diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index f42ec44c4025adc8533ce6433058689bea7c4d44..65d5fdb6174c4b8ecd840b57c7fab103bfc46cde 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -48,6 +48,7 @@ class COpendriveRoadNode
     const COpendriveLane &get_lane(unsigned int index) const;
     unsigned int get_num_geometries(void) const;
     const COpendriveGeometry &get_geometry(unsigned int index) const;
+    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
     ~COpendriveRoadNode();
 };
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 91b3e7442005b854ad2d6afac59fdf1be41a7689..eb9107732acf0ceb54c35b626e525d5d08db4390 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -1,6 +1,7 @@
 #include "opendrive_road_node.h"
 #include "opendrive_road.h"
 #include "exceptions.h"
+#include <math.h>
 
 COpendriveRoadNode::COpendriveRoadNode()
 {
@@ -42,7 +43,10 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
 
 void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark)
 {
+  TOpendriveWorldPoint lane_end,node_start;
+  double start_curvature,end_curvature,length;
   COpendriveLink *new_link;
+  bool lane_found=false;
 
   for(unsigned int i=0;i<this->links.size();i++)
     if(this->links[i]->prev==this && this->links[i]->next==node)
@@ -53,7 +57,31 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
   new_link->set_resolution(this->resolution);
   new_link->set_scale_factor(this->scale_factor);
   new_link->set_road_mark(mark);
-  new_link->generate();
+  // get the curvature
+  node_start=node->get_start_pose();
+  for(unsigned int i=0;i<this->lanes.size();i++)
+  {
+    lane_end=this->lanes[i]->get_end_point();
+    if(fabs(lane_end.x-node_start.x)<this->resolution && fabs(lane_end.y-node_start.y)<this->resolution)
+    {
+      this->geometries[i]->get_curvature(start_curvature,end_curvature);
+      length=this->geometries[i]->get_length();
+      if(this->lanes[i]->get_id()>0)
+      {
+        start_curvature*=-1.0;
+        end_curvature*=-1.0;
+      }
+      lane_found=true;
+      break;
+    }
+  }
+  if(!lane_found)
+  {
+    start_curvature=0.0;
+    end_curvature=0.0;
+    length=sqrt(pow(this->start_point.x-node_start.x,2.0)+pow(this->start_point.y-node_start.y,2.0));
+  }
+  new_link->generate(start_curvature,end_curvature,length,lane_found);
   this->links.push_back(new_link);
   node->links.push_back(new_link);
 }
@@ -244,6 +272,42 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
     throw CException(_HERE_,"Invalid geometry index");
 }
 
+double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
+{
+  double dist,min_dist=std::numeric_limits<double>::max();
+  double angle,min_angle=std::numeric_limits<double>::max();
+  double length,closest_length;
+  TOpendriveWorldPoint target;
+  TPoint point;
+
+  target.x=x;
+  target.y=y;
+  target.heading=heading;
+  for(unsigned int i=0;i<this->links.size();i++)
+  {
+    if(&this->links[i]->get_prev()==this)// links starting at this node
+    {
+      length=this->links[i]->find_closest_world_point(target,point);
+      angle=diff_angle(normalize_angle(heading),point.heading);
+      if(angle<min_angle)
+      {
+        dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+        if(dist<min_dist)
+        {
+          min_angle=angle;
+          min_dist=dist;
+          closest_point.x=point.x;
+          closest_point.y=point.y;
+          closest_point.heading=point.heading;
+          closest_length=length;
+        }
+      }
+    }
+  }
+
+  return closest_length;
+}
+
 std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
   out << "    Node: " << node.get_index() << std::endl;