I'm attempting to implement the A* pathfinding algorithm in Java. I thought it was working well, but then I found instances where it doesn't follow the shortest path
Green = start, red = target, black = walls (unwalkable), blue = path
Red # in top left is G value, green # in top right is H value, black # in middle is F value. Black lines show parents.
I have a Node class which has the following properties: Integers x, y == 0 Integers f, g== -1 Node parent
H is calculated using the diagonal shortcut method described here
int xDist = Math.abs(end.x - x);
int yDist = Math.abs(end.y - y);
//end == target node
if (xDist > yDist) {
h = 14 * yDist + 10 * (xDist - yDist);
} else {
h = 14 * xDist + 10 * (yDist - xDist);
}
G is calculated as follows:
public int getG(boolean force) { //force == true will recalculate G
if (g == -1 || force) {
if (equals(start) || parent == null) {
g = 0; //If its the start, g = 0
} else {
g = parent.getG(force) + (parent.x != x && parent.y != y ? 14 : 10);
//10 for orthogonal moves, 14 for diagonal
}
}
return g;
}
The path is calculated using this findPath method:
public static boolean findPath(Node start, Node end) {
if (start.block || end.block) {
//start or target has a wall on it
return false;
}
Node.start = start;
Node.end = end;
LinkedHashSet open = new LinkedHashSet();
LinkedHashSet closed = new LinkedHashSet();
//open and closed lists, respectively
boolean found = false;
open.add(start);
//add the starting node to open list
while (open.size() > 0) {
//while there is something in open list
int lowF = Integer.MAX_VALUE;
int curF = 0;
Node curN = null;
for (Node node : open) {
curF = node.getF();
if (curF < lowF) {
lowF = curF;
curN = node;
}
}
//curN == lowest F value in open list
if (curN == end) {
//target has been found
found = true;
break;
}
open.remove(curN);
closed.add(curN);
//switch the lowest F value Node to the closed list
for (Node node : curN.getAdjacent()) {
//iterate over non-wall adjacent nodes
if (closed.contains(node)) {
//already on closed list
continue;
}
if (!open.contains(node) || node.getG() < curN.getG()) {
//not on open list OR it has a lower G cost
node.parent = curN;
//set node's parent to the lowest F cost Node from before
node.getF(true);
//recalculates the F (and G) values
open.add(node);
//add to the open list.
//will NOT be readded if already present
}
}
}
return found;
//loop is over, return true
}
I lead myself to believe that the issue was in my heuristic, however I'm not sure that's the case anymore: shouldn't the shortest path still be found eventually?
The path begins by approaching the target the fastest way because of the heuristic, but then seems to "change it's mind." I've been baffled for days.
When I remove the H value (i.e. set it to 0) I get the expected result. When I remove diagonal movement I also get the expected result.
Answer
Here is the pseudo-code from wikipedia for A* with a consistent-heuristic:
1. while openset is not empty
2. current := the node in openset having the lowest f_score[] value
3. if current = goal
4. return reconstruct_path(came_from, goal)
5.
6. remove current from openset
7. add current to closedset
8. for each neighbor in neighbor_nodes(current)
9. tentative_g_score := g_score[current] + dist_between(current,neighbor)
10. if neighbor in closedset and tentative_g_score >= g_score[neighbor]
11. continue
12.
13. if neighbor not in openset or tentative_g_score < g_score[neighbor]
14. came_from[neighbor] := current
15. g_score[neighbor] := tentative_g_score
16. f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
17. if neighbor not in openset
18. add neighbor to openset
19.
20. return failure
The important lines are these:
9. tentative_g_score := g_score[current] + dist_between(current,neighbor)
...
13. if neighbor not in openset or tentative_g_score < g_score[neighbor]
The purpose of tentative_g_score
is that you may need to update the g-score of a node in the open-list if you find a closer neighbor to it. You are not doing that; you are instead comparing the g-score of the node to the g-score of its neighbor.
No comments:
Post a Comment