Added a function to get the cosest point to a given one.

Found a bug (not yet solved): The contact point information is needed when linking junction roads.

include/opendrive_road.h

@@ -42,6 +42,7 @@ class COpendriveRoad
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode& get_node(unsigned int index) const;
     const COpendriveRoadSegment& operator[](std::size_t index);
+    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
     void operator=(const COpendriveRoad& object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
     ~COpendriveRoad();

src/opendrive_road.cpp

 #include "opendrive_road.h"
+#include "exceptions.h"
 #include <sys/types.h>
 #include <sys/stat.h>
-#include "exceptions.h"
+#include <math.h>
 
 COpendriveRoad::COpendriveRoad()
 {
@@ -61,6 +62,7 @@ COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
 void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
 {
   std::string predecessor_id,successor_id;
+  std::string predecessor_contact,successor_contact;
 
   for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
   {
@@ -72,12 +74,18 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
       if(road_it->lane_link().get().predecessor().present())// predecessor present
       {
         if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road
+        {
           predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get();
+          predecessor_contact=road_it->lane_link().get().predecessor().get().contactPoint().get();
+        }
       }
       if(road_it->lane_link().get().successor().present())// successor present
       {
         if(road_it->lane_link().get().successor().get().elementType().get()=="road")
+        {
           successor_id=road_it->lane_link().get().successor().get().elementId().get();
+          successor_contact=road_it->lane_link().get().successor().get().contactPoint().get();
+        }
       }
     }
     if(std::stoi(road_it->junction().get())==-1)// non junction road segments
@@ -293,6 +301,33 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
     throw CException(_HERE_,"Invalid segment index");
 }
 
+double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
+{
+  double dist,min_dist=std::numeric_limits<double>::max();
+  double angle,min_angle=std::numeric_limits<double>::max();
+  TOpendriveWorldPoint point;
+  double length,closest_length;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    length=this->nodes[i]->get_closest_point(x,y,heading,point);
+    angle=diff_angle(normalize_angle(heading),point.heading);
+    if(angle<min_angle)
+    {
+      dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+      if(dist<min_dist)
+      {
+        min_angle=angle;
+        min_dist=dist;
+        closest_point=point;
+        closest_length=length;
+      }
+    }
+  }
+
+  return closest_length;
+}
+
 void COpendriveRoad::operator=(const COpendriveRoad& object)
 {
   COpendriveRoadSegment *segment;