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％