diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index 5896723aa29fe89c8aa23162b22c401e27a587c7..70736390774e5a71849852d64f09ea9740516606 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -66,7 +66,7 @@ class COpendriveRoad
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road);
- void get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road);
+ void get_sub_road(TOpendriveWorldPose &start_pose,double &length,double margin,COpendriveRoad &new_road);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 42a9a77a2e4fbac46f958e6c1eab6ea52759e1ad..486084a0a2bbe21e54cb2b2a717b0531abd5095e 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -907,66 +907,154 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
return new_road.update_path(new_node_ref,path_nodes);
}
-void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road)
+void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,double margin,COpendriveRoad &new_road)
{
segment_up_ref_t new_segment_ref;
lane_up_ref_t new_lane_ref;
node_up_ref_t new_node_ref;
link_up_ref_t new_link_ref;
- unsigned int segment_index,node_index;
- TOpendriveWorldPose end_pose;
- COpendriveRoadSegment *segment,*new_segment,*next_segment;
- COpendriveRoadNode *node;
- double rem_length=length,distance,start_length;
+ unsigned int segment_index;
+ TOpendriveWorldPose end_pose,int_pose;
+ const COpendriveRoadSegment *segment,*next_segment;
+ COpendriveRoadSegment *new_segment;
+ double rem_length=length+2.0*margin,distance,start_length;
int node_side;
+ bool valid;
new_road.free();
new_road.set_resolution(this->resolution);
new_road.set_scale_factor(this->scale_factor);
new_road.set_min_road_length(this->min_road_length);
- node_index=this->get_closest_node_index(start_pose,distance);
- if(node_index==(unsigned int)-1)
- throw CException(_HERE_,"Start position does not belang to the road");
- node=this->nodes[node_index];
+ // get the first segment
segment_index=this->get_closest_segment_index(start_pose,distance);
if(segment_index==(unsigned int)-1)
- throw CException(_HERE_,"Start position does not belang to the road");
+ throw CException(_HERE_,"Start position does not belong to the road");
segment=this->segments[segment_index];
- node_side=segment->get_node_side(node);
- new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,NULL);
- start_length=new_segment->get_length()-distance;
- if(rem_length<start_length)
+ node_side=segment->get_pose_side(start_pose);
+ if(margin>0.0)
{
if(node_side<0)
- end_pose=new_segment->get_pose_at(rem_length);
+ {
+ rem_length=distance-margin;
+ if(rem_length<0.0)// get the previous segment
+ {
+ next_segment=&segment->get_prev_segment(node_side,valid);
+ if(valid)
+ {
+ segment=next_segment;
+ if(node_side<0)
+ {
+ int_pose=segment->get_pose_at(segment->get_length()+rem_length);
+ distance=segment->get_length()+rem_length;
+ }
+ else
+ {
+ int_pose=segment->get_pose_at(-rem_length);
+ distance=-rem_length;
+ }
+ }
+ else
+ {
+ int_pose=segment->get_pose_at(0.0);
+ distance=0.0;
+ }
+ }
+ else
+ {
+ int_pose=segment->get_pose_at(rem_length);
+ distance-=margin;
+ }
+ }
else
- end_pose=new_segment->get_pose_at(new_segment->get_length()-rem_length);
- segment=new_segment;
- new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
- delete segment;
+ {
+ rem_length=distance+margin-segment->get_length();
+ if(rem_length>0.0)// get the prev segment
+ {
+ next_segment=&segment->get_prev_segment(node_side,valid);
+ if(valid)
+ {
+ segment=next_segment;
+ if(node_side<0)
+ {
+ int_pose=segment->get_pose_at(segment->get_length()-rem_length);
+ distance=segment->get_length()-rem_length;
+ }
+ else
+ {
+ int_pose=segment->get_pose_at(rem_length);
+ distance=rem_length;
+ }
+ }
+ else
+ {
+ int_pose=segment->get_pose_at(segment->get_length());
+ distance=0.0;
+ }
+ }
+ else
+ {
+ int_pose=segment->get_pose_at(distance+margin);
+ distance+=margin;
+ }
+ }
+ }
+ else
+ int_pose=start_pose;
+ rem_length=length+2.0*margin;
+ new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&int_pose,NULL);
+ valid=true;
+ if(new_segment->get_length()<this->min_road_length)
+ {
+ next_segment=&segment->get_next_segment(node_side,valid);
+ segment=next_segment;
+ delete new_segment;
+ if(valid)
+ {
+ new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
+ distance=0.0;
+ }
}
- rem_length-=new_segment->get_length();
- new_road.add_segment(new_segment);
- new_segment_ref[segment]=new_segment;
- while(rem_length>0)
+ if(valid)
{
- next_segment=segment->get_next_segment(node_side);
- if(next_segment==NULL)
- break;
- if((rem_length-next_segment->get_length())<0.0)
+ start_length=new_segment->get_length()-distance;
+ if(rem_length<start_length)
{
if(node_side<0)
- end_pose=next_segment->get_pose_at(rem_length);
+ end_pose=new_segment->get_pose_at(rem_length);
else
- end_pose=next_segment->get_pose_at(next_segment->get_length()-rem_length);
- new_segment=next_segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
+ end_pose=new_segment->get_pose_at(new_segment->get_length()-rem_length);
+ delete new_segment;
+ new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,&end_pose);
}
- else
- new_segment=next_segment->clone(new_node_ref,new_lane_ref,new_link_ref);
rem_length-=new_segment->get_length();
new_road.add_segment(new_segment);
- new_segment_ref[next_segment]=new_segment;
+ new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
+ }
+ while(rem_length>this->resolution && valid)
+ {
+ next_segment=&segment->get_next_segment(node_side,valid);
segment=next_segment;
+ if(valid)
+ {
+ if((rem_length-segment->get_length())<0.0)
+ {
+ if(node_side<0)
+ end_pose=segment->get_pose_at(rem_length);
+ else
+ end_pose=segment->get_pose_at(segment->get_length()-rem_length);
+ new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
+ }
+ else
+ new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
+ if(new_segment->get_length()>this->min_road_length)
+ {
+ rem_length-=new_segment->get_length();
+ new_road.add_segment(new_segment);
+ new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
+ }
+ else
+ break;
+ }
}
length-=rem_length;
new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);