Unity实现A*寻路算法学习1.0
一、A*寻路算法的原理
如果现在地图上存在两点A、B，这里设A为起点，B为目标点(终点)
这里为每一个地图节点定义了三个值
gCost：距离起点的Cost(距离)
hCost：距离目标点的Cost(距离)
fCost：gCost和gCost之和。
这里的Cost可以采用直线距离，也可以采用曼哈顿距离等，只要适合就行
那么先计算起点周围的所有节点的三个值
这里设每两个相邻节点间的距离为10，那么对角线距离为14
那么计算得出，F值最小的是A点左上角的方块，将节点放入列表(数组也行)将A设为该节点的父节点，然后计算周围方块的距离
因为是从A点移动过来的，所以下次比较时不再比较A点
再次计算得出F值最小的仍然为左上角的节点
这样就求出了A到B点的最短路径
如果A、B之间存在障碍物那么又该怎么办呢？
同样也是计算最小的 F 值
但这里出现了三个相同的F值
那么接下来优先选择 H 值最小的路径，即距离目标点最近的路径
但是移动过后 F 值 反而变大了
那么反过来寻找之前 F 值最小的路径，但接下来还是 F 值更大
那么仍然选择 F 值最小的路径
然后是下一个F值最小的路径
然后是下一个
直到距离目的地的hCost为0
再来一个例子，这里说明如何找出最短的路径，箭头表示父节点
第一次计算A点周围节点的F值后，找出最小的那个，将节点的父节点设为A点
再次计算，将周围节点设为子节点，然后后发现周围有两个58的点，选中gCost更小也就是下面A旁边那个58的点
再次计算
在计算过下面那个58节点后发现旁边节点从这里经过所需的cost更小，所以重新设置父节点
再次说明
如果经过黄线的路径，右下角节点的Cost会达到66
如果经过下面到达，Cost为58，会更小，会重新设置父节点
这里重新计算的fCost为 A — 58 — 58，fCost为58更小，说明新路径更小，重新设置父节点
按照此方法循环直至找到目标点
因为设置了父节点(图中箭头表示)，那么只需要从目标点开始，一直获取父节点即可，将获取到的所有节点存储进列表或数组然后进行翻转，就得到了A-B的最短路径
二、在Unity中设置路径点
然后添加Cuble视为障碍物
将Cube的层级设为UnWalkable
接着复制几个
新建脚本Node，节点目前只包含坐标位置，和是否能行走
public class Node
{
public bool walkable; //节点是否能走动
public Vector3 worldPos; //节点的空间坐标
public Node(bool _walkable, Vector3 _worldPos) //构造器
{
walkable = _walkable;
worldPos = _worldPos;
}
}
新建脚本MyGrid，添加到新建空物体A*上
public class MyGrid : MonoBehaviour
{
public LayerMask unwalkableMask; //节点是否能走动
public Vector2 gridWorldSize; //地图的范围，节点在地图内创建
public float nodeRadius; //节点的大小
Node[,] grid; //节点数组
private void OnDrawGizmos()
{
//首先画出地图的范围 //宽度 厚度 长度
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
}
}
然后设置节点地图大小
继续修改MyGrid
public class MyGrid : MonoBehaviour
{
public LayerMask unwalkableMask; //是否能行走
public Vector2 gridWorldSize; //需要寻路的地图大小
public float nodeRadius; //节点半径
Node[,] grid; //节点
float nodeDiameter; //节点的直径
int gridSizeX, gridSizeY;
void Start()
{
nodeDiameter = nodeRadius * 2;
gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter); //计算出x轴方向有多少个节点
gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter); //计算出z轴方向有多少个节点
CreateGrid();
}
void CreateGrid()
{
grid = new Node[gridSizeX, gridSizeY]; //初始化节点数组
//计算网格的起始点(原点)
Vector3 worldButtonLeft = transform.position
- Vector3.right * gridWorldSize.x / 2
- Vector3.forward * gridWorldSize.y / 2;
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
//计算节点的空间坐标
Vector3 worldPoint = worldButtonLeft
+ Vector3.right * (x * nodeDiameter + nodeRadius)
+ Vector3.forward * (y * nodeDiameter + nodeRadius);
//判断节点是否能行走，根据节点范围是否与障碍物碰撞
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
grid[x, y] = new Node(walkable, worldPoint); //将节点的数据添加进二位数组
}
}
}
private void OnDrawGizmos()
{
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
if (grid != null)
{
foreach (Node node in grid)
{
//绘制出所有节点，可以行走为白色，不能行走为红色
Gizmos.color = (node.walkable) ? Color.white : Color.red;
Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));//减少Gizmos方块的大小便于观察
}
}
}
}
运行结果
接下来添加一个起点和一个终点
新建两个Capsule
那么如何知道起点现在在哪个节点呢？
继续修改MyGrid
public class MyGrid : MonoBehaviour
{
......
public Node NodeFromWorldPos(Vector3 worldPos) //这里传入起点的位置
{
//这里 percentX 和 percentY 计算起点位置占地图区域横竖坐标的比例
float percentX = (worldPos.x + gridWorldSize.x / 2) / gridWorldSize.x;
float percentY = (worldPos.z + gridWorldSize.y / 2) / gridWorldSize.y;
//将起点的位置限定在地图范围之内
percentX = Mathf.Clamp01(percentX);
percentY = Mathf.Clamp01(percentY);
//总节点数量 * 所在区域比例 = 在第几个节点， -1 是为了从 0 开始计算，因为0也有一个节点
int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
return grid[x, y];
}
private void OnDrawGizmos()
{
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
if (grid != null)
{
//计算出起点的位置
Node playerNode = NodeFromWorldPos(player.position);
foreach (Node node in grid)
{
Gizmos.color = (node.walkable) ? Color.white : Color.red;
if (playerNode == node) //设置起点位置节点的颜色
{
Gizmos.color = Color.cyan;
}
Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
}
}
}
}
运行结果
三、实现寻路算法
修改Node
public class Node
{
......
public int gridX; //地图中x方向第几个节点
public int gridY; //地图中y方向第几个节点
public int gCost; //g值
public int hCost; //h值
public Node parent; //父节点，最后用于存储实际路径
//重新添加了两个参数，便于计算邻近节点
public Node(bool _walkable, Vector3 _worldPos,int _gridX, int _gridY)
{
walkable = _walkable;
worldPos = _worldPos;
gridX = _gridX;
gridY = _gridY;
}
public int FCost //属性，F值
{
get
{
return gCost + hCost;
}
}
}
新建脚本PathFinding，并添加到物体A*上
public class PathFinding : MonoBehaviour
{
public Transform seeker, target; //声明两个坐标，起始点和目标点
private MyGrid grid;
......
private void Update()
{
FindPath(seeker.position, target.position); //计算路径
}
private void FindPath(Vector3 startPos, Vector3 targetPos)
{
Node startNode = grid.NodeFromWorldPos(startPos); //输入空间坐标，计算出起始点处于哪个节点位置
Node targwtNode = grid.NodeFromWorldPos(targetPos); //输入空间坐标，计算出目标点处于哪个节点位置
List<Node> openSet = new List<Node>(); //用于存储需要评估的节点
HashSet<Node> closedSet = new HashSet<Node>(); //用于存储已经评估的节点
openSet.Add(startNode); //将起始点加入openSet，进行评估
while (openSet.Count > 0) //如果还有待评估的节点
{
#region //获取待评估列表中 F 值最小的节点
Node currentNode = openSet[0]; //获取其中一个待评估的节点
for (int i = 0; i < openSet.Count; i++) //将该节点与所有待评估的节点比较，找出 F 值 最小的节点，F
//值相同就h值更小的节点
{
if (openSet[i].FCost < currentNode.FCost
|| openSet[i].FCost == currentNode.FCost
&& openSet[i].hCost < currentNode.hCost)
{
currentNode = openSet[i];
}
}
#endregion
openSet.Remove(currentNode); //待评估节点中去掉 F 值最小的节点
closedSet.Add(currentNode); //将该节点加入已评估的节点，之后不再参与评估
if (currentNode == targwtNode) //如果该节点为目标终点，就计算出实际路径并结束循环
{
RetracePath(startNode, targwtNode);
return;
}
//如果该节点不是目标点，遍历该点周围的所有节点
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
//如果周围某节点不能行走 或 周围某节点已经评估，为上一个节点，则跳过
// 说明某节点已经设置父节点
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
//计算前起始点前往某节点的 gCost 值，起始点的 gCost 值就是0
//经过循环这里会计算周围所有节点的g值
int newMovementCostToNeighbor = currentNode.gCost + GetDinstance(currentNode, neighbor);
//如果新路线 gCost 值更小(更近)， 或 某节点没有评估过(为全新的节点)
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor; //计算某节点gCost
neighbor.hCost = GetDinstance(neighbor, targwtNode); //计算某节点hCost
neighbor.parent = currentNode; //将中间节点设为某节点的父节点
//如果存在某节点gCost更小的节点，会重新将中间节点设为某节点父节点
if (!openSet.Contains(neighbor)) //如果某节点没有评估过
{
openSet.Add(neighbor); //将某节点加入待评估列表，在下一次循环进行评估，
//下一次循环又会找出这些周围节点 F 值最小的节点
}
}
}
}
}
private void RetracePath(Node startNode, Node endNode) //获取实际路径
{
List<Node> path = new List<Node>();
Node currentNode = endNode;
while (currentNode != startNode) //如果当前不为目标点
{
path.Add(currentNode); //将当前节点加入路径
currentNode = currentNode.parent;//获取下一个节点(当前节点的父节点)
}
path.Reverse(); //反转所有元素的顺序
grid.path = path; //返回实际路径
}
private int GetDinstance(Node nodeA, Node nodeB) //计算两个节点间的cost
{
int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
if (dstX > dstY)
{
return 14 * dstY + 10 * (dstX - dstY);
}
return 14 * dstX + 10 * (dstY - dstX);
}
}
修改脚本MyGrid
public class MyGrid : MonoBehaviour
{
......
public List<Node> path;
......
void CreateGrid()
{
......
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
...... //多了两个参数，方便计算周围节点
grid[x, y] = new Node(walkable, worldPoint, x, y); //将节点的数据添加进二位数组
}
}
}
......
public List<Node> GetNeighbors(Node node) //获取节点周围的所有节点
{
List<Node> neighbors = new List<Node>();
//节点的相对坐标左侧为x-1，右侧为x+1，下方y-1，上方y+1
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0) //跳过中间的节点
{
continue;
}
//从x、y相对于中间节点的坐标 加上 中间节点位于地图的坐标，得到了周围节点位于地图的坐标
int checkX = node.gridX + x;
int checkY = node.gridY + y;
//限定节点范围，防止出现地图外的不存在的节点
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbors.Add(grid[checkX, checkY]);//添加周围节点
}
}
}
return neighbors;
}
private void OnDrawGizmos()
{
......
if (path != null)
{
if (path.Contains(node)) //给路径添加颜色
{
Gizmos.color = Color.yellow;
}
}
Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
}
}
}
}
自行在Inspector面板中设置相应的参数
运行结果
可以随时修改起点和终点的位置
演示视频：https://www.bilibili.com/video/BV14B4y127YN/
下一篇 A*寻路算法2.0 将使用数组实现堆来代替List列表存储节点，算法消耗的时间将减少约60％
