Steer to goals from just added nodes, too

diff --git a/src/rrts.cc b/src/rrts.cc
index e1886ecfb673fd0fdcc31a649e8b75427ead49e6..4428814acb210a3832142a4942a1f42b39225a4b 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -213,6 +213,7 @@ bool RRTS::next()
         if (!this->connect())
                 return next;
         this->rewire();
+        unsigned scnt = this->steered().size();
         this->steered().erase(this->steered().begin());
         while (this->steered().size() > 0) {
                 RRTNode *f = &this->nodes().back();
@@ -222,6 +223,26 @@ bool RRTS::next()
                 t->c(this->cost(*f, *t));
                 this->steered().erase(this->steered().begin());
         }
+        RRTNode *just_added = &this->nodes().back();
+        while (scnt > 0) {
+                scnt--;
+                for (auto &g: this->goals()) {
+                        this->steer(*just_added, g);
+                        if (this->collide_steered_from(*just_added))
+                                continue;
+                        RRTNode *f = just_added;
+                        while (this->steered().size() > 0) {
+                                RRTNode s = this->steered().front();
+                                this->nodes().push_back(s);
+                                RRTNode *t = &this->nodes().back();
+                                t->p(f);
+                                t->c(this->cost(*f, *t));
+                                this->steered().erase(this->steered().begin());
+                                f = t;
+                        }
+                }
+                just_added = just_added->p();
+        }
         for (auto &g: this->goals()) {
                 double cost = this->cost(this->nodes().back(), g);
                 double edist = sqrt(