tmp_cusps.push_back(n);
tmp_cusps.push_back(n->parent());
}
+ //tmp_cusps.push_back(n);
}
if (tmp_cusps.size() < 2)
return false;
RRTNode *RRTBase::nn(RRTNode *rs)
{
return nn4(this->iy_, rs, nullptr);
+ //return nn3(this->iy_, rs, nullptr);
}
std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)