Repeat optimization while improving

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 1917ddfefb6e1f396547b955755dfa145df9cf4c..8897216871c4dea33e7c13dca44c2159efa95a02 100644 (file)
--- 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();
        }