A*最短路径算法实现

A*算法原理

• 用来解决什么问题

  最短路径问题

  A*寻路就是用来计算玩家行进路径的，通过它可以计算出避开阻挡的最短路径

• 基本原理

  不停的找自己周围的点，选出一个新的点作为起点再循环找

• 详细原理

  1. 寻路消耗公式

     f (寻路消耗)= g（离起点的距离） + h（离终点的距离）

     • 离起点 g：勾股定理和直线距离，父对象离起点的距离+自己离父对象的距离
     • 离终点 h：曼哈顿距离，直接算x，y格子差

  2. 开启列表

     • 开启和关闭作为容器

  3. 关闭列表

     • 每次往关闭列表放点，都要判断该点和终点是否一样
     • 最后开启列表为空，关闭列表仍没有终点值说明是死路
     • 每次从新的点找周围的点时，如果周围的点已经在开启/关闭列表中，就不用查找

  4. 格子对象的父对象

     • 从关闭列表最后一个点（终点）依次找父对象，就是链表

       取父对象而不是关闭列表的原因：路径上有阻挡，关闭列表的点不对，可能要换掉前面的点，关闭列表里面的点可能有不对是绕路的，通过父对象找到的才是对的，通过父对象回溯。

• 步骤

  1. 起点放入关闭列表，设置起点父对象为空
  2. 遍历起点周围8个点，放到开启列表，并记录其父对象（上一步）
  3. 选出最合适的点，最优放到关闭列表
  4. 排除障碍遍历周围8个点，排除重复的点
  5. 直到找到终点

---

类图梳理

---

A*算法实现

AStarNode Class

• 寻路地图中每一个点的类

// AStarNode
// 对于type类型最好使用枚举，walk可以通过，stop不能通过
public enum E_Node_Type { walk, stop }

// A星格子类
public class AStarNode
{
    // 格子对象的坐标
    public int x;
    public int y;

    //寻路消耗
    public float f;
    //离起点的直线距离
    public float g;
    //离终点的曼哈顿距离
    public float h;

    // 父对象
    public AStarNode father;

    // 格子的类型
    public E_Node_Type type;

    // 有参构造函数
    public AStarNode(int x, int y, E_Node_Type type)
    {
        this.x = x;
        this.y = y;
        this.type = type;
    }
}

AStarManager A星管理器

• 管理A星寻路整个地图

// AStarManager
// 管理器一般不需要继承，使用单例模式
// A星寻路管理器
public class AStarManager
{
    // 单例模式
    private static AStarManager instance;

    public static AStarManager Instance
    {
        get
        {
            if (instance == null)
                instance = new AStarManager();
            return instance;
        }
    }
    //地图的宽高
    private int mapW;
    private int mapH;

    // 地图所有相关格子对象容器
    public AStarNode[,] nodes;
    private List<AStarNode> openList = new List<AStarNode>();   // 开启列表
    private List<AStarNode> closeList = new List<AStarNode>();  // 关闭列表

    /// <summary>
    /// 初始化地图信息
    /// </summary>
    /// <param name="w"></param>
    /// <param name="h"></param>
    public void InitMapInfo(int w, int h)
    {
        // 记录宽高 范围
        this.mapH = h;
        this.mapW = w;
        // 根据宽高 创建格子
        nodes = new AStarNode[w, h];    // 声明容器大小
        for (int i = 0; i < w; i++) 
            for (int j = 0; j < h; j++)
            {
                AStarNode node = new AStarNode(i, j, Random.Range(0, 100) < 20 ?  E_Node_Type.stop : E_Node_Type.walk); // 百分之20概率是障碍
                nodes[i, j] = node;
            }      
    }

    /// <summary>
    /// 寻路方法 提供给外部使用
    /// </summary>
    /// <param name="startPos"></param> 起点
    /// <param name="endPos"></param>   终点
    /// <returns></returns>
    public List<AStarNode> FindPath(Vector2 startPos, Vector3 endPos)
    {
        // 得到起点终点 对应的格子
        AStarNode starNode = nodes[(int)startPos.x, (int)startPos.y];
        AStarNode endNode = nodes[(int)endPos.x, (int)endPos.y];
        //首先判断传入的两个点是否合法
        //1, 地图范围内
        if (startPos.x >= mapW || startPos.x < 0||
            startPos.y >= mapH || startPos.y < 0||
            endPos.x >= mapW || endPos.x < 0 ||
            endPos.y >= mapH || endPos.y < 0)
        {
            Debug.Log("开始或者结束点在地图格子范围外");
            return null;
        }
        // 2.不是阻挡
        // 在nodes容器里面取点
        AStarNode start = nodes[(int)startPos.x, (int)startPos.y];
        AStarNode end = nodes[(int)endPos.x, (int)endPos.y];
        if (start.type == E_Node_Type.stop || end.type == E_Node_Type.stop)
        {
            Debug.Log("开始或者结束点是障碍");
            return null;
        }
        // 如果不合法 返回null 不能寻路 

        // 清空上一次相关的数据，避免他们影响这一次的数据
        // 清空关闭和开启列表
        closeList.Clear();
        openList.Clear();

        // 把开始点放入开启列表中
        start.father = null;
        start.f = 0;
        start.g = 0;
        start.h = 0;
        closeList.Add(start);   // 第一次 起点放进去

        // 死循环
        while(true)
        {
            // 从起点开始 找周围的点 并放入开启列表中
            // 左上
            FindNearOpenList(start.x - 1, start.y - 1, 1.4f, start, end);
            // 上
            FindNearOpenList(start.x, start.y - 1, 1, start, end);
            // 右上
            FindNearOpenList(start.x + 1, start.y - 1, 1.4f, start, end);
            // 左
            FindNearOpenList(start.x - 1, start.y, 1, start, end);
            // 右
            FindNearOpenList(start.x + 1, start.y, 1, start, end);
            // 左下
            FindNearOpenList(start.x - 1, start.y + 1, 1.4f, start, end);
            // 下
            FindNearOpenList(start.x, start.y + 1, 1, start, end);
            // 右下
            FindNearOpenList(start.x + 1, start.y + 1, 1.4f, start, end);

            // 死路判断 开启列表为空 都没找到终点 死路
            if (openList.Count == 0)
            {
                Debug.Log("死路");
                return null;
            }

            // 选出开启列表中 寻路消耗最小的点 List.Sort（函数名） 委托
            openList.Sort(SortOpenList);

            // 放入关闭列表中 然后再从开启列表中移除
            closeList.Add(openList[0]); // 排序后第0个就是最小的                                     
            start = openList[0];        // 找到这个点 又变成新的起点 运行下一次寻路计算
            openList.RemoveAt(0);

            // 如果这个点已经是终点 得到最终结果 返回
            // 如果这个点不是终点 那么继续循环
            if (start == end)
            {
                // 找到路径了：将列表里父节点放进去
                List<AStarNode> path = new List<AStarNode>();
                path.Add(end);
                while (end.father != null)  // 开始father置空
                {
                    path.Add(end.father);
                    end = end.father;
                }
                // 列表反转api
                path.Reverse();
                return path;
            }
        }
    }

    /// <summary>
    /// 排序函数
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <returns></returns>
    private int SortOpenList(AStarNode a, AStarNode b)
    {
        if (a.f > b.f)
            return 1;
        else if (a.f == b.f)
            return 1;
        else
            return -1;
    }

    /// <summary>
    /// 把临近节点放入开启列表中的函数
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    private void FindNearOpenList(int x, int y, float g, AStarNode father, AStarNode end)
    {
        // 边界判断
        if (x >= mapW || x < 0 || y >= mapH || y < 0 )
            return;

        AStarNode node = nodes[x, y];
        // 判断这些点 是否是边界 是否是阻挡 是否在开启/关闭列表中 如果都不是 才放入开启
        if (node == null ||
            node.type == E_Node_Type.stop ||
            closeList.Contains(node)||
            openList.Contains(node))
            return;
        // 计算f值：f = g + h
        // 记录父对象
        node.father = father;
        // 计算g，我离起点的距离 = 我父亲离起点的距离 + 我离父亲的距离
        node.g = father.g + g;
        node.h = Mathf.Abs(end.x - node.x) + Mathf.Abs(end.y - node.y); // 离终点的曼哈顿距离
        node.f = node.g + node.h;
        // 如果通过了上面的合法验证，就存到开启列表中
        openList.Add(node);
    }
}

测试

• 用cube名字做坐标

public class TestAStar : MonoBehaviour
{
    // 左上角第一个立方体位置
    public int beginX = -3;
    public int beginY = 5;
    // 之后每一个立方体 偏移位置
    public int offsetX = 2;
    public int offsetY = 2;
    // 地图格子宽高
    public int mapW = 5;
    public int mapH = 5;

    private Vector2 beginPos = Vector2.right * -1;  // 开始点负坐标，right 是 Vector2(1, 0)简写

    private Dictionary<string, GameObject> cubes = new Dictionary<string, GameObject>();
    private List<AStarNode> list;

    public Material red;
    public Material yellow;
    public Material green;
    public Material normal;

    // Start is called before the first frame update
    void Start()
    {
        AStarManager.Instance.InitMapInfo(mapW, mapH);  

        for (int i = 0; i < mapW; i++)
            for (int j = 0; j < mapH; j++)
            {
                // 创建立方体
                GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Cube);    // 创建默认模型
                obj.transform.position = new Vector3(beginX + i * offsetX, beginY + j * offsetY, 0);
                // 名字
                obj.name = i + "_" + j;
                // 存储立方体到字典中
                cubes.Add(obj.name, obj);

                // 得到格子，判断是不是阻挡
                AStarNode node = AStarManager.Instance.nodes[i, j];
                if (node.type == E_Node_Type.stop)
                {
                    obj.GetComponent<MeshRenderer>().material = red;
                }
            }
    }

    // Update is called once per frame
    void Update()
    {
        if (Input.GetMouseButtonDown(0))
        {
            AStarFindPath();
        }         
    }

    public void AStarFindPath()
    {
        // 射线检测
        RaycastHit info;
        // 得到屏幕鼠标位置发出去的射线
        Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);

        // 射线检测
        if (Physics.Raycast(ray, out info, 1000))
        {
                    
            if (beginPos == Vector2.right * -1)
            {
                // 清理上一次的路径 把绿色立方体变成白色   
                if (list != null)
                {
                    for (int i = 0; i < list.Count; i++)
                    {
                        // 路径经过的格子名字为ix_iy，材质改为绿色
                        cubes[list[i].x + "_" + list[i].y].GetComponent<MeshRenderer>().material = normal;
                    }
                }
                // 得到点击的立方体，才能知道是几行几列的
                string[] strs = info.collider.gameObject.name.Split('_');    // 用下划线分开字符串
                // 得到行列位置
                beginPos = new Vector2(int.Parse(strs[0]), int.Parse(strs[1])); // int.Parse(strs[0]) 字符转int
                // 点击到的对象改为黄色
                info.collider.gameObject.GetComponent<MeshRenderer>().material = yellow;
            }
            // 有起点 那就点出终点
            else
            {
                // 得到终点
                string[] strs = info.collider.gameObject.name.Split('_');    // 用下划线分开字符串
                Vector2 endPos = new Vector2(int.Parse(strs[0]), int.Parse(strs[1]));

                // 寻路
                list = AStarManager.Instance.FindPath(beginPos, endPos);
                // 如果不为空 找成功
                if (list != null)
                {
                    for (int i = 0; i < list.Count; i++)
                    {
                        // 路径经过的格子名字为ix_iy，材质改为绿色
                        cubes[list[i].x + "_" + list[i].y].GetComponent<MeshRenderer>().material = green;
                    }
                }
                // 清除开始点 把他变成初始值
                beginPos = Vector2.right * -1;
            }
        }
    }
}

Reference：【手把手教你】Unity中实现A星寻路算法 https://www.bilibili.com/video/BV147411u7r5?p=4&share_source=copy_web&vd_source=d83ce8fdf322bae01b4efc0e4798d0b9

---