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