Solved a bug in the computation of the start and ened distances.

include/osm/osm_road_segment.h

@@ -21,7 +21,9 @@ class COSMRoadSegment
     COSMJunction *start_junction;
     COSMJunction *end_junction;
     double start_distance;
+    double original_start_distance;
     double end_distance;
+    double original_end_distance;
   protected:
     void set_start_distance(double dist);
     void set_end_distance(double dist);

src/examples/osm_import_test.cpp

@@ -5,7 +5,7 @@
 
 //const std::string osm_file="/home/shernand/Downloads/test_osm.osm";
 //const std::string osm_file="/home/shernand/Downloads/test_osm2.osm";
-const std::string osm_file="/home/shernand/Downloads/diagonal.osm";
+const std::string osm_file="/home/shernand/Downloads/diagonal2.osm";
 
 int main(int argc, char *argv[])
 {
@@ -17,6 +17,13 @@ int main(int argc, char *argv[])
  
     map.load_osm(osm_file);
 
+    /*
+    map.get_segment_geometry(x,y,heading);
+    for(unsigned int i=0;i<x.size();i++)
+      std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;
+
+    return 0;
+*/
     map.get_lane_geometry(x,y,heading);
     for(unsigned int i=0;i<x.size();i++)
       std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl;

src/osm/osm_road_segment.cpp

@@ -8,7 +8,9 @@ COSMRoadSegment::COSMRoadSegment(COSMWay *way)
   this->start_junction=NULL;
   this->end_junction=NULL;
   this->start_distance=0.0;
+  this->original_start_distance=0.0;
   this->end_distance=0.0;
+  this->original_end_distance=0.0;
 }
 
 void COSMRoadSegment::set_start_distance(double dist)
@@ -16,6 +18,8 @@ void COSMRoadSegment::set_start_distance(double dist)
   COSMNode *node1,*node2;
   double length,start_end_distance;
 
+  if(dist>this->original_start_distance)
+    this->original_start_distance=dist;
   if(dist>this->start_distance)
   {
     node1=&this->parent_way->get_segment_start_node(FIRST_SEGMENT);
@@ -25,12 +29,12 @@ void COSMRoadSegment::set_start_distance(double dist)
     {
       if((length-this->end_distance-dist)<MIN_ROAD_LENGTH)
       {
-        if(this->end_distance==0.0)
+        if(this->original_end_distance==0.0)
           this->start_distance=length-MIN_ROAD_LENGTH;
         else
         {
-          start_end_distance=this->end_distance+dist;
-          this->end_distance*=length/start_end_distance;
+          start_end_distance=this->original_end_distance+dist;
+          this->end_distance=this->original_end_distance*length/start_end_distance;
           this->end_distance-=MIN_ROAD_LENGTH/2.0;
           this->start_distance=dist*length/start_end_distance;
           this->start_distance-=MIN_ROAD_LENGTH/2.0;
@@ -54,6 +58,8 @@ void COSMRoadSegment::set_end_distance(double dist)
   COSMNode *node1,*node2;
   double length,start_end_distance;
 
+  if(dist>this->original_end_distance)
+    this->original_end_distance=dist;
   if(dist>this->end_distance)
   {
     node1=&this->parent_way->get_segment_start_node(LAST_SEGMENT);
@@ -63,14 +69,14 @@ void COSMRoadSegment::set_end_distance(double dist)
     {
       if((length-this->start_distance-dist)<MIN_ROAD_LENGTH)
       {
-        if(this->start_distance==0.0)
+        if(this->original_start_distance==0.0)
           this->end_distance=length-MIN_ROAD_LENGTH;
         else
         {
-          start_end_distance=dist+this->start_distance;
+          start_end_distance=dist+this->original_start_distance;
           this->end_distance=dist*length/start_end_distance;
           this->end_distance-=MIN_ROAD_LENGTH/2.0;
-          this->start_distance*=length/start_end_distance;
+          this->start_distance=this->original_start_distance*length/start_end_distance;
           this->start_distance-=MIN_ROAD_LENGTH/2.0;
         }
       }
@@ -112,15 +118,13 @@ bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
       start_point.heading=heading1;
       start_point.curvature=0.0;
       road->set_start_point(start_point);
-      //prev_heading=heading1;
+//      prev_heading=heading1;
     }
     if((i+1)==(this->parent_way->get_num_nodes()-1))// straight line
     {
 //      angle2=atan2(y_offset,length1-this->end_distance);
 //      angle3=diff_angle(heading1,prev_heading)+angle2;
-//      if(fabs(angle3)<1.5707)
-//      {
-      if((length1-this->end_distance-prev_dist)>0.0)
+      if((length1-this->end_distance-prev_dist)>resolution)// && fabs(angle3)<1.5707)
       {
         end_point.x=x+(length1-this->end_distance)*cos(heading1)-y_offset*sin(heading1);
         end_point.y=y+(length1-this->end_distance)*sin(heading1)+y_offset*cos(heading1);    
@@ -128,7 +132,6 @@ bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
         end_point.curvature=0.0;
         road->add_segment(end_point);
       }
-//      }
     }
     else 
     {
@@ -141,23 +144,23 @@ bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
         if(i==0)
         {
           node2->get_location(x,y);
-          start_point.x=x-y_offset*sin(heading1);
-          start_point.y=y+y_offset*cos(heading1);
+          start_point.x=x-y_offset*sin(heading2);
+          start_point.y=y+y_offset*cos(heading2);
           start_point.heading=heading2;
           start_point.curvature=0.0;
           road->set_start_point(start_point);
           prev_dist=0.0;
-          //prev_heading=heading2;
+//          prev_heading=heading2;
         }
         else if((i+2)==(this->parent_way->get_num_nodes()-1))
         {
           prev_dist=dist;
-          //prev_heading=heading2;
+//          prev_heading=heading2;
         }
         else
         {
           prev_dist=dist;
-          //prev_heading=heading1;
+//          prev_heading=heading1;
         }
         continue;
       }
@@ -213,15 +216,14 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
       start_point.heading=heading1;
       start_point.curvature=0.0;
       road->set_start_point(start_point);
-      //prev_heading=heading1;
+//      prev_heading=heading1;
     }
     if((i-1)==0)// straight line
     {
 //      angle2=atan2(y_offset,length1-this->end_distance);
 //      angle3=diff_angle(heading1,prev_heading)+angle2;
 //      if(fabs(angle3)<1.5707)
-//      {
-      if((length1-this->start_distance-prev_dist)>0.0)
+      if((length1-this->start_distance-prev_dist)>resolution)// && fabs(angle3)<1.5707)
       {
         end_point.x=x+(length1-this->start_distance)*cos(heading1)-y_offset*sin(heading1);
         end_point.y=y+(length1-this->start_distance)*sin(heading1)+y_offset*cos(heading1);
@@ -229,7 +231,6 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
         end_point.curvature=0.0;
         road->add_segment(end_point);
       }
-//      }
     }
     else 
     {
@@ -242,23 +243,23 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
         if(i==(this->parent_way->get_num_nodes()-1))
         {
           node2->get_location(x,y);
-          start_point.x=x-y_offset*sin(heading1);
-          start_point.y=y+y_offset*cos(heading1);
+          start_point.x=x-y_offset*sin(heading2);
+          start_point.y=y+y_offset*cos(heading2);
           start_point.heading=heading2;
           start_point.curvature=0.0;
           road->set_start_point(start_point);
           prev_dist=0.0;
-          //prev_heading=heading2;
+//          prev_heading=heading2;
         }
         else if((i-2)==0)
         {
           prev_dist=dist;
-          //prev_heading=heading2;
+//          prev_heading=heading2;
         }
         else
         {
           prev_dist=dist;
-          //prev_heading=heading1;
+//          prev_heading=heading1;
         }
         continue;
       }
@@ -280,7 +281,7 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
       end_point.curvature=0.0;
       road->add_segment(end_point);
       prev_dist=dist;
-      //prev_heading=heading1;
+//      prev_heading=heading1;
     } 
   }
   if(road->get_num_segments()==0)