diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp
index 3af705b92ae8d21c38f4aea6bac9c989ef74da03..e3089e29244c33660356ea4f3ef06a2632dfdcf3 100644
--- a/src/dijkstra.cpp
+++ b/src/dijkstra.cpp
@@ -106,6 +106,8 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n
     d2=new_d2;
   }
   path.clear();
+  if(w(1,end_node)==w(w.rows()-1,end_node) && w(1,end_node)==std::numeric_limits<double>::max()/2.0)
+    return std::numeric_limits<double>::max()/2.0;
   if(end_node==start_node)
     path.push_back(0); 
   else