Algorithm * not working properly

Question

I need help implementing the A * algorithm. When I run the algorithm, it finds the target, but the path is definitely not the shortest: -P

Here is my code, please help me find bugs! I think this may be the recovery path, which is my problem, but I'm not sure.

public class Pathfinder { public List<Node> aStar(Node start, Node goal, WeightedGraph graph) { Node x, y; int tentative_g_score; boolean tentative_is_better; FScoreComparator comparator = new FScoreComparator(); List<Node> closedset = new ArrayList<Node>(); Queue<Node> openset = new PriorityQueue<Node>(10, comparator); openset.add(start); start.g_score = 0; start.h_score = heuristic_cost_estimate(start, goal); start.f_score = start.h_score; while (!openset.isEmpty()) { x = openset.peek(); if (x == goal) { return reconstruct_path(goal); } x = openset.remove(); closedset.add(x); for (Edge e : graph.adj(x)) { if (ev == x) { y = ew; } else { y = ev; } if (closedset.contains(y) || y.illegal) { continue; } tentative_g_score = x.g_score + e.weight; if (!openset.contains(y)) { openset.add(y); tentative_is_better = true; } else if (tentative_g_score < y.g_score) { tentative_is_better = true; } else { tentative_is_better = false; } if (tentative_is_better) { y.g_score = tentative_g_score; y.h_score = heuristic_cost_estimate(y, goal); y.f_score = y.g_score + y.h_score; y.parent = x; } } } return null; } private int heuristic_cost_estimate(Node start, Node goal) { return Math.abs(start.x - goal.x) + Math.abs(start.y - goal.y); } private List<Node> reconstruct_path(Node current_node) { List<Node> result = new ArrayList<Node>(); while (current_node != null) { result.add(current_node); current_node = current_node.parent; } return result; } private class FScoreComparator implements Comparator<Node> { public int compare(Node n1, Node n2) { if (n1.f_score < n2.f_score) { return 1; } else if (n1.f_score > n2.f_score) { return -1; } else { return 0; } } } 

}

Thanks everyone for the great answers! My A * algorithm now works great thanks to you guys! :-)

This was my first post, and this forum is really awesome!

Answer 1

You change the priority of an item in PriorityQueue after setting it. This is not supported because the priority queue does not know that the object has changed. What you can do is delete and re-add the object when it changes.

The priority changes in the line: y.f_score = y.g_score + y.h_score; . This line occurs after adding y to the priority queue. Note that simply moving the line openset.add(y); after calculating the cost will be insufficient, since y can be added to the previous iteration.

It is also clear from your code whether heuristics are used, admissible . If this is not the case, it will also lead to suboptimal paths.

Finally, a performance note: the contains method on ArrayList and PriorityQueue takes linear time to run, which will make the execution time of your implementation sub-optimal. You can improve this by adding logical properties to nodes to indicate whether they are in closed / open sets or using an established data structure.

Answer 2

The priority queue does not update the position of the element when changing the priority. Therefore, the heap property is not satisfied. Changed priority affects the addition / removal of other elements, but it does not restore the heap property.

therefore you do not get the best item from open → you will not find the shortest path.

You can: 1) write your own heap and save the index in it 2) add another object in PQ and mark the old one as invalid (instead of node, you need to put some object with the validity flag and refer to node in the queue).

2) have worse performance, and I advise against this, but some navigation software uses this approach (or at least a few years ago).

edit: Best practice is to insert immutable objects (or at least with non-denyable parts, which means priority) in PriorityQueue