From c7354e5d47a38c31427b9bf01c3bc41bb9d1cf38 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 30 Aug 2021 12:57:56 +0200 Subject: [PATCH 1/1] Repeat optimization while improving --- src/rrtext13.cc | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/rrtext13.cc b/src/rrtext13.cc index 1917ddf..8897216 100644 --- a/src/rrtext13.cc +++ b/src/rrtext13.cc @@ -196,14 +196,19 @@ RRTExt13::compute_path() if (measure_time) { this->otime_ = -this->ter_.scnt(); } - this->pick_interesting(); - this->dijkstra_forward(); -#if 0 // TODO Fix as the code does not always finish. - RRTS::compute_path(); - this->pick_interesting(); - this->dijkstra_backward(); -#endif - RRTS::compute_path(); + double curr_cc = this->goal_.cc(); + double last_cc = curr_cc + 1.0; + while (curr_cc < last_cc) { + last_cc = curr_cc; + RRTS::compute_path(); + this->interesting_forward(); + this->dijkstra_forward(); + RRTS::compute_path(); + this->interesting_backward(); + this->dijkstra_backward(); + RRTS::compute_path(); + curr_cc = this->goal_.cc(); + } if (measure_time) { this->otime_ += this->ter_.scnt(); } -- 2.39.2