Try steering from root to just added node

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

diff --git a/src/rrts.cc b/src/rrts.cc
index 100c8cc243d770b4e979a1b5aefb3b0cc61155be..33600fcf8147631ddff4999ea3f577d840b904a9 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -280,6 +280,16 @@ RRTS::next()
        }
        this->icnt_ += 1;
        auto rs = this->sample();
+#if 1 // anytime RRTs
+{
+       double d1 = this->cost_search(this->nodes_.front(), rs);
+       double d2 = this->cost_search(rs, this->goal_);
+       if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
+               this->icnt_ -= 1;
+               return this->should_continue();
+       }
+}
+#endif
        this->find_nn(rs);
        this->steer(this->nn(), rs);
        if (this->collide_steered()) {
@@ -322,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();
        ////}
@@ -349,6 +368,9 @@ RRTS::next()
 void
 RRTS::reset()
 {
+       if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
+               this->last_goal_cc_ = this->goal_.cc();
+       }
        this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
                this->goal_.e());
        this->path_.clear();