Implement naive next

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index d0e30b8c879cbd0cc06f0936444e51382dfaa9cb..d5faf1f75ebf9f45205119c2a2b3f5900d19c94f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -109,6 +109,29 @@ std::vector<RRTNode *> RRTS::path()
 bool RRTS::next()
 {
         bool next = true;
+        this->icnt_++;
+        this->sample();
+        this->steer(
+                *this->nn(this->samples().back()),
+                this->samples().back()
+        );
+        this->connect();
+        this->rewire();
+        for (auto &n: this->goals()) {
+                double cost = this->cost(this->nodes().back(), n);
+                if (cost < ETA) {
+                        next = false;
+                        if (
+                                n.p() == nullptr
+                                || this->nodes().back().c() + cost < n.c()
+                        ) {
+                                n.p(&this->nodes().back());
+                                n.c(this->nodes().back().c() + cost);
+                        }
+                }
+        }
+        if (this->icnt_ > 999)
+                next = false;
         return next;
 }