Commit cf165668 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Improved the way to create the sub road.

parent e8d22de9
......@@ -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);
......
......@@ -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:
......
......@@ -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;
}
}
......
......@@ -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();
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment