Try steering from root to just added node

diff --git a/src/rrts.cc b/src/rrts.cc
index 2700fd7b929de5938c790d5bd5b46a7c39202ebe..33600fcf8147631ddff4999ea3f577d840b904a9 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -332,7 +332,16 @@ RRTS::next()
                ss--;
                just_added = just_added->p();
        }
-
+#if 1 // test if root -> just_added can be steered directly
+       this->steer(this->nodes_.front(), *just_added);
+       ss = this->steered_.size();
+       if (!this->collide_steered() && this->steered_.size() == ss) {
+               this->join_steered(&this->nodes_.front());
+               just_added->p(this->nodes_.back());
+               just_added->c(this->cost_build(this->nodes_.back(),
+                       *just_added));
+       }
+#endif
        ////if (!this->goal_drivable_from(this->nodes_.back())) {
        ////    return this->should_continue();
        ////}