From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 9 Oct 2019 15:04:59 +0000 (+0200)
Subject: Implement Dijkstra algorithm
X-Git-Tag: v0.3.0~8^2~4
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/a719fe88678e5882cbc547b605ff8619aa8576ee

Implement Dijkstra algorithm
---

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;