From e494945b15bfb8e77a4da36f18ee8defcf3fccbc Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 28 Apr 2020 16:57:47 +0200
Subject: [PATCH] =?UTF-8?q?Changes=20to=20allow=20the=20creation=20junctio?=
 =?UTF-8?q?n=20roads=20with=20any=20angle=20(except=20for=20values=20close?=
 =?UTF-8?q?=20to=20180=C2=BA).?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 OpenRoadEd/create_junction_road.cpp | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/OpenRoadEd/create_junction_road.cpp b/OpenRoadEd/create_junction_road.cpp
index 27f14a7..1a1082a 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;
-- 
GitLab