Improved the way to create the sub road.

include/opendrive_road.h

@@ -32,6 +32,7 @@ class COpendriveRoad
     void remove_lane(COpendriveLane *lane);
     void complete_open_lanes(void);
     void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
+    bool has_segment(COpendriveRoadSegment *segment);
     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 prune(std::vector<unsigned int> &path_nodes);

include/opendrive_road_segment.h

@@ -45,6 +45,7 @@ class COpendriveRoadSegment
     void set_id(unsigned int id);
     void set_center_mark(opendrive_mark_t mark);
     bool updated(segment_up_ref_t &segment_refs);
+    COpendriveRoadSegment *get_original_segment(segment_up_ref_t &segment_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 add_lanes(lanes::laneSection_type &lane_section);
@@ -58,7 +59,7 @@ class COpendriveRoadSegment
     void link_segment(COpendriveRoadSegment &segment);
     void link_segment(COpendriveRoadSegment &segment,int from,bool from_start, int to,bool to_start);
     bool connects_to(COpendriveRoadSegment *segment);
-    bool connects_nodes(COpendriveRoadNode *node1,COpendriveRoadNode *node2);
+    bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
     COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
     COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
   public:

src/opendrive_lane.cpp

@@ -45,35 +45,20 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
   if(lane!=NULL)
   {
     if(this->get_num_nodes()!=lane->get_num_nodes())
-      throw CException(_HERE_,"Impossible to link lanes because they have different number of nodes");
-    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
+      return;
+    if(this->get_num_nodes()==0 || lane->get_num_nodes()==0)
+      return;
+    for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
     {
-      if(this->id*lane->get_id()<0) // oposite directions
-      {
-        for(unsigned int i=0;i<this->get_num_nodes();i++)
-        {
-          this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark,this->segment,this);
-          lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark,this->segment,this);
-          lane->right_mark=this->left_mark;
-        }
-      }
-      else
-      {
-        for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
-        {
-          this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark,this->segment,this);
-          lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,this);
-          if(this->id>0)// left lanes
-            this->right_mark=lane->left_mark;
-          else// right lanes
-            this->left_mark=lane->right_mark;
-        }
-      } 
-      this->left_lane=lane;
-      lane->right_lane=this;
-    }
-    else
-      throw CException(_HERE_,"One of the lanes to link has no nodes");
+      this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark,this->segment,this);
+      lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,this);
+      if(this->id>0)// left lanes
+        this->right_mark=lane->left_mark;
+      else// right lanes
+        this->left_mark=lane->right_mark;
+    } 
+    this->left_lane=lane;
+    lane->right_lane=this;
   }
 }
 

src/opendrive_road.cpp

@@ -266,6 +266,8 @@ void COpendriveRoad::complete_open_lanes(void)
         new_node->set_index(new_node_index);
         (*lane_it)->add_node(new_node);
         (*lane_it)->segment->add_node(new_node);
+        (*lane_it)->link_neightbour_lane((*lane_it)->left_lane);
+        (*lane_it)->link_neightbour_lane((*lane_it)->right_lane);
       }
       else
         lane_it++;
@@ -275,6 +277,15 @@ void COpendriveRoad::complete_open_lanes(void)
   }   
 }
 
+bool COpendriveRoad::has_segment(COpendriveRoadSegment *segment)
+{
+  for(unsigned int i=0;i<this->segments.size();i++)
+    if(this->segments[i]==segment)
+      return true;
+  
+  return false;
+}
+
 void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path)
 {
   for(unsigned int i=0;i<this->segments.size();i++)
@@ -455,15 +466,8 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
   {
     for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
     {
-      for(unsigned int k=0;k<path_nodes.size()-1;k++)
-      {
-        if(path_nodes[k]==this->nodes[i]->links[j]->prev->index)
-          if(path_nodes[k+1]!=this->nodes[i]->links[j]->next->index)
-          {
-            this->nodes[i]->remove_link(this->nodes[i]->links[j]);
-            break;
-          }
-      }
+      if(!this->has_segment(this->nodes[i]->links[j]->segment))
+        this->nodes[i]->remove_link(this->nodes[i]->links[j]);
     }
   } 
 }
@@ -676,12 +680,11 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   segment_up_ref_t new_segment_ref;
   lane_up_ref_t new_lane_ref;
   node_up_ref_t new_node_ref;
-  COpendriveRoadNode *node,*next_node,*repeat_node,*repeat_next_node;
-  COpendriveRoadSegment *segment,*new_segment,*repeat_segment;
+  COpendriveRoadNode *node,*next_node;
+  COpendriveRoadSegment *segment,*new_segment,*original_seg1,original_seg2;
+  COpendriveLink *link;
   std::vector<unsigned int> new_path_nodes;
   unsigned int link_index;
-  bool added,repeated;
-  int node_side;
 
   new_path_nodes.resize(path_nodes.size());
 
@@ -690,110 +693,60 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   new_road.set_scale_factor(this->scale_factor);
   new_road.set_min_road_length(this->min_road_length);
 
-  if(path_nodes.size()==1)
+  for(unsigned int i=0;i<path_nodes.size()-1;i++)
   {
-    node=this->nodes[path_nodes[path_nodes.size()-1]];
-    link_index=node->get_closest_link(end_point);
-    segment=node->links[link_index]->segment;
-    node_side=segment->get_node_side(node);
-    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,&end_point);
+    node=this->nodes[path_nodes[i]];
+    next_node=this->nodes[path_nodes[i+1]];
+    link=node->get_link_with(next_node);
+    if(link==NULL)
+      throw CException(_HERE_,"The provided path has unconnected nodes");
+    segment=link->segment;
+    if(new_segment_ref.find(segment)==new_segment_ref.end())
+    {
+      new_segment=segment->clone(new_node_ref,new_lane_ref);
+      new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+      new_segment_ref[segment]=new_segment;
+    }
+  }
+  // add the last segment
+  node=this->nodes[path_nodes[path_nodes.size()-1]];
+  link_index=node->get_closest_link(end_point,3.14159);
+  link=node->links[link_index];
+  if(link==NULL)
+    throw CException(_HERE_,"The provided path has unconnected nodes");
+  segment=link->segment;
+  if(new_segment_ref.find(segment)==new_segment_ref.end())
+  {
+    new_segment=segment->clone(new_node_ref,new_lane_ref);
     new_road.add_segment(new_segment,path_nodes,new_path_nodes);
     new_segment_ref[segment]=new_segment;
   }
-  else
+
+  // add additional nodes not explicitly in the path
+/*
+  for(unsigned int i=0;i<this->segments.size();i++)
   {
-    for(unsigned int i=0;i<path_nodes.size()-1;i++)
+    for(unsigned int j=0;j<new_road.segments.size();j++)
     {
-      node=this->nodes[path_nodes[i]];
-      next_node=this->nodes[path_nodes[i+1]];
-      segment=node->get_link_with(next_node)->segment;
-      if(segment->has_node(next_node))
-        continue;
-      else
+      original_seg1=new_road.segments[j]->get_original_segment(new_segment_ref);
+      for(unsigned int k=j+1;k<new_road.segments.size();k++)
       {
-        if(i==0)
+        original_seg2=new_road.segments[k]->get_original_segment(new_segment_ref);
+        if(this->segments[i]->connects_segments(original_seg1,original_seg2)
         {
-          node_side=segment->get_node_side(node);
-          repeated=false;
-          for(unsigned int j=1;j<path_nodes.size();j++)
+          if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
           {
-            repeat_node=this->nodes[path_nodes[j]];
-            if(j==path_nodes.size()-1)
-            {
-              link_index=node->get_closest_link(end_point,3.14159);
-              repeat_segment=node->links[link_index]->segment;
-            }
-            else
-            {
-              repeat_next_node=this->nodes[path_nodes[j+1]];
-              repeat_segment=repeat_node->get_link_with(repeat_next_node)->segment;
-            }
-            if(segment==repeat_segment)
-            {
-              repeated=true;
-              break;
-            }
+            new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref);
+            new_road.add_segment(new_segment,path_nodes,new_path_nodes);
+            new_segment_ref[segment]=new_segment;
           }
-          if(repeated)
-            new_segment=segment->clone(new_node_ref,new_lane_ref);
-          else
-            new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,&start_point,NULL);
         }
-        else
-          new_segment=segment->clone(new_node_ref,new_lane_ref);
-        new_road.add_segment(new_segment,path_nodes,new_path_nodes);
-        new_segment_ref[segment]=new_segment;
       }
     }
-    // add the last segment
-    node=this->nodes[path_nodes[path_nodes.size()-1]];
-    link_index=node->get_closest_link(end_point,3.14159);
-    segment=node->links[link_index]->segment;
-    repeated=false;
-    for(unsigned int j=0;j<path_nodes.size()-1;j++)
-    {
-      repeat_node=this->nodes[path_nodes[j]];
-      repeat_next_node=this->nodes[path_nodes[j+1]];
-      repeat_segment=repeat_node->get_link_with(repeat_next_node)->segment;
-      if(segment==repeat_segment)
-      {
-        repeated=true;
-        break;
-      }
-    }
-    if(!repeated)
-    {
-      node_side=segment->get_node_side(node);
-      new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,node_side,NULL,&end_point);
-      new_road.add_segment(new_segment,path_nodes,new_path_nodes);
-      new_segment_ref[segment]=new_segment;
-    }
   }
+*/
   new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
   new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
-  // add additional nodes not explicitly in the path
-/*
-  for(unsigned int i=0;i<path_nodes.size();i++)
-  {
-    added=false;
-    node=this->nodes[path_nodes[i]];
-    for(unsigned int j=i+1;j<path_nodes.size();j++)
-    {
-      next_node=this->nodes[path_nodes[j]];
-      for(unsigned int k=0;k<this->segments.size();k++)
-        if(this->segments[k]->connects_nodes(node,next_node))
-        {
-          new_segment=this->segments[k]->clone(new_node_ref,new_lane_ref);
-          new_road.add_segment(new_segment,path_nodes,new_path_nodes);
-          new_segment_ref[segment]=new_segment;
-          added=true;
-          break;
-        }
-      if(added)
-        break;
-    }
-  }
-*/
   // remove unconnected elements
   new_road.prune(new_path_nodes);
   new_road.complete_open_lanes();

src/opendrive_road_segment.cpp

@@ -130,6 +130,17 @@ bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
   return false;
 }
 
+COpendriveRoadSegment *COpendriveRoadSegment::get_original_segment(segment_up_ref_t &segment_refs)
+{
+  segment_up_ref_t::iterator updated_it;
+
+  for(updated_it=segment_refs.begin();updated_it!=segment_refs.end();updated_it++)
+    if(updated_it->second==this)
+      return updated_it->first;
+
+  return NULL;
+}
+
 void COpendriveRoadSegment::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
 {
   std::vector<COpendriveRoadSegment *>::iterator seg_it;
@@ -481,21 +492,15 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
     if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists 
       this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
     else
-      this->lanes[i]->left_mark=OD_MARK_NONE;
+      this->lanes[i]->left_mark=this->center_mark;
   }
   for(int i=this->num_left_lanes;i>0;i--)
   {
     if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
       this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
     else
-      this->lanes[i]->right_mark=OD_MARK_NONE;
+      this->lanes[i]->right_mark=this->center_mark;
   }
-
-  if(this->lanes.find(1)!=this->lanes.end())
-    this->lanes[1]->right_mark=this->center_mark;
-  if(this->lanes.find(-1)!=this->lanes.end())
-    this->lanes[-1]->left_mark=this->center_mark;
-//  this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
 }
 
 void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
@@ -574,6 +579,14 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from
     if(this->lanes.find(from+1)!=this->lanes.end())
       this->lanes[from+1]->link_lane(segment.lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
   }
+/*
+  if(segment.lanes.find(to-1)!=segment.lanes.end())
+    if(this->lanes.find(from)!=this->lanes.end())
+      this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start);
+  if(segment.lanes.find(to+1)!=segment.lanes.end())
+    if(this->lanes.find(from)!=this->lanes.end())
+      this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start);
+*/
 }
 
 bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
@@ -585,9 +598,9 @@ bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
   return false;
 }
 
-bool COpendriveRoadSegment::connects_nodes(COpendriveRoadNode *node1,COpendriveRoadNode *node2)
+bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2)
 {
-  if(this->has_node(node1) && this->has_node(node2))
+  if(this->connects_to(segment1) && this->connects_to(segment2))
     return true;
   else
     return false;