diff --git a/include/osm/osm_road_segment.h b/include/osm/osm_road_segment.h
index 4218653ae8529821206585a8304de504381d518e..98c39bb41f9477d6fb84b8e465d07b8b15630cda 100644
--- a/include/osm/osm_road_segment.h
+++ b/include/osm/osm_road_segment.h
@@ -9,6 +9,7 @@
#include <iostream>
+
class COSMRoadSegment
{
friend class COSMJunction;
diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp
index 09f2f51caca0c0455375c1849633fe725f23e546..851c488c17f3b751be3fbfe5359e76e8f9c67a22 100644
--- a/src/osm/osm_road_segment.cpp
+++ b/src/osm/osm_road_segment.cpp
@@ -14,7 +14,7 @@ COSMRoadSegment::COSMRoadSegment(COSMWay *way)
void COSMRoadSegment::set_start_distance(double dist)
{
COSMNode *node1,*node2;
- double length;
+ double length,start_end_distance;
if(dist>this->start_distance)
{
@@ -25,8 +25,16 @@ void COSMRoadSegment::set_start_distance(double dist)
{
if((length-this->end_distance-dist)<MIN_ROAD_LENGTH)
{
- this->end_distance=(length-MIN_ROAD_LENGTH)/2.0;
- this->start_distance=(length-MIN_ROAD_LENGTH)/2.0;
+ if(this->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;
+ this->end_distance-=MIN_ROAD_LENGTH/2.0;
+ this->start_distance=dist*length/start_end_distance;
+ this->start_distance-=MIN_ROAD_LENGTH/2.0;
+ }
}
else
this->start_distance=dist;
@@ -44,7 +52,7 @@ void COSMRoadSegment::set_start_distance(double dist)
void COSMRoadSegment::set_end_distance(double dist)
{
COSMNode *node1,*node2;
- double length;
+ double length,start_end_distance;
if(dist>this->end_distance)
{
@@ -55,8 +63,16 @@ void COSMRoadSegment::set_end_distance(double dist)
{
if((length-this->start_distance-dist)<MIN_ROAD_LENGTH)
{
- this->start_distance=(length-MIN_ROAD_LENGTH)/2.0;
- this->end_distance=(length-MIN_ROAD_LENGTH)/2.0;
+ if(this->start_distance==0.0)
+ this->end_distance=length-MIN_ROAD_LENGTH;
+ else
+ {
+ start_end_distance=dist+this->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-=MIN_ROAD_LENGTH/2.0;
+ }
}
else
this->end_distance=dist;
@@ -73,7 +89,8 @@ void COSMRoadSegment::set_end_distance(double dist)
void COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
{
- double heading1,heading2,x,y,angle,radius,length1,length2,dist,prev_dist,y_offset;
+ double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset;
+ double angle2,angle3,prev_heading;
TPoint start_point,end_point;
COSMNode *node1,*node2,*node3;
@@ -95,45 +112,54 @@ void 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;
}
if((i+1)==(this->parent_way->get_num_nodes()-1))// straight line
{
- 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);
- end_point.heading=heading1;
- end_point.curvature=0.0;
- road->add_segment(end_point);
+ angle2=atan2(y_offset,length1-this->end_distance);
+ angle3=diff_angle(heading1,prev_heading)+angle2;
+ if(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);
+ end_point.heading=heading1;
+ end_point.curvature=0.0;
+ road->add_segment(end_point);
+ }
}
else
{
node3=&this->parent_way->get_node_by_index(i+2);
- length2=node2->compute_distance(*node3);
heading2=node2->compute_heading(*node3);
angle=node2->compute_angle(*node1,*node3);
- radius=DEFAULT_MIN_RADIUS+std::max(this->parent_way->get_num_forward_lanes(),this->parent_way->get_num_backward_lanes())*this->parent_way->get_lane_width();
- dist=radius*cos(angle/2.0);
- if(length1<length2)
+ dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0));
+ if(dist>(length1-prev_dist))
{
- if(dist>(length1-prev_dist))
+ if(i==0)
{
- dist=length1-prev_dist;
- radius=dist/cos(angle/2.0);
+ node2->get_location(x,y);
+ start_point.x=x-y_offset*sin(heading1);
+ start_point.y=y+y_offset*cos(heading1);
+ start_point.heading=heading2;
+ start_point.curvature=0.0;
+ road->set_start_point(start_point);
+ prev_dist=0.0;
+ prev_heading=heading2;
}
- }
- else
- {
- if((i+2)==(this->parent_way->get_num_nodes()-1))// last node
+ else if((i+2)==(this->parent_way->get_num_nodes()-1))
{
- if(dist>(length2-this->end_distance))
- dist=(length2-this->end_distance);
+ prev_dist=dist;
+ prev_heading=heading2;
}
else
{
- if(dist>length2/2.0)
- dist=length2/2.0;
+ prev_dist=dist;
+ prev_heading=heading1;
}
- radius=dist/cos(angle/2.0);
+ continue;
}
+ else if(dist<MIN_ROAD_LENGTH/2.0)
+ dist=MIN_ROAD_LENGTH/2.0;
if(length1-prev_dist-dist>resolution)// straight segment
{
end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
@@ -150,17 +176,19 @@ void COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
end_point.curvature=0.0;
road->add_segment(end_point);
prev_dist=dist;
+ prev_heading=heading1;
}
}
}
void COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
{
- double heading1,heading2,x,y,angle,radius,length1,length2,dist,prev_dist,y_offset;
+ double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset;
+ double angle2,angle3,prev_heading;
TPoint start_point,end_point;
COSMNode *node1,*node2,*node3;
- y_offset=((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width();
+ y_offset=-((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width();
if(this->parent_way->get_num_nodes()<2)
throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry");
prev_dist=this->end_distance;
@@ -178,49 +206,58 @@ void 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;
}
if((i-1)==0)// straight line
{
- 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);
- end_point.heading=heading1;
- end_point.curvature=0.0;
- road->add_segment(end_point);
+ angle2=atan2(y_offset,length1-this->end_distance);
+ angle3=diff_angle(heading1,prev_heading)+angle2;
+ if(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);
+ end_point.heading=heading1;
+ end_point.curvature=0.0;
+ road->add_segment(end_point);
+ }
}
else
{
node3=&this->parent_way->get_node_by_index(i-2);
- length2=node2->compute_distance(*node3);
heading2=node2->compute_heading(*node3);
angle=node2->compute_angle(*node1,*node3);
- radius=DEFAULT_MIN_RADIUS+std::max(this->parent_way->get_num_forward_lanes(),this->parent_way->get_num_backward_lanes())*this->parent_way->get_lane_width();
- dist=radius*cos(angle/2.0);
- if(length1<length2)
+ dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0));
+ if(dist>(length1-prev_dist))
{
- if(dist>(length1-prev_dist))
+ if(i==(this->parent_way->get_num_nodes()-1))
{
- dist=length1-prev_dist;
- radius=dist/cos(angle/2.0);
+ node2->get_location(x,y);
+ start_point.x=x-y_offset*sin(heading1);
+ start_point.y=y+y_offset*cos(heading1);
+ start_point.heading=heading2;
+ start_point.curvature=0.0;
+ road->set_start_point(start_point);
+ prev_dist=0.0;
+ prev_heading=heading2;
}
- }
- else
- {
- if((i-2)==0)// last segment
+ else if((i-2)==0)
{
- if(dist>(length2-this->start_distance))
- dist=(length2-this->start_distance);
+ prev_dist=dist;
+ prev_heading=heading2;
}
else
{
- if(dist>length2/2.0)
- dist=length2/2.0;
+ prev_dist=dist;
+ prev_heading=heading1;
}
- radius=dist/cos(angle/2.0);
+ continue;
}
+ else if(dist<MIN_ROAD_LENGTH/2.0)
+ dist=MIN_ROAD_LENGTH/2.0;
if(length1-prev_dist-dist>resolution)// straight segment
{
- end_point.x=x+(length1-prev_dist)*cos(heading1)-y_offset*sin(heading1);
- end_point.y=y+(length1-prev_dist)*sin(heading1)+y_offset*cos(heading1);
+ end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
+ end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1);
end_point.heading=heading1;
end_point.curvature=0.0;
road->add_segment(end_point);
@@ -233,6 +270,7 @@ void COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
end_point.curvature=0.0;
road->add_segment(end_point);
prev_dist=dist;
+ prev_heading=heading1;
}
}
}
@@ -289,9 +327,9 @@ void COSMRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolu
for(unsigned int i=0;i<(*left_road)->get_num_segments();i++)
{
segment=(*left_road)->get_segment_by_index(i);
- for(unsigned int j=this->get_parent_way().get_num_forward_lanes();j<this->get_parent_way().get_num_lanes();j++)
+ for(unsigned int j=0;j<num_lanes;j++)
{
- restrictions=this->get_parent_way().get_lane_restriction(j);
+ restrictions=this->get_parent_way().get_lane_restriction(j+this->get_parent_way().get_num_forward_lanes());
if(restrictions&RESTRICTION_RIGHT)
{
for(unsigned int k=j+1;k<num_lanes;k++)