Change spacing

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

diff --git a/src/rrtext12.cc b/src/rrtext12.cc
index b47a7cebae612e3f0a534b56416f5f9eec53a2cc..bb6babfda9586e44acd0c687a4c2ffffcf8d663c 100644 (file)
--- a/src/rrtext12.cc
+++ b/src/rrtext12.cc
@@ -2,43 +2,132 @@
 
 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
 {
-        // assume that `t` is circle uniformly sampled
-        auto bc = BicycleCar();
-        bc.x(f.x());
-        bc.y(f.y());
-        bc.h(f.h());
-        bc.set_max_steer();
-        auto max_steer = bc.st();
-        // map t.h() in -PI...+PI to -max_steer...+max_steer
-        bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
-        bc.sp((this->udy_(this->gen_) > 0.5) ? 0.2 : -0.2);
-        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();
-        bc.sp(-1*bc.sp());
-        this->samples().back().x(bc.x());
-        this->samples().back().y(bc.y());
-        this->samples().back().h(bc.h());
+       // assume that `t` is circle uniformly sampled
+       auto bc = BicycleCar();
+       bc.x(f.x());
+       bc.y(f.y());
+       bc.h(f.h());
+       bc.set_max_steer();
+       auto max_steer = bc.st();
+       // map t.h() in -PI...+PI to -max_steer...+max_steer
+       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();
+       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;
 }