c# - 2D 配列での A* パスファインディングの実装

Question

私は 2D タイル マップを作成中ですが、現在 A* パスファインディングを実装しようとしています。A* のウィキペディア擬似コードに従っています。

アルゴリズムによって行われる決定でのいくつかの奇妙な動作を除いて、物事は非常にうまくいっています。

これまでの私のコード:

void Pathfinding(Point from, Point destination) {

    goalNode = new Node(destination, 0, 0);
    startNode = new Node(from, 0, ManhattanDistance(from, destination));

    open = new List<Node>();            //list of nodes
    closed = new List<Node>();
    open.Add(startNode);                //Add starting point

    while(open.Count > 0) {

        node = getBestNode();                   //Get node with lowest F value
        if(node.position == goalNode.position) {
            Debug.Log("Goal reached");
            getPath(node);
            break;
        }
        removeNode(node, open);
        closed.Add(node);

        List<Node> neighbors = getNeighbors(node);
        foreach(Node n in neighbors) {
            float g_score = node.G + 1;
            float h_score = ManhattanDistance(n.position, goalNode.position);
            float f_score = g_score + h_score;

            if(isValueInList(n, closed) && f_score >= n.F) 
                continue;

            if(!isValueInList(n, open) || f_score < n.F) {
                n.parent = node;
                n.G = g_score;
                n.G = h_score;
                if(!isValueInList(n, open)) {
                    map_data[n.position.x, n.position.y] = 4;
                    open.Add(n);
                }
            }
        }
    }
}

このコードを実行した結果:

青は開いているリストからのノードで、緑は目的のノードへの選択されたパスです。

解決：

void Pathfinding(Point from, Point destination) {

    goalNode = new Node(destination, 0, 0);
    startNode = new Node(from, 0, ManhattanDistance(from, destination));

    open = new List<Node>();            //list of nodes
    closed = new List<Node>();
    open.Add(startNode);                //Add starting point

    while(open.Count > 0) {

        node = getBestNode();                   //Get node with lowest F value
        if(node.position == goalNode.position) {
            Debug.Log("Goal reached");
            getPath(node);
            break;
        }
        removeNode(node, open);
        closed.Add(node);

        List<Node> neighbors = getNeighbors(node);
        foreach(Node n in neighbors) {
            float g_score = node.G + 1;
            float h_score = ManhattanDistance(n.position, goalNode.position);
            float f_score = g_score + h_score;

            if(isValueInList(n, closed) && f_score >= n.F) 
                continue;

            if(!isValueInList(n, open) || f_score < n.F) {
                n.parent = node;
                n.G = g_score;
                n.H = h_score;
                if(!isValueInList(n, open)) {
                    map_data[n.position.x, n.position.y] = 4;
                    open.Add(n);
                }
            }
        }
    }
}