A *寻路算法-难以计算最短路径

Question

我正在尝试实现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。 例如：

Example grid具有3个权重节点-绿色节点是开始节点，红色节点是结束节点，哑铃节点是权重节点。

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

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

提前谢谢！

Answer 1

@jdweng

我实际上通过实现一种额外的方法来修复该错误，该方法增加了GScore的权重

蓝色方块-算法为找到最短的最终路径而采取的所有步骤

A *带权重的算法

A *无权重算法 [

Dijkstra Algo配重

Dijkstra Algo没有重量

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

，然后添加NodeToHeapMethod（）

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