From e74d068c03166b4462de5c01477c98c477983e1f Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 24 Aug 2021 09:59:53 +0200
Subject: [PATCH] Compute path only at the end of each iteration

---
 src/rrts.cc | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/rrts.cc b/src/rrts.cc
index 33600fc..8e1280e 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -303,6 +303,7 @@ RRTS::next()
 	unsigned int ss = this->steered_.size();
 	this->join_steered(&this->nodes_.back());
 	RRTNode* just_added = &this->nodes_.back();
+	bool gf = false;
 	while (ss > 0 && just_added->p() != nullptr) {
 		//if (!this->goal_drivable_from(*just_added)) {
 		//	ss--;
@@ -326,7 +327,7 @@ RRTS::next()
 					|| ncc < this->goal_.cc()) {
 				this->goal_.p(this->nodes_.back());
 				this->goal_.c(nc);
-				this->compute_path();
+				gf = true;
 			}
 		}
 		ss--;
@@ -342,6 +343,9 @@ RRTS::next()
 			*just_added));
 	}
 #endif
+	if (gf) {
+		this->compute_path();
+	}
 	////if (!this->goal_drivable_from(this->nodes_.back())) {
 	////	return this->should_continue();
 	////}
-- 
2.39.2