RRTNode* just_added = &this->nodes_.back();
bool gf = false;
while (ss > 0 && just_added->p() != nullptr) {
- //if (!this->goal_drivable_from(*just_added)) {
- // ss--;
- // just_added = just_added->p();
- // continue;
- //}
this->steer(*just_added, this->goal_);
if (this->collide_steered()) {
ss--;
if (gf) {
this->compute_path();
}
- ////if (!this->goal_drivable_from(this->nodes_.back())) {
- //// return this->should_continue();
- ////}
- //this->steer(this->nodes_.back(), this->goal_);
- //if (this->collide_steered()) {
- // return this->should_continue();
- //}
- //this->join_steered(&this->nodes_.back());
- //bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
- //bool gd = this->goal_drivable_from(this->nodes_.back());
- //if (gn && gd) {
- // double nc = this->cost_build(this->nodes_.back(), this->goal_);
- // double ncc = this->nodes_.back().cc() + nc;
- // if (this->goal_.p() == nullptr || ncc < this->goal_.cc()) {
- // this->goal_.p(this->nodes_.back());
- // this->goal_.c(nc);
- // this->compute_path();
- // }
- //}
this->time_ = this->ter_.scnt();
return this->should_continue();
}