A*算法实现未找到最短路径，反而返回首个路径的问题排查

我实现的A*算法未计算出最短路径，反而返回了它找到的首个路径，无法查明原因。想问问有没有我忽略的明显问题？

我遵循的A*实现步骤 #

• 用起始节点初始化open集合，closed集合为空。
• 当open集合不为空时，选择起始节点到该节点的成本+该节点到目标节点的估算成本最低的节点，将其从open集合移除。
• 如果选中的节点是目标节点，算法终止，从目标节点回溯到起始节点得到路径。
• 否则，将选中的节点加入closed集合，评估其相邻节点。对于每个不在closed集合且不是障碍物的相邻节点，计算从起始节点到该节点的临时成本和启发函数估算的到目标节点的成本。如果相邻节点不在open集合中，将其加入。
• 重复步骤2-4，直到找到目标节点或open集合为空。

A*算法在启发函数可采纳（即从不高估到目标节点的实际成本）且网格无环或负边权的情况下，应能保证找到从起始节点到目标节点的最短路径。

实现代码 #

World类代码 #

public class World
{
    public int Width { get; set; }
    public int Height { get; set; }

    public Node[,] Nodes { get; set; }
    public List<Node> Path { get; set; }

    public World(int width, int height)
    {
        Width = width;
        Height = height;
        Nodes = new Node[Width, Height];

        Build();
    }

    private void Build()
    {
        for (int y = 0; y < Height; y++)
        {
            for (int x = 0; x < Width; x++)
            {
                Nodes[x, y] = new Node(x, y);
            }
        }
    }

    public void Find()
    {
        Node Start = GetStartNode(Nodes);
        Node Destination = GetDestinationNode(Nodes);

        var openSet = new List<Node>();
        var closedSet = new List<Node>();
        openSet.Add(Start);

        while (openSet.Any())
        {
            Node n = GetLowestFCostNode(openSet);
            
            if (n.NodeState == NodeState.Destination)
            {
                /* Trace Back Path */
                Path = TracebackPath(n, Start);
                break;
            }

            closedSet.Add(n);
            openSet.Remove(n);

            foreach (var node in GetAdjacentNodes(n, Nodes))
            {
                if (closedSet.Contains(node) || node.NodeState == NodeState.Obstruction)
                    continue;

                int currentGCost = node.GCost + NodeDistance(n, node);

                bool recalculate;
                if (!openSet.Contains(node))
                {
                    openSet.Add(node);
                    recalculate = true;
                }
                else if (currentGCost < node.GCost)
                {
                    recalculate = true;
                }
                else
                {
                    recalculate = false;
                }

                if (recalculate)
                {
                    node.Parent = n;
                    node.GCost = currentGCost;
                    node.HCost = HueristicsCost(node, Destination);
                    node.CalculateFCost();
                }
            }
        }
    }

    private int HueristicsCost(Node node, Node destination)
    {
        int dx = Math.Abs(node.X - destination.X);
        int dy = Math.Abs(node.Y - destination.Y);
        return 10 * (dx + dy);
    }

    private int NodeDistance(Node a, Node b)
    {
        if (Math.Abs(a.X - b.X) == 1 && Math.Abs(a.Y - b.Y) == 1)
            return 14;
        return 10;
    }

    private List<Node> GetAdjacentNodes(Node candidate, Node[,] fields)
    {
        var fieldList = new List<Node>();
        var width = fields.GetLength(0);
        var height = fields.GetLength(1);

        /* Check Lateral Neighbors */
        for (var x = candidate.X - 1; x <= candidate.X + 1; x++)
        {
            /* Check Vertical Neighbors */
            for (var y = candidate.Y - 1; y <= candidate.Y + 1; y++)
            {
                /* Bounds Check */
                if (x >= 0 && x < width && y >= 0 && y < height && (x != candidate.X || y != candidate.Y))
                {
                    fieldList.Add(fields[x, y]);
                }
            }
        }

        return fieldList;
    }


    private Node GetLowestFCostNode(List<Node> openSet)
    {
        Node lowestFCostNode = null;
        int lowestFCost = int.MaxValue;

        foreach (Node node in openSet)
        {
            if (node.FCost < lowestFCost || (node.FCost == lowestFCost && node.HCost < lowestFCostNode.HCost))
            {
                lowestFCost = node.FCost;
                lowestFCostNode = node;
            }
        }

        return lowestFCostNode;
    }

    private List<Node> TracebackPath(Node node, Node start)
    {
        List<Node> list = new List<Node>();
        while (node != start)
        {
            list.Add(node);
            node.Tile.Fill = Brushes.Cyan;
            node = node.Parent;
        }

        list.Add(start);

        return list;
    }

    private Node GetDestinationNode(Node[,] nodes)
    {
        foreach (var node in nodes)
        {
            if (node.NodeState == NodeState.Destination)
                return node;
        }

        throw new Exception("No Destination Field.");
    }

    private Node GetStartNode(Node[,] nodes)
    {
        foreach (var node in nodes)
        {
            if (node.NodeState == NodeState.Start)
                return node;
        }

        throw new Exception("Could not find a starting node.");
    }
}

Node类代码 #

public class Node
{
    public int X { get; }
    public int Y { get; }
    public Rectangle Tile { get; set; }

    /* Estimated Distance from CurrentNode to the StartNode */
    public int GCost { get; set; }

    /* Estimated Distance From CurrentNode To DestinationNode */
    public int HCost { get; set; }

    /* G + HCost Combined */
    public int FCost { get; set; }
    public Node Parent { get; set; }

    private NodeState _nodeState;

    public NodeState NodeState
    {
        get { return _nodeState; }
        set
        {
            InvokeNodeState(value);
            _nodeState = value;
        }
    }

    public Node(int x, int y)
    {
        X = x;
        Y = y;

        CreateTile();
    }

    private void CreateTile()
    {
        Tile = new Rectangle()
        {
            Width = 25,
            Height = 25,
            Fill = Brushes.ForestGreen,
            Stroke = Brushes.Black,
            StrokeThickness = 2
        };

        Tile.MouseDown += (sender, args) =>
        {
            switch (NodeState)
            {
                case NodeState.None:
                    NodeState = NodeState.Obstruction;
                    break;
                case NodeState.Obstruction:
                    NodeState = NodeState.Start;
                    break;
                case NodeState.Start:
                    NodeState = NodeState.Destination;
                    break;
                case NodeState.Destination:
                    NodeState = NodeState.None;
                    break;
                default:
                    throw new ArgumentOutOfRangeException();
            }
        };

        Canvas.SetLeft(Tile, X * 25);
        Canvas.SetTop(Tile, Y * 25);
    }

    private void InvokeNodeState(NodeState value)
    {
        switch (value)
        {
            case NodeState.None:
                Tile.Fill = Brushes.ForestGreen;
                break;
            case NodeState.Obstruction:
                Tile.Fill = Brushes.SaddleBrown;
                break;
            case NodeState.Start:
                Tile.Fill = Brushes.Yellow;
                break;
            case NodeState.Destination:
                Tile.Fill = Brushes.Red;
                break;
            default:
                throw new ArgumentOutOfRangeException(nameof(value), value, null);
        }
    }

    public void CalculateFCost()
    {
        FCost = GCost + HCost;
        Tile.Fill = Brushes.DarkGreen;
        Tile.ToolTip = $"FCost: {FCost} - GCost: {GCost} - HCost: {HCost}";
    }
}

public enum NodeState
{
    None,
    Obstruction,
    Start,
    Destination
}

内容的提问来源于stack exchange，提问作者Jess Chan