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!

Answer 1

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.