Unity A*寻路算法

1.节点//是否可以行走 public bool isWalkable; //节点的位置 public Vector2Int gridPos; //起点-当前节点准确值 public float gCost; //当前节点-终点预测值 public float hCost; //总代价 public float FCost gCost hCost; //上一个节点 public Node parent;2.网格管理1生成网格地图public void CreateMap() { Map new Node[mapX,mapY]; for (int x 0; x mapX; x) { for (int y 0; y mapY; y) { //确定一个地图的原点第一象限 Vector2Int pos new Vector2Int(mapOrigin.x x,mapOrigin.y y); GameObject obj Instantiate(nodePrefab, new Vector3(pos.x , pos.y , 0), Quaternion.identity,mapRoot); Node node obj.GetComponentNode(); node.gridPos new Vector2Int(x,y);//将真实的世界坐标改变为虚拟坐标 node.isWalkable true; Map[x,y] node; } } }注黑色为真实世界坐标红色为虚拟坐标参与实际路径计算。2获取网格任意一个节点周围的节点public ListNode GetNeighbors(Node node) { ListNode neighbors new ListNode(); for (int x - 1; x 1; x) { for (int y - 1; y 1; y) { //指的是中心节点 if(x 0 y 0) continue; //x与y其实是node的偏移值 int nodeX node.gridPos.x x; int nodeY node.gridPos.y y; //如果在地图里面就添加到列表中 if (nodeX 0 nodeX mapX nodeY 0 nodeY mapY) { neighbors.Add(Map[nodeX,nodeY]); } } } return neighbors; }3.算法核心1定义待探索队列与已探索走过队列ListNode openList new ListNode();//待探索队列 HashSetNode closeList new HashSetNode();//已探索队列 openList.Add(startNode);2当探索队列还有时一直循环3找出探索队里fCost最小的节点意味着总代价最小路径更短如果fCost相同就比较hCost预估距离最小gCost因为是确定值不做考虑找出其最小的。Node currentNode openList[0]; for (int i 1; i openList.Count; i) { if (openList[i].FCost currentNode.FCost || openList[i].FCost currentNode.FCost openList[i].hCost currentNode.hCost) { currentNode openList[i]; } }4找到最小的节点就将其从待探索队列移到已探索队列。openList.Remove(currentNode); closeList.Add(currentNode);5如果当前节点等于目标节点就退出循环并且回溯路径。//到达终点 if (currentNode targetNode) { RetracePath(startNode, targetNode); return; }void RetracePath(Node startNode, Node endNode) { ListNode path new ListNode(); Node currentNode endNode; while (currentNode ! startNode) { currentNode.spriteRenderer.color Color.green; path.Add(currentNode); currentNode currentNode.parent; } path.Reverse(); }注根据节点的Parent进行回溯操作。6如果没有到达终点就获取当前节点的周围邻居节点。周围邻居节点中如果时是障碍或者已经探索过了就跳过。其余的就计算其gCost如果比自己之前的小距离初始点更近周围节点一开始gCost为零因此newGCost node.gCost但是之前没待过任何队列一定会加入待探索队列中。 之后gCost赋值后就比较谁的路径更短往回走的话newCost会更大或者节点不在探索队列不在任何队列就计算这个邻居节点g/h/fCost并且将当前节点标记为该节点的父节点放入待探索队列。foreach (var node in gridManager.GetNeighbors(currentNode)) { if(!node.isWalkable || closeList.Contains(node)) continue; float newGCost currentNode.gCost GetDistance(currentNode, node); if (newGCost node.gCost || !openList.Contains(node)) { node.gCost newGCost; node.hCost GetDistance(node, targetNode); node.parent currentNode; if(!openList.Contains(node)) openList.Add(node); } }7预估代价的距离计算1四方向哈曼顿距离int xDis Mathf.Abs(node1.gridPos.x - node2.gridPos.x); int yDis Mathf.Abs(node1.gridPos.y - node2.gridPos.y); return xDis yDis;2八方向切比雪夫距离int xDis Mathf.Abs(node1.gridPos.x - node2.gridPos.x); int yDis Mathf.Abs(node1.gridPos.y - node2.gridPos.y); //横竖移动代价10斜向移动代价14√2≈1.414取整 const int straightCost 10; const int diagonalCost 14; //先斜向走min(dx,dy)步再横竖走|dx-dy|步 return diagonalCost * Mathf.Min(xDis, yDis) straightCost * Mathf.Abs(xDis - yDis);4.多单位A*算法的优化1待探索队列用Priority Queue优先队列方便查找最小值已探索队列用HashSet哈希表查找方便。注优先队列逻辑上是完全二叉实际上是数组通过公式计算索引根为最小值查找更快。2起始位置相同直接复用缓存路径。3起点不同终点相同用迪杰斯特拉算法从终点反向扩散生成每个节点的方向。