}
for (auto &n: this->goals()) {
double cost = this->cost(this->nodes().back(), n);
- if (cost < ETA) {
+ double edist = sqrt(
+ pow(this->nodes().back().x() - n.x(), 2)
+ + pow(this->nodes().back().y() - n.y(), 2)
+ );
+ double adist = std::abs(this->nodes().back().h() - n.h());
+ if (edist < 0.05 && adist < M_PI / 32) {
next = false;
if (
n.p() == nullptr