Skip to content
Snippets Groups Projects
Commit da235f86 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added some functions:

  * A function to get the closes node to a point.
  * A function to get the set of closest points to a point.
parent 19d30ab3
No related branches found
No related tags found
No related merge requests found
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
#define _OPENDRIVE_ROAD_H #define _OPENDRIVE_ROAD_H
#ifdef _HAVE_XSD #ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx" #include "xml/OpenDRIVE_1.4H.hxx"
#endif #endif
#include "opendrive_road_segment.h" #include "opendrive_road_segment.h"
...@@ -42,7 +42,9 @@ class COpendriveRoad ...@@ -42,7 +42,9 @@ class COpendriveRoad
unsigned int get_num_nodes(void) const; unsigned int get_num_nodes(void) const;
const COpendriveRoadNode& get_node(unsigned int index) const; const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index); const COpendriveRoadSegment& operator[](std::size_t index);
unsigned int get_closest_node(double x, double y,double heading,double angle_tol=0.1);
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1); double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
void operator=(const COpendriveRoad& object); void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road); friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad(); ~COpendriveRoad();
......
...@@ -316,10 +316,30 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index) ...@@ -316,10 +316,30 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
throw CException(_HERE_,"Invalid segment index"); throw CException(_HERE_,"Invalid segment index");
} }
unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPoint point;
unsigned int closest_index;
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
if(dist<min_dist)
{
min_dist=dist;
closest_index=i;
}
}
return closest_index;
}
double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
{ {
double dist,min_dist=std::numeric_limits<double>::max(); double dist,min_dist=std::numeric_limits<double>::max();
double angle;
TOpendriveWorldPoint point; TOpendriveWorldPoint point;
double length,closest_length; double length,closest_length;
...@@ -327,22 +347,35 @@ double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpen ...@@ -327,22 +347,35 @@ double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpen
{ {
length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol); length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0)); dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
std::cout << point.x << "," << point.y << "," << point.heading << "," << length << "," << dist << std::endl; if(dist<min_dist)
angle=diff_angle(normalize_angle(heading),point.heading);
if(fabs(angle)<angle_tol)
{ {
if(dist<min_dist) min_dist=dist;
{ closest_point=point;
min_dist=dist; closest_length=length;
closest_point=point;
closest_length=length;
}
} }
} }
return closest_length; return closest_length;
} }
void COpendriveRoad::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
{
std::vector<TOpendriveWorldPoint> points;
std::vector<double> lengths;
closest_points.clear();
closest_lengths.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_points(x,y,heading,points,lengths,dist_tol,angle_tol);
for(unsigned int j=0;j<points.size();i++)
{
closest_points.push_back(points[i]);
closest_lengths.push_back(lengths[i]);
}
}
}
void COpendriveRoad::operator=(const COpendriveRoad& object) void COpendriveRoad::operator=(const COpendriveRoad& object)
{ {
COpendriveRoadSegment *segment; COpendriveRoadSegment *segment;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment