From 100e888490f064663dfb3ce9c71ab65592bccb6a Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 24 Aug 2021 09:59:01 +0200 Subject: [PATCH 1/1] Try steering from root to just added node --- src/rrts.cc | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/rrts.cc b/src/rrts.cc index 2700fd7..33600fc 100644 --- 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(); ////} -- 2.39.2