diff --git a/include/osm/osm_road_segment.h b/include/osm/osm_road_segment.h
index b0108a1358d1d88eea36ec32bf5800f30ffd2276..14b0205e71b71557eaf214432a05c54717a19568 100644
--- a/include/osm/osm_road_segment.h
+++ b/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);
diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp
index adfd30a46b4543561591ebdff47db12327db770c..4d15f586aabc7f31fac924ad24233caf2f9ed2d1 100644
--- a/src/examples/osm_import_test.cpp
+++ b/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;
diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp
index d92ecec82974a64cd843864d3f5d31c2268886fb..7ed8da2e771be666316ba716f513c68d18d02870 100644
--- a/src/osm/osm_road_segment.cpp
+++ b/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)