diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 5c65a9eeed7265309abe14d807199a7b8016e668..1a615d4c9b8a03327a848223ed1e431138210287 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -5,16 +5,20 @@
 #include "xml/OpenDRIVE_1.4H.hxx"
 #endif
 
+#include "opendrive_common.h"
 #include "opendrive_road_node.h"
 #include "opendrive_road_segment.h"
-
-class COpendriveRoadSegment;
+#include "opendrive_lane.h"
 
 class COpendriveLane
 {
   friend class COpendriveRoadSegment;
+  friend class COpendriveRoadNode;
+  friend class COpendriveRoad;
   private:
     std::vector<COpendriveRoadNode *> nodes;
+    std::vector<COpendriveLane *> next;
+    std::vector<COpendriveLane *> prev;
     COpendriveLane *left_lane;
     opendrive_mark_t left_mark;
     COpendriveLane *right_lane;
@@ -26,30 +30,52 @@ class COpendriveLane
     double speed;
     double offset;
     int id;
+    unsigned int index;
   protected:
     COpendriveLane();
-    COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref);
+    COpendriveLane(const COpendriveLane &object);
     void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
     void link_neightbour_lane(COpendriveLane *lane);
     void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start);
+    void add_next_lane(COpendriveLane *lane);
+    void add_prev_lane(COpendriveLane *lane);
     void add_node(COpendriveRoadNode *node);
+    bool has_node(COpendriveRoadNode *node);
+    bool has_node(unsigned int index);
+    void set_parent_segment(COpendriveRoadSegment *segment);
     void set_resolution(double res);
     void set_scale_factor(double scale);
+    void set_width(double width);
+    void set_speed(double speed);
     void set_offset(double offset);
-    void update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs);
+    void set_id(int id);
+    void set_index(unsigned int index);
+    bool updated(lane_up_ref_t &refs);
+    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
+    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
+    void update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL);
+    COpendriveRoadNode *update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end=NULL);
+    COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,COpendriveRoadNode **new_node,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
+    COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
   public:
     unsigned int get_num_nodes(void) const;
     const COpendriveRoadNode &get_node(unsigned int index) const;
     const COpendriveRoadSegment &get_segment(void) const;
+    unsigned int get_num_next_lanes(void) const;
+    const COpendriveLane &get_next_lane(unsigned int index) const;
+    unsigned int get_num_prev_lanes(void) const;
+    const COpendriveLane &get_prev_lane(unsigned int index) const;
     double get_width(void) const;
     double get_speed(void) const;
     double get_offset(void) const;
+    unsigned int get_index(void) const;
     int get_id(void) const;
     TOpendriveWorldPoint get_start_point(void) const;
     TOpendriveWorldPoint get_end_point(void) const;
     double get_length(void) const;
     TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
     TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
+    unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();
 };
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 01e25428bd61acda5bbb858fbd4511a9e13b3199..a523f0b26176c523d690065be6f4d6bd13ac0243 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -1,9 +1,12 @@
 #include "opendrive_lane.h"
 #include "exceptions.h"
+#include <math.h>
 
 COpendriveLane::COpendriveLane()
 {
   this->nodes.clear();
+  this->next.clear();
+  this->prev.clear();
   this->left_lane=NULL;
   this->left_mark=OD_MARK_NONE;
   this->right_lane=NULL;
@@ -15,24 +18,26 @@ COpendriveLane::COpendriveLane()
   this->speed=0.0;
   this->offset=0.0;
   this->id=0;
+  this->index=-1;
 }
 
-COpendriveLane::COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref)
+COpendriveLane::COpendriveLane(const COpendriveLane &object)
 {
-  this->nodes.resize(object.nodes.size());
-  for(unsigned int i=0;i<object.nodes.size();i++)
-    this->nodes[i]=node_refs[object.nodes[i]];
+  this->nodes=object.nodes;
+  this->next=object.next;
+  this->prev=object.prev;
   this->left_lane=object.left_lane;
   this->left_mark=object.left_mark;
   this->right_lane=object.right_lane;
   this->right_mark=object.right_mark;
-  this->segment=segment_ref;
+  this->segment=object.segment;
   this->resolution=object.resolution;
   this->scale_factor=object.scale_factor;
   this->width=object.width;
   this->speed=object.speed;
   this->offset=object.offset;
   this->id=object.id;
+  this->index=object.index;
 }
 
 void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
@@ -47,8 +52,8 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
       {
         for(unsigned int i=0;i<this->get_num_nodes();i++)
         {
-          this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark);
-          lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark);
+          this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark,this->segment,this);
+          lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark,this->segment,this);
           lane->right_mark=this->left_mark;
         }
       }
@@ -56,8 +61,8 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
       {
         for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
         {
-          this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark);
-          lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark);
+          this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark,this->segment,this);
+          lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,this);
           if(this->id>0)// left lanes
             this->right_mark=lane->left_mark;
           else// right lanes
@@ -122,25 +127,71 @@ void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool f
             end=lane->nodes[0];
         }
       }
-      start->add_link(end,mark);
+      start->add_link(end,mark,this->segment,this);
+      // link lane
+      // link laness
+      this->add_next_lane(lane);
+      lane->add_prev_lane(this);
     }
     else 
       throw CException(_HERE_,"One of the lanes to link has no nodes");
   }
 }
 
-void COpendriveLane::add_node(COpendriveRoadNode *node)
+void COpendriveLane::add_next_lane(COpendriveLane *lane)
 {
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    if(this->nodes[i]==node)
+  for(unsigned int i=0;i<this->next.size();i++)
+    if(this->next[i]==lane)// lane is already linked
       return;
+
+  // add the lane
+  this->next.push_back(lane);
+}
+
+void COpendriveLane::add_prev_lane(COpendriveLane *lane)
+{
+  for(unsigned int i=0;i<this->prev.size();i++)
+    if(this->prev[i]==lane)// lane is already linked
+      return;
+
+  // add the lane
+  this->prev.push_back(lane);
+}
+
+void COpendriveLane::add_node(COpendriveRoadNode *node)
+{
+  if(this->has_node(node))
+    return;
   // link the new node with the previous one in the current lane, if any
   if(this->nodes.size()>0)
-    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
+    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE,this->segment,this);
   // add the new node
   this->nodes.push_back(node);
 }
 
+bool COpendriveLane::has_node(COpendriveRoadNode *node)
+{
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]==node)
+      return true;
+
+  return false;
+}
+
+bool COpendriveLane::has_node(unsigned int index)
+{
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]->get_index()==index)
+      return true;
+
+  return false;
+}
+
+void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment)
+{
+  this->segment=segment;
+}
+
 void COpendriveLane::set_resolution(double res)
 {
   this->resolution=res;
@@ -157,18 +208,259 @@ void COpendriveLane::set_scale_factor(double scale)
     this->nodes[i]->set_scale_factor(scale);
 }
 
+void COpendriveLane::set_width(double width)
+{
+  this->width=width;
+}
+
+void COpendriveLane::set_speed(double speed)
+{
+  this->speed=speed;
+}
+
 void COpendriveLane::set_offset(double offset)
 {
   this->offset=offset;
 }
 
-void COpendriveLane::update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs)
+void COpendriveLane::set_id(int id)
 {
-  this->left_lane=lane_refs[this->left_lane];
-  this->right_lane=lane_refs[this->right_lane];
+  this->id=id;
+}
 
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    this->nodes[i]->update_references(lane_refs);
+void COpendriveLane::set_index(unsigned int index)
+{
+  this->index=index;
+}
+
+bool COpendriveLane::updated(lane_up_ref_t &refs)
+{
+  lane_up_ref_t::iterator updated_it;
+
+  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
+    if(updated_it->second==this)
+      return true;
+
+  return false;
+}
+
+void COpendriveLane::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
+{
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveRoadNode *>::iterator node_it;
+ 
+  if(this->updated(lane_refs))
+  {
+    for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
+    {
+      if(node_refs.find(*node_it)!=node_refs.end())
+      {
+        (*node_it)=node_refs[*node_it];
+        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
+      }
+      else if((*node_it)->updated(node_refs))
+        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
+    }
+    for(lane_it=this->next.begin();lane_it!=this->next.end();lane_it++)
+      if(lane_refs.find(*lane_it)!=lane_refs.end())
+        (*lane_it)=lane_refs[*lane_it];
+    for(lane_it=this->prev.begin();lane_it!=this->prev.end();lane_it++)
+      if(lane_refs.find(*lane_it)!=lane_refs.end())
+        (*lane_it)=lane_refs[*lane_it];
+    if(lane_refs.find(this->left_lane)!=lane_refs.end())
+      this->left_lane=lane_refs[this->left_lane];
+    if(lane_refs.find(this->right_lane)!=lane_refs.end())
+      this->right_lane=lane_refs[this->right_lane];
+    if(segment_refs.find(this->segment)!=segment_refs.end())
+      this->segment=segment_refs[this->segment];
+  }
+}
+
+void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
+{
+  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<COpendriveRoadNode *>::iterator node_it;
+
+  if(this->updated(lane_refs))
+  {
+    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
+    {
+      if(node_refs.find(*node_it)!=node_refs.end())
+      {
+        (*node_it)=node_refs[*node_it];
+        (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
+        node_it++;
+      }
+      else if((*node_it)->updated(node_refs))
+      {
+        (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
+        node_it++;
+      }
+      else
+        node_it=this->nodes.erase(node_it);
+    }
+    for(lane_it=this->next.begin();lane_it!=this->next.end();)
+    {
+      if(lane_refs.find(*lane_it)!=lane_refs.end())
+      {
+        (*lane_it)=lane_refs[*lane_it];
+        lane_it++;
+      }
+      else if(!(*lane_it)->updated(lane_refs))
+        lane_it=this->next.erase(lane_it);
+      else
+        lane_it++;
+    }
+    for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
+    {
+      if(lane_refs.find(*lane_it)!=lane_refs.end())
+      {
+        (*lane_it)=lane_refs[*lane_it];
+        lane_it++;
+      }
+      else if(!(*lane_it)->updated(lane_refs))
+        lane_it=this->prev.erase(lane_it);
+      else
+        lane_it++;
+    }
+    if(lane_refs.find(this->left_lane)!=lane_refs.end())
+      this->left_lane=lane_refs[this->left_lane];
+    else if(!this->left_lane->updated(lane_refs))
+      this->left_lane=NULL;
+    if(lane_refs.find(this->right_lane)!=lane_refs.end())
+      this->right_lane=lane_refs[this->right_lane];
+    else if(!this->right_lane->updated(lane_refs))
+      this->right_lane=NULL;
+  }
+}
+
+void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start)
+{
+  std::vector<COpendriveRoadNode *>::iterator it;
+  segment_up_ref_t new_segment_ref;
+  node_up_ref_t::iterator exist_it;
+  unsigned int start_node_index,i;
+  COpendriveRoadNode *new_node;
+  bool exists;
+  
+  if(start==NULL)
+    return;
+  start_node_index=this->get_closest_node(*start,3.14159);
+  // eliminate all the node before the start one
+  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
+  {
+    if(i<start_node_index)
+      it=this->nodes.erase(it);
+    else
+    {
+      exists=false;
+      // avoid creating the node for a second time
+      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
+        if((*it)==exist_it->second)
+        {
+          exists=true;
+          break;
+        } 
+      if(!exists)
+      {
+        new_node=new COpendriveRoadNode(**it);
+        new_node_ref[*it]=new_node;
+      }
+      it++;
+    }
+  }
+  this->prev.clear();// it is no longer connected to any previous lane
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
+  this->nodes[0]->update_start_pose(this,start);
+}
+
+COpendriveRoadNode *COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end)
+{
+  std::vector<COpendriveRoadNode *>::iterator it;
+  segment_up_ref_t new_segment_ref;
+  node_up_ref_t::iterator exist_it;
+  unsigned int end_node_index,i;
+  COpendriveRoadNode *new_node;
+  bool exists;
+    
+  if(end==NULL)
+    return NULL;
+  end_node_index=this->get_closest_node(*end,3.14159);
+  // eliminate all the node before the start one
+  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
+  {
+    if(i>end_node_index)
+      it=this->nodes.erase(it);
+    else
+    {
+      exists=false;
+      // avoid creating the node for a second time
+      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
+        if((*it)==exist_it->second)
+        {
+          exists=true;
+          break;
+        } 
+      if(!exists)
+      {
+        new_node=new COpendriveRoadNode(**it);
+        new_node_ref[*it]=new_node;
+      }
+      it++;
+    }
+  }
+  this->next.clear();// it is no longer connected to any next lane
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
+  new_node=this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
+  if(new_node!=NULL)
+  {
+    this->add_node(new_node);
+    new_node_ref[new_node]=new_node;
+    this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
+  }
+
+  return new_node;
+}
+
+COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,COpendriveRoadNode **new_node,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
+{
+  COpendriveLane *new_lane;
+
+  if(start==NULL && end==NULL)
+    return this->clone(new_node_ref,new_lane_ref);
+  new_lane=new COpendriveLane(*this);
+  new_lane_ref[this]=new_lane;
+  if(this->id<0)
+  {
+    new_lane->update_start_node(new_node_ref,new_lane_ref,start);
+    (*new_node)=new_lane->update_end_node(new_node_ref,new_lane_ref,end);
+  }
+  else
+  {
+    new_lane->update_start_node(new_node_ref,new_lane_ref,end);
+    (*new_node)=new_lane->update_end_node(new_node_ref,new_lane_ref,start);
+  }
+
+  return new_lane;
+}
+
+COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
+{
+  COpendriveLane *new_lane;
+  std::vector<COpendriveRoadNode *>::iterator it;
+  COpendriveRoadNode *new_node;
+  segment_up_ref_t new_segment_ref;
+
+  new_lane=new COpendriveLane(*this);
+  new_lane_ref[this]=new_lane;
+  for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++)
+  {
+    new_node=new COpendriveRoadNode(**it);
+    new_node_ref[*it]=new_node;
+  }
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
+
+  return new_lane;
 }
 
 void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
@@ -186,21 +478,21 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
   this->left_mark=OD_MARK_NONE;
   this->right_lane=NULL;
   this->right_mark=OD_MARK_NONE;
-  this->id=lane_info.id().get();
+  this->set_id(lane_info.id().get());
   // import lane width
   if(lane_info.width().size()!=1)
   {
     error << "Only one width record supported for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
-  this->width=lane_info.width().begin()->a().get();
+  this->set_width(lane_info.width().begin()->a().get());
   // import lane speed
   if(lane_info.speed().size()!=1)
   {
     error << "Only one speed record supported for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
-  this->speed=lane_info.speed().begin()->max().get();
+  this->set_speed(lane_info.speed().begin()->max().get());
   //lane mark
   if(lane_info.roadMark().size()==0)
     mark=OD_MARK_NONE;
@@ -238,7 +530,7 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
   else
     this->left_mark=mark;
 
-  this->segment=segment;
+  this->set_parent_segment(segment);
 }
 
 unsigned int COpendriveLane::get_num_nodes(void) const
@@ -254,6 +546,32 @@ const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
     throw CException(_HERE_,"Invalid node index");
 }
 
+unsigned int COpendriveLane::get_num_next_lanes(void) const
+{
+  return this->next.size();
+}
+
+const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const
+{
+  if(index>=0 && index<this->next.size())
+    return *this->next[index];
+  else
+    throw CException(_HERE_,"Invalid next lane index");
+}
+
+unsigned int COpendriveLane::get_num_prev_lanes(void) const
+{
+  return this->prev.size();
+}
+
+const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const
+{
+  if(index>=0 && index<this->prev.size())
+    return *this->prev[index];
+  else
+    throw CException(_HERE_,"Invalid previous lane index");
+}
+
 const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
 {
   return *this->segment;
@@ -274,6 +592,11 @@ double COpendriveLane::get_offset(void) const
   return this->offset/this->scale_factor;
 }
 
+unsigned int COpendriveLane::get_index(void) const
+{
+  return this->index;
+}
+
 int COpendriveLane::get_id(void) const
 {
   return this->id;
@@ -409,6 +732,26 @@ TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoi
   throw CException(_HERE_,"Track point does not belong to lane");
 }
 
+unsigned int COpendriveLane::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
+{
+  double dist,min_dist=std::numeric_limits<double>::max();
+  TOpendriveWorldPoint found_point;
+  unsigned int closest_index;
+
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    this->nodes[i]->get_closest_point(point,found_point,angle_tol);
+    dist=sqrt(pow(found_point.x-point.x,2.0)+pow(found_point.y-point.y,2.0));
+    if(dist<min_dist)
+    {
+      min_dist=dist;
+      closest_index=i;
+    }
+  }
+
+  return closest_index;
+}
+
 std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
 {
   out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
@@ -484,6 +827,8 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
 COpendriveLane::~COpendriveLane()
 {
   this->nodes.clear();
+  this->prev.clear();
+  this->next.clear();
   this->left_lane=NULL;
   this->left_mark=OD_MARK_NONE;
   this->right_lane=NULL;
@@ -494,4 +839,5 @@ COpendriveLane::~COpendriveLane()
   this->width=0.0;
   this->speed=0.0;
   this->offset=0.0;
+  this->index=-1;
 }