Added some functions:

* to get the pose at a given distance in the link.
* to get the curvature at a given distance in the link.
The length is no longer used to generate the spline. It is internally computed.
The curvature is provided when updating the start or end positions.

include/opendrive_link.h

@@ -33,11 +33,11 @@ class COpendriveLink
     void set_parent_lane(COpendriveLane *lane);
     void set_resolution(double res);
     void set_scale_factor(double scale);
-    void generate(double start_curvature,double end_curvature,double length,bool lane);
+    void generate(double start_curvature,double end_curvature);
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lanei_refs,node_up_ref_t &node_refs);
     bool clean_references(node_up_ref_t &refs);
-    TPoint update_start_pose(TOpendriveWorldPose *start=NULL);
-    TPoint update_end_pose(TOpendriveWorldPose *end=NULL);
+    void update_start_pose(TOpendriveWorldPose &start,double curvature);
+    void update_end_pose(TOpendriveWorldPose &end,double curvature);
   public:
     const COpendriveRoadNode &get_prev(void) const;
     const COpendriveRoadNode &get_next(void) const;
@@ -50,6 +50,8 @@ class COpendriveLink
     void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
     void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
     double get_length(void) const;
+    TOpendriveWorldPose get_pose_at(double length);
+    double get_curvature_at(double length);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
     ~COpendriveLink();
 };

src/opendrive_link.cpp

 #include "opendrive_link.h"
 #include "opendrive_road_node.h"
+#include "exceptions.h"
 
 COpendriveLink::COpendriveLink()
 {
@@ -77,13 +78,10 @@ void COpendriveLink::set_scale_factor(double scale)
     this->spline->generate(this->resolution);
   }
 
-
   this->scale_factor=scale;
-
-
 }
 
-void COpendriveLink::generate(double start_curvature,double end_curvature,double length,bool lane)
+void COpendriveLink::generate(double start_curvature,double end_curvature)
 {
   TOpendriveWorldPose node_start,node_end;
   TPoint start,end;
@@ -99,7 +97,7 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
   end.heading=node_end.heading;
   end.curvature=end_curvature;
   this->spline=new CG2Spline(start,end);
-  this->spline->generate(this->resolution,length);
+  this->spline->generate(this->resolution);
 }
 
 void COpendriveLink::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
@@ -128,39 +126,34 @@ bool COpendriveLink::clean_references(node_up_ref_t &refs)
   return false;
 }
 
-TPoint COpendriveLink::update_start_pose(TOpendriveWorldPose *start)
+void COpendriveLink::update_start_pose(TOpendriveWorldPose &start,double curvature)
 {
-  TPoint start_pose={0};
-  double length;
+  TPoint start_pose;
 
-  if(start==NULL)
-    return start_pose;
   if(this->spline!=NULL)
   {
-    length=this->spline->find_closest_point(start->x,start->y,start_pose);
-    length=this->spline->get_length()-length;
+    start_pose.x=start.x;
+    start_pose.y=start.y;
+    start_pose.heading=start.heading;
+    start_pose.curvature=curvature;
     this->spline->set_start_point(start_pose);
-    this->spline->generate(this->resolution,length);
+    this->spline->generate(this->resolution);
   }
-
-  return start_pose;
 }
 
-TPoint COpendriveLink::update_end_pose(TOpendriveWorldPose *end)
+void COpendriveLink::update_end_pose(TOpendriveWorldPose &end,double curvature)
 {
-  TPoint end_pose={0};
-  double length;
+  TPoint end_pose;
 
-  if(end==NULL)
-    return end_pose;
   if(this->spline!=NULL)
   {
-    length=this->spline->find_closest_point(end->x,end->y,end_pose);
+    end_pose.x=end.x;
+    end_pose.y=end.y;
+    end_pose.heading=end.heading;
+    end_pose.curvature=curvature;
     this->spline->set_end_point(end_pose);
-    this->spline->generate(this->resolution,length);
+    this->spline->generate(this->resolution);
   }
-
-  return end_pose;
 }
 
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const
@@ -185,7 +178,15 @@ const COpendriveRoadSegment &COpendriveLink::get_parent_segment(void) const
 
 const COpendriveLane &COpendriveLink::get_parent_lane(void) const
 {
-  return *this->lane;
+  std::stringstream error;
+
+  if(this->lane!=NULL)
+    return *this->lane;
+  else
+  {
+    error << "Link from node " << this->prev->get_index() << " to node " << this->next->get_index() << " has no parent lane";
+    throw CException(_HERE_,error.str());
+  }
 }
 
 double COpendriveLink::get_resolution(void) const
@@ -256,11 +257,44 @@ double COpendriveLink::get_length(void) const
     return 0.0;
 }
 
+TOpendriveWorldPose COpendriveLink::get_pose_at(double length)
+{
+  TPoint spline_pose;
+  TOpendriveWorldPose world_pose={0};
+
+  if(this->spline!=NULL)
+  {
+    spline_pose=this->spline->evaluate(length);
+    world_pose.x=spline_pose.x;
+    world_pose.y=spline_pose.y;
+    world_pose.heading=spline_pose.heading;
+  }
+
+  return world_pose;
+}
+
+double COpendriveLink::get_curvature_at(double length)
+{
+  TPoint spline_pose;
+  double curvature=0.0;
+
+  if(this->spline!=NULL)
+  {
+    spline_pose=this->spline->evaluate(length);
+    curvature=spline_pose.curvature;
+  }
+
+  return curvature;
+}
+
 std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
 {
   out << "        Previous node: " << link.get_prev().get_index() << std::endl;
   out << "        Next node: " << link.get_next().get_index() << std::endl;
-  out << "        Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl;
+  if(link.lane!=NULL)
+    out << "        Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl;
+  else
+    out << "        Parent segment: " << link.get_parent_segment().get_name() << " (no lane)" << std::endl;
   out << "        Road mark: ";
   switch(link.get_road_mark())
   {