Change steer step to 0.5 m

[hubacji1/rrts.git] / src / rrtext12.cc

diff --git a/src/rrtext12.cc b/src/rrtext12.cc
index 561b23484c5e2f8f95d8a2395cae2df3d9e3d303..4dacd107e8bc13869f9f7d0effa6909346f126f3 100644 (file)
--- a/src/rrtext12.cc
+++ b/src/rrtext12.cc
@@ -1,5 +1,4 @@
 #include "rrtext.h"
-#include <iostream>
 
 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
 {
@@ -14,6 +13,121 @@ void RRTExt12::steer1(RRTNode &f, RRTNode &t)
         bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
         bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
         this->steered().clear();
+        while (true) {
+                bc.next();
+                auto n = RRTNode(bc);
+                if (
+                        this->steered().size() == 0
+                        && std::get<0>(this->collide_two_nodes(f, n))
+                ) {
+                    return;
+                } else if (
+                        (
+                                this->steered().size() > 0
+                                && std::get<0>(this->collide_two_nodes(
+                                        this->steered().back(),
+                                        n
+                                ))
+                        )
+                        || this->steered().size() > 25
+                ) {
+                        break;
+                }
+                this->steered().push_back(RRTNode(bc));
+        }
+        bc.sp(-1*bc.sp());
         bc.next();
-        this->steered().push_back(RRTNode(bc));
+        bc.sp(-1*bc.sp());
+        this->samples().back().x(bc.x());
+        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.5)
+                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->icnt_ > this->log_path_iter_)
+            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;
 }