Added a reference to the segment and lane to which the link belongs.

Added functions to update the start and end positions of the link.
Used a single update_reference() function with all the new segment,lane and node references.

include/opendrive_link.h

 #ifndef _OPENDRIVE_LINK_H
 #define _OPENDRIVE_LINK_H
 
+#include "opendrive_common.h"
 #include "g2_spline.h"
 #include "opendrive_line.h"
 #include "opendrive_spiral.h"
 #include "opendrive_arc.h"
 #include "opendrive_param_poly3.h"
-
-class COpendriveRoadNode;
+#include "opendrive_road_node.h"
 
 class COpendriveLink
 {
+  friend class COpendriveRoadSegment;
   friend class COpendriveRoadNode;
+  friend class COpendriveRoad;
   private:
     COpendriveRoadNode *prev;
     COpendriveRoadNode *next;
+    COpendriveRoadSegment *segment;
+    COpendriveLane *lane;
     CG2Spline *spline;
     opendrive_mark_t mark;
     double resolution;
@@ -25,14 +29,21 @@ class COpendriveLink
     void set_prev(COpendriveRoadNode *node);
     void set_next(COpendriveRoadNode *node);
     void set_road_mark(opendrive_mark_t mark);
+    void set_parent_segment(COpendriveRoadSegment *segment);
+    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 update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs);
+    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);
+    void update_start_pose(TOpendriveWorldPoint *start=NULL);
+    void update_end_pose(TOpendriveWorldPoint *end=NULL);
   public:
     const COpendriveRoadNode &get_prev(void) const;
     const COpendriveRoadNode &get_next(void) const;
     opendrive_mark_t get_road_mark(void) const;
+    const COpendriveRoadSegment &get_parent_segment(void) const;
+    const COpendriveLane &get_parent_lane(void) const;
     double get_resolution(void) const;
     double get_scale_factor(void) const;
     double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);

src/opendrive_link.cpp

@@ -5,6 +5,8 @@ COpendriveLink::COpendriveLink()
 {
   this->prev=NULL;
   this->next=NULL;
+  this->segment=NULL;
+  this->lane=NULL;
   this->spline=NULL;
   this->mark=OD_MARK_NONE;
   this->resolution=DEFAULT_RESOLUTION;
@@ -15,6 +17,8 @@ COpendriveLink::COpendriveLink(const COpendriveLink &object)
 {
   this->prev=object.prev;
   this->next=object.next;
+  this->segment=object.segment;
+  this->lane=object.lane;
   this->spline=new CG2Spline(*object.spline);
   this->mark=object.mark;
   this->resolution=object.resolution;
@@ -36,6 +40,16 @@ void COpendriveLink::set_road_mark(opendrive_mark_t mark)
   this->mark=mark;
 }
 
+void COpendriveLink::set_parent_segment(COpendriveRoadSegment *segment)
+{
+  this->segment=segment;
+}
+
+void COpendriveLink::set_parent_lane(COpendriveLane *lane)
+{
+  this->lane=lane;
+}
+
 void COpendriveLink::set_resolution(double res)
 {
   this->resolution=res;
@@ -65,10 +79,55 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
   this->spline->generate(this->resolution,length);
 }
 
-void COpendriveLink::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
+void COpendriveLink::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
+{
+  if(node_refs.find(this->prev)!=node_refs.end())
+    this->prev=node_refs[this->prev];
+  if(node_refs.find(this->next)!=node_refs.end())
+    this->next=node_refs[this->next];
+  if(segment_refs.find(this->segment)!=segment_refs.end())
+    this->segment=segment_refs[this->segment];
+  if(lane_refs.find(this->lane)!=lane_refs.end())
+    this->lane=lane_refs[this->lane];
+}
+
+bool COpendriveLink::clean_references(node_up_ref_t &refs)
+{
+  if(refs.find(this->prev)!=refs.end())
+    this->prev=refs[this->prev];
+  else if(!this->prev->updated(refs))
+    return true;
+  if(refs.find(this->next)!=refs.end())
+    this->next=refs[this->next];
+  else if(!this->next->updated(refs))
+    return true;
+
+  return false;
+}
+
+void COpendriveLink::update_start_pose(TOpendriveWorldPoint *start)
+{
+  TPoint spline_start;
+  double length;
+
+  if(start==NULL)
+    return;
+  length=this->find_closest_world_point(*start,spline_start);
+  length=this->spline->get_length()-length;
+  this->spline->set_start_point(spline_start);
+  this->spline->generate(this->resolution,length);
+}
+
+void COpendriveLink::update_end_pose(TOpendriveWorldPoint *end)
 {
-  this->prev=refs[this->prev];
-  this->next=refs[this->next];
+  TPoint spline_end;
+  double length;
+
+  if(end==NULL)
+    return;
+  length=this->find_closest_world_point(*end,spline_end);
+  this->spline->set_end_point(spline_end);
+  this->spline->generate(this->resolution,length);
 }
 
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const
@@ -86,6 +145,16 @@ opendrive_mark_t COpendriveLink::get_road_mark(void) const
   return this->mark;
 }
 
+const COpendriveRoadSegment &COpendriveLink::get_parent_segment(void) const
+{
+  return *this->segment;
+}
+
+const COpendriveLane &COpendriveLink::get_parent_lane(void) const
+{
+  return *this->lane;
+}
+
 double COpendriveLink::get_resolution(void) const
 {
   return this->resolution;
@@ -101,7 +170,10 @@ double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoi
   double length;
 
   if(this->spline!=NULL)
+  {
     length=this->spline->find_closest_point(world.x,world.y,point);
+    point.heading=normalize_angle(point.heading);
+  }
   else
     length=std::numeric_limits<double>::max();
 
@@ -160,8 +232,8 @@ double COpendriveLink::get_length(void) const
 
 std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
 {
-  out << "        Previous node: " << link.get_prev().get_index() << " of road " << link.get_prev().get_parent_segment().get_name() << std::endl;
-  out << "        Next node: " << link.get_next().get_index() << " of road " << link.get_next().get_parent_segment().get_name() << std::endl;
+  out << "        Previous node: " << link.get_prev().get_index() << std::endl;
+  out << "        Next node: " << link.get_next().get_index() << std::endl;
   out << "        Road mark: ";
   switch(link.get_road_mark())
   {
@@ -195,6 +267,8 @@ COpendriveLink::~COpendriveLink()
 {
   this->prev=NULL;
   this->next=NULL;
+  this->segment=NULL;
+  this->lane=NULL;
   if(this->spline!=NULL)
   {
     delete this->spline;