diff --git a/OpenRoadEd/create_junction_road.cpp b/OpenRoadEd/create_junction_road.cpp index 27f14a727d64ca9577da1678eb119220d849f9f9..1a1082ab8f0591b5b9b0ef1fc4d8bcaa19206aa2 100644 --- a/OpenRoadEd/create_junction_road.cpp +++ b/OpenRoadEd/create_junction_road.cpp @@ -16,7 +16,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns GeometryBlock *geometry,new_geometry; Eigen::Matrix2d A; Eigen::Vector2d b,sol; - double curvature,length,start_length,end_length,angle; + double radius,curvature,length,start_length,end_length,angle; LaneSection *lane_section,new_lane_section(0.0); double lane_start_width; double s,start_x,start_y,start_heading,original_start_heading; @@ -198,31 +198,33 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns sol=A.inverse()*b; if(fabs(sol(0))>fabs(sol(1))) { - curvature=1.0/fabs(sol(1)); angle=end_heading-start_heading; if(angle>3.14158) angle-=2.0*3.14158; else if(angle<-3.14158) angle+=2.0*3.14158; + radius=fabs(sol(1))*tan((3.14159-fabs(angle))/2.0); + curvature=1.0/radius; if(angle<0.0) curvature*=-1.0; - length=fabs(angle)*fabs(sol(1)); - start_length=fabs(sol(0))-fabs(sol(1))*tan(fabs(angle)/2.0); - end_length=fabs(sol(1))*(1-tan(fabs(angle)/2.0)); + length=fabs(angle)*radius; + start_length=fabs(sol(0))-fabs(sol(1)); + end_length=0.0; } else { - curvature=1.0/fabs(sol(0)); angle=end_heading-start_heading; if(angle>3.14158) angle-=2.0*3.14158; else if(angle<-3.14158) angle+=2.0*3.14158; + radius=fabs(sol(0))*tan((3.14159-fabs(angle))/2.0); + curvature=1.0/radius; if(angle<0.0) curvature*=-1.0; - length=fabs(angle)*fabs(sol(0)); - start_length=fabs(sol(0))*(1-tan(fabs(angle)/2.0)); - end_length=fabs(sol(1))-fabs(sol(0))*tan(fabs(angle)/2.0); + length=fabs(angle)*radius; + start_length=0.0; + end_length=fabs(sol(1))-fabs(sol(0)); } // add geometry s=0;