}
if (this->samples().size() <= 1)
return this->goal_found();
+ // steer to goal
for (auto na: newly_added) {
pn = na;
en_add = true;
}
}
}
+ // steer to goals
+ for (auto na: newly_added) {
+ for (auto go: this->goals()) {
+ pn = na;
+ en_add = true;
+ cusps = 0;
+ for (auto ns: this->steer(na, go)) {
+ ns->rs(rs);
+ if (!en_add) {
+ delete ns;
+ } else if (IS_NEAR(pn, ns)) {
+ delete ns;
+ } else {
+ if (sgn(pn->s()) != sgn(ns->s()))
+ cusps++;
+ if (cusps > 4)
+ en_add = false;
+ this->nodes().push_back(ns);
+ this->add_iy(ns);
+ pn->add_child(ns, this->cost(pn, ns));
+ if (this->collide(pn, ns)) {
+ pn->children().pop_back();
+ ns->remove_parent();
+ this->iy_[IYI(ns->y())].pop_back();
+ this->nodes().pop_back();
+ delete ns;
+ en_add = false;
+ } else {
+ this->add_ixy(ns);
+ this->ocost(ns);
+ pn = ns;
+ RRTNode *oldgoal = this->goal();
+ this->goal(go);
+ if (this->goal_found(pn, CO)) {
+ this->goal_cost();
+ this->tlog(this->findt());
+ en_add = false;
+ } else {
+ this->goal(oldgoal);
+ }
+ }
+ }
+ }
+ }}
return this->goal_found();
}