Implement naive connect

diff --git a/src/rrts.cc b/src/rrts.cc
index 4c5c004e3d0ff3ca3206cc3864b6764e33c5b5de..8ddd06d765a7148967c2b0ead41fb79a24a40428 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -71,6 +71,23 @@ void RRTS::steer(RRTNode &f, RRTNode &t)
 // RRT* procedures
 bool RRTS::connect()
 {
+        bool conn = false;
+        RRTNode *t = &this->steered().front();
+        RRTNode *f = this->nn(this->samples().back());
+        double cost = this->cost(*f, *t);
+        for (auto n: this->nv(*t)) {
+                if (this->cost(*n, *t) < cost) {
+                        f = n;
+                        cost = this->cost(*n, *t);
+                }
+        }
+        this->nodes().push_back(this->steered().front());
+        this->steered().erase(this->steered().begin());
+        t = &this->nodes().back();
+        t->p(f);
+        t->c(f->c() + this->cost(*f, *t));
+        conn = true;
+        return conn;
 }
 
 void RRTS::rewire()