diff --git a/include/road_segment.h b/include/road_segment.h
index 60c62f3d3157ec09a0373a9724782007edd132eb..72310e13aed80b3d469d89de93e2fa64109dbf41 100644
--- a/include/road_segment.h
+++ b/include/road_segment.h
@@ -63,6 +63,7 @@ class CRoadSegment
     void unlink_lanes(unsigned int lane1,unsigned int lane2);
     bool are_lanes_linked(unsigned int lane1,unsigned int lane2);
     void get_connectivity_matrix(Eigen::MatrixXd &connectivity);
+    bool is_input_lane_connected(unsigned int lane);
     double get_length(void);
     bool get_point_at(double distance, double lateral_offset,TPoint &point);
     bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
diff --git a/src/road_segment.cpp b/src/road_segment.cpp
index 33b3b6aa6adb767770cf36bd57ecd5e6e40ecdb3..3dca6c5f1c16f667809a29b0091f1e797d5d84fd 100644
--- a/src/road_segment.cpp
+++ b/src/road_segment.cpp
@@ -350,6 +350,21 @@ void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity)
   connectivity=this->connectivity;
 }
 
+bool CRoadSegment::is_input_lane_connected(unsigned int lane)
+{
+  unsigned int num=0;
+
+  if(lane>=this->connectivity.rows())
+    throw CException(_HERE_,"Invalid lane index");
+  for(unsigned int i=0;i<this->connectivity.cols();i++)
+    if(this->connectivity(lane,i)==1.0)
+      num++;
+  if(num>0)
+    return true;
+  else
+    return false;
+}
+
 double CRoadSegment::get_length(void)
 {
   return this->spline.get_length();
@@ -388,7 +403,7 @@ bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point,
     { 
       lateral_offset=sqrt(pow(target_point.x-closest_point.x,2.0)+pow(target_point.y-closest_point.y,2.0));
       /* check the side */
-      cross=(this->end_point.x-this->start_point.x)*(closest_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(closest_point.x-this->start_point.x);
+      cross=(this->end_point.x-this->start_point.x)*(target_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(target_point.x-this->start_point.x);
       if(cross<0.0)
         lateral_offset*=-1.0;
       return true;