C# A*寻路算法-无法计算最短路径

我试图实现一个*算法，以便在给定的网格中找到最短路径

我的节点类：

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);
        }
    }

公共类节点：IComparable
{
公共节点（int行，int列，节点previousNode=null，双距离=double.PositiveInfinity）
{
this.Row=行；
this.Col=Col；
this.PreviousNode=PreviousNode；
这个。距离=距离；
}
公共int行{get；}
公共整数列{get；}
公共布尔值{get；内部集合；}
公共双距离{get；set；}
公共整数权重{get；set；}=1；
公共双GScore{get；set；}=double.PositiveInfinity；
公共双H{get；set；}
public-double-FScore=>this.GScore+this.H；
公共节点类型？节点类型{get；内部集合；}
公共节点PreviousNode{get；set；}
公共覆盖布尔等于（对象对象对象）
{
var otherNode=obj作为节点；
返回此.Equals（其他节点）；
}
受保护的布尔等于（节点其他）
=>this.Row==other.Row&&this.Col==other.Col；
公共覆盖int GetHashCode（）
{
未经检查
{
返回（this.Row*397）^this.Col；
}
}
公共整数比较（对象对象对象）
{
var otherNode=obj作为节点；
if（this.FScore==otherNode.FScore）
{
if（this.H>=otherNode.H）
{
返回1；
}
else if（this.H

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;
        }

执行公共覆盖结果（节点[，]网格、节点开始节点、节点结束节点） { var heap=new MinHeap（）； var allSteps=new HashSet（）； startNode.GScore=0； startNode.H=曼哈顿站（startNode，endNode）； startNode.IsVisited=true； 添加（startNode）； while（heap.Count！=0） { var currentNode=heap.Pop（）； if（currentNode.NodeType==NodeType.Wall） 继续； 添加（当前节点）； if（currentNode.Equals（endNode）） { 返回新结果（allSteps，this.getAllNodeInshorteTPathOrder（currentNode））； } var rowDirection=new[]{-1，+1,0,0}； var columnDirection=new[]{0,0，+1，-1}； 对于（int i=0；i<4；i++） { var currentRowDirection=currentNode.Row+rowDirection[i]； var currentColDirection=currentNode.Col+columnDirection[i]； 如果（（当前行方向<0 | |当前列方向<0） ||（currentRowDirection>=grid.GetLength（0） ||currentColDirection>=grid.GetLength（1））） { 继续； } var nextNode=grid[currentRowDirection，currentColDirection]； AddNodeToHeap（currentNode、nextNode、endNode、heap）； } } 返回新结果（所有步骤）； } 私有void AddNodeToHeap（节点currentNode、节点nextNode、节点endNode、MinHeap heap） { if（nextNode.isvisted | | nextNode.GScore 自定义堆类：

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;
            }
        }
    }

公共类MinHeap
{
专用只读IComparer比较器；
私有只读列表=新列表{default}；
公共垃圾堆（）
：此（默认值（IComparer））
{
}
公共MinHeap（IComparer比较器）
{
this.comparer=comparer？？comparer.Default；
}
公共MinHeap（比较）
：此（比较器。创建（比较））
{
}
public int Count=>this.list.Count-1；
公共无效添加（T元素）
{
此.list.Add（元素）；
this.ShiftUp（this.list.Count-1）；
}
公共广播电台
{
T result=this.list[1]；
this.list[1]=this.list[^1]；
this.list.RemoveAt（this.list.Count-1）；
本发明涉及移位（1）；
返回结果；
}
私有静态int父级（int i）=>i/2；
私有静态int Left（int i）=>i*2；
私有静态整数权限（整数i）=>i*2+1；
私有无效移位（int i）
{
而（i>1）
{
int parent=父（i）；
如果（this.comparer.Compare（this.list[i]，this.list[parent]）>0
{
返回；
}
（this.list[parent]，this.list[i]）=（this.list[i]，this.list[parent]）；
i=父母；
}
}
私人空位移位（int i）
{
for（int left=left（i）；leftprivate (double weight, NodeDirection? Direction) GetDistanceAndDirection(Node nodeOne, Node nodeTwo)
        {
            var x1 = nodeOne.Row;
            var y1 = nodeOne.Col;
            var x2 = nodeTwo.Row;
            var y2 = nodeTwo.Col;

            if (x2 < x1 && y1 == y2)
            {
                switch (nodeOne.Direction)
                {
                    case NodeDirection.Up:
                        return (1, NodeDirection.Up);
                    case NodeDirection.Right:
                        return (2, NodeDirection.Up);
                    case NodeDirection.Left:
                        return (2, NodeDirection.Up);
                    case NodeDirection.Down:
                        return (3, NodeDirection.Up);
                }
            }
            else if (x2 > x1 && y1 == y2)
            {
                switch (nodeOne.Direction)
                {
                    case NodeDirection.Up:
                        return (3, NodeDirection.Down);
                    case NodeDirection.Right:
                        return (2, NodeDirection.Down);
                    case NodeDirection.Left:
                        return (2, NodeDirection.Down);
                    case NodeDirection.Down:
                        return (1, NodeDirection.Down);
                }
            }
            if (y2 < y1 && x1 == x2)
            {
                switch (nodeOne.Direction)
                {
                    case NodeDirection.Up:
                        return (2, NodeDirection.Left);
                    case NodeDirection.Right:
                        return (3, NodeDirection.Left);
                    case NodeDirection.Left:
                        return (1, NodeDirection.Left);
                    case NodeDirection.Down:
                        return (2, NodeDirection.Left);
                }
            }
            else if (y2 > y1 && x1 == x2)
            {
                switch (nodeOne.Direction)
                {
                    case NodeDirection.Up:
                        return (2, NodeDirection.Right);
                    case NodeDirection.Right:
                        return (1, NodeDirection.Right);
                    case NodeDirection.Left:
                        return (3, NodeDirection.Right);
                    case NodeDirection.Down:
                        return (2, NodeDirection.Right);
                }
            }

            return default;
        }

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

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

            if (g < nextNode.GScore)
            {

                nextNode.GScore= g;
                nextNode.H = h;
                nextNode.PreviousNode = currentNode;
                nextNode.IsVisited = true;
            }

            currentNode.Direction = direction;

            heap.Add(nextNode);
        }