Unity-A-Star寻路算法

A-Star寻路

最短路径

将地图存成二维数组，通过行列查找；

每一步都查找周围四个方向，或者八方向的所有点，放入开表；

• 是否边缘
• 是否障碍
• 是否已经在确定的路线中

计算每个方向上路径消耗，一般斜着走消耗小，收益大；

开表排序，筛选出最小消耗的点放入闭表，并将该点设置为新的起点，从开表中去除该点；

重复上面操作，直到起点=终点退出；

中间查找八个方向可移动点位时，需要将这些可移动点的父节点设置为起点，这样可通过最后筛选出的点，一层层找出最短路径；

x步内可移动所有点位

将所有方向上可以移动的节点放入开表，同时放入闭表；

遍历闭表，将每个节点周围可移动的节点放入开表，同时放入新表；

清空闭表，新表赋值给闭表（实际代码不这么写）；

重复上面操作，可移动几步，就循环几次；

最终返回开表；

代码实现：

public enum ObsType
{
    None,
    Wall,
    Water,
    Cliff
}

public class AStarNode
{
    public ObsType type;
    public int x;
    public int y;
    public float g;
    public float h;
    public float f;
    public AStarNode father;
    public AStarNode(int _x,int _y,ObsType _type)
    {
        type = _type;
        x = _x;
        y = _y;
    }
}

[Serializable]
public class GridPos
{
    public GridPos(int col, int row)
    {
        Col = col;
        Row = row;
    }
    public int Col;
    public int Row;
}

public class AStarMgr
{
    //地图的宽高
    private int mapRow;
    private int mapCol;
    //地图相关的所有的格子容器
    public AStarNode[,] nodes;
    //开启列表
    private List<AStarNode> openList = new List<AStarNode>();
    //关闭列表
    private List<AStarNode> closeList = new List<AStarNode>();
    
    //初始化地图信息
    public void InItMapInfo(int col, int row)
    {
        this.mapCol = col;
        this.mapRow = row;
        
        nodes = new AStarNode[col, row];
        for (int i = 0; i < col; i++)
        {
            for (int j = 0; j < row; j++)
            {
                AStarNode node = new AStarNode(i, j,
                    Random.Range(0, 100) < 20 ? ObsType.Wall : Ob
                nodes[i, j] = node;
            }
        }
    }
    
        public List<AStarNode> FindPath(GridPos startPos, GridPos endPos)
        {
            //判断起始点是不是在地图的范围内
            if (startPos.x < 0 || startPos.x >= mapCol
                                 || startPos.y < 0 || startPos.y >= mapRow
                                 || endPos.x < 0 || endPos.x >= mapCol
                                 || endPos.y < 0 || endPos.y >= mapRow
               )
                return null;
            //判断起始点是不是不能通行的点
            AStarNode start = nodes[startPos.x, startPos.y];
            AStarNode end = nodes[endPos.x, endPos.y];

            if (start.type != ObsType.Hero || end.type != ObsType.None)
                return null;

            closeList.Clear();
            openList.Clear();
            //开始点放入关闭列表中
            start.father = null;
            start.f = 0;
            start.g = 0;
            start.h = 0;

            closeList.Add(start);

            while (true)
            {
                //周围的点
                //FindNearlyToOpenList(start.x - 1, start.y - 1, 1.4f, start, end);
                FindNearlyToOpenList(start.x, start.y - 1, 1.4f, start, end);
                //FindNearlyToOpenList(start.x + 1, start.y - 1, 1.4f, start, end);
                FindNearlyToOpenList(start.x - 1, start.y, 1.4f, start, end);
                FindNearlyToOpenList(start.x + 1, start.y, 1.4f, start, end);
                //FindNearlyToOpenList(start.x - 1, start.y + 1, 1.4f, start, end);
                FindNearlyToOpenList(start.x, start.y + 1, 1.4f, start, end);
                //FindNearlyToOpenList(start.x + 1, start.y + 1, 1.4f, start, end);

                if (openList.Count == 0)
                    return null;

                //排序选出最小的点
                openList.Sort(SortOpenList);

                //放入关闭列表，然后从开启列表中移除
                closeList.Add(openList[0]);
                //找到这个点，进行下一次寻路
                start = openList[0];
                openList.RemoveAt(0);

                if (start == end)
                {
                    //结束
                    List<AStarNode> path = new List<AStarNode>();
                    path.Add(end);
                    while (end.father != null)
                    {
                        path.Add(end.father);
                        end = end.father;
                    }

                    path.Reverse();
                    return path;
                }
            }
        }

    //找出周围几步之内可到达的格子
    public List<AStarNode> FindRange(GridPos startPos, int step)
    {
        if (startPos.Col < 0 || startPos.Col >= mapCol
                             || startPos.Row < 0 || startPos.Row 
            return null;
        AStarNode start = nodes[startPos.Col, startPos.Row];
        if (start.type != ObsType.None)
            return null;
        closeList.Clear();
        openList.Clear();
        closeList.Add(start);
        while (step-- > 0)
        {
            AddNearOneStep(closeList.ToArray());
        }
        return openList;
    }
    
    //排序函数
    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;
    }
    
    //临近的点放入开启列表
    private void FindNearlyToOpenList(int x, int y, float g, AStarNode father, AStarNode end)
    {
        if (x < 0 || x >= mapCol || y < 0 || y >= mapRow)
            return;
        AStarNode node = nodes[x, y];
        if (node == null || node.type != ObsType.None
                         || closeList.Contains(node)
                         || openList.Contains(node)
           )
            return;
        //计算f值 f=g+h;
        node.father = father;
        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);
    }
    //当前步数的所有Node放入open
    private void AddNearOneStep(AStarNode[] curStepNodes)
    {
        closeList.Clear();
        foreach (var it in curStepNodes)
        {
            AddNearNode(it.x - 1, it.y);
            AddNearNode(it.x, it.y - 1);
            AddNearNode(it.x + 1, it.y);
            AddNearNode(it.x, it.y + 1);
        }
    }
    
    //单个格子周围的点加入open
    private void AddNearNode(int x, int y)
    {
        if (x < 0 || x >= mapCol || y < 0 || y >= mapRow)
            return;
        AStarNode node = nodes[x, y];
        if (node == null || node.type != ObsType.None)
            return;
        openList.Add(node);
        closeList.Add(node);
    }
}