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