return this == &n;
}
+void
+RRTS::recompute_path_cc()
+{
+ this->path_.clear();
+ RRTNode* g = &this->goal_;
+ while (g != nullptr) {
+ this->path_.push_back(g);
+ g = g->p();
+ }
+ std::reverse(this->path_.begin(), this->path_.end());
+ for (unsigned int i = 1; i < this->path_.size(); i++) {
+ this->path_[i]->c(this->cost_build(*this->path_[i - 1],
+ *this->path_[i]));
+ }
+}
+
double
RRTS::min_gamma_eta() const
{
double d1 = this->cost_search(this->nodes_.front(), rs);
double d2 = this->cost_search(rs, this->goal_);
if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
- this->icnt_ -= 1;
- return this->should_continue();
+ rs = this->last_path_[rand() % this->last_path_.size()];
}
}
#endif
{
if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
this->last_goal_cc_ = this->goal_.cc();
+ this->last_path_.clear();
+ for (auto n: this->path_) {
+ this->last_path_.push_back(*n);
+ }
}
this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
this->goal_.e());