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.

include/opendrive_road.h

@@ -2,7 +2,7 @@
 #define _OPENDRIVE_ROAD_H
 
 #ifdef _HAVE_XSD
-#include "../src/xml/OpenDRIVE_1.4H.hxx"
+#include "xml/OpenDRIVE_1.4H.hxx"
 #endif
 
 #include "opendrive_road_segment.h"
@@ -42,7 +42,9 @@ class COpendriveRoad
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode& get_node(unsigned int index) const;
     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);
+    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);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
     ~COpendriveRoad();

src/opendrive_road.cpp

@@ -316,10 +316,30 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t 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 dist,min_dist=std::numeric_limits<double>::max();
-  double angle;
   TOpendriveWorldPoint point;
   double length,closest_length;
 
@@ -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);
     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;
-    angle=diff_angle(normalize_angle(heading),point.heading);
-    if(fabs(angle)<angle_tol)
+    if(dist<min_dist)
     {
-      if(dist<min_dist)
-      {
-        min_dist=dist;
-        closest_point=point;
-        closest_length=length;
-      }
+      min_dist=dist;
+      closest_point=point;
+      closest_length=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)
 {
   COpendriveRoadSegment *segment;