A* 优化：二叉堆、启发权重与路径平滑

问题：从“能用”到“好用”

上一篇的 astarPath 在演示里表现很好。放进一个真实的游戏，问题就来了：

• 地图是 256×256，几百个单位，每个单位每隔一会儿就要重新寻路，每帧只有几毫秒的预算。
• 单位沿着网格路径行走，一路“直角转弯”，看起来像机器人。

优化可以从三个方向入手：让每次展开更便宜（数据结构与内存），少展开一些格子（启发权重），让结果更自然（路径平滑）。这一章依次讨论，并且每一项都用实测数据说话。

优先队列的内部：二叉堆

前两篇一直在用 std::priority_queue，它的内部是一个二叉堆：一棵完全二叉树，每个节点都不大于它的子节点，所以最小值永远在树根。

完全二叉树可以不用指针，直接按层序存进数组：下标 i 的子节点是 2i+1 和 2i+2，父节点是 (i-1)/2。

图 1.4同一个堆的两种视角：逻辑上是一棵树，实际存储是一个数组。树根（下标 0）总是最小值。

两个基本操作都只沿一条从根到叶的路径移动，所以是 O(log n)：

• 插入：放到数组末尾，然后上浮，只要比父节点小就和父节点交换。
• 取出最小值：拿走树根，把数组最后一个元素放到树根，然后下沉，和较小的子节点交换，直到两个子节点都不比它小。

教科书写法：就地降低优先级

前两篇的写法有一个看起来不太优雅的地方：一个格子找到更便宜的路时，我们往堆里再塞一个条目，旧条目留在原地，出堆时再跳过（惰性删除）。

教科书上的 Dijkstra 和 A* 用的是另一种操作：decrease-key，直接修改堆里已有元素的优先级，再让它上浮。为此需要额外记录每个格子在堆数组中的位置：

cpp/pathfinding/astar_opt/binary_heap.h

// 带索引的二叉小顶堆。元素是 0..capacity-1 的整数 id（这里就是格子下标），
// 每个 id 在堆里至多出现一次。额外记录每个 id 在数组中的位置 pos_，于是可以就地"降低优先级"，
// 不再需要往堆里塞重复条目、出堆时再跳过。
// 上浮 / 下沉时用"挪空位"代替反复交换：先把节点拿出来，其他节点依次移动，最后放回一次。
//
// 数组布局：下标 i 的父节点是 (i-1)/2，子节点是 2i+1 和 2i+2。
template <class Key>
class IndexedMinHeap {
public:
    void reset(int capacity) {
        heap_.clear();
        pos_.assign(capacity, -1);
    }
    // 清空堆，只重置仍在堆里的元素：O(堆大小)，而不是 O(capacity)
    void clear() {
        for (const Node& n : heap_) pos_[n.id] = -1;
        heap_.clear();
    }

    bool empty() const { return heap_.empty(); }
    int size() const { return static_cast<int>(heap_.size()); }
    bool contains(int id) const { return pos_[id] >= 0; }
    const Key& key(int id) const { return heap_[pos_[id]].key; }
    const Key& topKey() const { return heap_[0].key; }
    int idAt(int i) const { return heap_[i].id; }

    void push(int id, const Key& key) {
        heap_.push_back({key, id});
        siftUp(size() - 1);
    }
    // 新的 key 必须不大于原来的 key
    void decreaseKey(int id, const Key& key) {
        int i = pos_[id];
        heap_[i].key = key;
        siftUp(i);
    }
    // 取出 key 最小的 id
    int pop() {
        int top = heap_[0].id;
        pos_[top] = -1;
        Node last = heap_.back();
        heap_.pop_back();
        if (!heap_.empty()) siftDown(0, last);
        return top;
    }

private:
    struct Node {
        Key key;  // key 和 id 放在一起，比较时不必再查一次表
        int id;
    };

    // 上浮：把 i 处的节点和父节点比较，比父节点小就让父节点下移一层
    void siftUp(int i) {
        Node node = heap_[i];
        while (i > 0) {
            int parent = (i - 1) / 2;
            if (!(node.key < heap_[parent].key)) break;
            place(i, heap_[parent]);
            i = parent;
        }
        place(i, node);
    }
    // 下沉：从 i 开始为 node 找位置，较小的子节点逐层上移
    void siftDown(int i, Node node) {
        const int n = size();
        for (;;) {
            int child = 2 * i + 1;
            if (child >= n) break;
            if (child + 1 < n && heap_[child + 1].key < heap_[child].key) ++child;
            if (!(heap_[child].key < node.key)) break;
            place(i, heap_[child]);
            i = child;
        }
        place(i, node);
    }
    void place(int i, const Node& node) {
        heap_[i] = node;
        pos_[node.id] = i;
    }

    std::vector<Node> heap_;  // 堆数组
    std::vector<int> pos_;    // pos_[id] = id 在 heap_ 中的下标，-1 表示不在堆里
};

这样堆里没有重复条目，也不需要“跳过过期条目”。理论上更干净，那实际上更快吗？

实测：别相信直觉

在一张 256×256 的随机地图上（20% 墙、15% 森林、7% 沼泽），用同一批起终点测试三种实现：

实现	长寻路（平均展开 6709 格）	短寻路（平均展开 47 格）
1.3 的 astarPath：每次分配数组，惰性删除	430 μs	10.3 μs
复用数组 + 带索引的堆（decrease-key）	620 μs	3.6 μs
复用数组 + 惰性删除（下文的 Pathfinder）	460 μs	2.7 μs

测试环境

Apple M3，Apple clang 21，-O2，取 7 次运行中最快的一次。完整代码在 cpp/pathfinding/astar_opt/astar_bench.cpp，不同机器上的绝对数值会不同，但相对关系值得在你自己的平台上验证。

结论和直觉相反：

• decrease-key 比惰性删除慢了三到四成。每次在堆里移动元素，都要顺带写一次 pos_ 数组，而这些写入分散在一个很大的数组中，对缓存很不友好。惰性删除省下的只是一些重复条目，在 4 邻域网格上，每个格子最多被松弛 4 次，重复条目本来就不多。
• 复用数组对长寻路几乎没有影响，对短寻路却快了将近 4 倍。长寻路的时间花在展开格子上；而短寻路只展开几十个格子，每次把两个 65536 元素的数组分配并清零，反倒成了大头。

游戏里的寻路请求，大多数恰恰是短距离的：“走到旁边那棵树”“绕开前面的单位”。所以下面的 Pathfinder 选择复用数组 + 惰性删除。

复用内存：代数标记

复用数组的难点在于：每次搜索开始前，costSoFar 这类数组需要“清空”，而清空本身就是 O(V)。

技巧是给每次搜索编一个代数（generation）。数组里除了存数据，还存“这是第几次搜索写入的”。读取时，代数不等于当前代数，就当作“本次搜索还没访问过”：

cpp/pathfinding/astar_opt/pathfinder.h

// 可以反复使用的 A* 寻路器：
//   - 所有数组只在构造时分配一次，之后每次寻路复用
//   - 用"代数"（generation）标记数组内容是否属于本次搜索，省去每次 O(V) 的清零
//   - 堆用 vector + std::push_heap / pop_heap，惰性删除；vector 的容量也跨搜索复用
//   - 支持启发权重 weight：f = g + weight × h（用 1/16 精度的定点数，堆里全是整数比较）
class Pathfinder {
public:
    explicit Pathfinder(const CostGrid& grid)
        : grid_(&grid), seen_(grid.size(), 0), closed_(grid.size(), 0),
          g_(grid.size(), 0), cameFrom_(grid.size(), -1) {}

    std::vector<Cell> find(Cell start, Cell goal, float weight = 1.0f) {
        begin(start, goal, weight);
        while (step()) {}
        return path();
    }

cpp/pathfinding/astar_opt/pathfinder.h

void begin(Cell start, Cell goal, float weight = 1.0f) {
    start_ = start;
    goal_ = goal;
    weight16_ = static_cast<int>(weight * 16.0f + 0.5f);
    found_ = false;
    expanded_ = 0;
    open_.clear();  // 只清空元素，保留已分配的容量
    if (++generation_ == 0) {  // 32 位计数器回绕（约 40 亿次搜索后）：真正清零一次
        std::fill(seen_.begin(), seen_.end(), 0);
        std::fill(closed_.begin(), closed_.end(), 0);
        generation_ = 1;
    }
    if (!grid_->passable(start) || !grid_->passable(goal)) return;
    int s = grid_->index(start);
    visit(s, 0, s);
    push(start, s, 0);
}

开始一次新搜索只需要 ++generation_，所有旧数据自动失效。32 位计数器大约 40 亿次搜索后才会回绕，届时真正清零一次即可。堆用的 vector 调用 clear() 时也会保留已分配的容量，同样不必重新分配。

cpp/pathfinding/astar_opt/pathfinder.h

// 展开一个格子。搜索结束（找到或不可达）时返回 false
bool step() {
    while (!found_ && !open_.empty()) {
        std::pop_heap(open_.begin(), open_.end(), Worse{});
        Entry top = open_.back();
        open_.pop_back();
        if (top.g > g_[top.index]) continue;  // 过期条目：这个格子后来找到了更便宜的路

        closed_[top.index] = generation_;  // 只用于演示显示，搜索本身不读它
        ++expanded_;
        Cell current = grid_->cellAt(top.index);
        if (current == goal_) {
            found_ = true;
            return false;
        }
        grid_->forEachNeighbor(current, [&](Cell next) {
            int ni = grid_->index(next);
            int g = top.g + grid_->cost(next);
            if (seen_[ni] != generation_ || g < g_[ni]) {
                visit(ni, g, top.index);
                push(next, ni, g);
            }
        });
        return true;
    }
    return false;
}

启发权重：用最优性换速度

上一篇提到，h 越大，A* 越“急于”奔向终点；h 一旦高估，就不再保证最优。加权 A* 主动利用这一点：

f(n) = g(n) + w × h(n)，w ≥ 1

它有一个很漂亮的保证：只要原来的 h 是可采纳的，加权 A* 找到的路径代价不超过最优代价的 w 倍。w = 1.5 意味着最坏比最优路径贵 50%，实际上通常远小于这个上限。

图 1.5w 滑块调整启发权重，右上角显示路径代价比最优多了多少；smooth 开关把网格路径拉直（虚线为原始网格路径）。数字是 g。单独打开 ↗

默认地图上，终点前方有一大片沼泽，最优路线从下方绕过去：

w	展开格子数	路径代价
1.0	183	38（最优）
1.5	163	38
2.0	103	38
2.5	98	53（+39%）
4.0	37	53

w 从 1 增加到 2，展开数减少了将近一半，路径依然最优；再往上，搜索“等不及”绕路，直接蹚过了沼泽。在前面的 256×256 随机地图上，规律是一样的：

w	耗时	展开格子数	代价（平均 / 最坏）
1.0	452 μs	6709	×1.000 / ×1.000
1.2	232 μs	3147	×1.003 / ×1.085
1.5	90 μs	1147	×1.065 / ×1.253
2.0	28 μs	369	×1.160 / ×1.459
3.0	19 μs	266	×1.289 / ×1.676

w = 1.5 换来了 5 倍的速度，路径平均只贵 6.5%。和前面那些“优化数据结构”省下的百分之几十相比，少展开格子才是数量级的提升。

w 怎么选

玩家很少能察觉一条路径比最优路径贵 5%10%，却能明显察觉卡顿。常见的做法是：玩家亲自操控的单位用 w = 1；大量 NPC 用 1.21.5；寻路请求排队过多时，临时调高 w。

路径平滑：拉直网格路径

4 方向网格上的路径只能横平竖直，即使在一片空地上斜着走，也会变成一串台阶。玩家看到的是单位一路“直角转弯”。

最简单有效的修正是视线平滑（string pulling）：从起点出发，沿着路径往前找，只要某个点和起点之间没有遮挡，就可以直接走直线过去。找到最远的那个可见点，把它当作新的起点，重复这个过程。

判断“能不能直接走过去”

需要知道一条线段经过了哪些格子。这里有一个容易忽略的细节：线段恰好穿过格子的角点时，角点两侧的格子都必须检查，否则单位会从两堵墙之间斜着“挤”过去。这种把所有接触到的格子都算上的画线算法叫超覆盖（supercover）：

cpp/pathfinding/astar_opt/smoothing.h

// 从格子 a 的中心到格子 b 的中心画一条线段，按顺序访问它经过的每一个格子。
// "超覆盖"：线段恰好穿过格子角点时，角点两侧的格子都算经过，这样线段不会从两堵墙的缝隙里斜穿过去。
// visit 返回 false 时提前停止，函数也返回 false。
template <class F>
bool forEachCellOnLine(Cell a, Cell b, F&& visit) {
    const int nx = std::abs(b.x - a.x), ny = std::abs(b.y - a.y);
    const int sx = b.x > a.x ? 1 : -1, sy = b.y > a.y ? 1 : -1;
    Cell p = a;
    if (!visit(p)) return false;
    for (int ix = 0, iy = 0; ix < nx || iy < ny;) {
        // 比较线段下一次穿过竖直格线和水平格线的"时刻"，先穿过哪条就往哪个方向走一格
        int decision = (1 + 2 * ix) * ny - (1 + 2 * iy) * nx;
        if (decision == 0) {  // 同时穿过：正好经过角点
            if (!visit(Cell{p.x + sx, p.y}) || !visit(Cell{p.x, p.y + sy})) return false;
            p.x += sx;
            p.y += sy;
            ++ix;
            ++iy;
        } else if (decision < 0) {
            p.x += sx;
            ++ix;
        } else {
            p.y += sy;
            ++iy;
        }
        if (!visit(p)) return false;
    }
    return true;
}

// 视线检查：线段经过的格子都可通行，且代价都不超过 maxCost
inline bool lineOfSight(const CostGrid& grid, Cell a, Cell b, int maxCost) {
    return forEachCellOnLine(a, b, [&](Cell c) { return grid.passable(c) && grid.cost(c) <= maxCost; });
}

拉直

cpp/pathfinding/astar_opt/smoothing.h

// 拉直路径：从当前锚点出发，找到路径上"能直接看到"的最远一点，连成一条直线，再以它为新锚点继续。
// 返回的是拐点（路标）序列，单位沿着相邻路标之间的直线移动。
//
// 只要求"没有墙"会让捷径穿过原路径特意绕开的沼泽，所以额外要求：
// 捷径经过的格子，代价不能超过被它替换掉的那段原路径上的最高代价。
inline std::vector<Cell> smoothPath(const CostGrid& grid, const std::vector<Cell>& path) {
    if (path.size() < 3) return path;
    std::vector<Cell> result{path.front()};
    size_t anchor = 0;
    while (anchor + 1 < path.size()) {
        size_t best = anchor + 1;
        int segmentMax = std::max(grid.cost(path[anchor]), grid.cost(path[anchor + 1]));
        for (size_t j = anchor + 2; j < path.size(); ++j) {
            segmentMax = std::max(segmentMax, grid.cost(path[j]));
            if (lineOfSight(grid, path[anchor], path[j], segmentMax)) best = j;
        }
        result.push_back(path[best]);
        anchor = best;
    }
    return result;
}

// 路标序列的几何长度（以格子边长为单位）
inline float polylineLength(const std::vector<Cell>& points) {
    float total = 0;
    for (size_t i = 1; i < points.size(); ++i) {
        total += std::hypot(static_cast<float>(points[i].x - points[i - 1].x),
                            static_cast<float>(points[i].y - points[i - 1].y));
    }
    return total;
}

带地形代价的地图需要额外小心。原路径特意绕开了一片沼泽，如果只检查“有没有墙”，捷径就可能直接穿过沼泽，看起来更短，实际更慢。所以这里额外要求：捷径经过的格子，代价不能超过被替换的那段原路径上的最高代价。

在演示中打开 smooth：默认地图上，总长 38.0 的台阶路径变成了 6 段直线，总长 34.8，而且仍然从沼泽下方绕过去。

平滑不是免费的

smoothPath 对每个锚点都要向后检查所有点的视线，最坏是 O(n² × L)（n 为路径长度，L 为线段长度）。路径很长时，可以只检查前方固定数量的点，或者让单位边走边平滑：每帧只检查从当前位置能否看到更远的路径点。

常见陷阱

问题	原因	解决
“优化”之后反而更慢	凭直觉选择数据结构	在真实的地图和请求分布上测量
短距离寻路也很慢	每次寻路都分配并清零整张图的数组	复用缓冲区，用代数标记代替清零
加权后路径明显绕远或穿过危险区域	w 太大	控制在 1.2~1.5；对路径质量敏感的单位用 1
平滑后单位穿过墙角	画线算法只检查了线段经过的“主要”格子	使用超覆盖，角点两侧的格子都检查
平滑后单位蹚过沼泽	只检查墙，没有检查地形代价	捷径的地形代价不能超过原路径
平滑后路径贴着墙走，单位模型被墙挡住	视线检查把单位当成一个点	视线检查时把墙“加粗”单位的半径，或使用导航网格

在游戏中的应用

• 大型 RTS / 模拟经营：寻路请求排队，每帧只处理一部分，并用单步版本把一次长寻路分摊到多帧；队列积压时动态调高 w。
• 任意角度寻路：Theta* 把视线检查直接放进 A* 的松弛步骤里，搜索过程中就允许“抄近路”，得到的路径比先搜索再平滑更短。
• 路径跟随：平滑后的拐点交给转向行为（第三卷“转向行为与 Boids”）处理，单位在拐点附近提前转弯，运动会更自然。

练习

1. 运行 astar_bench.cpp，在你的机器上重现本章的数据。把地图改成 1024×1024，结论有变化吗？
2. 实现“动态权重”：w 随着离起点的距离逐渐降低，即离起点越近越“急”，离终点越近越“细致”。和固定权重比较展开数与路径质量。
3. 修改 Pathfinder，加入“最多展开 N 个格子”的上限：超过上限时，返回到目前为止离终点最近（h 最小）的格子的路径。这在游戏中有什么用？
4. 在 smoothPath 的视线检查中加入“单位半径”：线段两侧各偏移半个格子，三条线都通行才算可见。

延伸阅读

• Amit Patel，Amit’s A* Pages: Implementation notes：堆、tie-breaking 与各种工程细节
• Mo Chen 等，Priority Queues and Dijkstra’s Algorithm（UTCS 技术报告 TR-07-54，2007）：系统比较了 decrease-key 与重复插入两种写法
• Kenny Daniel、Alex Nash、Sven Koenig、Ariel Felner，Theta*: Any-Angle Path Planning on Grids：在搜索过程中直接得到任意角度路径
• Ira Pohl，Heuristic Search Viewed as Path Finding in a Graph（1970）：加权 A* 的出处