导航网格与漏斗算法

问题：网格不够用了

前五篇都在网格上寻路。网格简单可靠，但在很多游戏里并不合适：

• 太浪费：一片 200 米 × 200 米的开阔广场，按 0.5 米一格要 16 万个格子，而它本质上只是“一块空地”。
• 路径不自然：网格路径只能沿 4 个或 8 个方向走，即使做了平滑，也是在一个本不该有的约束上修修补补。
• 不贴合地形：3D 场景里有斜坡、台阶、桥，可行走区域的边界是任意形状，很难用方格准确描述。

商业引擎给出的答案是导航网格（navigation mesh，NavMesh）：把可行走区域划分成若干个凸多边形，在多边形之间寻路。Unity 和 Unreal 内置的寻路系统，底层都是 Mikko Mononen 的开源库 Recast & Detour。

导航网格是什么

导航网格由两样东西组成：

• 凸多边形：凸多边形内任意两点之间的连线都在多边形内部，所以单位在同一个多边形里可以直线行走，不需要再寻路。
• 门（portal）：相邻两个多边形共享的那段边。单位从一个多边形走进另一个，必须穿过这扇门。

cpp/pathfinding/navmesh/navmesh.h

// 导航网格：可行走区域被划分成若干个凸多边形。
// 相邻多边形之间共享的线段叫"门"（portal），单位从一个多边形穿过门进入另一个。
struct NavLink {
    int to;    // 相邻多边形的下标
    Vec2 a, b; // 门的两个端点
};

struct NavPoly {
    std::vector<Vec2> verts;  // 凸多边形顶点
    Vec2 center;
    std::vector<NavLink> links;
};

using Polygon = std::vector<Vec2>;

// 点是否在凸多边形内（含边界）：对每条边，点都必须在同一侧
inline bool insideConvex(const Polygon& poly, Vec2 p, float eps = 1e-3f) {
    bool pos = false, neg = false;
    for (size_t i = 0; i < poly.size(); ++i) {
        float c = cross(poly[(i + 1) % poly.size()] - poly[i], p - poly[i]);
        if (c > eps) pos = true;
        if (c < -eps) neg = true;
        if (pos && neg) return false;
    }
    return true;
}

class NavMesh {
public:
    std::vector<NavPoly> polys;

    // 找到包含点 p 的多边形，找不到（在障碍里或地图外）返回 -1
    int findPoly(Vec2 p) const {
        for (int i = 0; i < static_cast<int>(polys.size()); ++i) {
            if (insideConvex(polys[i].verts, p)) return i;
        }
        return -1;
    }
};

一块空旷的广场，无论多大，都可以只用一个多边形表示。演示中整张 800×450 的地图只需要 32 个多边形，换成 10 像素一格的网格则需要 3600 个格子。

亲手试一试

图 1.8拖动 S、G 移动起点终点。浅色区域是 A* 找到的通道，赭黄线段是通道依次穿过的门（实心点为左端点，空心点为右端点）。每一步处理一扇门：圆圈是漏斗尖，实线箭头和虚线分别是漏斗的左右边。单独打开 ↗

几个值得做的小实验：

1. 单步执行，观察漏斗：每处理一扇门，左右两条边就尝试向内收紧。当一侧越过另一侧时，路径在那个端点处拐弯，漏斗尖移到拐点。
2. 打开 midpoints，对比“直接连接门中点”的路径：默认地图上它长 1351，漏斗算法的结果只有 741。
3. 把 S 和 G 拖到同一个多边形里，路径就是一条直线，漏斗算法一步就结束。

生成导航网格

实际项目中，导航网格通常由 Recast 这样的工具离线生成，大致流程是：

1. 把场景几何体体素化，标记哪些体素可以站立（坡度、台阶高度、头顶空间）。
2. 按单位的半径向内收缩可行走区域，这样后面就可以把单位当作一个点。
3. 把可行走区域划分成区域，提取轮廓，再剖分成凸多边形。

这套流程很复杂，不是本章的重点。为了得到一个可以交互的演示，这里用一个简单得多的方法：梯形分解。在每个障碍物顶点的 x 坐标处画一条竖线，把地图切成若干竖条。每个竖条里没有顶点，穿过它的障碍物边可以按高度排序，相邻两条边之间如果不在障碍物里，就是一块梯形。

cpp/pathfinding/navmesh/navmesh.h

// 用"梯形分解"从障碍物生成导航网格：
//   1. 在每个障碍物顶点的 x 坐标处画一条竖线，把地图切成若干竖条
//   2. 每个竖条内部没有顶点，穿过它的障碍物边互不相交，可以按高度排序
//   3. 相邻两条边之间如果不在障碍物里，就是一块可行走的梯形
//   4. 左右相邻、上下边界来自同一对边的梯形合并成一个
//   5. 左右两侧竖边有重叠的梯形互相连通，重叠部分就是门
// 障碍物必须是互不重叠的凸多边形，且完全位于 [min, max] 矩形内部。
inline NavMesh buildNavMesh(Vec2 min, Vec2 max, const std::vector<Polygon>& obstacles) {
    constexpr float kEps = 1e-3f;  // 横坐标相差小于它视为同一条竖线，保证每个竖条都有一定宽度
    struct Edge {
        Vec2 p, q;  // p.x < q.x
        float yAt(float x) const { return p.y + (q.y - p.y) * (x - p.x) / (q.x - p.x); }
    };
    std::vector<Edge> edges{{{min.x, min.y}, {max.x, min.y}}, {{min.x, max.y}, {max.x, max.y}}};  // 上下边界
    std::vector<float> xs{min.x, max.x};
    for (const Polygon& o : obstacles) {
        for (size_t i = 0; i < o.size(); ++i) {
            Vec2 a = o[i], b = o[(i + 1) % o.size()];
            xs.push_back(a.x);
            if (std::abs(a.x - b.x) > kEps) edges.push_back(a.x < b.x ? Edge{a, b} : Edge{b, a});
        }
    }
    std::sort(xs.begin(), xs.end());
    xs.erase(std::unique(xs.begin(), xs.end(), [](float a, float b) { return b - a < kEps; }), xs.end());

    // 一块梯形：左右两条竖边 x0 / x1，上边界是 edges[top]，下边界是 edges[bottom]
    struct Trap {
        float x0, x1;
        int top, bottom;
    };
    std::vector<Trap> traps;
    std::vector<Trap> prevSlab;
    for (size_t s = 0; s + 1 < xs.size(); ++s) {
        float x0 = xs[s], x1 = xs[s + 1], xm = (x0 + x1) / 2;
        std::vector<int> crossing;  // 穿过这个竖条的边
        for (int e = 0; e < static_cast<int>(edges.size()); ++e) {
            if (edges[e].p.x <= x0 + kEps && edges[e].q.x >= x1 - kEps) crossing.push_back(e);
        }
        std::sort(crossing.begin(), crossing.end(),
                  [&](int a, int b) { return edges[a].yAt(xm) < edges[b].yAt(xm); });

        std::vector<Trap> slab;
        for (size_t k = 0; k + 1 < crossing.size(); ++k) {
            int top = crossing[k], bottom = crossing[k + 1];
            Vec2 mid{xm, (edges[top].yAt(xm) + edges[bottom].yAt(xm)) / 2};
            bool blocked = std::any_of(obstacles.begin(), obstacles.end(),
                                       [&](const Polygon& o) { return insidePolygon(o, mid); });
            if (blocked) continue;
            // 和左边竖条里上下边界相同的梯形合并
            auto same = std::find_if(prevSlab.begin(), prevSlab.end(), [&](const Trap& t) {
                return t.top == top && t.bottom == bottom && std::abs(t.x1 - x0) < kEps;
            });
            if (same != prevSlab.end()) {
                Trap merged = *same;
                merged.x1 = x1;
                slab.push_back(merged);
                prevSlab.erase(same);
            } else {
                slab.push_back({x0, x1, top, bottom});
            }
        }
        traps.insert(traps.end(), prevSlab.begin(), prevSlab.end());  // 左边没被合并的已经完整
        prevSlab = std::move(slab);
    }
    traps.insert(traps.end(), prevSlab.begin(), prevSlab.end());

    NavMesh mesh;
    for (const Trap& t : traps) {
        const Edge &top = edges[t.top], &bottom = edges[t.bottom];
        Polygon v{{t.x0, top.yAt(t.x0)}, {t.x1, top.yAt(t.x1)}, {t.x1, bottom.yAt(t.x1)}, {t.x0, bottom.yAt(t.x0)}};
        // 上下边界在端点处相交时，梯形退化成三角形：去掉重复顶点
        Polygon verts;
        for (Vec2 p : v) {
            if (verts.empty() || distance(verts.back(), p) > 1e-4f) verts.push_back(p);
        }
        if (verts.size() > 1 && distance(verts.front(), verts.back()) < 1e-4f) verts.pop_back();
        Vec2 c{0, 0};
        for (Vec2 p : verts) c += p;
        mesh.polys.push_back({verts, c / static_cast<float>(verts.size()), {}});
    }
    // 连通关系：a 的右边与 b 的左边在同一条竖线上，且竖直方向有重叠
    for (size_t a = 0; a < traps.size(); ++a) {
        for (size_t b = 0; b < traps.size(); ++b) {
            const Trap &ta = traps[a], &tb = traps[b];
            if (std::abs(ta.x1 - tb.x0) > kEps) continue;
            float x = ta.x1;
            float y0 = std::max(edges[ta.top].yAt(x), edges[tb.top].yAt(x));
            float y1 = std::min(edges[ta.bottom].yAt(x), edges[tb.bottom].yAt(x));
            if (y1 - y0 < 1e-3f) continue;
            mesh.polys[a].links.push_back({static_cast<int>(b), {x, y0}, {x, y1}});
            mesh.polys[b].links.push_back({static_cast<int>(a), {x, y0}, {x, y1}});
        }
    }
    return mesh;
}

打开演示的 mesh 开关就能看到结果：很多又高又窄的梯形。Recast 生成的多边形形状会规整得多，但对于寻路算法来说，两者没有区别，都是“一组凸多边形 + 它们之间的门”。

第一步：找到通道

先在多边形之间跑 A*，得到一串需要依次穿过的多边形，称为通道（channel）。

这里有一个前几篇没有遇到的问题：多边形的“位置”在哪里？网格寻路中，每个格子就是一个点；而一个多边形可能很大，用它的中心计算距离会非常不准。常见的做法是用单位穿过门的位置代替，而这个位置又该取在哪里？

cpp/pathfinding/navmesh/funnel.h

// 估计单位穿过门 a-b 的位置：从 from 指向终点的直线与门的交点，夹在门的范围内。
// 比直接取门的中点准确得多（门很长时，中点可能离真实路线很远）
inline Vec2 portalPoint(Vec2 a, Vec2 b, Vec2 from, Vec2 goal) {
    Vec2 ab = b - a, d = goal - from;
    float denom = cross(d, ab);
    float t = 0.5f;
    if (std::abs(denom) > 1e-6f) t = cross(d, from - a) / denom;  // 交点在 a-b 上的参数
    t = std::clamp(t, 0.02f, 0.98f);  // 稍微离开端点，端点通常紧贴障碍物
    return a + ab * t;
}

cpp/pathfinding/navmesh/funnel.h

// 在多边形之间跑 A*，得到一串需要依次穿过的多边形（通道，channel）。
// 每个多边形的"位置"取单位进入它时穿过门的估计位置，代价是这些点之间的直线距离。
inline std::vector<int> findChannel(const NavMesh& mesh, Vec2 start, Vec2 goal, int* expanded = nullptr) {
    int startPoly = mesh.findPoly(start), goalPoly = mesh.findPoly(goal);
    if (startPoly < 0 || goalPoly < 0) return {};

    const int n = static_cast<int>(mesh.polys.size());
    std::vector<float> g(n, 1e30f);
    std::vector<Vec2> pos(n);
    std::vector<int> cameFrom(n, -1);
    std::vector<char> closed(n, 0);
    struct Entry {
        float f;
        int poly;
        bool operator<(const Entry& o) const { return f > o.f; }  // 小顶堆
    };
    std::vector<Entry> open{{distance(start, goal), startPoly}};
    g[startPoly] = 0;
    pos[startPoly] = start;
    cameFrom[startPoly] = startPoly;
    int count = 0;

    while (!open.empty()) {
        std::pop_heap(open.begin(), open.end());
        int cur = open.back().poly;
        open.pop_back();
        if (closed[cur]) continue;
        closed[cur] = 1;
        ++count;
        if (cur == goalPoly) break;
        for (const NavLink& link : mesh.polys[cur].links) {
            Vec2 entry = portalPoint(link.a, link.b, pos[cur], goal);
            float ng = g[cur] + distance(pos[cur], entry);
            if (!closed[link.to] && ng < g[link.to]) {
                g[link.to] = ng;
                pos[link.to] = entry;
                cameFrom[link.to] = cur;
                open.push_back({ng + distance(entry, goal), link.to});
                std::push_heap(open.begin(), open.end());
            }
        }
    }
    if (expanded) *expanded = count;
    if (cameFrom[goalPoly] < 0) return {};
    std::vector<int> channel;
    for (int p = goalPoly;; p = cameFrom[p]) {
        channel.push_back(p);
        if (p == startPoly) break;
    }
    std::reverse(channel.begin(), channel.end());
    return channel;
}

最直接的选择是门的中点，但门可能很长（演示里有的门从地图顶部一直延伸到障碍物），中点离真实路线很远。在 400 张随机地图、7855 次寻路上，用两种方式选出通道，再和真正的最短路径（可见性图暴力求解）比较：

穿门位置	得到全局最短路径	平均多走	多走 10% 以上	多走 50% 以上
门的中点	72%	3.77%	1006 次	58 次
朝向终点的直线与门的交点	94%	0.23%	28 次	2 次

只改了一行代码，绝大多数情况下就能选出正确的通道。剩下的少数情况是因为 A* 在估计通道的长度，而真实长度要等漏斗算法拉直之后才知道。需要严格保证最短路径的场合，可以使用 Polyanya 这类在导航网格上精确搜索的算法。

第二步：漏斗算法

有了通道，就有了一串门。单位必须依次穿过每扇门，问题变成：穿过这些门的最短折线是什么？

想象从起点拉一根绳子，依次穿过每扇门，到达终点后把绳子拉紧。拉紧后的绳子只会在某些门的端点处拐弯，这就是拉绳（string pulling）问题，漏斗算法就是它的高效解法。

左与右

每扇门都有两个端点。站在门前、面朝前进方向，左手边的是左端点，右手边的是右端点。判断方法正是第 0.2 篇的叉积：从多边形中心看向门，另一个端点在右边，则这个端点是左端点。

cpp/pathfinding/navmesh/funnel.h

struct Portal {
    Vec2 left, right;  // 站在门前、面朝前进方向时的左端点和右端点
};

// 把通道转换成门的序列：起点和终点各视为一扇宽度为 0 的门
inline std::vector<Portal> channelPortals(const NavMesh& mesh, const std::vector<int>& channel, Vec2 start, Vec2 goal) {
    std::vector<Portal> portals{{start, start}};
    for (size_t i = 0; i + 1 < channel.size(); ++i) {
        const NavPoly& from = mesh.polys[channel[i]];
        for (const NavLink& link : from.links) {
            if (link.to != channel[i + 1]) continue;
            // 从多边形中心看向门：b 在 a 的右边（屏幕坐标 y 向下时叉积为正），则 a 是左端点
            bool aIsLeft = cross(link.a - from.center, link.b - from.center) > 0;
            portals.push_back(aIsLeft ? Portal{link.a, link.b} : Portal{link.b, link.a});
            break;
        }
    }
    portals.push_back({goal, goal});
    return portals;
}

收紧漏斗

从起点出发，向第一扇门的两个端点各引一条射线，形成一个漏斗。之后依次处理每扇门：

• 新的右端点如果在漏斗内侧，就把漏斗的右边移过去，漏斗变窄。
• 如果新的右端点越过了漏斗的左边，说明不可能直线到达，路径必须在左边的端点处拐弯。把这个端点加入路径，作为新的漏斗尖，从它之后的门重新开始。
• 左边的处理完全对称。

终点被当作一扇宽度为 0 的门，处理完它，路径就完整了。

cpp/pathfinding/navmesh/funnel.h

// 漏斗算法（Simple Stupid Funnel Algorithm，Mikko Mononen）：
// 从起点（漏斗尖）出发，用左右两条边夹住依次经过的门，不断收紧。
// 某一侧收紧时越过了另一侧，说明路径必须在另一侧的端点处拐弯：
// 把那个端点加入路径，作为新的漏斗尖，从它之后重新开始。
class Funnel {
public:
    explicit Funnel(std::vector<Portal> portals) : portals_(std::move(portals)) {
        if (portals_.empty()) return;
        apex_ = left_ = right_ = portals_[0].left;
        path_.push_back(apex_);
    }

    // 处理下一扇门。全部处理完时返回 false
    bool step() {
        if (i_ >= portals_.size()) return false;
        Vec2 l = portals_[i_].left, r = portals_[i_].right;

        // 收紧右边：新的右端点没有在当前右边的外侧（更右）
        if (cross(right_ - apex_, r - apex_) <= 0) {
            if (apex_ == right_ || cross(left_ - apex_, r - apex_) > 0) {
                right_ = r;  // 仍在左边的右侧：漏斗变窄
                rightIndex_ = i_;
            } else {
                restartAt(left_, leftIndex_);  // 越过了左边：在左端点拐弯
                return true;
            }
        }
        // 收紧左边：对称
        if (cross(left_ - apex_, l - apex_) >= 0) {
            if (apex_ == left_ || cross(right_ - apex_, l - apex_) < 0) {
                left_ = l;
                leftIndex_ = i_;
            } else {
                restartAt(right_, rightIndex_);
                return true;
            }
        }
        if (++i_ == portals_.size() && !(path_.back() == portals_.back().left)) {
            path_.push_back(portals_.back().left);  // 终点
        }
        return i_ < portals_.size();
    }

    const std::vector<Vec2>& path() const { return path_; }
    Vec2 apex() const { return apex_; }
    Vec2 left() const { return left_; }
    Vec2 right() const { return right_; }
    size_t index() const { return i_; }

private:
    void restartAt(Vec2 corner, size_t index) {
        path_.push_back(corner);
        apex_ = left_ = right_ = corner;
        leftIndex_ = rightIndex_ = index;
        i_ = index + 1;
    }

    std::vector<Portal> portals_;
    std::vector<Vec2> path_;
    Vec2 apex_{}, left_{}, right_{};
    size_t leftIndex_ = 0, rightIndex_ = 0;
    size_t i_ = 1;
};

inline std::vector<Vec2> stringPull(const std::vector<Portal>& portals) {
    Funnel f(portals);
    while (f.step()) {}
    return f.path();
}

“内侧”和“越过”的判断，又是叉积。这个算法总共不到 50 行，却是几乎所有商业寻路系统的标准组成部分。

复杂度

每次拐弯后，算法会从拐点之后的门重新扫描，所以最坏情况是 O(n²)（n 为门的数量）。实际上拐点很少、回退很短，通常接近线性。另有严格 O(n) 的版本，但实现复杂得多，收益很小。

常见陷阱

问题	原因	解决
单位贴着墙角走，模型穿进墙里	导航网格按“点”生成，没有考虑单位的半径	生成时把可行走区域向内收缩单位半径；不同体型的单位使用不同的导航网格
路径明显绕远	用门的中点估计通道长度	用朝向终点的直线与门的交点，或者使用精确算法
漏斗算法拐错方向	门的左右端点颠倒	用叉积统一判断；注意屏幕坐标 y 向下时，叉积的符号与数学课本相反
起点或终点不在任何多边形里	点击在障碍物上，或者在导航网格外	找最近的多边形，把点投影到它的边界上
动态障碍（开关的门、移动的车）无法绕开	导航网格是离线生成的	局部重建受影响的区域（Recast 的分块），或用局部避让处理移动障碍

在游戏中的应用

• 3D 动作 / 开放世界游戏：几乎都使用导航网格。Unity 的 NavMesh 和 Unreal 的 Navigation System 都基于 Recast & Detour。
• 多种体型：小怪、大型 Boss、载具各用一张导航网格，生成时按各自的半径收缩。
• 跳跃与攀爬：在多边形之间加入特殊的连接（Off-Mesh Link），比如“从这里可以跳到那个平台”，寻路时当作普通的门处理，只是代价不同。
• 与局部避让结合：导航网格负责全局路线，RVO 等局部避让算法负责单位之间不互相碰撞，这是大多数游戏的分工方式。

练习

1. 修改 findChannel，把穿门位置改回门的中点，在演示中找一组起终点，使两种方式选出的通道明显不同。
2. 实现“单位半径”：在 buildNavMesh 之前，把每个障碍物向外扩张 r。凸多边形向外扩张后，拐角处应该是圆弧，可以用多段折线近似。
3. 给漏斗算法加入“拐角偏移”：路径在障碍物拐角处拐弯时，把拐点沿外角平分线方向推开 r，让单位不要贴着拐角走。
4. 实现“起点在障碍物里”的处理：找到离起点最近的多边形边界点，从那里开始寻路。

延伸阅读

• Mikko Mononen，Simple Stupid Funnel Algorithm：本章漏斗算法的出处，Recast 作者的博客
• Recast & Detour：工业级的导航网格生成与寻路库
• Michael Cui、Daniel Harabor、Alban Grastien，Compromise-free Pathfinding on a Navigation Mesh（IJCAI 2017）：Polyanya，在导航网格上精确求最短路径
• D. T. Lee、F. P. Preparata，Euclidean Shortest Paths in the Presence of Rectilinear Barriers（1984）：漏斗算法的早期形式