diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 37dd02c066f6cff8add1c72b956c003e7ad8148c..fb263febcec659ee1675d451d9b074e7f6ff9a09 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -71,8 +71,9 @@ class COpendriveRoadSegment
     void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
     bool connects_to(COpendriveRoadSegment *segment);
     bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
-    COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
-    COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
+    COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
+    COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
+    COpendriveRoadSegment *get_next_segment(int &side);
   public:
     std::string get_name(void) const;
     unsigned int get_id(void) const;
@@ -89,8 +90,15 @@ class COpendriveRoadSegment
     unsigned int get_num_connecting(void) const;
     const COpendriveRoadSegment &get_connecting(unsigned int index) const;
     double get_length(void);
+    TOpendriveWorldPose get_pose_at(double length);
+    double get_curvature_at(double length);
     TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
     TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
+    const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1);
+    int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
     ~COpendriveRoadSegment();
 };
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 5b40b4745e34ec60153259eeec36638eb0f50ca6..60186394132d3276dc74e92e3a0646d11ee41195 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -94,6 +94,7 @@ void COpendriveRoadSegment::set_resolution(double res)
 void COpendriveRoadSegment::set_scale_factor(double scale)
 {
   std::map<int,COpendriveLane *>::const_iterator lane_it;
+  TPoint spline_start,spline_end;
 
   this->scale_factor=scale;
 
@@ -105,7 +106,20 @@ void COpendriveRoadSegment::set_scale_factor(double scale)
     this->objects[i]->set_scale_factor(scale);
 
   for(unsigned int i=0;i<this->geometries.size();i++)
+  {
     this->geometries[i].opendrive->set_scale_factor(scale);
+    this->geometries[i].spline->get_start_point(spline_start);
+    spline_start.x*=this->scale_factor/scale;
+    spline_start.y*=this->scale_factor/scale;
+    spline_start.curvature*=scale/this->scale_factor;
+    this->geometries[i].spline->set_start_point(spline_start);
+    this->geometries[i].spline->get_end_point(spline_end);
+    spline_end.x*=this->scale_factor/scale;
+    spline_end.y*=this->scale_factor/scale;
+    spline_end.curvature*=scale/this->scale_factor;
+    this->geometries[i].spline->set_end_point(spline_end);
+    this->geometries[i].spline->generate(this->resolution);
+  }
 }
 
 void COpendriveRoadSegment::set_min_road_length(double length)
@@ -396,7 +410,7 @@ void COpendriveRoadSegment::add_nodes(void)
 void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane)
 {
   TOpendriveTrackPose track_pose;
-  double start_curvature,end_curvature,length;
+  double start_curvature,end_curvature;
   COpendriveRoadNode *new_node;
   TOpendriveWorldPose node_pose;
   unsigned int node_index;
@@ -440,11 +454,10 @@ void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COp
       end_curvature=1.0/((1.0/end_curvature)-lane->get_center_offset());
     else
       end_curvature=1.0/((1.0/end_curvature)+lane->get_center_offset());
-    length=geometry.opendrive->get_length();
     if(lane->get_id()<0)
-      new_node->add_parent(lane,this,length,start_curvature,end_curvature);
+      new_node->add_parent(lane,this,start_curvature,end_curvature);
     else
-      new_node->add_parent(lane,this,length,-end_curvature,-start_curvature);
+      new_node->add_parent(lane,this,-end_curvature,-start_curvature);
     for(unsigned int i=0;i<this->nodes.size();i++)
       if(this->nodes[i]==new_node)
       {
@@ -591,7 +604,7 @@ void COpendriveRoadSegment::link_neightbour_lanes(void)
     if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
       this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
     else
-      this->lanes[i]->set_right_lane(NULL,this->center_mark);
+      this->lanes[i]->set_left_lane(NULL,this->center_mark);
   }
 }
 
@@ -609,11 +622,11 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment)
       if(segment->lanes.find(j)!=segment->lanes.end())
       {
         if(j==i-1)
-          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true);
+          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true,false);
         else if(j==i)
-          this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true);
+          this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true,true);
         else
-          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true);
+          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true,false);
       }
     }
   }
@@ -624,11 +637,11 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment)
       if(this->lanes.find(j)!=this->lanes.end())
       {
         if(j==i-1)
-          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true);
+          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true,false);
         else if(j==i)
-          segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
+          segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true,true);
         else
-          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true);
+          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true,false);
       }
     }
   }
@@ -644,19 +657,19 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment,int from
   if(segment->lanes.find(to)!=segment->lanes.end())
   {
     if(this->lanes.find(from-1)!=this->lanes.end())
-      this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
+      this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start,false);
     if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start);
+      this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start,true);
     if(this->lanes.find(from+1)!=this->lanes.end())
-      this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
+      this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start,false);
   }
 /*
   if(segment.lanes.find(to-1)!=segment.lanes.end())
     if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start);
+      this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start,false);
   if(segment.lanes.find(to+1)!=segment.lanes.end())
     if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start);
+      this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start,false);
 */
 }
 
@@ -677,8 +690,9 @@ bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,CO
     return false;
 }
 
-COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
+COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
 {
+  TOpendriveWorldPose *start_pose,*end_pose;
   std::map<int,COpendriveLane *>::iterator lane_it;
   lane_up_ref_t::iterator lane_ref_it;
   COpendriveLane *new_lane;
@@ -692,48 +706,60 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
   double length;
 
   if(start==NULL && end==NULL)
-    return this->clone(new_node_ref,new_lane_ref);
+    return this->clone(new_node_ref,new_lane_ref,new_link_ref);
   new_segment=new COpendriveRoadSegment(*this);
   new_segment_ref[this]=new_segment;
+  if(node_side<0)
+  {
+    start_pose=start;
+    end_pose=end;
+  }
+  else
+  {
+    start_pose=end;
+    end_pose=start;
+  }
   /* get the sublanes */
   for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
   {
-    if(node_side<0)
-      new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,start,end);
-    else
-      new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,end,start);
+    new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,new_link_ref,start_pose,end_pose);
     new_lane_ref[lane_it->second]=new_lane;
   }
   new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
   // update geometry
-  for(geom_it=this->geometries.begin();geom_it!=this->geometries.end();)
+  if(start_pose!=NULL)
   {
-    if(start!=NULL)
+    for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
     {
-      length=geom_it->spline->find_closest_point(start->x,start->y,new_point);
-      if(length>geom_it->opendrive->get_length())
-        geom_it=this->geometries.erase(geom_it);
+      length=geom_it->spline->find_closest_point(start_pose->x,start_pose->y,new_point);
+      if(fabs(length-geom_it->spline->get_length())<this->resolution)
+        geom_it=new_segment->geometries.erase(geom_it);
       else
       {
         geom_it->spline->set_start_point(new_point);
         geom_it->spline->generate(this->resolution);
-        geom_it->opendrive->set_start_pose(*start);
+        geom_it->opendrive->set_start_pose(*start_pose);
         geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
         break;
       }
     }
+    for(unsigned int i=1;i<new_segment->geometries.size();i++)
+    {
+      new_segment->geometries[i].opendrive->set_min_s(new_segment->geometries[i].opendrive->get_min_s()-length);
+      new_segment->geometries[i].opendrive->set_max_s(new_segment->geometries[i].opendrive->get_max_s()-length);
+    }  
   }
-  for(geom_it=this->geometries.begin();geom_it!=this->geometries.end();)
+  if(end_pose!=NULL)
   {
-    if(end!=NULL)
+    for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
     {
-      length=geom_it->spline->find_closest_point(end->x,end->y,new_point);
-      if(length<geom_it->opendrive->get_length())
+      length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point);
+      if(length<geom_it->spline->get_length())
       {
         geom_it->spline->set_end_point(new_point);
         geom_it->spline->generate(this->resolution);
-        geom_it->opendrive->set_max_s(length);;
-        geom_it=this->geometries.erase(geom_it+1,this->geometries.end());
+        geom_it->opendrive->set_max_s(geom_it->opendrive->get_min_s()+length);;
+        geom_it=new_segment->geometries.erase(++geom_it,new_segment->geometries.end());
         break;
       }
       else
@@ -744,7 +770,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
   return new_segment; 
 }
 
-COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
+COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
 {
   std::map<int,COpendriveLane *>::iterator lane_it;
   lane_up_ref_t::iterator lane_ref_it;
@@ -759,7 +785,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
   /* get the sublanes */
   for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
   {
-    new_lane=lane_it->second->clone(new_node_ref,new_lane_ref);
+    new_lane=lane_it->second->clone(new_node_ref,new_lane_ref,new_link_ref);
     new_lane_ref[lane_it->second]=new_lane;
   }
   new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
@@ -767,6 +793,71 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
   return new_segment;
 }
 
+COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
+{
+  std::vector<COpendriveRoadSegment *> segment_candidates;
+  std::vector<int> side_candidates;
+  bool already_present;
+
+  if(side<0)
+  {
+    for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
+    {
+      const COpendriveLane &lane=this->get_lane(-i);
+      if(lane.get_num_next_lanes()>1)
+        throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+      if(lane.get_num_next_lanes()==1)
+      {
+        already_present=false;
+        for(unsigned int k=0;k<segment_candidates.size();k++)
+          if(segment_candidates[k]==lane.get_next_lane(0).segment)
+          {
+            already_present=true;
+            break;
+          }
+        if(!already_present)
+        {
+          segment_candidates.push_back(lane.get_next_lane(0).segment);
+          side_candidates.push_back(lane.get_next_lane(0).get_id());
+        }
+      }
+    }
+  }
+  else
+  {
+    for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
+    {
+      const COpendriveLane &lane=this->get_lane(i);
+      if(lane.get_num_next_lanes()>1)
+        throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+      if(lane.get_num_next_lanes()==1)
+      {
+        already_present=false;
+        for(unsigned int k=0;k<segment_candidates.size();k++)
+          if(segment_candidates[k]==lane.get_next_lane(0).segment)
+          {
+            already_present=true;
+            break;
+          }
+        if(!already_present)
+        {
+          segment_candidates.push_back(lane.get_next_lane(0).segment);
+          side_candidates.push_back(lane.get_next_lane(0).get_id());
+        }
+      }
+    }
+  }
+  if(segment_candidates.size()==0)
+    return NULL;
+  else if(segment_candidates.size()>1)
+    throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
+  else
+  {
+    side=side_candidates[0];
+    return segment_candidates[0];
+  }
+}
+
 void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
 {
   std::map<int,COpendriveLane *>::const_iterator lane_it;
@@ -953,6 +1044,48 @@ double COpendriveRoadSegment::get_length(void)
   return length;
 }
 
+TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
+{
+  TOpendriveWorldPose world_pose{0};
+  TPoint spline_pose;
+
+  for(unsigned int i=0;i<this->geometries.size();i++)
+  {
+    if((length-this->geometries[i].spline->get_length())<0.0)
+    {
+      spline_pose=this->geometries[i].spline->evaluate(length);
+      world_pose.x=spline_pose.x;
+      world_pose.y=spline_pose.y;
+      world_pose.heading=spline_pose.heading;
+      return world_pose;
+    }
+    else
+      length-=this->geometries[i].spline->get_length();
+  }
+
+  return world_pose;
+}
+
+double COpendriveRoadSegment::get_curvature_at(double length)
+{
+  double curvature;
+  TPoint spline_pose;
+
+  for(unsigned int i=0;i<this->geometries.size();i++)
+  {
+    if((length-this->geometries[i].spline->get_length())<0.0)
+    {
+      spline_pose=this->geometries[i].spline->evaluate(length);
+      curvature=spline_pose.curvature;
+      return curvature;
+    }
+    else
+      length-=this->geometries[i].spline->get_length();
+  }
+
+  return 0.0;
+}
+
 TOpendriveLocalPose COpendriveRoadSegment::transform_to_local_pose(const TOpendriveTrackPose &track)
 {
   TOpendriveTrackPose pose;
@@ -1015,6 +1148,96 @@ TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendr
   throw CException(_HERE_,error.str());
 }
 
+unsigned int COpendriveRoadSegment::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
+{
+  double dist,min_dist=std::numeric_limits<double>::max();
+  TOpendriveWorldPose closest_pose;
+  unsigned int closest_index=-1;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
+    dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+    if(dist<min_dist)
+    {
+      min_dist=dist;
+      closest_index=i;
+    }
+  }
+
+  return closest_index;
+}
+
+const COpendriveRoadNode &COpendriveRoadSegment::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
+{
+  unsigned int closest_index;
+
+  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
+  if(closest_index==(unsigned int)-1)
+    throw CException(_HERE_,"Impossible to find the closest node");
+  return *this->nodes[closest_index];
+}
+
+int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
+{
+  double dist,min_dist=std::numeric_limits<double>::max();
+  TOpendriveWorldPose closest_pose;
+  int closest_id=0;
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
+
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
+  {
+    lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
+    dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+    if(dist<min_dist)
+    {
+      min_dist=dist;
+      closest_id=lane_it->first;
+    }
+  }
+
+  return closest_id;
+}
+
+const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) 
+{
+  int closest_id=this->get_closest_lane_id(pose,distance,angle_tol);
+  if(closest_id==0)
+    throw CException(_HERE_,"Impossible to find the closest lane");
+  return *this->lanes[closest_id];
+}
+
+double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
+{
+  double dist,min_dist=std::numeric_limits<double>::max(),distance;
+  unsigned int closest_index=-1;
+  TPoint closest_spline_point;
+
+  closest_pose.x=std::numeric_limits<double>::max();
+  closest_pose.y=std::numeric_limits<double>::max();
+  closest_pose.heading=std::numeric_limits<double>::max();
+  for(unsigned int i=0;i<this->geometries.size();i++)
+  {
+    this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
+    dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
+    if(dist<min_dist)
+    {
+      min_dist=dist;
+      closest_index=i;
+      closest_pose.x=closest_spline_point.x;
+      closest_pose.y=closest_spline_point.y;
+      closest_pose.heading=normalize_angle(closest_spline_point.heading);
+    }
+  }
+
+  distance=0.0;
+  for(unsigned int i=0;i<closest_index;i++)
+    distance+=this->geometries[i].spline->get_length();
+  distance+=min_dist;
+
+  return distance;
+}
+
 std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
 {
   out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;