【Unity】A星寻路算法 + Demo示例

A寻路算法的估量代价*

在A算法中核心的寻路依据就是估量代价，在A中通常用 F 表示。
F = G + H
其中G表示当前点到起始点的估量代价 ，H表示当前点到终点的代价。

G的计算方式：
最开始，以起点为中心开始计算周边的八个格子，然后在以这算出来的八个格子为中心，继续计算他们周围的八个格子的G值。
以此类推，直到找到终端，或遍历完所有的格子。
G值的计算结果为：中心点的G值+距离值【10 or 14】

A*算法步骤

1.设置开放列表OpenList和关闭列表CloseList
2.将起点放置到OpenList
3.开启循环While(OpenList.count > 0)
3.1 将OpenList排序【F值从小到大】
3.2 OpenList[0]必定是F值最小的，命名为Center
3.2.1 发现Center就是终点，回溯找到导航路径
3.3 以这个点为中心，去发现它周围的8个格子
3.4 计算发现的每个格子G H F三个值
3.5 如果这个格子没有被计算过或原来G值，比这次计算出来的G要大
3.5.1 此时，设置新的FGH值给这个格子，并设置该格子的发现者是Center
3.6 如果这个格子被计算过，且原G值，比这次计算出来的G要小
3.6.1 此时，就不能替换原来FGH值
3.7 将发现的每个格子放入到OpenList
3.7.1 放入的时候要做检测【该格子不在OpenList、该格子不在CloseList】
3.8 将此次循环的发现者Center放入到CloseList
3.8 判断OpenList空了
3.8.1 说明所有的可发现的格子都被遍历过了，始终没有找到中，说明无法到达终点

实现代码

1 、 格子类 ：

里面声明了F G H值 ， 格子的 x y 下标

设置格子类型 、 按照格子类型设置颜色 ， 根据格子的G值对格子排序（实现IComparable接口）方法

using System;
using UnityEngine;

/// <summary>
/// 格子类型
/// </summary>
public enum GridType
{
    Normal,
    Obstacle,
    Start,
    End
}

public class CubeGrid : MonoBehaviour,IComparable<CubeGrid>
{
    private MeshRenderer _meshRenderer;

    #region Grid Info

    //格子类型
    private GridType _gridType;
    
    //格子下标
    [HideInInspector]
    public int x;
    [HideInInspector]
    public int y;
    
    //格子的估量代价
    [HideInInspector]
    public int F;
    [HideInInspector]
    public int G;
    [HideInInspector]
    public int H;
    
    //发现者
    public CubeGrid finder;

    #endregion
    
    private void Awake()
    {
        _meshRenderer = GetComponent<MeshRenderer>();
    }

    public GridType GetGridType()
    {
        return _gridType;
    }

    public void SetGridType(GridType gridType)
    {
        _gridType = gridType;

        switch (gridType)
        {
            case GridType.Start:
                SetGridColor(Color.red);
                break;
            case GridType.End:
                SetGridColor(Color.green);
                break;
            case GridType.Obstacle:
                SetGridColor(Color.blue);
                break;
        }
    }

    public void SetGridColor(Color color)
    {
        _meshRenderer.material.color = color;
    }

    public int CompareTo(CubeGrid other)
    {
        if (this.F > other.F)
        {
            return 1;
        }
        else if(this.F < other.F)
        {
            return -1;
        }
        else
        {
            return 0;
        }
    }
}

算法逻辑

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Random = UnityEngine.Random;

public class AStarPathFinding : MonoBehaviour
{
    [Header("方块预设体")]
    public GameObject cubePrefab;
    [Range(0,100)]
    [Header("格子障碍物概率")]
    public int obstacleProbability = 30;
    [Header("起点和终点下标")]
    public Vector4 startAndEnd;
    
    //长度
    private int length;
    //宽度
    private int width;
    //所有的格子
    private CubeGrid[,] allGrid;

    private List<CubeGrid> openList;
    private List<CubeGrid> closeList;
    private Stack<CubeGrid> pathStack;

    private void Awake()
    {
        openList = new List<CubeGrid>();
        closeList = new List<CubeGrid>();
        pathStack = new Stack<CubeGrid>();
    }

    private IEnumerator Start()
    {
        length = (int)(transform.localScale.x * 10 / 0.5f);
        width = (int)(transform.localScale.z * 10 / 0.5f);

        //初始化二维数组
        allGrid = new CubeGrid[length,width];
        
        for (int i = 0; i < length; i++)
        {
            for (int j = 0; j < width; j++)
            {
                //生成Cube
                GameObject crtCube = Instantiate(cubePrefab);
                //设置位置
                crtCube.transform.position = new Vector3(i,0,j) / 2;
                //整体偏移
                crtCube.transform.position -= 
                    new Vector3(transform.localScale.x * 5,0,
                        transform.localScale.z * 5);
                crtCube.transform.position += 0.25f * new Vector3(1, 0, 1);
                //获取脚本
                CubeGrid grid = crtCube.GetComponent<CubeGrid>();
                
                if (Random.Range(1, 101) <= obstacleProbability)
                {
                    //设置格子类型
                    grid.SetGridType(GridType.Obstacle);
                }
                
                //设置格子的下标
                grid.x = i;
                grid.y = j;
                
                //将格子保存到二维数组
                allGrid[i, j] = grid;
            }
        }
        
        //设置起点和终点
        allGrid[(int)startAndEnd.x,(int)startAndEnd.y].SetGridType(GridType.Start);
        allGrid[(int)startAndEnd.z,(int)startAndEnd.w].SetGridType(GridType.End);

        yield return new WaitForSeconds(1);
        
        //启动寻路算法
        AStar();
    }

    private void AStar()
    {
        //先将起点放置到OpenList
        openList.Add(allGrid[(int)startAndEnd.x,(int)startAndEnd.y]);
        
        while (openList.Count > 0)
        {
            //对OpenList进行排序【F值从小到大】
            openList.Sort();
            //获取到中心格子
            CubeGrid centerGrid = openList[0];
            //如果找到了终点
            if (centerGrid.GetGridType() == GridType.End)
            {
                //TODO:回溯查找路径
                GeneratePath(centerGrid);  
                break;
            }

            for (int i = -1; i < 2; i++)
            {
                for (int j = -1; j < 2; j++)
                {
                    //先把中心格子剔除
                    if(i == 0 && j == 0)
                        continue;
                    //计算出新格子的下标
                    int x = centerGrid.x + i;
                    int y = centerGrid.y + j;
                    
                    //剔除越界的下标
                    if(x < 0 || y < 0)
                        continue;
                    if(x >= length || y >= width)
                        continue;
                    //获取当前遍历的格子
                    CubeGrid currentGrid = allGrid[x, y];
                    //剔除障碍物
                    if(currentGrid.GetGridType() == GridType.Obstacle)
                        continue;
                    if(closeList.Contains(currentGrid))
                        continue;
                    int g = 0;

                    if (i == 0 || j == 0)
                    {
                        //计算格子的G值
                        g = centerGrid.G + 10;
                    }
                    else
                    {
                        //计算格子的G值
                        g = centerGrid.G + 14;
                    }

                    if (currentGrid.G == 0 || currentGrid.G > g)
                    {
                        //更新G值
                        currentGrid.G = g;
                        //计算H值
                        currentGrid.H = (int)(Mathf.Abs(startAndEnd.z - x)
                                            + Mathf.Abs(startAndEnd.w - y)) * 10;
                        //计算F值
                        currentGrid.F = g + currentGrid.H;
                        //设置或更新【发现者】
                        currentGrid.finder = centerGrid;
                        //openlist和closelist中都不包含该格子
                        if (!openList.Contains(currentGrid) &&
                            !closeList.Contains(currentGrid))
                        {
                            //将当前格子放置到openlist
                            openList.Add(currentGrid);
                        }
                    }
                }
            }
            
            //将center放入CloseList
            closeList.Add(centerGrid);
            //将center从Openlist中删除
            openList.Remove(centerGrid);

            if (openList.Count == 0)
            {
                Debug.Log("没有找到路径！！！");
                break;
            }
        }
    }

    /// <summary>
    /// 生成路径
    /// </summary>
    /// <param name="cubeGrid"></param>
    private void GeneratePath(CubeGrid cubeGrid)
    {
        pathStack.Push(cubeGrid);
        
        if (cubeGrid.finder != null)
        {
            GeneratePath(cubeGrid.finder);
        }
        else
        {
            //展示路径
            StartCoroutine(ShowPath());
        }
    }

    private IEnumerator ShowPath()
    {
        //总路径数
        int length = pathStack.Count;
        
        while (pathStack.Count > 0)
        {
            yield return new WaitForSeconds(0.05f);
            //获取到当前格子
            CubeGrid currentGrid = pathStack.Pop();
            //渐变色
            Color col = Color.Lerp(Color.red, Color.green, (length - pathStack.Count) / (float)length);
            //更换颜色以标记格子
            currentGrid.SetGridColor(col);
        }
    }

    private void Update()
    {
        if (Input.GetKeyDown(KeyCode.Space))
        {
            UnityEngine.SceneManagement.SceneManager.LoadScene(0);
        }
    }
}

效果图