从遍历到A星寻路算法

在游戏当中，经常需要找一个点到其它点的路径。在之前的一篇博文(地图编辑器开发（三）)中也有使用到到A*寻路算法。我们期望能找到最短的路径，同时也需要考虑到查找路径的时间消耗。游戏中的地图可以图的数据结构来表示，然后使用图的搜索算法，来搜索最短路径。在图的搜索算法中，使用最为广泛的的是A*寻路算法，它是对图广度优先搜索的优化，图广度优先搜索又是一种图的遍历，万丈高楼平地起，我们先从基础数据结构的遍历讲起，到广度优先遍历，最后拓展到A*寻路算法。

寻路过程

一遍历

遍历（Traversal）是指沿着某条搜索路线，依次对多元素集合中的每个元素均做且仅做一次访问。根据遍历代码的实现方式，可以分为两种，迭代遍历和递归遍历。

1.1 线性表的遍历

对于最简单的数据结构线性表（比如数组、链表等），其遍历方式的实现，通常使用的是迭代遍历，递归遍历用的比较少。在遍历的过程中，根据元素的访问顺序，又可以分为前序遍历和后续遍历，利用后续遍历可以实现逆序访问效果。线性表几种遍历方式的核心代码如下：

// 数组迭代遍历
void traverse(vector<int>& arr) {
    for (int i = 0; i < arr.size(); i++) {
        // 迭代访问 arr[i]
    }
}

// 链表迭代遍历
void traverse(ListNode* head) {
    for (ListNode* p = head; p != nullptr; p = p->neighbor) {
        // 迭代访问 p -> val
    }
}

// 递归遍历数组
void traverse(vector<int>& arr, int i) {
    //前序访问 arr[i]
    traverse(arr, i + 1);
    //后序访问 arr[i]
}

// 递归遍历单链表
void traverse(ListNode* head) {
    //前序访问 head -> val
    traverse(head -> neighbor);
    //后序访问 head -> val
}

1.2 二叉树的遍历

相对于线性表而言，二叉树多了一个后继节点，结构复杂了一点。对于非线性表的遍历，通过是使用递归的方式实现。

1.2.1 二叉树的深度优先

因为二叉树有两个后继节点，便有三个访问位置，多出了一个中序访问位置。二叉树的深度优先遍历通常都是使用递归实现，代码清晰，容易理解；当然也可以使用迭代实现，不过迭代实现的代码比较长，且难以写出像递归那样风格统一的代码。迭代遍历的过程就是利用栈来模拟递归调用过程，因为栈是后进先出，因此入栈的顺序于访问的顺序相仿。下面这种二叉树迭代遍历的写法，是目前能收集到风格比较统一的非递归实现方式，它巧妙地使用了一个空节点作为是否访问过的标记。两种迭代方式实现的核心代码如下：

// 递归实现
void traverse(TreeNode* root) {
    // 前序访问 root->val
    traverse(root->left);
    // 中序访问 root->val
    traverse(root->right);
    // 后序访问 root->val
}

// 迭代实现
vector<int> traverse(TreeNode* root) {
    vector<int> result;
    stack<TreeNode*> st;
    if (root) st.push(root);

    while (!st.empty()) {
        TreeNode* node = st.top();
        st.pop();
        if (node) { // 入栈，节点顺序于访问顺序相反
            // 前序遍历，中左右
            if (node->right) st.push(node->right); // 右
            if (node->left) st.push(node->left); // 左
            st.push(node); // 中
            st.push(nullptr); // 空，路过，但未访问，加入空节点做为标记

            // 中序遍历，左中右
            if (node->right) st.push(node->right); // 右
            st.push(node); // 中
            st.push(nullptr); // 空
            if (node->left) st.push(node->left); // 左

            // 后序遍历，左右中
            st.push(node); // 中
            st.push(nullptr); // 空
            if (node->right) st.push(node->right); // 右
            if (node->left) st.push(node->left); // 左
        } else { // 出栈，只在出栈时加入到返回值
            node = st.top();
            st.pop();
            result.push_back(node->val);
        }
    }
    return result;
}

1.2.2 二叉树的广度优先

因为二叉树有两个后继节点，除了像深度优先遍历那样，从跟节点一直访问到叶子节点外，多出一种访问路线，即广度优先，它先访问二叉树同一深度的所有节点，然后访问下一深度的所有节点。二叉树的广度优先，也叫二叉树的层次遍历，通常都是使用队列来实现。

// 迭代实现 - BFS
void traverse(TreeNode* root) {
    if (!root) return;

    queue<TreeNode*> q;
    q.push(root);

    // 从上到下遍历二叉树的每一层
    int depth = 0;
    while (!q.empty()) {
        depth++;

        int breadth = q.size();
        // 从左到右遍历每一层的每个节点
        for (int i = 0; i < breadth; i++) {
            TreeNode* cur = q.front();
            q.pop();
            // 访问 cur -> val

            // 将下一层节点放入队列
            if (cur->left) q.push(cur->left);
            if (cur->right) q.push(cur->right);
        }
    }
}

// 递归实现 - BFS:利用递归访问每一层的节点
vector<vector<int>> res;
void traverse(queue<TreeNode*>& curLevelNodes) {
    if (curLevelNodes.empty()) {
        return;
    }

    vector<int> nodeValues;
    queue<TreeNode*> neighborLevelNodes;
    while (!curLevelNodes.empty()) {
        // 取出当前层的节点
        TreeNode* node = curLevelNodes.front();
        curLevelNodes.pop();
        nodeValues.push_back(node->val);

        // 将下一层的节点加入队列
        if (node->left) neighborLevelNodes.push(node->left);
        if (node->right) neighborLevelNodes.push(node->right);
    }
    // 记录当前层的节点值，取得自上而下的遍历结果
    res.push_back(nodeValues);
    traverse(neighborLevelNodes);
}

1.3 多叉树的遍历

多叉树是在二叉树的基础上，多出1~N个后续节点。节点变多后，中序难以定义，所以多叉树没有了中序遍历，只有前序遍历和后续遍历。对照二叉树的遍历方式，可以得出多叉树的遍历核心代码如下：

// 迭代实现 - DFS
void traverse(TreeNode* root) {
    // 前序访问 root->val
    for (TreeNode* child : root->children)
        traverse(child);
    // 后序访问 root->val
}

// 迭代实现 - BFS
void traverse(TreeNode* root) {
    if (!root) return;
    queue<TreeNode*> q;
    q.push(root);

    int depth = 0;
    while (!q.empty()) {
        depth++;

        int breadth = q.size();
        for (int i = 0; i < breadth; i++) {
            TreeNode* cur = q.front();
            q.pop();
            // 访问 cur -> val

            for (auto child : cur->children) {
                q.push(child);
            }
        }
    }
}

1.4 图的遍历

多叉树后续节点有0_{N个，但前驱节点最多只有一个。图的结构是在多叉树的基础上，新增了前驱节点的数量，也可以有0}N个，这也导致了图中可能有环的村在，所以在图的遍历过程中，需要一个 visited 数组，来记录节点是否被访问过，防止节点被多次访问，避免死循环的出现。图通常是用迭代遍历，两种遍历方式的核心代码如下：

// 图的广度优先
void traverse(Node* root) {
    if (!root) return;
    queue<TreeNode*> q;
    q.push(root);

    int depth = 0;
    while (!q.empty()) {
        depth++;

        int breadth = q.size();
        for (int i = 0; i < breadth; i++) {
            Node* cur = q.front();
            q.pop();
            // 访问 cur -> val

            for (auto child : cur->neighbors) {
                if (!visited[child]) q.push(child);
            }
        }
    }
}

// 图的深度优先
vector<bool> visited; // 记录被遍历过的节点
vector<bool> onPath; // 记录从起点到当前节点的路径
void traverse(Graph graph, Node s) {
    if (visited[s]) return;
    // 经过节点 s，标记为已遍历
    visited[s] = true;

    // 做选择：标记节点 s 在路径上
    onPath[s] = true;
    for (Node neighbor : graph.neighbors(s)) {
        traverse(graph, neighbor);
    }
    // 撤销选择：节点 s 离开路径
    onPath[s] = false;
}

以上就是基础数据结构遍历的全部内容，从至多只有一个前驱和后继节点的线性表开始，到有多个后继节点的树，再到有多个前驱节点的图。接下来，基于图的遍历，开始讨论寻路算法。

二寻路算法

2.1 地图的表示

在学习一个算法时，要做的第一件事是要理解数据，弄清楚它的输入是什么、输出是什么？

输入：图搜索算法（包括A*寻路算法）都是用“图”作为输入，图是由一组点及其之间连接的边组成；除此之外，算法不知道任何其它内容，诸如地图上是室内或是室外、是房间或者门廊、或者面积多大等，它知道的唯一内容就是图。无论地图上的内容是什么样，只要图是一致的，对算法来说就没有任何区别。
输出：寻路算法找到的路径是由图中的节点和边组成。边是抽象的数学概念，寻路算法只会告诉你从哪个点移动到哪个点，但不会关注如何移动。记住，算法不知道任何的房间或门，它只知道图。我们必须自己决定，算法返回的边表示的是什么意思，是从一个网格移动到另一个网格，还是从一个点直线走到另一个点，或是开一个门，或是游泳过去，亦或是沿着一个曲线路跑。

对于任何给定的游戏地图，都有许多不同的方法可以制作寻路图，给到寻路算法。例如，地图上的门，既作为节点，也可以作为边，还可以作为可行走的网格，寻路图无须跟游戏中使用的地图保持一模一样，网格地图也可以使用非网格的寻路图，反之亦然。寻路网格易于制作和展示，虽然缺点是节点太多（地图中的节点越少，寻路算法运行就越快），但在此讨论的是寻路算法，而不讨论地图的设计，因此后续将使用寻路网格来讨论。

有很多基于图的算法，接下来讨论的内容包括：

广度优先搜索：它从起点开始，平等地向所有方向进行探索。毋庸置疑，这是一个非常有用的算法，不仅可以用于常规的寻路，还可以用于地图的生成、流场寻路（flow field pathfinding）、距离图（distance maps）以及其它类型的地图分析。
Dijkstra 算法：它让我们确定要探索的路径的优先级，不是平等地探索所有可能的路径，而是偏向于成本较低的路径。我们可以在推荐走的路径上设置更低的成本，在有敌人的路径上设置更高的成本，等等。当移动成本不同时，我们使用Dijkstra 算法而不是广度优先搜索。
A*寻路算法：它是 Dijkstra 算法的改进版，针对单个目标点进行了优化。Dijkstra 算法可以搜索起点到所有节点的最短路径，A*寻路算法只能找到起点到一个目标点的最短路径，它优先考虑那些似乎更接近目标的路径。

接下来，我将从最简单的广度优先搜索开始，然后一次添加一个功能，直到将其转换为 A*寻路算法。

2.2 广度优先遍历（BFS）

所有这些算法的关键思想是，跟踪一个称为 openList 的扩展环。在网格地图上，这个过程有时被称为“洪水填充”，因为探索的过程是中起点开始，像水波一样向四周扩散，同样的技术也适用于非网格地图，算法的具体过程如下：

将起点放入到队列 openList 和 visited (记录已访问过的节点) 中；
重复以下步骤，直到 openList 为空：
1. 从队列 openList 中取出一个节点 current；
2. 遍历 current 所有相邻的节点 neighbors，跳过墙，将所有未被访问过的节点同时加入到 openList 和 visited 。

void breadth_first_search(Graph graph, Node start)
{
    queue<Node> openList;
    openList.push(start);

    // 记录节点是否已被访问过
    unordered_set<Node> visited;
    visited.insert(start);

    while (!openList.empty()) {
        Node current = openList.front();
        openList.pop();

        for (auto neighbor : graph.neighbors(current)) {
            if (visited.find(neighbor) == visited.end()) {
                openList.push(neighbor);
                visited.insert(neighbor);
            }
        }
    }
}

这段代码中的循环是这个系列图搜索算法（包括A*寻路算法）的基础。我们怎么才能找到最短路径呢？这个函数实际上并没有生成一个路径，它仅仅是告诉我们如何遍历整个地图，这是因为广度优先搜索的用途远不止寻路。这里我们想用它来寻路，需要改一下这个循环，让它把移动轨迹保存下来，对于每个访问到的节点，记录它从哪里来。我们将 visited 重命名为 came_from，并记录上一个节点。

unordered_map<Node, Node> breadth_first_search(Graph graph, Node start)
{
    queue<Node> openList;
    openList.push(start);

    // 记录节点的来路
    unordered_map<Node, Node> came_from;
    came_from[start] = start;

    while (!openList.empty()) {
        Node current = openList.front();
        openList.pop();

        for (auto neighbor : graph.neighbors(current)) {
            if (came_from.find(neighbor) == came_from.end()) {
                openList.push(neighbor);
                came_from[neighbor] = current;
            }
        }
    }
    return came_from;
}

现在 came_from 中记录了每个到达过的节点从哪里来。他们虽然看起比较零散，但可以重建整个路径。重建的过程也很简单，从目标点开始，一个一个往回找它从哪里来，直到找到起点，经过的所有节点，就是从终点到起点的路径，逆序之后就是从起点到终点的路径。路径理论上是一组边的集合，但通常保存节点更加容易。

vector<Node> reconstruct_path(Node start, Node end, unordered_map<Node, Node> came_from)
{
    vector<Node> path;
    if (came_from.find(goal) == came_from.end()) {
        return path; // 没有找到路径
    }

    auto current = goal;
    while(current != start) {
        path.push_back(current);
        current = came_from[current]
    }
    reverse(path.begin(), path.end());
    return path;
}

这个就是最简单的寻路算法，此算法既可以在网格地图寻路中使用，也适用于其它任何类型的图结构。

提前退出：上面的代码找到了起点到其他所有节点的路径，但通常只需要找到起点到另外一个点，即目标点的路径，我们可以在找到目标点后，立即停止探索 openList。

unordered_map<Node, Node> breadth_first_search(Graph graph, Node start)
{
    queue<Node> openList;
    openList.push(start);

    unordered_map<Node, Node> came_from;
    came_from[start] = start;

    while (!openList.empty()) {
        Node current = openList.front();
        openList.pop();

        // 找到目标点后，则停止搜索
        if (current == goal) break;

        for (auto neighbor : graph.neighbors(current)) {
            if (came_from.find(neighbor) == came_from.end()) {
                openList.push(neighbor);
                came_from[neighbor] = current;
            }
        }
    }

    return came_from;
}

移动成本：目前为止我们认为所有的移动都有相同的成本，在一些寻路场景中不同类型的移动具有不同的成本，比如在游戏《文明》中，在平地或沙漠上移动消耗1点能量，但在冰川或山上移动需要消耗5个点能量，比如在穿过水的成本是穿过草地的10倍，另一个例子是在网格地图中对角线上的移动成本高于在轴向上的移动成本，所以我们需要在寻路的过程中考虑到移动的成本，即 Dijkstra 算法。

2.3 Dijkstra 算法

Dijkstra 算法与广度优先搜索有什么不同呢？我们需要记录移动的成本，所以我们添加一个新的变量 cost_so_far，保存从起点开始的总移动成本。当我们在选择下一个要探索的节点时，希望将移动成本考虑在内。让我们将队列改为小数优先队列，以保证每次选择的都是成本最小的节点。不太明显的是，我们最终可能会以不同的成本多次访问一个节点，因此我们需要稍微改变一下逻辑 —— 将如果从未到达过该节点，则将节点直接添加到 openList，改为当通往该节点的新路径优于之前的最佳路径时也添加该节点。

void dijkstra_search(Graph graph, Node start, Node goal)
{
    priority_queue <Node, vector<Node>, lessNodeCompare> openList; // 使用小值优先队列替代普通队列
    openList.push(start, 0);

    unordered_map<Node, Node> came_from;
    came_from[start] = start;

    unordered_map<Node, int> cost_so_far; // 记录移动到节点时的最低成本
    cost_so_far[start] = 0;

    while (!openList.empty()) {
        Node current = openList.front();
        openList.pop();

        if (current == goal) break;

        for (auto neighbor : graph.neighbors(current)) {
            double new_cost = cost_so_far[current] + graph.cost(current, neighbor);
            // 节点没有访问过或新路径成本更低，则更新移动节点的成本
            if (cost_so_far.find(neighbor) == cost_so_far.end() || new_cost < cost_so_far[neighbor]) {
                cost_so_far[neighbor] = new_cost;
                came_from[neighbor] = current;
                openList.push(neighbor, new_cost);
            }
        }
    }
}

使用优先队列替代普通队列，改变了 openList 队列探索的方式，同时探索的速度也变慢了。Dijkstra 算法可是计算除 1 之外的移动成本，从而允许我们探索更有趣的图形，而不仅仅是网格。

ps: 对比一下 Dijkstra 算法和 Prim 算法？
Dijkstra 算法用于求单源最短路径，从优先队列中存的是离起点的距离最近的点
Prim 算法用于求最小生成树，从优先队列中存的是与MSG相连权值最小边

2.4 A*寻路算法

2.4.1 贪婪最佳优先搜索（GBFS）

在广度优先搜索和 Dijkstra 算法中，openList 队列都是等价地向所有方向探索。如果我们希望找到所有目标点或多个目标点的路径，这是一个情有可原的选择；但通常情况我们只是要找起点到一个目标点的路径。我们希望让 openList 优先朝着目标点的方向探索，而不是其他方向。首先，我们定义一个启发函数 heuristic，来预估我们离目标点有多远。

double heuristic(Point from, Point to) {
    // 网格坐标上两个节点的曼哈顿距离，通常我们使用的欧拉距离，但需要开更号，运算效率低
    return abs(from.x - to.x) + abs(from.y - to.y);
}

在 Dijkstra 算法中我们使用距离离起点的实际距离给优先队列排序，这里我们使用到目标点的预估距离给优先队列排序，离目标点最近的节点最优先被考虑。对比 Dijkstra 算法的实现，仍然使用最小优先队列，不过没有了 cost_so_far。


void greedy_best_first_search(Graph graph, Node start, Node goal)
{
    priority_queue <Node, vector<Node>, lessNodeCompare> openList; 
    openList.push(start, 0);

    unordered_map<Node, Node> came_from;
    came_from[start] = start;

    while (!openList.empty()) {
        Node current = openList.front();
        openList.pop();

        if (current == goal) break;

        for (auto neighbor : graph.neighbors(current)) {
            if (came_from.find(neighbor) == came_from.end()) {
                double priority = heuristic(goal, neighbor);// 预估距离
                openList.push(neighbor, priority);
                came_from[neighbor] = current;
            }
        }
    }
}

不过，这个算法找到的路径不一定是最短路径，虽然这个算法在阻挡不多时运算很快，只是找到的路径还不够好，我们有办法解决这个问题吗？当然！

2.4.2 A*寻路算法实现

Dijkstra 算法在寻找最短路径时效果很好，但它耗了大量时间去探索那些非预期的方向；贪婪最佳优先搜索算法的时预期额方向，但找到的不是路径。A*寻路算法同时使用距离起点的实际距离和到目标点的预估距离。它的代码跟 Dijkstra 算法的代码非常类似。

void a_star_search(Graph graph, Node start, Node goal)
{
    priority_queue <Node, vector<Node>, lessNodeCompare> openList;
    openList.push(start, 0);

    unordered_map<Node, Node> came_from;
    came_from[start] = start;

    unordered_map<Node, int> cost_so_far;
    cost_so_far[start] = 0;

    while (!openList.empty()) {
        Node current = openList.front();
        openList.pop();

        if (current == goal) break;

        for (auto neighbor : graph.neighbors(current)) {
            double new_cost = cost_so_far[current] + graph.cost(current, neighbor);
            if (cost_so_far.find(neighbor) == cost_so_far.end() || new_cost < cost_so_far[neighbor]) {
                cost_so_far[neighbor] = new_cost;
                double priority = heuristic(neighbor, goal);// 实际距离 + 预估距离 
                openList.push(neighbor, priority);
                came_from[neighbor] = current;
            }
        }
    }
}

比较这几个算法：

Dijkstra 算法：计算距离起点的距离 g(n)；
贪婪最佳优先搜索算法：使用到终点的预估距离 h(n)；
A*寻路算法：使用了距离起点的距离和到终点的预估距离之和 f(n) = g(n) + h(n)；

在贪婪最佳优先搜索算法能正确搜索最短路径的情况下，A*寻路算法也能找到正确的结果；在在贪婪最佳优先搜索算法不能正确找到最短路径的情况下，A*寻路算法还是可以找到正确的结果，像Dijkstra 算法那样，而且A*寻路算法探索更少的格子。A*寻路算法可以两全其美，只要启发算法评估正确，就可以像Dijkstra 算法那样找到最短路径。A*寻路算法使用启发式方法对节点进行重新排序，以便更有可能更快地找到目标节点。

如何在游戏地图中选择合适的寻路算法呢？

如果需要找出从所有节点开始或是到所有节点路径，使用广度优先搜索或者Dijkstra 算法；
如果移动的消耗都是一样的，使用广度优先搜索；如果移动的消耗是变化的，使用Dijkstra 算法。
如果需要找到到一个或几个目标点的最短路径，使用贪婪最佳优先搜索算法或 A*寻路算法。当您想使用贪婪的最佳优先搜索时，请考虑使用带有“不可接受”启发式的 A*寻路算法。

关于最佳路径：广度优先搜索和 Dijkstra 算法可以保证给定的图中找到的最短路径，贪婪的最佳优先搜索不能保证。如果启发式方法永远不会大于真实距离，则保证 A*寻路算法找到最短路径。随着启发式算法变小，A*寻路算法变成了 Dijkstra 算法。随着启发式方法变大，A*寻路算法变为贪婪的最佳首次搜索。

关于性能：最好的办法是减少图表中不必要的节点。如果使用网格地图，减小图形的大小有助于所有图形搜索算法。然后，就是尽可能使用最简单的算法。贪婪最佳优先搜索算法通常比 Dijkstra 算法运行得更快，但不会产生最佳路径。对于大多数寻路需求来说，A*寻路算法是一个不错的选择。

关于非地图：图的搜索算法可以用于任何类型的图，不仅仅是游戏地图。移动代价表示图中边的权重。启发式方法无法通用，我们必须为不同类型图设计相应的启发式方法。对于平面地图，距离是一个不错的选择，所以这就是这里使用的。

请记住，图形搜索只是我们需要内容的一部分。A*寻路算法本身不处理诸如协作移动、移动障碍物、地图更改、危险区域评估、编队、转弯半径、对象大小、动画、路径平滑或许多其他主题等内容。