Solved several bugs to use the correct curvature for the splines.

include/opendrive_link.h

@@ -36,7 +36,6 @@ class COpendriveLink
     double get_resolution(void) const;
     double get_scale_factor(void) const;
     double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
-    double find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point);
     double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
     double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
     void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;

include/opendrive_road_segment.h

@@ -31,6 +31,7 @@ class COpendriveRoadSegment
     std::vector<COpendriveRoadSegment *> connecting;
     std::string name;
     unsigned int id;
+    opendrive_mark_t center_mark;
   protected:
     COpendriveRoadSegment();
     COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref);

src/opendrive_lane.cpp

@@ -296,12 +296,10 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
   else
   {
     track_point.t=this->get_offset()+this->get_width()/2.0;
+    track_point.s=0.0;
     for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
       if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
-      {
-        track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
         this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
-      }
   }
   if(this->get_id()>0)
     world_point.heading=normalize_angle(world_point.heading+3.14159);

src/opendrive_link.cpp

@@ -61,7 +61,6 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
   end.y=node_end.y;
   end.heading=node_end.heading;
   end.curvature=end_curvature;
-//  std::cout << start.x << "," << start.y << "," << start.heading << "," << start_curvature << "," << end.x << "," << end.y << "," << end.heading << "," << end_curvature << std::endl;
   this->spline=new CG2Spline(start,end);
   this->spline->generate(this->resolution,length);
 }
@@ -99,12 +98,14 @@ double COpendriveLink::get_scale_factor(void) const
 
 double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
 {
+  double length;
 
-}
-
-double COpendriveLink::find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point)
-{
+  if(this->spline!=NULL)
+    length=this->spline->find_closest_point(world.x,world.y,point);
+  else
+    length=std::numeric_limits<double>::max();
 
+  return length;
 }
 
 double COpendriveLink::get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)

src/opendrive_road_node.cpp

@@ -61,7 +61,10 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
   node_start=node->get_start_pose();
   for(unsigned int i=0;i<this->lanes.size();i++)
   {
-    lane_end=this->lanes[i]->get_start_point();
+    if(this->lanes[i]->get_id()<0)
+      lane_end=this->lanes[i]->get_end_point();
+    else
+      lane_end=this->lanes[i]->get_start_point();
     if(fabs(lane_end.x-node_start.x)<this->resolution && fabs(lane_end.y-node_start.y)<this->resolution)
     {
       this->geometries[i]->get_curvature(start_curvature,end_curvature);

src/opendrive_road_segment.cpp

@@ -252,7 +252,6 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
 
 void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
 {
-  opendrive_mark_t center_mark;
   center::lane_type center_lane;
 
   if(lane_section.center().lane().present())
@@ -261,30 +260,30 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
     if(center_lane.roadMark().size()>1)
       throw CException(_HERE_,"Only one road mark supported for lane");
     else if(center_lane.roadMark().size()==0)
-      center_mark=OD_MARK_NONE;
+      this->center_mark=OD_MARK_NONE;
     else if(center_lane.roadMark().size()==1)
     {
       if(center_lane.roadMark().begin()->type1().present())
       {
         if(center_lane.roadMark().begin()->type1().get()=="none")
-          center_mark=OD_MARK_NONE;
+          this->center_mark=OD_MARK_NONE;
         else if(center_lane.roadMark().begin()->type1().get()=="solid")
-          center_mark=OD_MARK_SOLID;
+          this->center_mark=OD_MARK_SOLID;
         else if(center_lane.roadMark().begin()->type1().get()=="broken")
-          center_mark=OD_MARK_BROKEN;
+          this->center_mark=OD_MARK_BROKEN;
         else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
-          center_mark=OD_MARK_SOLID_SOLID;
+          this->center_mark=OD_MARK_SOLID_SOLID;
         else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
-          center_mark=OD_MARK_SOLID_BROKEN;
+          this->center_mark=OD_MARK_SOLID_BROKEN;
         else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
-         center_mark=OD_MARK_BROKEN_SOLID;
+          this->center_mark=OD_MARK_BROKEN_SOLID;
         else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
-          center_mark=OD_MARK_BROKEN_BROKEN;
+          this->center_mark=OD_MARK_BROKEN_BROKEN;
         else
-          center_mark=OD_MARK_NONE;
+          this->center_mark=OD_MARK_NONE;
       }
       else
-        center_mark=OD_MARK_NONE;
+        this->center_mark=OD_MARK_NONE;
     }
   }
   for(int i=-this->num_right_lanes;i<0;i++)
@@ -522,7 +521,22 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
 
 TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
 {
+  TOpendriveTrackPoint point;
+  TOpendriveLocalPoint local;
+
+  if(track.s<0.0)
+    throw CException(_HERE_,"Invalid track point");
+  point=track;
+  if(this->num_right_lanes>0)
+    local=this->lanes[-1]->transform_to_local_point(point);
+  else
+  {
+    point.s=this->lanes[1]->get_length()-track.s;
+    point.heading=normalize_angle(track.heading+3.14159);
+    local=this->lanes[1]->transform_to_local_point(point);
+  }
 
+  return local;
 }
 
 TOpendriveWorldPoint COpendriveRoadSegment::transform_to_world_point(const TOpendriveTrackPoint &track)