diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index afa462d5445cbd9a342406150a21dc78595b60a6..5dda2fdefcc418b4e1a93d6d76b454d9451c0a02 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -88,11 +88,11 @@ void COpendriveLane::add_node(COpendriveRoadNode *node)
   for(unsigned int i=0;i<this->nodes.size();i++)
     if(this->nodes[i]==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);
   // add the new node
   this->nodes.push_back(node);
-  // link the new node with the previous one in the current lane, if any
-  if(this->nodes.size()>1)
-    this->nodes[this->nodes.size()-2]->add_link(node,OD_MARK_NONE);
 }
 
 void COpendriveLane::set_resolution(double res)
@@ -244,14 +244,14 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void)
     track_point.t=this->get_offset()-this->get_width()/2.0;
     track_point.s=0.0;
     for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
-      if(this->nodes[0]->get_lane(i).get_id()==this->id)
+      if(&this->nodes[0]->get_lane(i)==this)
         this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
   }
   else
   {
     track_point.t=this->get_offset()+this->get_width()/2.0;
     for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
-      if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
       {
         track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
         this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
@@ -271,17 +271,17 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void)
   track_point.heading=0.0;
   if(this->id>0)// left lane
   { 
-    track_point.t=this->get_offset()-this->get_width()/2.0;
+    track_point.t=this->get_offset()+this->get_width()/2.0;
     track_point.s=0.0;
-    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
-      if(this->nodes[0]->get_lane(i).get_id()==this->id)
-        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
+      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
+        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
   }
   else
   { 
-    track_point.t=this->get_offset()+this->get_width()/2.0;
+    track_point.t=this->get_offset()-this->get_width()/2.0;
     for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
-      if(this->nodes[this->nodes.size()-1]->get_lane(i).get_id()==this->id)
+      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
       {
         track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
         this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);