A* algorithm not working properly

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!

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.

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