From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 24 Aug 2021 07:59:01 +0000 (+0200)
Subject: Try steering from root to just added node
X-Git-Tag: v0.9.0~6^2~3
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/100e888490f064663dfb3ce9c71ab65592bccb6a?ds=sidebyside

Try steering from root to just added node
---

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();
 	////}