清晰简洁的A*寻路算法教程（含C#代码）

Dorry_

1260人浏览 · 2021-02-23 21:23:59

Dorry_ · 2021-02-23 21:23:59 发布

前言

本文主要讲述基于网格型地图的A*算法，对于基本图结构只需要将遍历的方法改为新的节点关系就可以实现。

（1）从OpenList中选出Cost最小的结点作为CurNode标记为CLOSE
添加到CloseList，并判断该节点是否为终点
如果为终点，则从CurNode一直找父节点，直到找到起点，将这些节点作为路径返回
（2）遍历CurNode周围可以一步到达的邻近结点。

① 不在两个列表的结点：计算移动代价Startcost和全部代价Cost，将邻近结点的父节点设为CurNode, 并标记为OPEN添加到OpenList

全部代价：Cost = Startcost + Esicost

移动代价：Startcost = CurNode的Startcost + CurNode与邻近结点的欧氏距离

估计代价：Esicost = 终点与邻近结点的欧氏距离

② 处于OpenList的结点：重新计算Startcost，如果新的Startcost更小，则更新Cost和Startcost然后把CurNode设为父节点。

二、核心思想

A*算法的核心步骤只有两个：

1、 找出最优的结点

2、 添加目前已经知道可以到达的结点

这就类似于人的思维，当我们要从起点走向终点，我们会：

① 从所知道的路中选最好的一条路。

② 再看周围有哪些路可以走。

三、经典例子（包含特殊情况）

浅蓝色：处于OpenList的结点，深蓝色：处于CloseList的结点，
红色：不可达结点，绿色：终点，
黄色箭头：指向子节点（编程时要记录父节点），式子：Startcost + Esicost 的形式

四、代码实现

【注】这里还是用网格图来举例，一段C#的代码，转换成其他代码应该不难。看起来有点长，标上了注释，结合上文的核心思想，理解起来会容易些。如果有代码上的疑惑或者转换成其他语言的问题都可以在评论区提出你的疑问。

1、节点定义

public enum MapSign // 节点标记
{
    FREE,            // 空闲节点
    OPEN,            // 在开启列表的节点
    CLOSE,          // 在关闭列表的节点
    BAN,            // 禁止节点
}

public class Mapnode                // 节点类
{
    public int[] _pos;              // 坐标
    public float _startcost;        // 移动代价
    public float _cost;            // 全部代价（ = 移动代价 + 估计代价）
    public MapSign _mapsign;        // 节点标记
    public Mapnode _fathernode;    // 父节点
}

2、寻路函数

private Stack<Mapnode> FindRoad(int[] startsite, int[] endsite, List<int[]> scan_list, Mapnode[,] map, int row, int col)
        /*
        * startsite :  起始坐标
        * endsite  :  终点坐标
        * scan_list :  遍历序列
        * row      :  地图行数
        * col      :  地图列数
        * map      :  地图
        *
        * return    :  路径
        */
    {
        // 异常检测
        if (startsite[0] == endsite[0] && startsite[1] == endsite[1]) return new Stack<Mapnode>();  // 起始坐标等于终点坐标，返回空路径
        if (endsite[0] < 0 || endsite[1] < 0 || endsite[0] >= row || endsite[1] >= col) return new Stack<Mapnode>();  // 终点坐标超出地图范围，返回空路径


        ////  (一) 创建寻路算法所需的数据结构

        List<Mapnode> _Openlt = new List<Mapnode>();    // 开启列表
        List<Mapnode> _Closelt = new List<Mapnode>();    // 关闭列表
        Mapnode curnode;                                // 当前节点
        Mapnode[,] t_map = new Mapnode[row, col];        // 复制地图
        for (int i = 0; i < row; i++)
        {
            for (int j = 0; j < col; j++)
            {
                t_map[i, j] = new Mapnode();
                t_map[i, j]._pos = new int[2] { i, j };
                t_map[i, j]._mapsign = map[i, j]._mapsign;
            }
        }


        ////  (二) 开始寻路流程

        // 1、设置起点Startdis为0，并将其标记为OPEN放入OpenList
        t_map[startsite[0], startsite[1]]._startcost = 0;
        t_map[startsite[0], startsite[1]]._mapsign = MapSign.OPEN;
        _Openlt.Add(t_map[startsite[0], startsite[1]]);
        // 2、重复寻路
        while (_Openlt.Count > 0)
        {
            // (1) 从OpenList中选出Cost最小的结点作为CurNode标记为CLOSE添加到CloseList，并判断该节点是否为终点
            curnode = _Openlt[0];
            foreach (Mapnode n in _Openlt)
            {
                if (n._cost < curnode._cost)
                {
                    curnode = n;
                }
            }
            _Openlt.Remove(curnode);
            curnode._mapsign = MapSign.CLOSE;
            _Closelt.Add(curnode);
            if (curnode._pos[0] == endsite[0] && curnode._pos[1] == endsite[1])

            // 找到路径，从CurNode一直找父节点，直到找到起点，将这些节点作为路径返
            {
                Stack<Mapnode> road = new Stack<Mapnode>();  // 目的路径
                while (curnode._fathernode != null)
                {
                    road.Push(curnode);
                    curnode = curnode._fathernode;
                }
                return road;
            }

            // (2) 遍历CurNode周围可以一步到达的结点
            int tx, tz;
            foreach(int[] scan_bias in scan_list)
            {
                // 获取周围某一节点的坐标
                tx = curnode._pos[0] + scan_bias[0];
                tz = curnode._pos[1] + scan_bias[1];
                if (tx >= 0 && tx < row && tz >= 0 && tz < col)  // 限制邻近节点在地图范围内
                {
                    Mapnode t_node = t_map[tx, tz];  // 获取该邻近节点
                    if (t_node._mapsign == MapSign.FREE)

                    // ① 不在两个列表的结点：计算移动代价Startcost和全部代价Cost，将这些结点的父节点设为CurNode, 并标记为OPEN添加到OpenList
                    {
                        t_node._startcost = curnode._startcost + Mathf.Sqrt(Mathf.Pow(scan_bias[0], 2) + Mathf.Pow(scan_bias[1], 2));
                        t_node._cost = t_node._startcost + Mathf.Sqrt(Mathf.Pow(t_node._pos[0] - endsite[0], 2) + Mathf.Pow(t_node._pos[1] - endsite[1], 2));
                        t_node._fathernode = curnode;
                        t_node._mapsign = MapSign.OPEN;
                        _Openlt.Add(t_node);
                    }
                    else if (t_node._mapsign == MapSign.OPEN)

                    // ② 处于OpenList的结点：重新计算Startcost，如果新的Startcost更小，则更新Cost和Startcost然后把CurNode设为父节点。
                    {
                        float new_startcost = curnode._startcost + Mathf.Sqrt(Mathf.Pow(scan_bias[0], 2) + Mathf.Pow(scan_bias[1], 2));
                        if ( new_startcost< t_node._startcost)
                        {
                            t_node._cost = t_node._cost - t_node._startcost + new_startcost;
                            t_node._startcost = new_startcost;
                            t_node._fathernode = curnode;
                        }
                    }
                }
            }
        }
        return new Stack<Mapnode>();  // 未找到，返回空路径
    }