diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 0e083dedf01bb0a5257b68f3a0752f0f8c07775c..e006cb5cf12c8dd44b5a7df820b75a6011b3ecfc 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -44,11 +44,14 @@ class COpendriveRoadNode
     TOpendriveWorldPoint get_start_pose(void) const;
     unsigned int get_num_links(void) const;
     const COpendriveLink &get_link(unsigned int index) const;
+    const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
     unsigned int get_num_lanes(void) const; 
     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,double angle_tol=0.1);
+    unsigned int get_closest_link(double x, double y,double heading,double angle_tol=0.1) const;
+    double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
+    void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
     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 3c0dfeaf6befd43b642b2f4dabc39ab7620469bf..31c212895ce180ce9fd812740914fed8a7717398 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -249,6 +249,17 @@ const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const
     throw CException(_HERE_,"Invalid link index");
 }
 
+const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_index) const
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+  {
+    if(this->links[i]->get_next().get_index()==node_index)
+      return *this->links[i];
+  }
+
+  throw CException(_HERE_,"No link in this node ends at the desired node");
+}
+
 unsigned int COpendriveRoadNode::get_num_lanes(void) const
 {
   return this->lanes.size();
@@ -275,7 +286,39 @@ 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 angle_tol)
+unsigned int COpendriveRoadNode::get_closest_link(double x, double y,double heading,double angle_tol)const 
+{
+  double dist,min_dist=std::numeric_limits<double>::max();
+  TOpendriveWorldPoint target;
+  unsigned int closest_index;
+  double angle;
+  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
+    {
+      this->links[i]->find_closest_world_point(target,point);
+      angle=diff_angle(normalize_angle(heading),point.heading);
+      if(fabs(angle)<angle_tol)
+      {
+        dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+        if(dist<min_dist)
+        {
+          min_dist=dist;
+          closest_index=i;
+        }
+      }
+    }
+  }
+
+  return closest_index;
+}
+
+double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol) const
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
@@ -291,7 +334,6 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
       length=this->links[i]->find_closest_world_point(target,point);
-      std::cout << "  " << point.x << "," << point.y << "," << point.heading << "," << length << std::endl;
       angle=diff_angle(normalize_angle(heading),point.heading);
       if(fabs(angle)<angle_tol)
       {
@@ -311,6 +353,41 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
   return closest_length;
 }
 
+void COpendriveRoadNode::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+{
+  double dist;
+  double angle;
+  double length;
+  TOpendriveWorldPoint target,new_point;
+  TPoint point;
+
+  target.x=x;
+  target.y=y;
+  target.heading=heading;
+  closest_points.clear();
+  closest_lengths.clear();
+  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(fabs(angle)<angle_tol)
+      {
+        dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
+        if(dist<=dist_tol)
+        {
+          new_point.x=point.x;
+          new_point.y=point.y;
+          new_point.heading=point.heading;
+          closest_points.push_back(new_point);
+          closest_lengths.push_back(length);
+        }
+      }
+    }
+  }
+}
+
 std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
   out << "    Node: " << node.get_index() << std::endl;