From 4e0fb284dc39f83bbbc7c42f93dd3fd5fb0a1b7b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 8 Jan 2021 15:24:55 +0100
Subject: [PATCH] Added a reference to all the segments to which the node
 belongs. Added functions to create a copy of the node and update its
 references. Added a function to clean any invalid reference.

---
 include/opendrive_road_node.h |  32 ++--
 src/opendrive_road_node.cpp   | 322 ++++++++++++++++++++++++++++------
 2 files changed, 288 insertions(+), 66 deletions(-)

diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index e006cb5..f660aeb 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -2,18 +2,17 @@
 #define _OPENDRIVE_ROAD_NODE_H
 
 #include <vector>
+#include "opendrive_common.h"
 #include "opendrive_link.h"
 #include "opendrive_lane.h"
 #include "opendrive_road_segment.h"
 
-class COpendriveLane;
-class COpendriveRoadSegment;
-
 class COpendriveRoadNode
 {
   friend class COpendriveLane;
   friend class COpendriveRoadSegment;
   friend class COpendriveRoad;
+  friend class COpendriveLink;
   private:
     std::vector<COpendriveLink *> links;
     double resolution;
@@ -21,26 +20,33 @@ class COpendriveRoadNode
     TOpendriveWorldPoint start_point;
     std::vector<COpendriveLane *> lanes;
     std::vector<COpendriveGeometry *> geometries;
-    COpendriveRoadSegment *parent_segment;
+    std::vector<COpendriveRoadSegment *>parents;
     unsigned int index;
   protected:
     COpendriveRoadNode();
     COpendriveRoadNode(const COpendriveRoadNode &object);
     void load(const planView::geometry_type &geom_info,COpendriveLane *lane);
-    void add_link(COpendriveRoadNode *node,opendrive_mark_t mark);
+    void add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
+    void add_link(COpendriveLink *link);
+    void remove_link(COpendriveLink *link);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_index(unsigned int index);
-    void set_parent_segment(COpendriveRoadSegment *segment);
+    void set_start_point(TOpendriveWorldPoint &start);
+    void add_parent_segment(COpendriveRoadSegment *segment);
     void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane);
-    void update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs);
-    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
-    void update_references(std::map<COpendriveLane *,COpendriveLane *> refs);
+    bool updated(node_up_ref_t &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,TOpendriveWorldPoint *start=NULL);
+    COpendriveRoadNode *update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
+    COpendriveLink *get_link_with(COpendriveRoadNode *end);
   public:
     double get_resolution(void) const;
     double get_scale_factor(void) const;
     unsigned int get_index(void) const;
-    const COpendriveRoadSegment& get_parent_segment(void) const;
+    unsigned int get_num_parent_segments(void) const;
+    const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
     TOpendriveWorldPoint get_start_pose(void) const;
     unsigned int get_num_links(void) const;
     const COpendriveLink &get_link(unsigned int index) const;
@@ -49,9 +55,9 @@ class COpendriveRoadNode
     const COpendriveLane &get_lane(unsigned int index) const;
     unsigned int get_num_geometries(void) const;
     const COpendriveGeometry &get_geometry(unsigned int index) const;
-    unsigned int get_closest_link(double x, double y,double heading,double angle_tol=0.1) const;
-    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
-    void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
+    unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
+    double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
+    void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
     ~COpendriveRoadNode();
 };
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 31c2128..edbf921 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -13,7 +13,7 @@ COpendriveRoadNode::COpendriveRoadNode()
   this->lanes.clear();
   this->geometries.clear();
   this->index=-1;
-  this->parent_segment=NULL;
+  this->parents.clear();
 }
 
 COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
@@ -25,9 +25,7 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
   this->start_point.x=object.start_point.x;
   this->start_point.y=object.start_point.y;
   this->start_point.heading=object.start_point.heading;
-  this->lanes.resize(object.lanes.size());
-  for(unsigned int i=0;i<object.lanes.size();i++)
-    this->lanes[i]=object.lanes[i];
+  this->lanes=object.lanes;
   this->geometries.resize(object.geometries.size());
   for(unsigned int i=0;i<object.geometries.size();i++)
     this->geometries[i]=object.geometries[i]->clone();
@@ -38,10 +36,10 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
     this->links.push_back(new_link);
   }
   this->index=object.index;
-  this->parent_segment=object.parent_segment;
+  this->parents=object.parents;
 }
 
-void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
+void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
 {
   TOpendriveWorldPoint lane_end,node_start;
   double start_curvature,end_curvature,length;
@@ -57,6 +55,8 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
   new_link->set_resolution(this->resolution);
   new_link->set_scale_factor(this->scale_factor);
   new_link->set_road_mark(mark);
+  new_link->set_parent_segment(segment);
+  new_link->set_parent_lane(lane);
   // get the curvature
   node_start=node->get_start_pose();
   for(unsigned int i=0;i<this->lanes.size();i++)
@@ -89,6 +89,32 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
   node->links.push_back(new_link);
 }
 
+void COpendriveRoadNode::add_link(COpendriveLink *link)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+    if(this->links[i]==link)
+      return;
+
+  this->links.push_back(link);
+}
+
+void COpendriveRoadNode::remove_link(COpendriveLink *link)
+{
+  std::vector<COpendriveLink *>::iterator it;
+
+  for(it=this->links.begin();it!=this->links.end();)
+  {
+    if((*it)==link)
+    {
+      delete (*it);
+      it=this->links.erase(it);
+      break;
+    }
+    else
+      it++;
+  }
+}
+
 void COpendriveRoadNode::set_resolution(double res)
 {
   this->resolution=res;
@@ -110,9 +136,18 @@ void COpendriveRoadNode::set_index(unsigned int index)
   this->index=index;
 }
 
-void COpendriveRoadNode::set_parent_segment(COpendriveRoadSegment *segment)
+void COpendriveRoadNode::set_start_point(TOpendriveWorldPoint &start)
+{
+  this->start_point=start;
+}
+
+void COpendriveRoadNode::add_parent_segment(COpendriveRoadSegment *segment)
 {
-  this->parent_segment=segment;
+  for(unsigned int i=0;i<this->parents.size();i++)
+    if(this->parents[i]==segment)
+      return;
+
+  this->parents.push_back(segment);
 }
 
 void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane)
@@ -194,21 +229,198 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
   this->lanes.push_back(lane);
 }
 
-void COpendriveRoadNode::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
+bool COpendriveRoadNode::updated(node_up_ref_t &refs)
 {
-  for(unsigned int i=0;i<this->links.size();i++)
-    this->links[i]->update_references(refs);
+  node_up_ref_t::iterator updated_it;
+
+  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
+    if(updated_it->second==this)
+      return true;
+
+  return false;
 }
 
-void COpendriveRoadNode::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+void COpendriveRoadNode::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
 {
-  this->parent_segment=refs[this->parent_segment];
+  std::vector<COpendriveRoadSegment *>::iterator seg_it;
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveLink *>::iterator link_it;
+
+  if(this->updated(node_refs))
+  {
+    for(link_it=this->links.begin();link_it!=this->links.end();link_it++)
+      (*link_it)->update_references(segment_refs,lane_refs,node_refs);
+    for(seg_it=this->parents.begin();seg_it!=this->parents.end();seg_it++)
+      if(segment_refs.find(*seg_it)!=segment_refs.end())
+        (*seg_it)=segment_refs[*seg_it];
+    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
+      if(lane_refs.find(*lane_it)!=lane_refs.end())
+        (*lane_it)=lane_refs[*lane_it];
+  }
 }
 
-void COpendriveRoadNode::update_references(std::map<COpendriveLane *,COpendriveLane *> refs)
+void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
 {
-  for(unsigned int i=0;i<this->lanes.size();i++)
-    this->lanes[i]=refs[this->lanes[i]];
+  std::vector<COpendriveRoadSegment *>::iterator seg_it;
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveLink *>::iterator link_it;
+  unsigned int index=0;
+
+  if(this->updated(node_refs))
+  {
+    for(link_it=this->links.begin();link_it!=this->links.end();)
+    {
+      if((*link_it)->clean_references(node_refs))
+        link_it=this->links.erase(link_it);
+      else
+        link_it++;
+    }
+    for(seg_it=this->parents.begin();seg_it!=this->parents.end();)
+    {
+      if(segment_refs.find(*seg_it)!=segment_refs.end())
+      {
+        (*seg_it)=segment_refs[*seg_it];
+        seg_it++;
+      }
+      else if(!(*seg_it)->updated(segment_refs))
+        seg_it=this->parents.erase(seg_it);
+      else
+        seg_it++;
+    }
+    for(index=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();index++)
+    {
+      if(lane_refs.find(*lane_it)!=lane_refs.end())
+      {
+        (*lane_it)=lane_refs[*lane_it];
+        lane_it++;
+      }
+      else if(!(*lane_it)->updated(lane_refs))
+      {
+        lane_it=this->lanes.erase(lane_it);
+        this->geometries.erase(this->geometries.begin()+index);
+      }
+      else
+        lane_it++;
+    }
+  }
+}
+
+void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start)
+{
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveLink *>::iterator link_it;
+  unsigned int i;
+  double length;
+
+  if(start==NULL)
+    return;
+  // remove the references to all lanes and segments except for lane
+  for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
+  {
+    if(*lane_it!=lane)
+    {
+      lane_it=this->lanes.erase(lane_it);
+      this->geometries.erase(this->geometries.begin()+i);
+      this->parents.erase(this->parents.begin()+i);
+    }
+    else
+      lane_it++;
+  }
+  length=this->get_closest_point(*start,this->start_point,3.14159);
+  // update geometry 
+  for(unsigned int i=0;i<this->geometries.size();i++)
+  {
+    if(lane->get_id()<0)
+    {
+      this->geometries[i]->pose.x=this->start_point.x*this->scale_factor;
+      this->geometries[i]->pose.y=this->start_point.y*this->scale_factor;
+      this->geometries[i]->pose.heading=this->start_point.heading;
+    }
+    this->geometries[i]->max_s-=length*this->scale_factor;
+  }
+  // update the links
+  for(link_it=this->links.begin();link_it!=this->links.end();)
+  {
+    if((*link_it)->next==this)
+    {
+      delete *link_it;
+      link_it=this->links.erase(link_it); 
+    }
+    else
+    {
+      (*link_it)->update_start_pose(start);
+      link_it++;
+    }
+  }
+}
+
+COpendriveRoadNode *COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end)
+{
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveLink *>::iterator link_it;
+  COpendriveRoadNode *new_node=NULL;
+  TOpendriveWorldPoint end_point;
+  unsigned int i;
+  double length;
+
+  if(end==NULL)
+    return NULL;
+  // remove the references to all lanes and segments except for lane
+  for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
+  { 
+    if(*lane_it!=lane)
+    {
+      lane_it=this->lanes.erase(lane_it);
+      this->geometries.erase(this->geometries.begin()+i);
+      this->parents.erase(this->parents.begin()+i);
+    }
+    else
+      lane_it++;
+  }
+  length=this->get_closest_point(*end,end_point,3.14159);
+  // update geometry 
+  for(unsigned int i=0;i<this->geometries.size();i++)
+    this->geometries[i]->max_s=length*this->scale_factor;
+  // update the links
+  for(i=0,link_it=this->links.begin();link_it!=this->links.end();i++)
+  {
+    if((*link_it)->prev==this)
+    {
+      if((*link_it)->lane==lane)
+      {
+        // create a dummy node
+        new_node=new COpendriveRoadNode();
+        new_node->set_resolution(this->resolution);
+        new_node->set_scale_factor(this->scale_factor);
+        new_node->set_start_point(end_point);
+        (*link_it)->set_next(new_node);
+        new_node->add_link(*link_it);
+        // update the end position of the link
+        (*link_it)->update_end_pose(end);
+        link_it++;
+      }
+      else
+      {
+        delete *link_it;
+        link_it=this->links.erase(link_it);
+      }
+    }
+    else
+      link_it++;
+  }
+
+  return new_node;
+}
+
+COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *end)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+  {
+    if(this->links[i]->prev==end || this->links[i]->next==end)
+      return this->links[i];
+  }
+
+  return NULL;
 }
 
 double COpendriveRoadNode::get_resolution(void) const
@@ -226,9 +438,17 @@ unsigned int COpendriveRoadNode::get_index(void) const
  return this->index;
 }
 
-const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(void) const
+unsigned int COpendriveRoadNode::get_num_parent_segments(void) const
+{
+  return this->parents.size();
+}
+
+const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(unsigned int index) const
 {
-  return *this->parent_segment;
+  if(index>=0 && index<this->parents.size())
+    return *this->parents[index];
+  else
+    throw CException(_HERE_,"Invalid parent index");
 }
 
 TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
@@ -286,26 +506,22 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
     throw CException(_HERE_,"Invalid geometry index");
 }
 
-unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double heading,double angle_tol)const 
+unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPoint &point,double angle_tol)const 
 {
   double dist,min_dist=std::numeric_limits<double>::max();
-  TOpendriveWorldPoint target;
-  unsigned int closest_index;
+  unsigned int closest_index=-1;
   double angle;
-  TPoint point;
+  TPoint spline_point;
 
-  target.x=x;
-  target.y=y;
-  target.heading=heading;
   for(unsigned int i=0;i<this->links.size();i++)
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      this->links[i]->find_closest_world_point(target,point);
-      angle=diff_angle(normalize_angle(heading),point.heading);
+      this->links[i]->find_closest_world_point(point,spline_point);
+      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
         if(dist<min_dist)
         {
           min_dist=dist;
@@ -318,32 +534,31 @@ unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double head
   return closest_index;
 }
 
-double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) const
+double COpendriveRoadNode::get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol) const
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
-  double length,closest_length;
-  TOpendriveWorldPoint target;
-  TPoint point;
+  double length,closest_length=std::numeric_limits<double>::max();
+  TPoint spline_point;
 
-  target.x=x;
-  target.y=y;
-  target.heading=heading;
+  closest_point.x=std::numeric_limits<double>::max();
+  closest_point.y=std::numeric_limits<double>::max();
+  closest_point.heading=std::numeric_limits<double>::max();
   for(unsigned int i=0;i<this->links.size();i++)
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_point(target,point);
-      angle=diff_angle(normalize_angle(heading),point.heading);
+      length=this->links[i]->find_closest_world_point(point,spline_point);
+      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
         if(dist<min_dist)
         {
           min_dist=dist;
-          closest_point.x=point.x;
-          closest_point.y=point.y;
-          closest_point.heading=point.heading;
+          closest_point.x=spline_point.x;
+          closest_point.y=spline_point.y;
+          closest_point.heading=spline_point.heading;
           closest_length=length;
         }
       }
@@ -353,33 +568,30 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
   return closest_length;
 }
 
-void COpendriveRoadNode::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+void COpendriveRoadNode::get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
 {
   double dist;
   double angle;
   double length;
-  TOpendriveWorldPoint target,new_point;
-  TPoint point;
+  TOpendriveWorldPoint new_point;
+  TPoint spline_point;
 
-  target.x=x;
-  target.y=y;
-  target.heading=heading;
   closest_points.clear();
   closest_lengths.clear();
   for(unsigned int i=0;i<this->links.size();i++)
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_point(target,point);
-      angle=diff_angle(normalize_angle(heading),point.heading);
+      length=this->links[i]->find_closest_world_point(point,spline_point);
+      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
         if(dist<=dist_tol)
         {
-          new_point.x=point.x;
-          new_point.y=point.y;
-          new_point.heading=point.heading;
+          new_point.x=spline_point.x;
+          new_point.y=spline_point.y;
+          new_point.heading=spline_point.heading;
           closest_points.push_back(new_point);
           closest_lengths.push_back(length);
         }
@@ -392,6 +604,9 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
   out << "    Node: " << node.get_index() << std::endl;
   out << "      start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl;
+  out << "      Number of parent segments: " << node.get_num_parent_segments() << std::endl;
+  for(unsigned int i=0;i<node.get_num_parent_segments();i++)
+    out << "        " << node.get_parent_segment(i).get_name() << std::endl; 
   out << "      Number of parent lanes: " << node.lanes.size() << std::endl;
   for(unsigned int i=0;i<node.lanes.size();i++)
   {
@@ -430,6 +645,7 @@ COpendriveRoadNode::~COpendriveRoadNode()
       this->links[i]=NULL;
     }
   }
+  this->parents.clear();
   this->links.clear();
 }
 
-- 
GitLab