From a719fe88678e5882cbc547b605ff8619aa8576ee Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 9 Oct 2019 17:04:59 +0200
Subject: [PATCH] Implement Dijkstra algorithm

---
 src/rrtext3.cc | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 211e5b5..06c35c4 100644
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -66,6 +66,20 @@ std::vector<RRTNode *> RRTExt3::path()
         while (!pq.empty()) {
                 DijkstraNode f = pq.top();
                 pq.pop();
+                for (unsigned int i = f.i + 1; i < dncnt; i++) {
+                        double cost = f.cc + this->cost_search(f, dn[i]);
+                        this->steer(f, dn[i]);
+                        if (this->steered().size() == 0)
+                                break; // TODO why not continue?
+                        if (std::get<0>(this->collide_steered_from(f)))
+                                continue;
+                        if (cost < dn[i].cc) {
+                                dn[i].cc = cost;
+                                dn[i].p(f.s);
+                                if (!dn[i].vi())
+                                        pq.push(dn[i]);
+                        }
+                }
         }
         std::vector<RRTNode *> path;
         return path;
-- 
2.39.2