Implement Dijkstra algorithm

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 211e5b5e285cb8ba59babfeb1760d25b9304ee03..06c35c473a3f47c2a8ab6729f83ea286c074777a 100644 (file)
--- 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;