Point EscapeObstaclesPathPlanner::findNonBlockedGoal(
Point goal, boost::optional<Point> prevGoal, const ShapeSet& obstacles,
int maxItr) {
if (obstacles.hit(goal)) {
FixedStepTree goalTree;
goalTree.init(goal, &obstacles);
goalTree.step = stepSize();
// The starting point is in an obstacle, extend the tree until we find
// an unobstructed point
Point newGoal;
for (int i = 0; i < maxItr; ++i) {
// extend towards a random point
Tree::Point* newPoint = goalTree.extend(RandomFieldLocation());
// if the new point is not blocked, it becomes the new goal
if (newPoint && newPoint->hit.empty()) {
newGoal = newPoint->pos;
break;
}
}
if (!prevGoal || obstacles.hit(*prevGoal)) return newGoal;
// Only use this newly-found point if it's closer to the desired goal by
// at least a certain threshold
float oldDist = (*prevGoal - goal).mag();
float newDist = (newGoal - goal).mag();
if (newDist + *_goalChangeThreshold < oldDist) {
return newGoal;
} else {
return *prevGoal;
}
}
return goal;
}
