选择错误路径的星图

Question

我写了一个a-star算法但它似乎没有正常工作，我已经将问题缩小到我的if语句，检查我是否达到了目标。基本上，A星有一个起始点（总是在0,0）并且它必须穿过图中的每个点，在那里它在边缘（从一个点到下一个点）绘制一条线。用户要求。

public LinkedList <Edge> AStar (Point start) {

            PriorityQueue <Node> openSet = new PriorityQueue <Node> ();
            ArrayList <Node> closedSet = new ArrayList <Node> ();
            Set <Point> tempNodes = new TreeSet <Point> ();
            LinkedList <Edge> path = null;
            Node x = null;
            boolean line = false;
            int nodesExplored = 0;

            // creating the initial state and add it into the open set
            Node initial = new Node (new LinkedList <Edge> (), new ArrayList <Edge> (), start, 0);
            openSet.add(initial);
            x = initial;

            // loop through and make sure the open set is never empty
            while (!openSet.isEmpty()) {

                // checks if there is anymore edges left to look at
                if (edges.size() == x.edgeList().size()) {
                    System.out.println(nodesExplored + " nodes explored");
                    for (Edge e : x.getHistory()) totalCost += e.getDistance(); 
                    System.out.printf("cost = %.2f\n", totalCost);
                    return x.getHistory();  
                }

                // pops off the head of the queue and add the current node to the closed set 
                x = openSet.poll();
                nodesExplored++;
                closedSet.add(x);
                tempNodes.addAll(nodes);
                tempNodes.remove(x.curNode());

                // loop through and find the adjacent nodes
                for (Point n : tempNodes) {

                    if (findEdge(x.curNode(), n)) line = true;
                    else line = false;
                    Edge edge = new Edge (x.curNode(), n, line);
                    ArrayList <Edge> tempList =  new ArrayList <Edge> ();
                    tempList.addAll(x.edgeList());

                    path = new LinkedList<Edge> ();
                    path.addAll(x.getHistory());
                    if (line && !checkEdge(x.curNode(), n, path)) tempList.add(edge);
                    if (!checkEdge(x.curNode(), n, path)) path.add(edge);

                    Node y = new Node (path, tempList, n, calcHeu(tempList));

                    // if the node has already been placed in the open or closed set
                    if (closedSet.contains(y)) continue;

                    if (!openSet.contains(y)) openSet.add(y);

                }

            }

            return null;

        }

我的输出结果如下：

15 nodes explored
cost = 34.82
Draw from 0 0 to 0 5
Move from 0 5 to 7 5
Draw from 7 5 to 7 2
Move from 7 2 to 5 5
Move from 5 5 to 7 5
Move from 7 5 to 1 0
Draw from 1 0 to 5 5

我的输入是：

Line between 0 0 and 0 5
Line between 1 0 and 5 5
Line between 7 5 and 7 2

我知道这是不正确的，因为最短距离将从0,0 to 0,5 to 7,5 to 7,2 to 1,0 to 5,5开始，这意味着我的优先级队列没有正确存储它。

编辑：我的Node类及其与

的比较

public class Node implements Comparable <Node> {


    @Override public boolean equals (Object other) {

        if (!(other instanceof Node)) {
              return false;
            }

        Node temp = (Node) other;

        return currNode.equals(temp.curNode()) && pathHistory.size() == temp.getHistory().size();
    }

@Override public int compareTo(Node s) {

    if (gscore > s.gScore()) return -1;

    return gscore == s.gScore() ? 1 : 0;
}

private ArrayList <Edge> drawnLines;
private LinkedList <Edge> pathHistory;
private Point currNode;
private int heuristic;
private double gscore;

}

EDIT2：http://pastebin.com/GsZvbv3t代表gscore。