bool
RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
{
- if (p.sp() == 0) {
- assert(this->sp() != 0);
+ if (std::abs(p.sp()) < 1e-3) {
+ // It should not be possible to have two zero speed nodes next
+ // to each other. In practice, this sometimes happens.
+ //assert(std::abs(this->sp()) > 1e-3);
if (p.p()) {
if (sgn(p.p()->sp()) != sgn(this->sp())) {
return true;
return true; // only root has no parent and it is cusp
}
} else {
- if (this->sp() == 0) {
+ if (std::abs(this->sp()) < 1e-3) {
return false; // this is cusp, not the parent
} else if (sgn(p.sp()) != sgn(this->sp())) {
return true;
RRTNode* f = this->_nn;
RRTNode* t = &this->_steered.front();
// Require the steer method to return first node equal to nn:
- assert(std::abs(t->x() - f->x()) < 1e-3);
- assert(std::abs(t->y() - f->x()) < 1e-3);
- assert(std::abs(t->h() - f->x()) < 1e-3);
+ assert(std::abs(t->x() - f->x()) < 1e-3
+ && std::abs(t->y() - f->y()) < 1e-3
+ && std::abs(t->h() - f->h()) < 1e-3);
this->_steered.erase(this->_steered.begin());
t = &this->_steered.front();
+ assert(!(std::abs(t->x() - f->x()) < 1e-3
+ && std::abs(t->y() - f->y()) < 1e-3
+ && std::abs(t->h() - f->h()) < 1e-3));
#if USE_RRTS
double cost = f->cc() + this->cost_build(*f, *t);
for (auto n: this->nv_) {
this->join_steered(&this->_nodes.back());
RRTNode* just_added = &this->_nodes.back();
bool gf = false;
- while (ss > 0 && just_added->p() != nullptr) {
+ while (ss > 0 && just_added != nullptr) {
this->steer(*just_added, this->_goal);
if (this->collide_steered()) {
ss--;
just_added = just_added->p();
continue;
}
+ // The first of steered is the same as just_added.
+ this->_steered.erase(this->_steered.begin());
this->join_steered(just_added);
bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
bool gd = this->goal_drivable_from(this->_nodes.back());