A-Star Pathfinding выбирает плохие путевые точки

РЕШЕНО: извините. Я неправильно реконструировал путь. Я думал, что в CloseSet есть все путевые точки только от начала до конца, но у него есть и некоторые другие путевые точки. Я не понял концепции. Теперь все работает!

У меня все еще возникают проблемы с A*.

Мой персонаж находит свой путь, но иногда, в зависимости от того, где я нажимаю на карту, алгоритм находит кратчайший путь или путь, но со многими узлами, которые не следует выбирать.

Я пытался следовать реализации Википедии и A* Pathfinding for Beginner, но они дают тот же результат. Не знаю, эвристика это или сам алгоритм, но что-то не так.

А это пример проблемы с нажатием двух разных узлов: http://i.imgur.com/gtgxi.jpg

Вот класс Pathfind:

import java.util.ArrayList;
import java.util.Collections;
import java.util.TreeSet;

public class Pathfind {
public Pathfind(){

}

public ArrayList<Node> findPath(Node start, Node end, ArrayList<Node> nodes){

    ArrayList<Node> openSet = new ArrayList<Node>();
    ArrayList<Node> closedSet = new ArrayList<Node>();

    Node current;

    openSet.add(start);

    while(openSet.size() > 0){

        current = openSet.get(0);

        current.setH_cost(ManhattanDistance(current, end));

        if(start == end) return null;           
        else if(closedSet.contains(end)){
            System.out.println("Path found!");
            return closedSet;
        }

        openSet.remove(current);
        closedSet.add(current);

        for(Node n : current.getNeigbours()){           
            if(!closedSet.contains(n)){     
                if(!openSet.contains(n) || (n.getG_cost() < (current.getG_cost()+10))){ 
                    n.setParent(current);
                    n.setG_cost(current.getG_cost()+10);
                    n.setH_cost(ManhattanDistance(n, end));

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

                    Collections.sort(openSet);
                }
            }
        }
    }       

    return null;
}

private int ManhattanDistance(Node start, Node end){
    int cost = start.getPenalty();

    int fromX = start.x, fromY = start.y;
    int toX = end.x, toY = end.y;

    return cost * (Math.abs(fromX - toX) + Math.abs(fromY - toY));
}

}