From 4dd90b07f5693ae3946ce905f0ddaaf0ed76e53b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 22 Dec 2020 17:03:56 +0100
Subject: [PATCH] Moved the constructor and default constructor to protected
 members. Completed the feature to copy the whole structure and update the
 references.

---
 include/opendrive_lane.h         |  7 ++---
 include/opendrive_link.h         |  6 ++--
 include/opendrive_road.h         |  4 +++
 include/opendrive_road_node.h    | 10 +++++--
 include/opendrive_road_segment.h |  8 ++---
 src/opendrive_lane.cpp           | 40 +++++++------------------
 src/opendrive_link.cpp           | 18 +++++------
 src/opendrive_road.cpp           | 27 +++++++++++------
 src/opendrive_road_node.cpp      | 51 +++++++++++---------------------
 src/opendrive_road_segment.cpp   | 51 +++++++-------------------------
 10 files changed, 83 insertions(+), 139 deletions(-)

diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index f64730e..8e461aa 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -27,6 +27,8 @@ class COpendriveLane
     double offset;
     int id;
   protected:
+    COpendriveLane();
+    COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref);
     void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
     void link_neightbour_lane(COpendriveLane *lane);
     void link_lane(COpendriveLane *lane,opendrive_mark_t mark);
@@ -34,10 +36,8 @@ class COpendriveLane
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_offset(double offset);
-    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
+    void update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs);
   public:
-    COpendriveLane();
-    COpendriveLane(const COpendriveLane &object);
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode &get_node(unsigned int index) const;
     const COpendriveRoadSegment &get_segment(void) const;
@@ -48,7 +48,6 @@ class COpendriveLane
     TOpendriveWorldPoint get_start_point(void);
     TOpendriveWorldPoint get_end_point(void);
     double get_length(void);
-    void operator=(const COpendriveLane &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();
 };
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index 9dafa58..fa6946e 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -20,15 +20,16 @@ class COpendriveLink
     double resolution;
     double scale_factor;    
   protected:
+    COpendriveLink();
+    COpendriveLink(const COpendriveLink &object);
     void set_prev(COpendriveRoadNode *node);
     void set_next(COpendriveRoadNode *node);
     void set_road_mark(opendrive_mark_t mark);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void generate(void);
+    void update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs);
   public:
-    COpendriveLink();
-    COpendriveLink(const COpendriveLink &object);
     const COpendriveRoadNode &get_prev(void) const;
     const COpendriveRoadNode &get_next(void) const;
     opendrive_mark_t get_road_mark(void) const;
@@ -39,7 +40,6 @@ class COpendriveLink
     double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
     double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
     double get_length(void) const;
-    void operator=(const COpendriveLink &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
     ~COpendriveLink();
 };
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index e2607d3..7d3ec7e 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -6,6 +6,10 @@
 #endif
 
 #include "opendrive_road_segment.h"
+#include "opendrive_road_node.h"
+
+class COpendriveRoadSegment;
+class COpendriveRoadNode;
 
 class COpendriveRoad
 {
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index fe55f25..f42ec44 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -4,6 +4,7 @@
 #include <vector>
 #include "opendrive_link.h"
 #include "opendrive_lane.h"
+#include "opendrive_road_segment.h"
 
 class COpendriveLane;
 class COpendriveRoadSegment;
@@ -12,6 +13,7 @@ class COpendriveRoadNode
 {
   friend class COpendriveLane;
   friend class COpendriveRoadSegment;
+  friend class COpendriveRoad;
   private:
     std::vector<COpendriveLink *> links;
     double resolution;
@@ -22,6 +24,8 @@ class COpendriveRoadNode
     COpendriveRoadSegment *parent_segment;
     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 set_resolution(double res);
@@ -29,9 +33,10 @@ class COpendriveRoadNode
     void set_index(unsigned int index);
     void set_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);
   public:
-    COpendriveRoadNode();
-    COpendriveRoadNode(const COpendriveRoadNode &object);
     double get_resolution(void) const;
     double get_scale_factor(void) const;
     unsigned int get_index(void) const;
@@ -43,7 +48,6 @@ 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;
-    void operator=(const COpendriveRoadNode &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
     ~COpendriveRoadNode();
 };
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 255b411..51f15de 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -8,6 +8,7 @@
 #include "opendrive_lane.h"
 #include "opendrive_signal.h"
 #include "opendrive_object.h"
+#include "opendrive_road.h"
 
 class COpendriveLane;
 class COpendriveRoad;
@@ -29,21 +30,21 @@ class COpendriveRoadSegment
     std::string name;
     unsigned int id;
   protected:
+    COpendriveRoadSegment();
+    COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref);
     void load(OpenDRIVE::road_type &road_info);
     void free(void);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_min_road_length(double length);
     void set_parent_road(COpendriveRoad *parent);
-    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
+    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> &segment_refs);
     void add_lanes(lanes::laneSection_type &lane_section);
     void add_nodes(OpenDRIVE::road_type &road_info);
     void link_neightbour_lanes(lanes::laneSection_type &lane_section);
     void link_segment(COpendriveRoadSegment &segment);
     void link_segment(COpendriveRoadSegment &segment,int from, int to);
   public:
-    COpendriveRoadSegment();
-    COpendriveRoadSegment(const COpendriveRoadSegment &object);
     std::string get_name(void) const;
     unsigned int get_id(void) const;
     unsigned int get_num_right_lanes(void) const;
@@ -56,7 +57,6 @@ class COpendriveRoadSegment
     const COpendriveObject &get_object(unsigned int index) const;
     unsigned int get_num_connecting(void) const;
     const COpendriveRoadSegment &get_connecting(unsigned int index) const;
-    void operator=(const COpendriveRoadSegment &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
     ~COpendriveRoadSegment();
 };
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 501628b..491758f 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -17,21 +17,16 @@ COpendriveLane::COpendriveLane()
   this->id=0;
 }
 
-COpendriveLane::COpendriveLane(const COpendriveLane &object)
+COpendriveLane::COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref)
 {
-  COpendriveRoadNode *node;
-
-  this->nodes.clear();
+  this->nodes.resize(object.nodes.size());
   for(unsigned int i=0;i<object.nodes.size();i++)
-  {
-    node=new COpendriveRoadNode(*object.nodes[i]);
-    this->nodes.push_back(node);
-  }
+    this->nodes[i]=node_refs[object.nodes[i]];
   this->left_lane=object.left_lane;
   this->left_mark=object.left_mark;
   this->right_lane=object.right_lane;
   this->right_mark=object.right_mark;
-  this->segment=object.segment;
+  this->segment=segment_ref;
   this->resolution=object.resolution;
   this->scale_factor=object.scale_factor;
   this->width=object.width;
@@ -121,9 +116,13 @@ void COpendriveLane::set_offset(double offset)
   this->offset=offset;
 }
 
-void COpendriveLane::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+void COpendriveLane::update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs)
 {
-  this->segment=refs[this->segment];
+  this->left_lane=lane_refs[this->left_lane];
+  this->right_lane=lane_refs[this->right_lane];
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->update_references(lane_refs);
 }
 
 void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
@@ -310,25 +309,6 @@ double COpendriveLane::get_length(void)
   return length;
 }
 
-void COpendriveLane::operator=(const COpendriveLane &object)
-{
-  COpendriveRoadNode *node;
-
-  this->nodes.clear();
-  for(unsigned int i=0;i<object.nodes.size();i++)
-  {
-    node=new COpendriveRoadNode(*object.nodes[i]);
-    this->nodes.push_back(node);
-  }
-  this->segment=object.segment;
-  this->scale_factor=object.scale_factor;
-  this->resolution=object.resolution;
-  this->width=object.width;
-  this->speed=object.speed;
-  this->offset=object.offset;
-  this->id=object.id;
-}
-
 std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
 {
   out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 021d904..0c17e55 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -51,6 +51,12 @@ void COpendriveLink::generate(void)
 
 }
 
+void COpendriveLink::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
+{
+  this->prev=refs[this->prev];
+  this->next=refs[this->next];
+}
+
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const
 {
   return *this->prev;
@@ -101,22 +107,12 @@ double COpendriveLink::get_length(void) const
 
 }
 
-void COpendriveLink::operator=(const COpendriveLink &object)
-{
-  this->prev=object.prev;
-  this->next=object.next;
-  this->spline=new CG2Spline(*object.spline);
-  this->mark=object.mark;
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-}
-
 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 << "        Road mark: ";
-  switch(link.mark)
+  switch(link.get_road_mark())
   {
     case OD_MARK_NONE:
       out << "no mark" << std::endl;
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 1cc2e20..c65d186 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -16,26 +16,35 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
 {
   COpendriveRoadSegment *segment;
   COpendriveRoadNode *node;
-  std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_references;
+  std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> new_segment_ref;
+  std::map<COpendriveRoadNode *,COpendriveRoadNode *> new_node_ref;
 
   this->resolution=object.resolution;
   this->scale_factor=object.scale_factor;
   this->min_road_length=object.min_road_length;
   this->segments.clear();
-  for(unsigned int i=0;i<object.segments.size();i++)
-  {
-    segment=new COpendriveRoadSegment(*object.segments[i]);
-    this->segments.push_back(segment);
-    new_references[object.segments[i]]=segment;
-  }
-  this->nodes.clear();
   for(unsigned int i=0;i<object.nodes.size();i++)
   {
     node=new COpendriveRoadNode(*object.nodes[i]);
     this->nodes.push_back(node);
+    new_node_ref[object.nodes[i]]=node;
   }
+  // update references
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->update_references(new_node_ref);
+
+  for(unsigned int i=0;i<object.segments.size();i++)
+  {
+    segment=new COpendriveRoadSegment(*object.segments[i],new_node_ref,this);
+    this->segments.push_back(segment);
+    new_segment_ref[object.segments[i]]=segment;
+  }
+  this->nodes.clear();
+  // update references
   for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->update_references(new_references);
+    this->segments[i]->update_references(new_segment_ref);
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    this->nodes[i]->update_references(new_segment_ref);
 }
 
 COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 848ea1a..91b3e74 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -163,6 +163,23 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
   this->lanes.push_back(lane);
 }
 
+void COpendriveRoadNode::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+    this->links[i]->update_references(refs);
+}
+
+void COpendriveRoadNode::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+{
+  this->parent_segment=refs[this->parent_segment];
+}
+
+void COpendriveRoadNode::update_references(std::map<COpendriveLane *,COpendriveLane *> refs)
+{
+  for(unsigned int i=0;i<this->lanes.size();i++)
+    this->lanes[i]=refs[this->lanes[i]];
+}
+
 double COpendriveRoadNode::get_resolution(void) const
 {
   return this->resolution;
@@ -227,40 +244,6 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
     throw CException(_HERE_,"Invalid geometry index");
 }
 
-void COpendriveRoadNode::operator=(const COpendriveRoadNode &object) 
-{
-  COpendriveLink *new_link;
-
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  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];
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    delete this->geometries[i];
-    this->geometries[i]=NULL;
-  }
-  this->geometries.resize(object.geometries.size());
-  for(unsigned int i=0;i<object.geometries.size();i++)
-    this->geometries[i]=object.geometries[i]->clone();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    delete this->links[i];
-    this->links[i]=NULL;
-  }
-  this->links.clear();
-  for(unsigned int i=0;i<object.links.size();i++)
-  {
-    new_link=new COpendriveLink(*object.links[i]);
-    this->links.push_back(new_link);
-  }
-  this->index=object.index;
-}
-
 std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
   out << "    Node: " << node.get_index() << std::endl;
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 3eab20a..737342d 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -1,5 +1,4 @@
 #include "opendrive_road_segment.h"
-#include "opendrive_road.h"
 #include "exceptions.h"
 
 COpendriveRoadSegment::COpendriveRoadSegment()
@@ -17,21 +16,27 @@ COpendriveRoadSegment::COpendriveRoadSegment()
   this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
-COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
+COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref)
 {
   COpendriveLane *new_lane;
   COpendriveSignal *new_signal;
   COpendriveObject *new_object;
   std::map<int,COpendriveLane *>::const_iterator lane_it;
+  std::map<COpendriveLane *,COpendriveLane *> new_refs;
 
   this->name=object.name;
   this->id=object.id;
   this->lanes.clear();
   for(lane_it=object.lanes.begin();lane_it!=object.lanes.end();++lane_it)
   {
-    new_lane=new COpendriveLane(*lane_it->second);
+    new_lane=new COpendriveLane(*lane_it->second,node_refs,this);
     this->lanes[lane_it->first]=new_lane;
+    new_refs[lane_it->second]=new_lane;
   }
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
+    lane_it->second->update_references(new_refs);
+
+  this->parent_road=road_ref;
   this->signals.clear();
   for(unsigned int i=0;i<object.signals.size();i++)
   {
@@ -107,13 +112,10 @@ void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent)
   this->parent_road=parent;
 }
 
-void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> &segment_refs)
 {
   for(unsigned int i=0;i<this->connecting.size();i++)
-    this->connecting[i]=refs[this->connecting[i]];
-
-  for(unsigned int i=0;i<this->lanes.size();i++)
-    this->lanes[i]->update_references(refs);
+    this->connecting[i]=segment_refs[this->connecting[i]];
 }
 
 void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
@@ -498,39 +500,6 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
     throw CException(_HERE_,"Invalid connecting segment index");
 }
 
-void COpendriveRoadSegment::operator=(const COpendriveRoadSegment &object)
-{
-  COpendriveLane *new_lane;
-  COpendriveSignal *new_signal;
-  COpendriveObject *new_object;
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
-
-  this->free();
-  this->name=object.name;
-  this->id=object.id;
-  for(lane_it=object.lanes.begin();lane_it!=object.lanes.end();++lane_it)
-  {
-    new_lane=new COpendriveLane(*lane_it->second);
-    this->lanes[lane_it->first]=new_lane;
-  }
-  for(unsigned int i=0;i<object.signals.size();i++)
-  {
-    new_signal=new COpendriveSignal(*object.signals[i]);
-    this->signals.push_back(new_signal);
-  }
-  for(unsigned int i=0;i<object.objects.size();i++)
-  {
-    new_object=new COpendriveObject(*object.objects[i]);
-    this->objects.push_back(new_object);
-  }
-  this->connecting.resize(object.connecting.size());
-  for(unsigned int i=0;i<object.connecting.size();i++)
-    this->connecting[i]=object.connecting[i];
-  this->resolution=object.resolution;
-  this->scale_factor=object.scale_factor;
-  this->min_road_length=object.min_road_length;
-}
-
 std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
 {
   out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
-- 
GitLab