【Unity】多种寻路算法实现 —— BFS，DFS，Dijkstra，A*

本实验寻路算法均基于网格实现，整体称呼为Grid，单个瓦片称之为Tile

考虑程序处理的简洁性，所有算法使用同一种Tile，且权值点，A*所需的记录数值也全部放在Tile中记录

前排贴上代码仓库链接：

GitHub - Sirokus/Unity-Pathfinding-Project: 内置了BFS，DFS，GBFS，Dijkstra，A*有权，A*无权的寻路函数，以及一个能够控制大小，障碍物，起始点和终点的网格系统，仅供个人了解原理使用

数据结构

首先是Tile的数据结构

已剪除不必要的代码保证逻辑清晰

public class Tile : MonoBehaviour
{
    public bool walkable;
    public bool visited;
    public Tile pre;

    public float cost = 1;

    //A*所需
    public float G;
    public float H;
    public float F => G + H;

    public void setWalkable(bool canWalk)
    {
        walkable = canWalk;
    }

    public int GetManhattanDistance(Tile tile)
    {
        Vector2Int aCoord = Grid.getCoordByTile(this);
        Vector2Int bCoord = Grid.getCoordByTile(tile);
        return Mathf.Abs(aCoord.x - bCoord.x) + Mathf.Abs(aCoord.y - bCoord.y);
    }
}

接着是Grid

鉴于Grid中集中了大量业务代码，此处只贴出部分有用的函数内容

Grid中存储着所有的格子，并提供格子和索引之间的转换，以下是具体方式

public static Vector2Int getCoordByTile(Tile tile)
{
    return new Vector2Int((int)tile.transform.localPosition.x, (int)tile.transform.localPosition.y);
}
public static Tile getTileByCoord(Vector2Int coord)
{
    if (coord.x < 0 || coord.y < 0 || coord.x >= Grid.ins.gridSize.x || coord.y >= Grid.ins.gridSize.y)
        return null;
    return Grid.ins.tiles[coord.x][coord.y];
}

接着是关键的寻路代码

BFS寻路算法

以下是BFS代码（为了能够运行时观看寻路过程，因此所有寻路代码皆使用协程实现）

BFS的基本思想是从出发点开始，向四周所有可以行走的节点进行访问，并在碰到目标值后停止

BFS是不考虑权值的

IEnumerator BfsPathFindingTask()
{
    Queue<Tile> queue = new Queue<Tile>();                              //遍历队列
    Dictionary<Tile, Tile> came_from = new Dictionary<Tile, Tile>();    //前驱队列
    queue.Enqueue(start);
    came_from[start] = null;

    while(queue.Count > 0)
    {
        //访问当前Tile
        Tile cur = queue.Dequeue();
        Vector2Int curCoord = getCoordByTile(cur);

        //依次访问其四个邻居，若可行走且未被访问过则入队
        foreach(var d in dir)
        {
            //获取邻居坐标
            Vector2Int tmp = curCoord + d;

            //按坐标获取邻居
            Tile neighbor = getTileByCoord(tmp);

            //确保邻居可访问且没有前驱（没有访问过）
            if (neighbor && neighbor.walkable && !came_from.ContainsKey(neighbor))
            {
                if(neighbor != end)
                {
                    neighbor.setColor(Color.black, VisitedColorBlendLerp);
                }
                
                //入队该邻居，标记已访问，记录其父tile
                queue.Enqueue(neighbor);
                came_from[neighbor] = cur;

                //终止条件
                if(CheckFindPathComplete(neighbor, came_from))
                    yield break;
            }
        }

        if(ShowProcess)
            yield return new WaitForSeconds(0.001f / (0.001f * speedMultiple));
    }
}

DFS寻路算法

我写的这个就是纯搞笑的算法，它会向一个方向一直探测转向直到走无可走再从下一个相邻点进行行走

也就是说这个不是一个启发式算法，目标肯定是可以找到的，但中间要走多少个弯就只有天知道了

（不过review一下发现写的还挺长的）

IEnumerator DfsPathFindingTask(Tile tile, System.Action isDone)
{
    if(end.visited || !tile)
    {
        isDone();
        yield break;
    }

    tile.visited = true;
    Vector2Int cur = getCoordByTile(tile);

    Tile neighbor = null;
    float minDis = float.MaxValue;
    foreach(var d in dir)
    {
        Vector2Int tmp = cur + d;
        Tile tmpTile = getTileByCoord(tmp);
        if(tmpTile && tmpTile.walkable && !tmpTile.visited)
        {
            float dis = Vector2.Distance(tmp, endCoord);
            if(dis < minDis)
            {
                neighbor = tmpTile;
                minDis = dis;
            }
        }
    }  

    if(neighbor)
    {
        if(neighbor != end)
        {
            neighbor.setColor(Color.black, VisitedColorBlendLerp);
        }

        neighbor.visited = true;
        neighbor.pre = tile;

        if(neighbor == end)
        {
            float costCount = neighbor.cost;
            neighbor = neighbor.pre;
            List<Tile> path = new List<Tile>();

            while(neighbor != start)
            {
                costCount += neighbor.cost;
                path.Add(neighbor);
                neighbor = neighbor.pre;
            }
            costCount += start.cost;
            Debug.Log(costCount);
            path.Reverse();

            foreach(Tile t in path)
            {
                t.setColor(Color.green, 0.5f);

                yield return new WaitForSeconds(0.02f);
            }

            yield break;
        }

        if(ShowProcess)
            yield return new WaitForSeconds(0.01f / (0.01f * speedMultiple));

        bool isdone = false;
        coroutines.Add(StartCoroutine(DfsPathFindingTask(neighbor, () => { isdone = true; })));
        yield return new WaitUntil(() => isdone);
    }
    else
    {
        bool isdone = false;
        coroutines.Add(StartCoroutine(DfsPathFindingTask(tile.pre, () => { isdone = true; })));
        yield return new WaitUntil(() => isdone);
    }

    isDone();
}

GBFS寻路算法

GBFS是在BFS基础上进行改进的算法，G代表Greedy（贪心），这是一个启发式算法，这意味着其知道终点的位置，并且会一直向理论最靠近终点的位置靠近

这也代表如果其和目标中间有个墙角，其也会先深入墙角再沿着墙走出来找到目标（因为其不考虑已付出代价）

        IEnumerator GBfsPathFindingTask()
        {
            SortedDictionary<float, LinkedList<Tile>> queue = new SortedDictionary<float, LinkedList<Tile>>(){{0, new LinkedList<Tile>()}};
            Dictionary<Tile, Tile> came_from = new Dictionary<Tile, Tile>();
            queue[0].AddLast(start);
            came_from[start] = null;

            Vector2Int endCoord = getCoordByTile(end);

            while(queue.Count > 0)
            {
                //访问当前Tile
                Tile cur = queue.First().Value.First();
                //当前Tile出队
                if(queue.First().Value.Count > 1)
                    queue.First().Value.RemoveFirst();
                else
                    queue.Remove(queue.First().Key);

                //获取当前Tile坐标
                Vector2Int curCoord = getCoordByTile(cur);

                //依次访问其四个邻居，若可行走且未被访问过则入队
                foreach(var d in dir)
                {
                    //计算邻居坐标
                    Vector2Int tmp = curCoord + d;
                    //按坐标获取邻居
                    Tile neighbor = getTileByCoord(tmp);

                    //确保邻居可访问
                    if (neighbor && neighbor.walkable && !came_from.ContainsKey(neighbor))
                    {
                        //计算优先级
                        float priority = Vector2Int.Distance(tmp, endCoord);

                        //入队
                        if(!queue.ContainsKey(priority))
                            queue.Add(priority, new LinkedList<Tile>());
                        queue[priority].AddLast(neighbor);

                        //设置前驱
                        came_from[neighbor] = cur;

                        //终止条件
                        if(CheckFindPathComplete(neighbor, came_from))
                            yield break;
                    }
                }
                if(ShowProcess)
                    yield return new WaitForSeconds(0.01f / (0.01f * speedMultiple));
            }
        }

Dijkstra寻路算法

该算法不是一个启发式算法，但该算法能够根据不同权值找到最优路径（一定最优），其几乎相当于一个广度优先的有权版本，其会评估路上每一个的已付出代价，并选择付出代价最小的节点进行扩展，当找到一个已访问结点的更优路径时，还会修改其前驱

IEnumerator DijkstraPathFindingTask()
{
    SortedDictionary<float, LinkedList<Tile>> queue = new SortedDictionary<float, LinkedList<Tile>>(){{0, new LinkedList<Tile>()}};
    Dictionary<Tile, Tile> came_from = new Dictionary<Tile, Tile>();
    Dictionary<Tile, float> cost_so_far = new Dictionary<Tile, float>();
    queue[0].AddLast(start);
    came_from[start] = null;
    cost_so_far[start] = 0;

    while(queue.Count > 0)
    {
        //访问当前Tile
        Tile cur = queue.First().Value.First();
        //当前Tile出队
        if(queue.First().Value.Count > 1)
            queue.First().Value.RemoveFirst();
        else
            queue.Remove(queue.First().Key);

        //获取当前Tile坐标
        Vector2Int curCoord = getCoordByTile(cur);

        //依次访问其四个邻居，若可行走且未被访问过则入队
        foreach(var d in dir)
        {
            //计算邻居坐标
            Vector2Int tmp = curCoord + d;
            //按坐标获取邻居
            Tile neighbor = getTileByCoord(tmp);
            if(!neighbor)
                continue;

            //计算cost
            float new_cost = cost_so_far[cur] + neighbor.cost;
            //可行走，且第一次走或者此为更优路线
            if (neighbor.walkable && (!cost_so_far.ContainsKey(neighbor) || new_cost < cost_so_far[neighbor]))
            {
                if(neighbor != end)
                {
                    neighbor.setColor(Color.black, VisitedColorBlendLerp);
                }
                
                //入队
                if(!queue.ContainsKey(new_cost))
                    queue.Add(new_cost, new LinkedList<Tile>());
                queue[new_cost].AddLast(neighbor);
                //设置前驱
                came_from[neighbor] = cur;
                //更新cost
                cost_so_far[neighbor] = new_cost;

                //终止条件
                if(CheckFindPathComplete(neighbor, came_from))
                    yield break;
            }
        }

        if(ShowProcess)
            yield return new WaitForSeconds(0.001f / (0.001f * speedMultiple));
    }
}

A*寻路算法

A*寻路算法是非常著名的寻路算法，其相当于一个混合算法，其存在一个启发函数

即 F = G + H + C

G = 已经付出的代价（类似Dijkstra）

H = 预估到达目标代价（类似GBFS）

C = 用户自定义算法（可用于实现一些A*寻路的特殊偏好，如减少拐点等）

A*的H计算可以计算欧式距离或者曼哈顿距离，通常使用后者，因为计算机计算起来比需要平方根的欧氏距离更快，方便大量计算

Tips：我在算法中，还额外使用了向量的叉乘，使得寻路时会更倾向于扩展朝向目标的点而非扩展更远的点

IEnumerator AStarPathFindingTask()
{
    SortedDictionary<float, LinkedList<Tile>> openQueue = new SortedDictionary<float, LinkedList<Tile>>(){{0, new LinkedList<Tile>()}};
    Dictionary<Tile, Tile> preDic = new Dictionary<Tile, Tile>();
    Dictionary<Tile, float> costDic = new Dictionary<Tile, float>();

    //用start初始化容器
    openQueue[0].AddLast(start);
    preDic[start] = null;
    costDic[start] = 0;

    Vector2Int endCoord = getCoordByTile(end);

    bool wait;
    while(openQueue.Count > 0)
    {
        wait = false;

        //访问当前Tile
        Tile cur = openQueue.First().Value.First();
        //当前Tile出队
        if(openQueue.First().Value.Count > 1)
            openQueue.First().Value.RemoveFirst();
        else
            openQueue.Remove(openQueue.First().Key);

        //获取当前Tile坐标
        Vector2Int curCoord = getCoordByTile(cur);

        //依次访问其四个邻居，若可行走且未被访问过则入队
        foreach(var d in dir)
        {
            //计算邻居坐标
            Vector2Int tmp = curCoord + d;
            //按坐标获取邻居
            Tile neighbor = getTileByCoord(tmp);
            if(!neighbor)
                continue;

            //计算邻居的Cost
            float new_cost = costDic[cur] + neighbor.cost;
            //可行走，且第一次走或者此为更优路线
            if (neighbor.walkable && (!costDic.ContainsKey(neighbor) || new_cost < costDic[neighbor]))
            {
                if(neighbor != end)
                {
                    neighbor.setColor(Color.black, VisitedColorBlendLerp);
                }
                
                //更新cost（G)
                costDic[neighbor] = new_cost;

                //F = G+H，switch中主要是不同的H的计算方式
                switch(AStarType)
                {
                    case 0:
                        new_cost += Vector2Int.Distance(tmp, endCoord);
                    break;
                    case 1:
                        float dx = Mathf.Abs(tmp.x - endCoord.x);
                        float dy = Mathf.Abs(tmp.y - endCoord.y);
                        new_cost += dx + dy + (Mathf.Sqrt(2) - 2) * Mathf.Min(dx, dy);
                    break;
                    case 2:
                        float dx1 = Mathf.Abs(tmp.x - endCoord.x);
                        float dy1 = Mathf.Abs(tmp.y - endCoord.y);
                        float dx2 = Mathf.Abs(startCoord.x - endCoord.x);
                        float dy2 = Mathf.Abs(startCoord.y - endCoord.y);
                        float cross = dx1 * dy2 - dx2 * dy1;
                        new_cost += neighbor.GetManhattanDistance(end) + (cross < 0 ? (cross + 1) * -2 : cross) * AstarCrossMultiple;
                    break;
                }

                //入队
                if(!openQueue.ContainsKey(new_cost))
                    openQueue.Add(new_cost, new LinkedList<Tile>());
                openQueue[new_cost].AddLast(neighbor);

                //记录前驱
                preDic[neighbor] = cur;

                //终止条件
                if(CheckFindPathComplete(neighbor, preDic))
                    yield break;

                wait = true;
            }
        }

        if(wait && ShowProcess)
            yield return new WaitForSeconds(0.001f / (0.001f * speedMultiple));
    }
}