Added a clone() function instead of the copy constructor to be able to update the link references.

The get_closest*() functions have a new parameter to limit the search to the links belonging to a lane.
The parent information no longer includes the length of the link.

include/opendrive_road_node.h

@@ -13,7 +13,6 @@ typedef struct
   COpendriveRoadSegment *segment;
   double start_curvature;
   double end_curvature;
-  double length;
 }TOpendriveRoadNodeParent;
 
 class COpendriveRoadNode
@@ -31,7 +30,7 @@ class COpendriveRoadNode
     unsigned int index;
   protected:
     COpendriveRoadNode();
-    COpendriveRoadNode(const COpendriveRoadNode &object);
+    COpendriveRoadNode *clone(link_up_ref_t &new_link_ref);
     void free(void);
     bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
     void add_link(COpendriveLink *link);
@@ -43,15 +42,15 @@ class COpendriveRoadNode
     void set_scale_factor(double scale);
     void set_index(unsigned int index);
     void set_pose(TOpendriveWorldPose &start);
-    void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature);
+    void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature);
     TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
     TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
     bool updated(node_up_ref_t &refs);
     COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
-    void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start=NULL);
-    void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end=NULL);
+    void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
+    void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
   public:
     double get_resolution(void) const;
     double get_scale_factor(void) const;
@@ -64,9 +63,9 @@ class COpendriveRoadNode
     const COpendriveLink &get_link(unsigned int index) const;
     const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
     unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
-    double get_closest_distance(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
-    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
-    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
+    double get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes=false,double angle_tol=0.1) const;
+    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const;
+    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol=0.1) const;
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
     ~COpendriveRoadNode();
 };

src/opendrive_road_node.cpp

@@ -15,29 +15,37 @@ COpendriveRoadNode::COpendriveRoadNode()
   this->index=-1;
 }
 
-COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
+COpendriveRoadNode *COpendriveRoadNode::clone(link_up_ref_t &new_link_ref)
 {
-  COpendriveLink *new_link;
+  COpendriveRoadNode *new_node;
 
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  this->pose=object.pose;
-  this->parents=object.parents;
-  this->links.clear();
-  for(unsigned int i=0;i<object.links.size();i++)
+  new_node=new COpendriveRoadNode();
+  new_node->resolution=this->resolution;
+  new_node->scale_factor=this->scale_factor;
+  new_node->pose=this->pose;
+  new_node->parents=this->parents;
+  new_node->links.resize(this->links.size());
+  for(unsigned int i=0;i<this->links.size();i++)
   {
-    new_link=new COpendriveLink(*object.links[i]);
-    this->links.push_back(new_link);
+    if(new_link_ref.find(this->links[i])!=new_link_ref.end())
+      new_node->links[i]=new_link_ref[this->links[i]];
+    else
+    {
+      new_node->links[i]=new COpendriveLink(*this->links[i]);
+      new_link_ref[this->links[i]]=new_node->links[i];
+    }
   }
-  this->index=object.index;
+  new_node->index=this->index;
+
+  return new_node;
 }
 
 bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
 {
+  TOpendriveRoadNodeParent *parent;
   TOpendriveWorldPose node_pose;
-  double start_curvature,end_curvature,length;
+  double start_curvature,end_curvature;
   COpendriveLink *new_link;
-  bool lane_found=false;
 
   if(this->has_link_with(node) || node->has_link_with(this))
     return false;
@@ -51,24 +59,26 @@ bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
   new_link->set_parent_lane(lane);
   // get the curvature
   node_pose=node->get_pose();
-  for(unsigned int i=0;i<this->parents.size();i++)
+  parent=this->get_parent_with_lane(lane);
+  if(parent!=NULL)
   {
-    if(this->parents[i].lane==lane && this->parents[i].segment==segment)
-    {
-      start_curvature=this->parents[i].start_curvature;
-      end_curvature=this->parents[i].end_curvature;
-      length=this->parents[i].length;
-      lane_found=true;
-      break;
-    }
+    start_curvature=parent->start_curvature;
+    end_curvature=parent->end_curvature;
   }
-  if(!lane_found)
+  else
   {
-    start_curvature=0.0;
-    end_curvature=0.0;
-    length=sqrt(pow(this->pose.x-node_pose.x,2.0)+pow(this->pose.y-node_pose.y,2.0));
+    if(this->get_num_parents()==1)
+    {
+      start_curvature=this->parents[0].start_curvature;
+      end_curvature=this->parents[0].end_curvature;
+    }
+    else
+    {
+      start_curvature=0.0;
+      end_curvature=0.0;
+    }
   }
-  new_link->generate(start_curvature,end_curvature,length,lane_found);
+  new_link->generate(start_curvature,end_curvature);
   this->add_link(new_link);
   node->add_link(new_link);
 
@@ -89,7 +99,8 @@ void COpendriveRoadNode::remove_link(COpendriveLink *link)
   {
     if((*it)==link)
     {
-      delete (*it);
+      if((*it)->prev==this)
+        delete (*it);
       it=this->links.erase(it);
       break;
     }
@@ -146,7 +157,6 @@ void COpendriveRoadNode::set_scale_factor(double scale)
   {
     this->parents[i].start_curvature*=scale/this->scale_factor;
     this->parents[i].end_curvature*=scale/this->scale_factor;
-    this->parents[i].length*=this->scale_factor/scale;
   }
 }
 
@@ -160,7 +170,7 @@ void COpendriveRoadNode::set_pose(TOpendriveWorldPose &start)
   this->pose=start;
 }
 
-void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature)
+void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature)
 {
   TOpendriveRoadNodeParent new_parent;
 
@@ -171,7 +181,6 @@ void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *
   new_parent.segment=segment;
   new_parent.start_curvature=start_curvature;
   new_parent.end_curvature=end_curvature;
-  new_parent.length=length;
   this->parents.push_back(new_parent);
 }
 
@@ -272,15 +281,24 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
   }
 }
 
-void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start)
+void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance)
 {
   std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
   std::vector<COpendriveLink *>::iterator link_it;
-  double new_length;
-  TPoint new_pose;
+  TOpendriveRoadNodeParent *parent;
 
-  if(start==NULL)
-    return;
+  this->pose=start;
+  // remove the references to all lanes and segments except for lane
+  for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
+  {
+    if(parent_it->lane!=lane)
+      parent_it=this->parents.erase(parent_it);
+    else
+    {
+      parent=&(*parent_it);
+      parent_it++;
+    }
+  }
   // update the links
   for(link_it=this->links.begin();link_it!=this->links.end();)
   {
@@ -291,35 +309,35 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
     }
     else
     {
-      new_pose=(*link_it)->update_start_pose(start);
-      new_length=(*link_it)->get_length();
+      if((*link_it)->lane==lane)
+      {
+        parent->start_curvature=(*link_it)->get_curvature_at(distance);
+        (*link_it)->update_start_pose(start,parent->start_curvature);
+      }
+      else
+        (*link_it)->update_start_pose(start,0.0);
       link_it++;
     }
   }
+}
+
+void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance)
+{
+  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
+  std::vector<COpendriveLink *>::iterator link_it;
+  TOpendriveRoadNodeParent *parent;
+
   // remove the references to all lanes and segments except for lane
   for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
-  {
+  { 
     if(parent_it->lane!=lane)
       parent_it=this->parents.erase(parent_it);
     else
     {
+      parent=&(*parent_it);
       parent_it++;
-      parent_it->length=new_length;
-      parent_it->start_curvature=new_pose.curvature;
     }
   }
-
-}
-
-void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end)
-{
-  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
-  std::vector<COpendriveLink *>::iterator link_it;
-  double new_length;
-  TPoint new_pose;
-
-  if(end==NULL)
-    return;
   // update the links
   for(link_it=this->links.begin();link_it!=this->links.end();)
   {
@@ -329,23 +347,7 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
       link_it=this->links.erase(link_it);
     }
     else
-    {
-      new_pose=(*link_it)->update_end_pose(end);
-      new_length=(*link_it)->get_length();
       link_it++;
-    }
-  }
-  // remove the references to all lanes and segments except for lane
-  for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
-  { 
-    if(parent_it->lane!=lane)
-      parent_it=this->parents.erase(parent_it);
-    else
-    {
-      parent_it++;
-      parent_it->length=new_length;
-      parent_it->end_curvature=new_pose.curvature;
-    }
   }
 }
 
@@ -460,7 +462,7 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
   return closest_index;
 }
 
-double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double angle_tol)const 
+double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes,double angle_tol)const 
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
@@ -470,13 +472,16 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      this->links[i]->find_closest_world_pose(pose,closest_pose);
-      angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
-      if(fabs(angle)<angle_tol)
+      if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
       {
-        dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-        if(dist<min_dist)
-          min_dist=dist;
+        this->links[i]->find_closest_world_pose(pose,closest_pose);
+        angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+        if(fabs(angle)<angle_tol)
+        {
+          dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+          if(dist<min_dist)
+            min_dist=dist;
+        }
       }
     }
   }
@@ -484,7 +489,7 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double
   return min_dist;
 }
 
-double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
+double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes,double angle_tol) const
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
@@ -498,16 +503,19 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
-      angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
-      if(fabs(angle)<angle_tol)
+      if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
       {
-        dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
-        if(dist<min_dist)
+        length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
+        angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
+        if(fabs(angle)<angle_tol)
         {
-          min_dist=dist;
-          closest_length=length;
-          closest_pose=tmp_pose;
+          dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
+          if(dist<min_dist)
+          {
+            min_dist=dist;
+            closest_length=length;
+            closest_pose=tmp_pose;
+          }
         }
       }
     }
@@ -516,7 +524,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
   return closest_length;
 }
 
-void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol) const
 {
   double dist;
   double angle;
@@ -529,15 +537,18 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_pose(pose,closest_pose);
-      angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
-      if(fabs(angle)<angle_tol)
+      if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
       {
-        dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
-        if(dist<=dist_tol)
+        length=this->links[i]->find_closest_world_pose(pose,closest_pose);
+        angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+        if(fabs(angle)<angle_tol)
         {
-          closest_poses.push_back(closest_pose);
-          closest_lengths.push_back(length);
+          dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+          if(dist<=dist_tol)
+          {
+            closest_poses.push_back(closest_pose);
+            closest_lengths.push_back(length);
+          }
         }
       }
     }