Звездный график, выбирающий неправильный путь

Question

Я написал звездный алгоритм, и он, похоже, не работает должным образом, я сузил проблему до своего 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.