Retrieve furthest Dijkstra node

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 06c35c473a3f47c2a8ab6729f83ea286c074777a..e6d09739a858f4e25f476627f49dfaaf95e4045e 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -81,6 +81,13 @@ std::vector<RRTNode *> RRTExt3::path()
                         }
                 }
         }
+        DijkstraNode *ln = nullptr;
+        for (auto &n: dn) {
+                if (n.v)
+                        ln = &n;
+        }
+        if (ln == nullptr || ln->p() == nullptr)
+                return this->orig_path();
         std::vector<RRTNode *> path;
         return path;
 }