A* algorithm not working properly

Question

I need some help with my A* algorithm implementation. When I run the algorithm it does find the goal, but the path is definately not the shortest :-P

Here is my code, please help me spot the bugs! I think it might be the reconstruct path that 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 (e.v == x) {
                y = e.w;
            } else {
                y = e.v;
            }

            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 to everyone for all the great answers! My A* algorithm now works perfectly thanks to you guys! :-)

This was my first post and this forum is really amazing!

This is very hard, if not impossible, to answer without knowledge of your problem domain. Are you finding a path through an L1-space (e.g. a grid)? If not, you might want to switch to a Euclidean distance heuristic instead. — Fred Foo, Aug 09 '11 at 14:25

interjay · Accepted Answer · 2011-08-09T14:38:34.997

You are changing the priority of an element in the PriorityQueue after having inserted it. This isn't supported, as the priority queue isn't aware that an object has changed. What you can do is remove and re-add the object when it changes.

The priority is changed in the line: y.f_score = y.g_score + y.h_score;. This line happens after adding y to the priority queue. Note that simply moving the line openset.add(y); to after calculating the cost won't be enough, since y may have been added in a previous iteration.

It also isn't clear from your code whether the heuristic you used is admissible. If it isn't it will also cause you to get suboptimal paths.

Finally, a performance note: The contains method on ArrayList and PriorityQueue takes linear time to run, which will make the running time of your implememtation non-optimal. You can improve this by adding boolean properties to the nodes to indicate if they are in the closed/open sets, or by using a set data structure.

score 3 · Answer 2 · answered Aug 09 '11 at 14:29

3

Priority queue does not update position of item when you change its priority. Therefore heap property does not hold. Changed priority affect additions/removals of other items, but it does not repair heap property.

therefore you does not get best item from open -> you don't find shortest path.

You can: 1) write your own heap and maintain index into it 2) add another object into PQ and mark the old one as invalid (you must instead of node put some object with validity flag and referencing node into queue).

2) have worse performance and I advise against it, but some navigation software use this approach (or at least few years back it used).

edit: Best practice is, insert immutable (or at least with imutable parts that means priority) objects into PriorityQueue

answered Aug 09 '11 at 14:29

Alpedar

1,314
1
8
12

Well even if you write your own minheap or whatever, I don't see many possibilities how to change the priority of an item that is noticeably more efficient than removing the old item and inserting it anew (is there? I don't see how, but that'd be interesting). So I'd personally just remove and add the item with new priority. – Voo Aug 09 '11 at 14:33
Could you please help me with the code. I don't really understand where and how to remove and add the item with new priority. – Johan Berg Aug 09 '11 at 14:39
Some time ago I tried to do that (but I'm not sure whether I did it correctly, and whether it was worth it performance wise) by sifting up/down depending on direction of change of priority. This shoul be faster than remove+add by not always traversing whole height of heap. – Alpedar Aug 09 '11 at 14:40

A* algorithm not working properly

2 Answers2