AStar（A*）算法是一种静态网格中求解最短路径直接有效的搜索方法。
将地图按行列分成不同的网格节点 Node，每个节点可以是(正方形、六边形，三角形，多边形等)，下面例子采用 矩形节点
AStar 通过遍历节点根据节点评估代价值确定搜索路径。
其中 f(n) 是从初始点经由节点n到目标点的估价函数，
g(n) 是从初始节点到n节点的实际代价，
h(n) 是从n到目标节点的估计代价。
其中 f(n) = g(n) + h(n)，
估价函数h(n)有多种不同方案选取：如 哈夫曼 策略， h(n) =节点n和终点的 行row、列col 差值绝对值之和
h(n) = abs(n.row - destination.row) + abs(n.col - destination.col)
A*算法逻辑如如下
A*算法的运行机制：
1.创建两张表: closedList 存放 不可访问数据，openHeap 存放可访问数据, (openHeap 为小根堆，因为下面逻辑取open表数据都是取 f(n) 最小的Node，小根堆会提高性能)
2.将开始节点Node 放入 openHeap
3.如果 openHeap 表中数据个数 > 0，则取 openHeap中 f(n)最小的节点 currentNode，如果 currentNode 是需要查找的终点，则路径查找结束，返回 currentNode
4.遍历 currentNode 的所有相邻节点（例子采用矩形节点，所以有八个相邻节点），顺次将所有相邻节点 neighborNode 执行步骤 5
5.如果 neighborNode 是空节点、空地块、障碍物(不可通过) 等无效地块, 或者 neighborNode 已经包含在 closed 表。 则 什么也不做
如果 neighborNode 已经包含在 open 表
判断 neighborNode.G 与 currentNode.G + distance 的大小，（distance 值：从 currentNode 到 neighborNode 的距离，如果相邻则为 1，如果为对角相邻则约等于 1.4）
如果 neighborNode.G > currentNode.G + distance 则更新 neighborNode.G = currentNode.G + distance； 令neighborNode 的父节点指向currentNode ;
如果 neighborNode 没有包含在 open 表
设置 neighborNode.G = currentNode.G + distance;
设置 neighborNode.H = 哈夫曼策略计算 (neighborNode 和 终点节点的 row、col 之差的绝对值之和)
令neighborNode 的父节点指向currentNode ;
将 neighborNode 添加到 openHeap
6.待步骤 4 执行完所有相邻节点，跳转至步骤 3 继续执行
如果步骤 3 最终返回节点不为空，则说明已经找到路径，从 返回节点node 顺次向上遍历 node.Parent，最终会到 起点，逆序便是从 起点 到 终点的路径
代码片段
// 栈：FILO 先进后出,存放路径点
_stackPos.Clear();
while (null != pathNode)
{
Position pos = _mapQuad.NodeToPosition(pathNode);
// 数据入栈
_stackPos.Push(pos);
pathNode = pathNode.Parent;
}
while (_stackPos.Count > 0)
{
// 数据出栈
Position pos = _stackPos.Pop();
}核心查找代码如下
using DataStruct.Heap;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace AStar
{
public class AStar
{
// 地图数据
private IMap _map;
// 小根堆保存开放节点，目的在于提高获取最小F值点效率
private Heap<Node> openHeap;
// closed 表
private List<Node> closedList;
public AStar()
{
openHeap = new Heap<Node>();
openHeap.SetHeapType(false);
closedList = new List<Node>();
}
public void SetMap(IMap map)
{
_map = map;
}
public Node SearchPath(Position from, Position desitination)
{
// 重置上次访问过的节点
foreach (var node in closedList)
{
node.Clear();
}
foreach(var node in openHeap.DataList)
{
node.Clear();
}
openHeap.MakeEmpty();
closedList.Clear();
// 起点
Node fromNode = _map.PositionToNode(from.X, from.Y);
// 终点
Node desitinationNode = _map.PositionToNode(desitination.X, desitination.Y);
if (fromNode.Row == desitinationNode.Row && fromNode.Col == desitinationNode.Col)
{
return null;
}
fromNode.NodeState = NodeState.InOpenTable;
// 将起点加入到 open 表
openHeap.Insert(fromNode);
while (openHeap.Count() > 0)
{
// 取出 open 表中 F 值最小的节点
Node node = openHeap.DelRoot();
// 将 node 添加到 closed 表
node.NodeState = NodeState.InColsedTable;
closedList.Add(node);
// 如果 node 是终点 则路径查找成功，并退出
if (node.Row == desitinationNode.Row && node.Col == desitinationNode.Col)
{
return node;
}
Neighbor(node, desitinationNode);
}
return null;
}
/// <summary>
/// 获取 currentNode 所有的相邻节点加入到 open 表
/// </summary>
private void Neighbor(Node currentNode, Node desitinationNode)
{
// 遍历获取 currentNode 所有相邻节点
for (int i = 0; i < currentNode.neighborCount; ++i)
{
float distance = 0;
Node neighborNode = _map.NodeNeighbor(currentNode, i, ref distance);
InsertToOpenHeap(neighborNode, currentNode, desitinationNode, distance);
}
}
/// <summary>
/// 将 neighborNode 加入到 open 表
/// </summary>
private void InsertToOpenHeap(Node neighborNode, Node currentNode, Node desitinationNode, float distance)
{
// 空、不可通过节点不做处理
if (null == neighborNode || neighborNode.NodeType == NodeType.Obstacle || neighborNode.NodeType == NodeType.Null)
{
return;
}
// 已经加入到 closed 表的 node 不做处理
if (neighborNode.NodeState == NodeState.InColsedTable)
{
return;
}
// 在 open 表中
if (neighborNode.NodeState == NodeState.InOpenTable)
{
// 比较 neighborNode 记录的 G 值是否比 从 currentNode 到 neighborNode 的G 值更大
// 如果 neighborNode.G 更大，则更新 neighborNode.G 并设置 neighborNode.Parent = currentNode;
if (neighborNode.G > (currentNode.G + currentNode.Cost * distance))
{
neighborNode.G = currentNode.G + currentNode.Cost * distance;
neighborNode.Parent = currentNode;
// 改变了 G 值，小根堆需要重排序
openHeap.HeapCreate();
}
}
else
{
// 设置 neighborNode.G 值 = 从 起点 到 neighborNode 的总 G
neighborNode.G = currentNode.G + currentNode.Cost * distance;
// 使用 曼哈顿 方法计算 H 值，即(neighborNode 到 终点的 Row、Col 偏移量绝对值之和)
float h = Math.Abs(neighborNode.Row - desitinationNode.Row) + Math.Abs(neighborNode.Col - desitinationNode.Col);
neighborNode.H = h * neighborNode.Cost;
// 设置父节点
neighborNode.Parent = currentNode;
neighborNode.NodeState = NodeState.InOpenTable;
openHeap.Insert(neighborNode);
}
}
}
}Demo 代码，Unity 辅助展示
使用 Unity 只是为了方便展示图形界面， AStar 代码部分跟语言以及引擎均无关联
下面是一个寻路过程中访问过的节点动画
蓝色点所在节点是整个寻路过程中访问过的节点，绿色节点为f(n)较小者
