diff --git a/src/road_map.cpp b/src/road_map.cpp index ed31a1a52f9d11f12b858ee7fe7059ec4e860eaf..0f7b6e74aa25ffc3b4ee5f1385f4baf0bb121e19 100644 --- a/src/road_map.cpp +++ b/src/road_map.cpp @@ -276,7 +276,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in CJunction *new_junction; CRoadSegment *new_segment,*old_segment; TPoint start_point,end_point; - unsigned int next_id,prev_id; + unsigned int next_id,prev_id,num_lanes_in,num_lanes_out; std::vector<unsigned int> new_path; new_road_map.free(); @@ -330,7 +330,14 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in { if(junction_segments[i]->get_prev_segment_by_index(0).has_parent_road()) { - prev_id=road_ref_update[&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road()]->get_id(); + if(road_ref_update.find(&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road())!=road_ref_update.end()) + prev_id=road_ref_update[&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road()]->get_id(); + else + { + prev_id=-1; + junction_segments[i]->get_prev_segment_by_index(0).get_end_point(start_point); + num_lanes_in=junction_segments[i]->get_prev_segment_by_index(0).get_num_lanes_out(); + } } else continue; @@ -341,7 +348,14 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in { if(junction_segments[i]->get_next_segment_by_index(0).has_parent_road()) { - next_id=road_ref_update[&junction_segments[i]->get_next_segment_by_index(0).get_parent_road()]->get_id(); + if(road_ref_update.find(&junction_segments[i]->get_next_segment_by_index(0).get_parent_road())!=road_ref_update.end()) + next_id=road_ref_update[&junction_segments[i]->get_next_segment_by_index(0).get_parent_road()]->get_id(); + else + { + next_id=-1; + junction_segments[i]->get_next_segment_by_index(0).get_start_point(end_point); + num_lanes_out=junction_segments[i]->get_next_segment_by_index(0).get_num_lanes_in(); + } } else continue; @@ -349,10 +363,27 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in else continue; new_junction=new CJunction(); - new_junction->add_incomming_road(&new_road_map.get_road_by_id(prev_id)); - new_junction->add_outgoing_road(&new_road_map.get_road_by_id(next_id)); - new_junction->link_roads_by_id(prev_id,next_id); - new_segment=&new_junction->get_segment_between_by_id(prev_id,next_id); + if(prev_id!=(unsigned int)-1) + new_junction->add_incomming_road(&new_road_map.get_road_by_id(prev_id)); + if(next_id!=(unsigned int)-1) + new_junction->add_outgoing_road(&new_road_map.get_road_by_id(next_id)); + if(prev_id==(unsigned int)-1) + { + if(next_id==(unsigned int)-1) + new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,end_point,num_lanes_out); + else + new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,next_id); + } + else + { + if(next_id==(unsigned int)-1) + new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out); + else + { + new_junction->link_roads_by_id(prev_id,next_id); + new_segment=&new_junction->get_segment_between_by_id(prev_id,next_id); + } + } new_segment->connectivity=junction_segments[i]->connectivity; segment_ref_update[junction_segments[i]]=new_segment; new_road_map.add_junction(new_junction);