The parent element are only erased if neither the segment not the lane have been updated.

Improved the way to update the pose of the node.

include/opendrive_road_node.h

@@ -48,8 +48,8 @@ class COpendriveRoadNode
     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,link_up_ref_t &new_link_ref);
-    void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
-    void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
+    void update_start_pose(TOpendriveWorldPose &start,node_up_ref_t &node_refs,double distance);
+    void update_end_pose(TOpendriveWorldPose &end,node_up_ref_t &node_refs);
   public:
     /**
      * \brief Returns the resolution

src/opendrive_road_node.cpp

@@ -297,7 +297,7 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
     }
     for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
     {
-      if(!parent_it->segment->updated(segment_refs) || !parent_it->lane->updated(lane_refs))
+      if(!parent_it->segment->updated(segment_refs) && !parent_it->lane->updated(lane_refs))
         parent_it=this->parents.erase(parent_it);
       else
         parent_it++;
@@ -305,63 +305,35 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
 //  }
 }
 
-void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance)
+void COpendriveRoadNode::update_start_pose(TOpendriveWorldPose &start,node_up_ref_t &node_refs,double distance)
 {
-  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
   std::vector<COpendriveLink *>::iterator link_it;
-  TOpendriveRoadNodeParent *parent;
 
   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();)
   {
-    if((*link_it)->next==this)
+    if((*link_it)->next==this->get_original_node(node_refs))
     {
       delete *link_it;
       link_it=this->links.erase(link_it); 
     }
     else
     {
-      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)->update_start_pose(start,(*link_it)->get_curvature_at(distance));
       link_it++;
     }
   }
 }
 
-void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance)
+void COpendriveRoadNode::update_end_pose(TOpendriveWorldPose &end,node_up_ref_t &node_refs)
 {
-  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
   std::vector<COpendriveLink *>::iterator 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++;
-  }
   // update the links
   for(link_it=this->links.begin();link_it!=this->links.end();)
   {
-    if((*link_it)->prev==this)
+    if((*link_it)->prev==this->get_original_node(node_refs))
     {
       delete *link_it;
       link_it=this->links.erase(link_it);