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;
+}