Solved a bug when computing the parameters of an arc geometry.

src/osm/osm_road_segment.cpp

@@ -271,6 +271,7 @@ void COSMRoadSegment::generate_opendrive_geometry(void)
   COpendriveGeometry *new_geometry;
   TOpendriveWorldPose start_pose;
   double y_offset,heading1,length1,length2,heading2,x,y,angle,radius,prev_dist,dist,lane_offset,diff;
+  double arc_length;
   const COSMNode *node1,*node2,*node3;
 
   this->opendrive_geometries.clear();
@@ -302,10 +303,8 @@ void COSMRoadSegment::generate_opendrive_geometry(void)
       length2=node2->compute_distance(*node3);
       heading2=node2->compute_heading(*node3);
       angle=node2->compute_angle(*node1,*node3);
-      //lane_offset=((double)this->parent_way->get_num_lanes())*this->parent_way->get_lane_width()/2.0;
-//      lane_offset=((double)this->parent_way->get_num_lanes())*2.0;
-//      radius=DEFAULT_MIN_RADIUS+lane_offset;
-      radius=DEFAULT_MIN_RADIUS+y_offset;
+      lane_offset=((double)this->parent_way->get_num_lanes())*this->parent_way->get_lane_width()/2.0;
+      radius=DEFAULT_MIN_RADIUS+lane_offset;
       dist=radius*cos(angle/2.0);
       if(length1<length2)
       {
@@ -347,6 +346,7 @@ void COSMRoadSegment::generate_opendrive_geometry(void)
       }
       if(dist<MIN_ROAD_LENGTH)
         dist=MIN_ROAD_LENGTH;
+//      std::cout << "way " << this->parent_way->id << " segment " << i << " radius " << radius << std::endl;
       if((i+1)==(this->parent_way->get_num_segments()-1))
       {
         if((length2-this->end_distance-dist)<MIN_ROAD_LENGTH)
@@ -402,10 +402,17 @@ void COSMRoadSegment::generate_opendrive_geometry(void)
       start_pose.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
       start_pose.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
       start_pose.heading=heading1;
+      // compute the new laength and radius
       if(angle>0.0)
-        new_geometry=new COpendriveArc(start_pose,2.0*dist,-1.0/radius);
+      {
+        arc_length=2.0*dist*(radius+y_offset)/radius;
+        new_geometry=new COpendriveArc(start_pose,arc_length,-1.0/(radius+y_offset));
+      }
       else
-        new_geometry=new COpendriveArc(start_pose,2.0*dist,1.0/radius);
+      {
+        arc_length=2.0*dist*(radius-y_offset)/radius;
+        new_geometry=new COpendriveArc(start_pose,arc_length,1.0/(radius-y_offset));
+      }
       this->opendrive_geometries.push_back(new_geometry);
       if((i+1)==(this->parent_way->get_num_segments()-1))
       {