Switch to RRT algorithm

RRT* (RRTS) differst from RRT slightly. Therefore, it's possible to
"just" comment out some code of RRT* to make it RRT.

The reason to do this is to find out the performance of new search cost
that includes the number of backward-forward direction changes.

diff --git a/src/rrts.cc b/src/rrts.cc
index 15f2436e8d3142bb2f61b876428e124bac0b799e..e08fed97acef4c9c42a4a861b785fc6a5ffd5a25 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -8,6 +8,10 @@
 #include <cassert>
 #include "rrts.hh"
 
+#ifndef USE_RRTS
+#define USE_RRTS 0  // TODO improve, this solution isn't clear.
+#endif
+
 namespace rrts {
 
 void
@@ -140,6 +144,7 @@ RRTS::connect()
 {
        RRTNode* f = this->nn_;
        RRTNode* t = &this->steered_.front();
+#if USE_RRTS
        double cost = f->cc() + this->cost_build(*f, *t);
        for (auto n: this->nv_) {
                double nc = n->cc() + this->cost_build(*n, *t);
@@ -155,6 +160,7 @@ RRTS::connect()
        if (!this->bc_.drivable(*t)) {
                f = this->nn_;
        }
+#endif
        this->store(this->steered_.front());
        t = &this->nodes_.back();
        t->p(*f);
@@ -337,11 +343,15 @@ RRTS::next()
        if (this->collide_steered()) {
                return this->should_continue();
        }
+#if USE_RRTS
        this->find_nv(this->steered_.front());
+#endif
        if (!this->connect()) {
                return this->should_continue();
        }
+#if USE_RRTS
        this->rewire();
+#endif
        unsigned int ss = this->steered_.size();
        this->join_steered(&this->nodes_.back());
        RRTNode* just_added = &this->nodes_.back();