AStar寻路算法

前言：关于A*算法的写法用法网上有很多教程，我这里只讲解我目前用到的写法。

一、算法流程介绍

1.写一个信息类

这个类的作用主要是用于保存算法地图中每个格子的信息。

public class Point
{
    public Point parent { get; set; }//该格子的父节点
    public Vector2 worldpos;//该格子在场景中的坐标
    public int x;//算法中的x方向第几格
    public int y;//算法中的y方向第几格
    public float h;//该格子与父节点的距离
    public float g;//该格子与目标点的距离 
    public float f;//h与g的综合，也是该格子在线路中的优先级
    public bool isWall;//是否是障碍物，能不能通过

    public Point(Vector2 worldpos, int x, int y, Point parent = null)
    {
        this.parent = parent;
        this.worldpos = worldpos;
        this.x = x;
        this.y = y;
    }

    public void UpdateParent(Point parent, float g)
    {
        this.parent = parent;
        this.g = g;
        this.f = this.g + h;
    }
}

2.绘制算法地图

在A算法中首先要将我们在unity中用到的地图变换成算法中的数组。所以我们的第一步就是绘制算法地图，以确定哪些地方可以走，哪些地方是障碍物，不可以走。绘制地图要有一个起始原点，即图中摄像机的左下角（原点可以自定义位置，我的设定的绘制地图大小是1510，所以图中显示的方格数量是150个）。

3.确定起点和终点

在范例中我是直接将目前要移动的物体绑定为起点，终点可自己指定。

4.开始查找路径

1、我们要写两个列表，用于保存信息。

开启列表: List<Point> openList = new List<Point>(); 待检查方格的集合列表，寻找周围可以达到的点，加入到此表中，并保存中心点为父节点
关闭列表：List<Point> closeList = new List<Point>(); 列表中保存不需要再次检查的方格 2、把起始点和终点的二维坐标转换到算法地图中的格子位置；

 public Point GetIdem(Vector2 pos)
    {
        int i = Mathf.RoundToInt((pos.x - ((Vector2)meshOrigin.position).x) / lengthofbox);
        int j = Mathf.RoundToInt((pos.y - ((Vector2)meshOrigin.position).y) / lengthofbox);
        i = Mathf.Clamp(i, 0, mapWith - 1);
        j = Mathf.Clamp(j, 0, mapHeight - 1);
        return map[i, j];
    }

3、将起始位置添加到开启列表中。 4、开始循环。查找开启列表中F值最小的那个方格point（不知道F值是啥的看开头信息类代码）（如果是第一轮，此时开启列表内只有起始位置）

private Point FindMinFOfPoint(List<Point> openList)
    {
        float f = float.MaxValue;
        Point temp = null;
        foreach (Point p in openList)
        {
            if (p.f < f)
            {
                temp = p;
                f = p.f;
            }
        }
        //print("返回起始列表中最小的节点F：" + temp.f);
        return temp;
    }

5、如果point是终点就跳出循环，否则继续执行。 6、将point从开启列表中移除，并添加到关闭列表中。 7、写一个周围列表用于保存point周围的格子。

此时是第一次循环。“周围列表”中保存的为图中绿色格子，此时的A点已经从“开启列表”中移除，并放入到“关闭列表”中

List<Point> surroundPoints = GetSurroundPoints(point);

  /// <summary>
    /// 获取当前节点周围的四个节点
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    private List<Point> GetSurroundPoints(Point point)
    {
        //需要八个方向都可以走，将该方法中注释部分代码解开即可
        Point up = null, down = null, left = null, right = null;
        //Point lu = null, ru = null, ld = null, rd = null;
        if (point.y < mapHeight - 1)
        {
            up = map[point.x, point.y + 1];
        }
        if (point.y > 0)
        {
            down = map[point.x, point.y - 1];
        }
        if (point.x > 0)
        {
            left = map[point.x - 1, point.y];
        }
        if (point.x < mapWith - 1)
        {
            right = map[point.x + 1, point.y];
        }
        //if (up != null && left != null)
        //{
        //    lu = map[point.x - 1, point.y + 1];
        //}
        //if (up != null && right != null)
        //{
        //    ru = map[point.x + 1, point.y + 1];
        //}
        //if (down != null && left != null)
        //{
        //    ld = map[point.x - 1, point.y - 1];
        //}
        //if (down != null && right != null)
        //{
        //    rd = map[point.x + 1, point.y - 1];
        //}
        List<Point> list = new List<Point>();
        if (down != null && down.isWall == false)
        {
            list.Add(down);
        }
        if (left != null && left.isWall == false)
        {
            list.Add(left);
        }
        if (right != null && right.isWall == false)
        {
            list.Add(right);
        }
        if (up != null && up.isWall == false)
        {
            list.Add(up);
        }
        //if (lu != null && lu.isWall == false && left.isWall == false && up.isWall == false)
        //{
        //    list.Add(lu);
        //}
        //if (ld != null && ld.isWall == false && left.isWall == false && down.isWall == false)
        //{
        //    list.Add(ld);
        //}
        //if (ru != null && ru.isWall == false && right.isWall == false && up.isWall == false)
        //{
        //    list.Add(ru);
        //}
        //if (rd != null && rd.isWall == false && right.isWall == false && down.isWall == false)
        //{
        //    list.Add(rd);
        //}
        return list;
    }

8、移除周围列表里面已经存在于关闭列表里的格子。

注意：此时是第二次循环。因为A已经计算过一次，写入到在“关闭列表”里了，所以要从“周围列表”中移除，避免二次计算。

9、遍历周围列表

如果开启列表里存在周围列表里的格子，那么判断该格子是基于它的父节点优先级（f值）高一些，还是基于自己优先级高一些，如果基于自己优先级高一些则将它的父节点设为自己。
如果开启列表中不存在该格子的父节点设为自己，并计算它的f、h、g值，并添加到开启列表中

10、返回第4步循环，直到开启列表中没有格子为止。（地图全部走完）如果此时还没有得到完整路径，说明无法到达终点（终点可能在障碍物中的情况）。

5.生成路径泛型队列

此时我们已经在关闭列表中找到了终点，那么我们根据终点的父节点一直向上找就可以找到一条完整的路径。

终点的g值是68，它的父节点是58的格子，58格子的父节点是48，依次循环就可以找到起点，形成图中蓝色格子路径

 private List<Point> Generatepath(Point start, Point end)
    {
        List<Point> path = new List<Point>();
        if (end != null)
        {
            Point node = end;//从终点开始生成路线
            while (node != start)
            {
                path.Add(node);//将节点放入路径队列中
                node = node.parent;//下一个节点为该节点的父节点
            }
            path.Reverse();//将路径节点倒序
        }
        //Debug.LogWarning(path.Count);
        return path;
    }

6.绘制路径

这里需要有LineRenderer组件

private void ShowPath()
    {
        int count = path.Count;
        if (path.Count > 0)
        {
            pathline.positionCount = count;
            for (int i = 0; i < count; i++)
            {
                pathline.SetPosition(i, path[i].worldpos);
            }
        }
        else
        {
            pathline.positionCount = 0;
        }
    }

二、具体demo代码演示

这里由于我的demo里面对开启列表的数据结构进行了一些二叉堆优化，看不懂的同学自行去科普二叉堆。在控制人物移动方面可以把update里的按键操作代码取消注释，这样人物移动会跟精准。还有我的设定是如果障碍物周围有路面也可以到达，总而言之大家看着改吧。

using UnityEngine;

public class Point
{
    public Point parent { get; set; }//该格子的父节点
    public Vector2 worldpos;//该格子在场景中的坐标
    public int x;//算法中的x方向第几格
    public int y;//算法中的y方向第几格
    public float h;//该格子与目标点的距离
    public float g;//该格子与父节点的距离 
    public float f;//h与g的综合，也是该格子在线路中的优先级
    public bool isWall;//是否是障碍物，能不能通过

    public Point(Vector2 worldpos, int x, int y, Point parent = null)
    {
        this.parent = parent;
        this.worldpos = worldpos;
        this.x = x;
        this.y = y;
    }

    public void UpdateParent(Point parent, float g)
    {
        this.parent = parent;
        this.g = g;
        this.f = this.g + h;
    }
}

using System.Collections.Generic;
using UnityEngine;

public class Astar : MonoBehaviour
{
    public static Astar instance;

    public const int mapWith = 15;//地图的长
    public const int mapHeight = 10;//地图的宽
    private Point[,] map = new Point[mapWith, mapHeight];

    public GameObject Ground;//地图不可行走格子的图片
    public GameObject Way;//地图可行走格子的图片

    [Range(0.5f, 1.5f)]
    public float lengthofbox;//地图中每个格子大小，这个可以根据实际情况改

    public LayerMask NodeLayer;//选择障碍物所在的层
    public Transform meshOrigin;//网格地图起点对象

    private void Awake()
    {
        instance = this;

        for (int x = 0; x < mapWith; x++)
        {
            for (int y = 0; y < mapHeight; y++)
            {
                //从起始点meshOrigin开始绘制网格
                Vector2 pos = new Vector2(x * lengthofbox, y * lengthofbox) + (Vector2)meshOrigin.position;

                //绘制可视化地面背景
                GameObject gameObject = Instantiate(Ground);
                gameObject.transform.position = new Vector3(pos.x, pos.y, 100)；  
                gameObject.transform.SetParent(GameObject.Find("Ground").transform);

                //方形范围检测（这里如果判断我将能检测到的方形碰撞盒的地方设为可行走路面，如果想改为障碍将感叹号去掉即可）
                bool iswall = !Physics2D.BoxCast(pos, new Vector2(lengthofbox, lengthofbox), 0, new Vector2(0, 0), NodeLayer);
                map[x, y] = new Point(pos, x, y);
                map[x, y].isWall = iswall;
            }
        }
    }

    /// <summary>
    /// 绘制算法地图
    /// </summary>
    private void InitMap()
    {
        for (int x = 0; x < mapWith; x++)
        {
            for (int y = 0; y < mapHeight; y++)
            {
                //从起始点meshOrigin开始绘制网格
                Vector2 pos = new Vector2(x * lengthofbox, y * lengthofbox) + (Vector2)meshOrigin.position;
                //方形范围检测（这里如果判断我将能检测到的方形碰撞盒的地方设为可行走路面，如果想改为障碍将感叹号去掉即可）
                bool iswall = !Physics2D.BoxCast(pos, new Vector2(lengthofbox, lengthofbox), 0, new Vector2(0, 0), NodeLayer);
                map[x, y] = new Point(pos, x, y);
                map[x, y].isWall = iswall;
            }
        }
    }

    /// <summary>
    /// 二叉堆添加
    /// </summary>
    /// <param name="openList"></param>
    /// <param name="point"></param>
    private void AddPoint(List<Point> openList, Point point)
    {
        openList.Add(point);
        int last = openList.Count - 1;
        while (last >= 1)
        {
            int half = last >> 1;
            if (openList[last].f >= openList[half].f)
            {
                break;
            }
            Point temporary = openList[last];
            openList[last] = openList[half];
            openList[half] = temporary;
            last >>= 1;
        }
    }
    /// <summary>
    /// 二叉堆删除
    /// </summary>
    /// <param name="openList"></param>
    /// <param name="point"></param>
    private void RemovePoint(List<Point> openList, Point point)
    {
        int last = openList.Count;
        int head = openList.IndexOf(point) + 1;//防止索引为零，必须+1

        while ((head << 1) + 1 <= last)
        {
            int child1 = head << 1;
            int child2 = child1 + 1;
            int childMin = openList[child1 - 1].f < openList[child2 - 1].f ? child1 : child2;

            openList[head - 1] = openList[childMin - 1];

            head = childMin;

        }
        openList.Remove(openList[head - 1]);
    }

    /// <summary>
    /// 查找最优路径
    /// </summary>
    /// <param name="start">起点</param>
    /// <param name="end">终点</param>
    public List<Point> FindPath(Vector2 startPos, Vector2 endPos)
    {
        Point start = GetIdem(startPos);
        Point end = GetIdem(endPos);
        //Debug.LogWarning("起点算法坐标"+start.x + "+" + start.y);
        //Debug.LogWarning("终点算法坐标"+end.x + "+" + end.y);
        List<Point> openList = new List<Point>();
        List<Point> closeList = new List<Point>();
        AddPoint(openList, start);//将起始位置添加到Open列表

        //终点在路面上的情况
        while (openList.Count > 0)
        {
            Point point = openList[0];//获取开启列表中F值最小的格子，
            if (point == end)
            {
                return Generatepath(start, end);
            }
            RemovePoint(openList, point);
            closeList.Add(point);
            //四周节点列表
            List<Point> surroundPoints = GetSurroundPoints(point);
            PointsFilter(surroundPoints, closeList);

            foreach (Point surroundPoint in surroundPoints)
            {
                if (openList.IndexOf(surroundPoint) > -1)//如果周围节点在起始列表中
                {
                    // 计算经过的Open列表中最小f值到周围节点的G值
                    float nowG = CalcG(surroundPoint, surroundPoint.parent);
                    if (nowG < surroundPoint.g)
                    {
                        surroundPoint.UpdateParent(point, nowG);
                    }
                }
                else
                {
                    surroundPoint.parent = point;//设置周围列表的父节点
                    CalcF(surroundPoint, end);//计算周围节点的F，G,H值  
                    AddPoint(openList, surroundPoint); //最后将周围节点添加进Open列表  
                }
            }
        }

        //终点在障碍物上，且障碍物周围至少有一格为路面的情况
        List<Point> endSurroundPoints = GetSurroundPoints(end);//终点周围格子
        Point optimal = null;//最优点
        foreach (Point surroundPoint in endSurroundPoints)
        {
            if (closeList.IndexOf(surroundPoint) > -1)
            {
                if (optimal != null)
                {
                    if (surroundPoint.g < optimal.g)
                        optimal = surroundPoint;
                }
                else
                {
                    optimal = surroundPoint;
                }
            }
        }
        if (optimal != null)
        {
            return Generatepath(start, optimal);
        }

        //终点在障碍物上，无法到达
        Debug.LogWarning("找不到路径");
        return Generatepath(start, null);
    }
    /// <summary>
    /// 获取坐标点在算法地图中的坐标
    /// </summary>
    /// <param name="pos"></param>
    /// <returns></returns>
    public Point GetIdem(Vector2 pos)
    {
        int i = Mathf.RoundToInt((pos.x - ((Vector2)meshOrigin.position).x) / lengthofbox);
        int j = Mathf.RoundToInt((pos.y - ((Vector2)meshOrigin.position).y) / lengthofbox);
        i = Mathf.Clamp(i, 0, mapWith - 1);
        j = Mathf.Clamp(j, 0, mapHeight - 1);
        return map[i, j];
    }

    /// <summary>
    /// 获取当前节点周围的四个节点
    /// </summary>
    /// <param name="point"></param>
    /// <returns></returns>
    private List<Point> GetSurroundPoints(Point point)
    {
        //需要八个方向都可以走，将该方法中注释部分代码解开即可
        Point up = null, down = null, left = null, right = null;
        //Point lu = null, ru = null, ld = null, rd = null;
        if (point.y < mapHeight - 1)
        {
            up = map[point.x, point.y + 1];
        }
        if (point.y > 0)
        {
            down = map[point.x, point.y - 1];
        }
        if (point.x > 0)
        {
            left = map[point.x - 1, point.y];
        }
        if (point.x < mapWith - 1)
        {
            right = map[point.x + 1, point.y];
        }
        //if (up != null && left != null)
        //{
        //    lu = map[point.x - 1, point.y + 1];
        //}
        //if (up != null && right != null)
        //{
        //    ru = map[point.x + 1, point.y + 1];
        //}
        //if (down != null && left != null)
        //{
        //    ld = map[point.x - 1, point.y - 1];
        //}
        //if (down != null && right != null)
        //{
        //    rd = map[point.x + 1, point.y - 1];
        //}
        List<Point> list = new List<Point>();
        if (down != null && down.isWall == false)
        {
            list.Add(down);
        }
        if (left != null && left.isWall == false)
        {
            list.Add(left);
        }
        if (right != null && right.isWall == false)
        {
            list.Add(right);
        }
        if (up != null && up.isWall == false)
        {
            list.Add(up);
        }
        //if (lu != null && lu.isWall == false && left.isWall == false && up.isWall == false)
        //{
        //    list.Add(lu);
        //}
        //if (ld != null && ld.isWall == false && left.isWall == false && down.isWall == false)
        //{
        //    list.Add(ld);
        //}
        //if (ru != null && ru.isWall == false && right.isWall == false && up.isWall == false)
        //{
        //    list.Add(ru);
        //}
        //if (rd != null && rd.isWall == false && right.isWall == false && down.isWall == false)
        //{
        //    list.Add(rd);
        //}
        return list;
    }
    /// <summary>
    /// 将关闭列表中已经存在的节点从周围节点中移除
    /// </summary>
    /// <param name="surroundPoints">四周节点列表</param>
    /// <param name="closeList">关闭列表</param>
    private void PointsFilter(List<Point> surroundPoints, List<Point> closeList)
    {
        foreach (Point p in closeList)
        {
            if (surroundPoints.IndexOf(p) > -1)
            {
                surroundPoints.Remove(p);
            }
        }
    }

    /// <summary>
    /// 计算经过起始列表中最小f值到周围节点的G值
    /// </summary>
    /// <param name="surroundPoint"></param>
    /// <param name="parent"></param>
    /// <returns></returns>
    private float CalcG(Point surroundPoint, Point parent)
    {
        return Vector2.Distance(new Vector2(surroundPoint.x, surroundPoint.y), new Vector2(parent.x, parent.y)) + parent.g;
    }
    /// <summary>
    /// 计算周围节点的F，G，H值
    /// </summary>
    /// <param name="surroundPoint"></param>
    /// <param name="end"></param>
    private void CalcF(Point surroundPoint, Point end)
    {
        //F = G + H  
        float h = Mathf.Abs(end.x - surroundPoint.x) + Mathf.Abs(end.y - surroundPoint.y);
        float g = 0;
        if (surroundPoint.parent == null)
        {
            g = 0;
        }
        else
        {
            g = Vector2.Distance(new Vector2(surroundPoint.x, surroundPoint.y), new Vector2(surroundPoint.parent.x, surroundPoint.parent.y)) + surroundPoint.parent.g;
        }
        float f = g + h;
        surroundPoint.f = f;
        surroundPoint.g = g;
        surroundPoint.h = h;
    }

    /// <summary>
    /// 生成路径泛型队列
    /// </summary>
    /// <param name="start"></param>
    /// <param name="end"></param>
    private List<Point> Generatepath(Point start, Point end)
    {
        List<Point> path = new List<Point>();
        if (end != null)
        {
            Point node = end;//从终点开始生成路线
            while (node != start)
            {
                path.Add(node);//将节点放入路径队列中
                node = node.parent;//下一个节点为该节点的父节点
            }
            path.Reverse();//将路径节点倒序
        }
        //Debug.LogWarning(path.Count);
        return path;
    }

    private void Update()
    {
        //当地图发生改变后重新绘制算法地图
        if (Input.GetKeyDown(KeyCode.Q))
        {
            InitMap();
        }
    }
}

using System.Collections.Generic;
using UnityEngine;

public class Move : MonoBehaviour
{
    [Range(0.5f, 3f)]
    public float speed = 1.5f;//移动速度
    public GameObject end;//终点
    private LineRenderer pathline;//给对象添加LineRenderer组件
    [Range(0.1f, 2f)]
    public float lineWidth = 0.2f;//线宽

    private int order = 0;//路径队列中当前坐标序号
    private List<Point> path = new List<Point>();//路径
    private Vector3 vector3;//移动方向

    void Start()
    {
        //路径绘画初始化
        pathline = gameObject.GetComponent<LineRenderer>();
        pathline.startWidth = lineWidth;
        pathline.endWidth = lineWidth;
    }

    void Update()
    {
        //if (Input.GetKeyDown(KeyCode.W))
        {
            path.Clear();
            order = 0;
            //获取路径
            path = new List<Point>(Astar.instance.FindPath(gameObject.transform.position, end.transform.position));
            //计算初始行走方向
            if (path.Count > 0)
                vector3 = new Vector3(path[order].worldpos.x, path[order].worldpos.y, path[order].worldpos.y) - gameObject.transform.position;

            Debug.LogWarning(gameObject.name + ";" + path.Count);
            ShowPath();
        }
        //开始移动
        Run();
    }

    private void Run()
    {
        if (path.Count > 0)
        {
            //开始向下个路径点位移
            gameObject.transform.position += vector3.normalized * speed * Time.deltaTime;
            //如果到达下一个路径点
            if (Vector3.Distance(new Vector3(path[order].worldpos.x, path[order].worldpos.y, path[order].worldpos.y), gameObject.transform.position) <= 0.1f)
            {
                print(gameObject.name + ":" + path[order].f + ";" + order + "; " + path[order].worldpos);

                order++;
                //如果没有下个路径点了
                if (path.Count <= order)
                {
                    path.Clear();
                    order = 0;
                }
                else
                {
                    //计算下个路径点行走方向
                    vector3 = new Vector3(path[order].worldpos.x, path[order].worldpos.y, path[order].worldpos.y) - gameObject.transform.position;
                }
            }
        }
    }

    /// <summary>
    /// 绘制路径
    /// </summary>
    private void ShowPath()
    {
        int count = path.Count;
        if (path.Count > 0)
        {
            pathline.positionCount = count;
            for (int i = 0; i < count; i++)
            {
                pathline.SetPosition(i, path[i].worldpos);
            }
        }
        else
        {
            pathline.positionCount = 0;
        }
    }
}