diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index a00e0a9398528984ca5de063302612355b9cba91..763ef4a5d7f8d944f019556f9e22e8156adbf1b1 100644
--- a/include/opendrive_road.h
+++ b/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);
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index b4e96ff41a6fbb4d60487912ea7ccf86e0ed55e9..cb8f8beaab8617fa8c331b476ff6f8f00dbc0315 100644
--- a/include/opendrive_road_segment.h
+++ b/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:
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index d01bd8746f8a559c9e7d4ddda53ea706d459688d..abbb4616d574767d83b56ee13f8f859008880695 100644
--- a/src/opendrive_lane.cpp
+++ b/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;
}
}
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index c1318f345527b781ff95813f6e25c8ef6bf69443..4149774986fa6ba0de7cd3a60c6631429ecbc362 100644
--- a/src/opendrive_road.cpp
+++ b/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();
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index b33f8bd2093658b1d75a32ba5c5095feb86ab49b..d8e3b3ae037cb8d26306d2cf9ee4db038ee440cc 100644
--- a/src/opendrive_road_segment.cpp
+++ b/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;