A *寻路算法-卡住了计算最短路径的时间

我正在尝试实现A *算法，以便在给定的网格中找到最短的路径。

我的Node类：

public class Node : IComparable
    {
        public Node(int row, int col, Node previousNode = null, double distance = double.PositiveInfinity)
        {
            this.Row = row;
            this.Col = col;
            this.PreviousNode = previousNode;
            this.Distance = distance;
        }

        public int Row { get; }

        public int Col { get; }

        public bool IsVisited { get; internal set; }

        public double Distance { get; set; } 

        public int Weight { get; set; } = 1;

        public double GScore { get; set; } = double.PositiveInfinity;

        public double H { get; set; }

        public double FScore => this.GScore + this.H;

        public NodeType? NodeType { get; internal set; }

        public Node PreviousNode { get; set; }

        public override bool Equals(object obj)
        {
            var otherNode = obj as Node;
            return this.Equals(otherNode);
        }

        protected bool Equals(Node other)
            => this.Row == other.Row && this.Col == other.Col;

        public override int GetHashCode()
        {
            unchecked
            {
                return (this.Row * 397) ^ this.Col;
            }
        }

        public int CompareTo(object obj)
        {
            var otherNode = obj as Node;
            if (this.FScore == otherNode.FScore)
            {
                if (this.H >= otherNode.H)
                {
                    return 1;
                }
                else if (this.H < otherNode.H)
                {
                    return -1;
                }
            }

            return this.FScore.CompareTo(otherNode.FScore);
        }
    }

A *算法类：

public override Result Execute(Node[,] grid, Node startNode, Node endNode)
        {
            var heap = new MinHeap<Node>();
            var allSteps = new HashSet<Node>();

            startNode.GScore = 0;
            startNode.H = ManhattanDistance(startNode, endNode);
            startNode.IsVisited = true;
            heap.Add(startNode);

            while (heap.Count != 0)
            {
                var currentNode = heap.Pop();

                if (currentNode.NodeType == NodeType.Wall)
                    continue;

                allSteps.Add(currentNode);

                if (currentNode.Equals(endNode))
                {
                    return new Result(allSteps, this.GetAllNodesInShortestPathOrder(currentNode));
                }

                var rowDirection = new[] { -1, +1, 0, 0 };
                var columnDirection = new[] { 0, 0, +1, -1 };
                for (int i = 0; i < 4; i++)
                {
                    var currentRowDirection = currentNode.Row + rowDirection[i];
                    var currentColDirection = currentNode.Col + columnDirection[i];

                    if ((currentRowDirection < 0 || currentColDirection < 0)
                        || (currentRowDirection >= grid.GetLength(0)
                            || currentColDirection >= grid.GetLength(1)))
                    {
                        continue;
                    }

                    var nextNode = grid[currentRowDirection, currentColDirection];
                    AddNodeToHeap(currentNode, nextNode, endNode, heap);
                }
            }

            return new Result(allSteps);
        }

        private void AddNodeToHeap(Node currentNode, Node nextNode, Node endNode, MinHeap<Node> heap)
        {
            if (nextNode.IsVisited || nextNode.GScore < currentNode.GScore)
                return;

            var g = currentNode.GScore + nextNode.Weight;
            var h = ManhattanDistance(nextNode, endNode);

            if (g + h < nextNode.FScore)
            {
                nextNode.GScore = g;
                nextNode.H = h;
                nextNode.PreviousNode = currentNode;
                nextNode.IsVisited = true;
            }

            heap.Add(nextNode);
        }

        private static int ManhattanDistance(Node currentNode, Node endNode)
        {
            var dx = Math.Abs(currentNode.Row - endNode.Row);
            var dy = Math.Abs(currentNode.Col - endNode.Col);
            return dx + dy;
        }

自定义MinHeap类：

public class MinHeap<T>
    {
        private readonly IComparer<T> comparer;
        private readonly List<T> list = new List<T> { default };

        public MinHeap() 
            : this(default(IComparer<T>))
        {
        }

        public MinHeap(IComparer<T> comparer)
        {
            this.comparer = comparer ?? Comparer<T>.Default;
        }

        public MinHeap(Comparison<T> comparison) 
            : this(Comparer<T>.Create(comparison))
        {
        }

        public int Count => this.list.Count - 1;

        public void Add(T element)
        {
            this.list.Add(element);
            this.ShiftUp(this.list.Count - 1);
        }

        public T Pop()
        {
            T result = this.list[1];
            this.list[1] = this.list[^1];
            this.list.RemoveAt(this.list.Count - 1);
            this.ShiftDown(1);
            return result;
        }

        private static int Parent(int i) => i / 2;
        private static int Left(int i) => i * 2;
        private static int Right(int i) => i * 2 + 1;

        private void ShiftUp(int i)
        {
            while (i > 1)
            {
                int parent = Parent(i);
                if (this.comparer.Compare(this.list[i], this.list[parent]) > 0)
                {
                    return;
                }

                (this.list[parent], this.list[i]) = (this.list[i], this.list[parent]);
                i = parent;
            }
        }

        private void ShiftDown(int i)
        {
            for (int left = Left(i); left < this.list.Count; left = Left(i))
            {
                int smallest = this.comparer.Compare(this.list[left], this.list[i]) <= 0 ? left : i;
                int right = Right(i);
                if (right < this.list.Count && this.comparer.Compare(this.list[right], this.list[smallest]) <= 0)
                {
                    smallest = right;
                }

                if (smallest == i)
                {
                    return;
                }

                (this.list[i], this.list[smallest]) = (this.list[smallest], this.list[i]);
                i = smallest;
            }
        }
    }

问题是，当我在地图上有一些权重时，它找不到最佳路径。例如：网格上标记为权重节点的每个正方形的权重为10，否则为1。这是示例：

具有3个权重节点的示例网格-绿色节点是开始节点，红色节点是结束节点，哑铃节点是权重节点。

当我运行算法时，得到以下结果。显然，这并不是最短的路径，因为该算法将通过第一个权重为1的节点，然后通过权重为10的下一个节点，而不是仅传递一个权重为10的节点。最短的路径应该是我标记的红色路径。

PS我已经设法通过在计算GCost时添加新的启发式函数来尊重权重，现在它会计算路径，但不是一条直线，而是一条奇怪的路径：

先感谢您！