diff --git a/OpenRoadEd/create_junction_road.cpp b/OpenRoadEd/create_junction_road.cpp index 557b9ed43d30c72726606933f646f3eac65d125a..1194c60cc840ab5e8793cb28f9098a81241e7284 100644 --- a/OpenRoadEd/create_junction_road.cpp +++ b/OpenRoadEd/create_junction_road.cpp @@ -18,9 +18,9 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns Eigen::Vector2d b,sol; double radius,curvature,length,start_length,end_length,angle; LaneSection *lane_section,new_lane_section(0.0); - double lane_start_width; + double lane_start_width,lane_start_offset; double s,start_x,start_y,start_heading,original_start_heading; - double end_x,end_y,end_heading,original_end_heading; + double end_x,end_y,end_heading,original_end_heading,lane_end_offset; unsigned int left_lane_index,right_lane_index,center_lane_index; unsigned int right_lane_width_index; LaneWidth *lane_width; @@ -100,7 +100,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns std::cout << "Warning: the predecessor (" << connection->GetIncomingRoad() << ") has invalid predecessor element" << std::endl; return -1; } - // get lane size and offset + // get lane size lanes=predecessor->GetLastLaneSection()->GetLaneVector(); for(unsigned int i=0;i<lanes->size();i++) { @@ -111,22 +111,41 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns lane_start_width=lane->GetWidthValue(s); } } + // get lane offset + lane_start_offset=0.0; if(original_start_heading==start_heading) { - if(lane_link->GetFrom()>0) + for(unsigned int i=0;i<lanes->size();i++) { - start_x-=lane_start_width*sin(start_heading); - start_y+=lane_start_width*cos(start_heading); + lane=&lanes->at(i); + if(std::signbit(lane_link->GetFrom())==std::signbit(lane->GetId())) + { + if(lane->GetId()>0)//right lanes + lane_start_offset+=lane->GetWidthValue(s); + else if(abs(lane->GetId())<abs(lane_link->GetFrom())) + lane_start_offset-=lane->GetWidthValue(s); + } } } else { - if(lane_link->GetFrom()<0) + for(unsigned int i=0;i<lanes->size();i++) { - start_x-=lane_start_width*sin(start_heading); - start_y+=lane_start_width*cos(start_heading); + lane=&lanes->at(i); + if(std::signbit(lane_link->GetFrom())==std::signbit(lane->GetId())) + { + if(lane->GetId()<0)//right lanes + lane_start_offset+=lane->GetWidthValue(s); + else if(abs(lane->GetId())<abs(lane_link->GetFrom())) + lane_start_offset-=lane->GetWidthValue(s); + } } } + std::cout << "start offset: " << lane_start_offset << std::endl; + std::cout << "start position: " << start_x << "," << start_y << std::endl; + start_x-=lane_start_offset*sin(start_heading); + start_y+=lane_start_offset*cos(start_heading); + std::cout << "start position: " << start_x << "," << start_y << std::endl; // get successor road start position if(successor->GetPredecessor()!=NULL) { @@ -164,23 +183,43 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns std::cout << "Warning: the successor (" << connection->GetConnectingRoad() << ") has invalid predecessor element" << std::endl; return -1; } - - if(end_heading==original_end_heading) + // get lane offset + lanes=successor->GetLaneSection(0)->GetLaneVector(); + lane_end_offset=0.0; + std::cout << original_end_heading << "," << end_heading << std::endl; + if(original_end_heading==end_heading) { - if(lane_link->GetTo()>0) + for(unsigned int i=0;i<lanes->size();i++) { - end_x-=lane_start_width*sin(end_heading); - end_y+=lane_start_width*cos(end_heading); + lane=&lanes->at(i); + if(std::signbit(lane_link->GetTo())==std::signbit(lane->GetId())) + { + if(lane->GetId()>0)//right lanes + lane_end_offset+=lane->GetWidthValue(s); + else if(abs(lane->GetId())<abs(lane_link->GetTo())) + lane_end_offset-=lane->GetWidthValue(s); + } } } else { - if(lane_link->GetTo()<0) + for(unsigned int i=0;i<lanes->size();i++) { - end_x-=lane_start_width*sin(end_heading); - end_y+=lane_start_width*cos(end_heading); + lane=&lanes->at(i); + if(std::signbit(lane_link->GetTo())==std::signbit(lane->GetId())) + { + if(lane->GetId()<0)//right lanes + lane_end_offset+=lane->GetWidthValue(s); + else if(abs(lane->GetId())<abs(lane_link->GetTo())) + lane_end_offset-=lane->GetWidthValue(s); + } } } + std::cout << "end offset: " << lane_end_offset << std::endl; + std::cout << "end position: " << end_x << "," << end_y << std::endl; + end_x-=lane_end_offset*sin(end_heading); + end_y+=lane_end_offset*cos(end_heading); + std::cout << "end position: " << end_x << "," << end_y << std::endl; if(road==NULL) { @@ -202,7 +241,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns road->SetSuccessor("road",connection->GetConnectingRoad(), "start"); else road->SetSuccessor("road",connection->GetConnectingRoad(), "end"); - road->AddRoadType(0,"town"); + road->AddRoadType(0,"town");// que sigui el tipus del predecessor road_tree->AddRoadType(road_index,0,false); road_id++; } @@ -219,9 +258,17 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns } // compute geometry // LINE - line1=(end_x-start_x)*tan(start_heading); - line2=(end_y-start_y); - if(fabs(line1-line2)<0.01 && fabs(start_heading-end_heading)<0.01) + if(fabs(start_heading-1.5707)<0.01 || fabs(start_heading-4.71238898)<0.01) + { + line1=start_x; + line2=end_x; + } + else + { + line1=start_y-tan(start_heading)*start_x; + line2=end_y-tan(start_heading)*end_x; + } + if(fabs(line2-line1)<0.01 && fabs(start_heading-end_heading)<0.01) { s=0; length=sqrt(pow(end_x-start_x,2.0)+pow(end_y-start_y,2.0));