From bc7fc6f2607a5e84206d7ec710eadbb37ddddfe4 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 26 Jul 2019 10:50:03 +0200
Subject: [PATCH] Implement naive connect

---
 src/rrts.cc | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/rrts.cc b/src/rrts.cc
index 4c5c004..8ddd06d 100644
--- 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()
-- 
2.39.2