A* 寻路算法 - 在计算最短路径时卡住了
A* pathfinding algorithm - got stuck calculating shortest path
我正在尝试实现 A* 算法以便在给定网格中找到最短路径。
我的节点class:
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* 算法 class:
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;
}
自定义最小堆class:
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 时通过添加新的启发式函数设法让它尊重权重,它现在计算路径但不是一条直线我得到了一些奇怪的路径:
提前致谢!
@jdweng
我实际上通过实施一种额外的方法修复了这个错误,该方法增加了 GScore 的额外权重
蓝色方块 - 算法为找到最短最终路径所采取的所有步骤
A* 带权重的算法
A* 没有权重的算法
[
带权重的 Dijkstra 算法
没有权重的 Dijkstra 算法
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;
}
然后是 AddNodeToHeapMethod()
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);
}
我正在尝试实现 A* 算法以便在给定网格中找到最短路径。
我的节点class:
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* 算法 class:
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;
}
自定义最小堆class:
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 时通过添加新的启发式函数设法让它尊重权重,它现在计算路径但不是一条直线我得到了一些奇怪的路径:
提前致谢!
@jdweng
我实际上通过实施一种额外的方法修复了这个错误,该方法增加了 GScore 的额外权重
蓝色方块 - 算法为找到最短最终路径所采取的所有步骤
A* 带权重的算法
A* 没有权重的算法
[
带权重的 Dijkstra 算法
没有权重的 Dijkstra 算法
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;
}
然后是 AddNodeToHeapMethod()
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);
}