Unity A*寻路算法

1.节点

cs 复制代码

    //是否可以行走
    public bool isWalkable;
    //节点的位置
    public Vector2Int gridPos;
    //起点->当前节点（准确值）
    public float gCost;
    //当前节点->终点（预测值）
    public float hCost;
    //总代价
    public float FCost => gCost + hCost;
    //上一个节点
    public Node parent;

2.网格管理

1）生成网格地图

cs 复制代码

    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.GetComponent<Node>();
                node.gridPos = new Vector2Int(x,y);//将真实的世界坐标改变为虚拟坐标
                node.isWalkable = true;
                
                Map[x,y] = node;
            }
        }
    }

注：黑色为真实世界坐标，红色为虚拟坐标（参与实际路径计算）。

2）获取网格任意一个节点周围的节点

cs 复制代码

    public List<Node> GetNeighbors(Node node)
    {
        List<Node> neighbors = new List<Node>();
        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）定义待探索队列与已探索（走过）队列

cs 复制代码

List<Node> openList = new List<Node>();//待探索队列
HashSet<Node> closeList = new HashSet<Node>();//已探索队列
        
openList.Add(startNode);

2）当探索队列还有时一直循环

3）找出探索队里fCost最小的节点（意味着总代价最小，路径更短）；如果fCost相同就比较hCost（预估距离最小，gCost因为是确定值不做考虑），找出其最小的。

cs 复制代码

            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）找到最小的节点，就将其从待探索队列移到已探索队列。

cs 复制代码

openList.Remove(currentNode);
closeList.Add(currentNode);

5）如果当前节点等于目标节点就退出循环，并且回溯路径。

cs 复制代码

            //到达终点
            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode);
                return;
            }

cs 复制代码

        void RetracePath(Node startNode, Node endNode) {
        List<Node> path = new List<Node>();
        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，并且将当前节点标记为该节点的父节点，放入待探索队列。

cs 复制代码

            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）四方向：哈曼顿距离

cs 复制代码

        int xDis = Mathf.Abs(node1.gridPos.x - node2.gridPos.x);
        int yDis = Mathf.Abs(node1.gridPos.y - node2.gridPos.y);

        return xDis + yDis;

2）八方向：切比雪夫距离

cs 复制代码

        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）起点不同，终点相同：用迪杰斯特拉算法从终点反向扩散，生成每个节点的方向。