When two nodes are near themselves, do not use both.
return false;
std::vector<RRTNode *> cusps;
for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
return false;
std::vector<RRTNode *> cusps;
for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
- if (tmp_cusps[i] != tmp_cusps[(i + 1) % tmp_cusps.size()])
+ if (!IS_NEAR(
+ tmp_cusps[i],
+ tmp_cusps[(i + 1) % tmp_cusps.size()]
+ ))
cusps.push_back(tmp_cusps[i]);
}
std::reverse(cusps.begin(), cusps.end());
cusps.push_back(tmp_cusps[i]);
}
std::reverse(cusps.begin(), cusps.end());