From 5b4036591aba857312cf9d391edc22eee5a744d9 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 24 Aug 2021 09:50:54 +0200
Subject: [PATCH] When path not found, do not optimize it

Reseting ogoal_cc and otime is not desired as the original path is the
first path found (even with reset.)
---
 src/rrtext13.cc | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 8c5a451..6027886 100644
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -138,6 +138,9 @@ void
 RRTExt13::compute_path()
 {
 	RRTS::compute_path();
+	if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
+		return;
+	}
 #if 0 // TODO 0.59 should work for sc4-1-0 only.
 	if (this->goal_.cc() * 0.59 > this->last_goal_cc_
 			&& this->last_goal_cc_ != 0.0) {
@@ -199,8 +202,6 @@ RRTExt13::reset()
 {
 	RRTS::reset();
 	this->opath_.clear();
-	this->ogoal_cc_ = 0.0;
-	this->otime_ = 0.0;
 }
 
 } // namespace rrts
-- 
2.39.2