From d5139fdff547a48a1fa9fc7cba72460063f95baf Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 29 Mar 2021 12:28:36 +0200
Subject: [PATCH] Override connect and next methods

Due to using use_nn variable when sampling case 3, there is no need for the
nearest neighbor search during the steer 1. Also, connect procedure does not
need to check for better neighbors -- there are none.
---
 api/rrtext.h    |  3 ++
 api/rrts.h      |  4 +--
 src/rrtext12.cc | 89 +++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 94 insertions(+), 2 deletions(-)

diff --git a/api/rrtext.h b/api/rrtext.h
index 30a8fa5..5f7cdf8 100644
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -23,6 +23,9 @@ Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
 class RRTExt12 : public virtual RRTS {
         protected:
                 void steer1(RRTNode &f, RRTNode &t);
+                bool connect();
+        public:
+                bool next();
 };
 
 /*! \brief Goal Zone.
diff --git a/api/rrts.h b/api/rrts.h
index f64dc33..e5821a0 100644
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -211,7 +211,7 @@ class RRTS {
                 void join_tmp_steered(RRTNode *f);
                 virtual bool goal_found(RRTNode &f);
                 // RRT* procedures
-                bool connect();
+                virtual bool connect();
                 void rewire();
         public:
                 /// ---
@@ -254,7 +254,7 @@ class RRTS {
                 bool should_continue();
                 /*! \brief Run next RRT* iteration.
                 */
-                bool next();
+                virtual bool next();
                 /*! \brief Set sampling info.
 
                 Based on `sample_dist_type`, set proper distribution
diff --git a/src/rrtext12.cc b/src/rrtext12.cc
index b47a7ce..1565faf 100644
--- a/src/rrtext12.cc
+++ b/src/rrtext12.cc
@@ -42,3 +42,92 @@ void RRTExt12::steer1(RRTNode &f, RRTNode &t)
         this->samples().back().y(bc.y());
         this->samples().back().h(bc.h());
 }
+bool RRTExt12::connect()
+{
+        RRTNode *t = &this->steered().front();
+        RRTNode *f = this->use_nn;
+        double cost = this->cost_search(*f, *t);
+        // steer from f->t and then continue with the steered.
+        this->tmp_steer(*f, *t);
+        if (this->tmp_steered_.size() > 0) {
+                auto col = this->collide_tmp_steered_from(*f);
+                if (std::get<0>(col))
+                        return false;
+                this->join_tmp_steered(f);
+                f = &this->nodes().back();
+        }
+        if (sqrt(
+                pow(f->x() - t->x(), 2)
+                + pow(f->y() - t->y(), 2)
+        ) > 0.2)
+                return false;
+        // cont.
+        this->store_node(this->steered().front());
+        t = &this->nodes().back();
+        t->p(f);
+        t->c(this->cost_build(*f, *t));
+        t->set_t(RRTNodeType::connected);
+        return true;
+}
+bool RRTExt12::next()
+{
+        if (this->icnt_ == 0)
+                this->tstart_ = std::chrono::high_resolution_clock::now();
+        bool next = true;
+        if (this->scnt_ > this->log_path_time_)
+            this->log_path_cost();
+        if (this->should_stop())
+                return false;
+        if (this->samples().size() == 0) {
+                this->samples().push_back(RRTNode());
+                this->samples().back().x(this->goals().front().x());
+                this->samples().back().y(this->goals().front().y());
+                this->samples().back().h(this->goals().front().h());
+                this->use_nn = &this->nodes().front();
+        } else {
+                this->sample();
+        }
+        this->steer1(
+                *this->use_nn,
+                this->samples().back()
+        );
+        if (this->steered().size() == 0)
+                return next;
+        auto col = this->collide_steered_from(*this->use_nn);
+        if (std::get<0>(col)) {
+                auto rcnt = this->steered().size() - std::get<1>(col);
+                while (rcnt-- > 0) {
+                        this->steered().pop_back();
+                }
+        }
+        if (!this->connect())
+                return next;
+        this->rewire();
+        unsigned scnt = this->steered().size();
+        this->join_steered(&this->nodes().back());
+        RRTNode *just_added = &this->nodes().back();
+        while (scnt > 0) {
+                // store all the steered1 nodes
+                this->steered1_.push_back(just_added);
+                scnt--;
+                auto &g = this->goals().front();
+                this->steer2(*just_added, g);
+                auto col = this->collide_steered_from(*just_added);
+                if (std::get<0>(col)) {
+                        auto rcnt = this->steered().size() - std::get<1>(col);
+                        while (rcnt-- > 0) {
+                                this->steered().pop_back();
+                        }
+                }
+                this->join_steered(just_added);
+                // store all the steered2 nodes
+                RRTNode* jap = &this->nodes().back();
+                while (jap != just_added) {
+                        this->steered2_.push_back(jap);
+                        jap = jap->p();
+                }
+                this->gf(this->goal_found(this->nodes().back()));
+                just_added = just_added->p();
+        }
+        return next;
+}
-- 
2.39.2