From 4beec29941fa54ce09e442504708d02d950cff41 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 25 Aug 2023 13:12:35 +0200
Subject: [PATCH] If the end node is not reachable, it return the maximum cost.

---
 src/dijkstra.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp
index 3af705b..e3089e2 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
-- 
GitLab