diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 825bd68c79e28b6f2bddd897f432d1539530c222..84765bf57a014c316923531e0a28891035843625 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -369,6 +369,7 @@ void COpendriveRoad::add_segment(const COpendriveRoadSegment *segment,segment_up
std::stringstream road_name;
int lane_id,inner_lane_id=std::numeric_limits<int>::max();
unsigned int num_right_lanes=0,num_left_lanes=0;
+ double offset;
if(segment->is_junction())
{
@@ -386,9 +387,18 @@ void COpendriveRoad::add_segment(const COpendriveRoadSegment *segment,segment_up
cand_prev_segments=candidate_segment->get_prev_segments(-1,prev_sides);
if(prev_segments[0]==cand_prev_segments[0])
{
- lane_id=candidate_segment->get_lane(-1).get_prev_lane(0).get_id();
+ const COpendriveRoadNode &node=candidate_segment->get_node(0);
+ for(unsigned int j=0;j<node.get_num_links();j++)
+ {
+ if(node.get_link(j).segment==prev_segments[0])
+ if(node.get_link(j).lane!=NULL)
+ {
+ lane_id=node.get_link(j).lane->get_id();
+ break;
+ }
+ }
junction_segments[lane_id]=candidate_segment;
- if(abs(lane_id) < abs(inner_lane_id))
+ if(lane_id<0 && (abs(lane_id) < abs(inner_lane_id)))
inner_lane_id=lane_id;
if(lane_id>0 && ((unsigned int)lane_id)>num_left_lanes)
num_left_lanes=lane_id;
@@ -397,8 +407,19 @@ void COpendriveRoad::add_segment(const COpendriveRoadSegment *segment,segment_up
}
else
{
- lane_id=candidate_segment->get_lane(-1).get_next_lane(0).get_id();
+ const COpendriveRoadNode &node=candidate_segment->get_node(candidate_segment->get_num_nodes()-1);
+ for(unsigned int j=0;j<node.get_num_links();j++)
+ {
+ if(node.get_link(j).segment==next_segments[0])
+ if(node.get_link(j).lane!=NULL)
+ {
+ lane_id=node.get_link(j).lane->get_id();
+ break;
+ }
+ }
junction_segments[lane_id]=candidate_segment;
+ if(lane_id<0 && (abs(lane_id) < abs(inner_lane_id)))
+ inner_lane_id=lane_id;
if(lane_id>0 && ((unsigned int)lane_id)>num_left_lanes)
num_left_lanes=lane_id;
else if(lane_id<0 && ((unsigned int)-lane_id)>num_right_lanes)
@@ -421,22 +442,31 @@ void COpendriveRoad::add_segment(const COpendriveRoadSegment *segment,segment_up
const COpendriveRoadSegment *inner_segment=junction_segments[inner_lane_id];
for(unsigned int i=0;i<inner_segment->geometries.size();i++)
new_segment->add_geometry(inner_segment->geometries[i].opendrive);
+ offset=0.0;
for(unsigned int i=1;i<=num_right_lanes;i++)
{
new_segment->lanes[-i]=junction_segments[-i]->lanes[-1];
+ new_segment->lanes[-i]->id=-i;
+ new_segment->lanes[-i]->offset=-offset;
+ offset+=junction_segments[-i]->lanes[-1]->width;
+ for(unsigned int j=0;j<junction_segments[-i]->lanes[-1]->get_num_nodes();j++)
+ new_segment->nodes.push_back(junction_segments[-i]->lanes[-1]->nodes[j]);
+ junction_segments[-i]->lanes[-1]->clone(new_node_ref,new_lane_ref,new_link_ref);
new_segment->num_right_lanes++;
- new_lane_ref[junction_segments[-i]->lanes[-1]]=new_segment->lanes[-i];
- for(unsigned int j=0;j<junction_segments[-i]->lanes[-1]->nodes.size();j++)
- new_node_ref[junction_segments[-i]->lanes[-1]->nodes[j]]=new_segment->lanes[-i]->nodes[j];
}
+ offset=0.0;
for(unsigned int i=1;i<=num_left_lanes;i++)
{
new_segment->lanes[i]=junction_segments[i]->lanes[-1];
+ new_segment->lanes[i]->id=i;
+ new_segment->lanes[i]->offset=offset;
+ offset+=junction_segments[i]->lanes[-1]->width;
+ for(unsigned int j=0;j<junction_segments[i]->lanes[-1]->get_num_nodes();j++)
+ new_segment->nodes.push_back(junction_segments[i]->lanes[-1]->nodes[j]);
+ junction_segments[i]->lanes[-1]->clone(new_node_ref,new_lane_ref,new_link_ref);
new_segment->num_left_lanes++;
- new_lane_ref[junction_segments[i]->lanes[-1]]=new_segment->lanes[i];
- for(unsigned int j=0;j<junction_segments[i]->lanes[-1]->nodes.size();j++)
- new_node_ref[junction_segments[i]->lanes[-1]->nodes[j]]=new_segment->lanes[i]->nodes[j];
}
+ new_segment->connecting=segment->connecting;
this->add_segment(new_segment);
for(std::map<int,COpendriveRoadSegment *>::iterator it=junction_segments.begin();it!=junction_segments.end();it++)
new_segment_ref[(*it).second]=new_segment;
@@ -526,25 +556,25 @@ void COpendriveRoad::complete_open_lanes(void)
{
std::vector<COpendriveLane *>::iterator lane_it;
TOpendriveWorldPose end_pose;
- TOpendriveTrackPose track_pose;
+// TOpendriveTrackPose track_pose;
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
{
// try{
// end_pose=(*lane_it)->get_end_pose();
// }catch(CException &e){
- track_pose.t=(*lane_it)->get_center_offset();
- if((*lane_it)->get_id()<0)
- {
- track_pose.heading=0.0;
- track_pose.s=(*lane_it)->segment->get_length(true);
- }
- else
- {
- track_pose.s=0.0;
- track_pose.heading=3.14159;
- }
- end_pose=(*lane_it)->segment->transform_to_world_pose(track_pose);
+// track_pose.t=(*lane_it)->get_center_offset();
+// if((*lane_it)->get_id()<0)
+// {
+// track_pose.heading=0.0;
+// track_pose.s=(*lane_it)->segment->get_length(true);
+// }
+// else
+// {
+// track_pose.s=0.0;
+// track_pose.heading=3.14159;
+// }
+ end_pose=(*lane_it)->get_end_pose(true);
// }
if(!this->node_exists_at(end_pose))// there is no node at the end pose
{
@@ -1531,11 +1561,13 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(TOpendriveWorldPose &star
segment=segment->get_next_path_segment(path_nodes,node_side,node_side);
if((rem_length-segment->get_length())<0.0)
{
- int_end_pose=segment->get_pose_at(rem_length,node_side);
- if((rem_length-segment->get_length())>-0.5)
+ if((rem_length-segment->get_length())>-2.0)
new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
else
+ {
+ int_end_pose=segment->get_pose_at(rem_length+2.0,node_side);
new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&int_end_pose);
+ }
new_road.add_segment(new_segment);
new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);