recastnavigation源码解析

导语

recastnavigation源码分为recast和detour两个部分。 recast部分是将以三角形集合形式表示的空间场景转化为可供寻路使用的导航数据(navmesh)。 detour部分是根据recast生成的navmesh，为指定源点和终点生成直线段路径。 下面我通过走读源码来逐步解读recast和detour这两个过程。 这篇文章单纯只是自己在阅读源代码过程中的一些理解，难免有些错误，如有发现请指正。

recast

recast的输入

cellSize------x、z方向上的体素精度

walkableSlopeAngle------agent的可行走最大坡度

walkableHeight------agent的可行走的最小高度空间

walkableClimb------agent的可攀爬高度

walkableRadius------agent的行走半径

float* bmin和float* bmax------场景的AABB包围盒

int* tris数组------场景的三角形序列

ntris------场景的三角形个数

float* verts------场景三角形各个顶点的坐标

nverts------场景三角形的顶点总数

recast的输入举例

如上图这个场景，包含3个三角形和5个顶点。5个顶点的坐标用float* verts[3 * nverts]数组存储，分别表示nverts个顶点的x、y、z坐标，nverts的值为5；3个三角形用int* tris[3 * ntris]数组存储，分别表示ntris个三角形的3*ntris个顶点在verts数组中的下标，ntris的值为3。在这个例子中，verts[3 * nverts]数组的内容是[x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,x5,y5,z5]，而tris[3 * ntris]数组的内容是[0,1,2,1,2,4,2,3,4]。

标记可行走的三角形

如上图，三角形的坡度角为 ϕ ，我们利用向量叉乘 V2V1→×V2V3→ 得到三角平面的法向量，然后对法向量归一化成单位法向量，那么此时单位法向量的Y坐标就是坡度角的余弦值。当单位法向量的Y坐标大于最大可行走坡度角的余弦值时，说明该三角平面是可行走的。相应的代码如下：

体素化

在讲体素化之前，我们先来看下如何将一个凸多边形分隔成两个凸多边形。如下图的五边形，我们分析下 V6V7 这条切割线分隔凸多边形的流程。

当遍历完所有边之后，我们就得到了两个子凸多边形 V1V6V7V5 和 V6V2V3V4V7 。对应的代码为：

凸多边形的分割代码

基于上面分隔凸多边形的原理，我们每相隔cellsize个单位(体素精度)，分别在平行于x轴和z轴的方向设置分隔线就可以将三角形平面切割成cellsize精度的体素格子，体素格子的y坐标下沿取多边形顶点中的最小y坐标，体素格子的y坐标上沿取多边形顶点中的最大y坐标。如下图所示：

三角形的体素化

构建高度场HeightField

高度场是一个链表数组，每个链表是由一系列x、z坐标相同的体素格子链接而成。存储高度场的数据结构如下图所示：

存储高度场的数据结构示例

某个体素插入到链表数组里的哪个链表中，由x + z * width的值来决定，这个值代表所插入链表在数组中的下标。比如x=1,z=1处的体素(在图中被标记为紫色)就插入到下标为3的链表中。如果待插入体素的y坐标范围与链表中已有体素的y坐标范围有重合，需要做体素合并。构建高度场的代码如下：

构建高度场的代码

高度场的可行走标记修正

定义：

walkableHeight ---- actor行走所需要的最小垂直高度

walkableClimb ----actor所能攀爬的最大垂直高度

第一种场景，如下图：

如果体素A是可行走的，并且height < walkableClimb，则体素B必然也是可行走的。

第二种场景，如下图：

定义邻居体素可达的条件为：min(top, ntop) - max(bot, nbot) > walkableHeight。则对于某个体素，其所有可达的邻居体素中：

1.如果存在(bot - nbot > walkableClimb)，则将该体素修正为不可行走。

2.如果max(nbot) - min(nbot) > walkableClimb，则将该体素修正为不可行走。

第三种场景，如下图：

如果height < walkableHeight，则体素A需要修正为不可行走。

构建紧凑高度场CompactHeightfield(反体素化)

遍历之前的高度场数据，将体素信息转为反体素，反体素的y=体素的上沿y坐标，反体素的h=(链表下一个体素的下沿y坐标或者最大y坐标-该体素的上沿y坐标)，不可行走的体素不用转换为反体素，

反体素的数据存储如上图所示。某个地点(x、z坐标)处的体素访问，首先计算值(x + z * width)，用这个值去元信息数组中访问rcCompactCell数据。元信息数据中的index代表该处位置的反体素在反体素数组中的开始下标，count字段表示该地点(x、z坐标)有几层反体素

struct rcCompactCell
{
	unsigned int index : 24;	///< Index to the first span in the column.
	unsigned int count : 8;		///< Number of spans in the column.
};

计算反体素的连通性

如下图所示，2个反体素要连通，需要满足两个条件。

1.2个反体素的y坐标差值要小于等于agent的可攀爬高度walkableClimb。

2.2个反体素的重叠部分的h要大于等于walkableHeight。

某个反体素与左右前后4个邻居反体素的连通信息存储在反体素结构的con字段，每个方向占6个bit，相应bit值表征连通邻居反体素的layer层。

比如con字段的二进制值为000001 000010 000000 000100时，意义如下：

1) 左方向，该体素与layer为1的体素连通

2) 上方向，该体素与layer为2的体素连通

3) 右方向，该体素无连通体素

4) 下方向，该体素与layer为4的体素连通

裁剪可行走区域

我们采用dist数组来保存每个反体素与可行走区域边缘的最近距离。

对于上图的中间那个体素：

1.从左到右、由下及上扫描反体素时，绿色的那4个邻居体素已先被扫描到。

2.从右到左、由上及下扫描反体素时，蓝色的那4个邻居体素已先被扫描到。

因此，我们可以通过上述两次对所有反体素的扫描可以得到每个反体素与可行走区域边缘的最近距离。

对于dist值小于agent直径的反体素，将其标记为不可行走。

标记体素掩码值

通过部署一些多边形柱子，然后遍历所有反体素，对于在多边形柱子内的反体素，将其areaId标记为相应值。areaId表示该体素是否可行走，是否是山地、草地之类。

后续的区域划分会确保1个区域不会包含两种areaId，detour寻路也支持对于不同的areaId定义不同的单位路径损耗cost。

区域划分算法

1) 分水岭(watershed) :recast默认算法，效果好，速度慢。

2) Monotone：速度快。但是生成的 Region 可能会又细又长，效果一般。

3) layers：类同monotone，只是区域在生成过程中不会有叠层(不会跨相同x、z坐标的多个y坐标体素)

• 分水岭算法

首先也是构建距离场dist数组(反体素到边界的距离)，这个过程与前面裁剪可行走区域过程中构建距离场非常类似。区别仅在于对非边界体素的判定：前者是前后左右4个方向都存在连通邻居并且体素掩码相同；后者只需要前后左右4个方向都存在连通邻居。相关代码如下图所示:

距离场构建完后，需要再对dist数组进行均值降噪处理。体素到边界的距离修正为 (自身到边界距离 + 8个邻居到边界距离) /9，如果有邻居不连通，则用自身到边界的距离补位。

分水岭算法举例

1.首先根据前面构建的距离场将体素划分到不同的批次，每隔2个距离场1个批次(看代码发现如果dist数组的最大值d是偶数，则第一个批次会包含d-2、d-1、d 三个距离的体素)。

2.从距离场最大的批次开始填充水位，新水位填充后立刻进行深度优先泛洪邻居体素。

3.新批次的体素，进行广度优先泛洪邻居体素，尚未被泛洪到的体素进行新水位填充。

如上图所示，最终体素被划分成了A、B、C三个区域。

• Monotone算法

称某一行的连续体素成为一个sweepSpan，该算法从左到右逐行扫描体素，每扫描完一行体素，针对该行体素形成的一系列sweepSpan进行判断：如果某个sweepSpan A在-y方向上的邻居sweepSpan只有1个并且该邻居在+y方向上的邻居也只有1个(必然是sweepSpan A)，那么合并这两个sweepSpan。

monotone算法举例

区域裁剪

做区域裁剪前，需要找出每个区域的邻接区域。寻找邻接区域的流程如下图所示：

寻找邻接区域的流程

比如下面这个图中：

1.区域7的邻接区域为区域6、3、8、9、5

2.区域5的邻接区域为区域4、6、7、9

找出邻接区域后，再做如下处理来完成区域裁剪。

1.针对每个区域，采用深度优先遍历其所有邻接区域，如果最终包含的体素数目小于minRegionArea，则将遍历到的所有区域裁剪掉。我理解这个操作是为了减少比较小的孤立区域。

2.对体素数量过少的区域A进行合并，合并到最小的邻接区域B中。合并过程中，需要将A的邻接区域合并到B的邻接区域中，针对所有区域的邻接区域，需要将其中的A区域需要替换为B区域。

3.经过区域裁剪和合并后，region会变少，需要对区域的regionID重新remap赋值，以此来降低regionID的最大值。

生成轮廓线

与寻找邻接区域类似，都是沿着区域边界顺时针行走。行走过程中取轮廓点的规则为:

1) 体素左方是边界，轮廓点取其上方体素。

2) 体素上方是边界，轮廓点取其右上方体素。

3) 体素右方是边界，轮廓点取其右方体素。

4) 体素下方是边界，轮廓点取其自身。

这样做的目的是，使得各个区域的轮廓线多边形的边互相重合。最终效果如下图所示：

• 轮廓线简化

简化的目的是使用尽可能少的直线段来逼近带毛刺的边界。整个简化过程如下：

1) 左下角和右上角顶点作为初始轮廓。

2) 对于轮廓线段，遍历线段中间的其它顶点，找到偏离线段最远的顶点，如果偏离距离大于指定值，则将该顶点加入轮廓。

3) 一直迭代，直到所有顶点与轮廓的距离在指定值内。

• 检查轮廓线的空洞

在说检测空洞之前，先讲下三角形面积与向量叉乘的关系。

对于上面的三角形ABC：

如果顺时针取顶点，面积 = (→×→+→×→+→×→)∕2 ，结果是负数。

如果逆时针取顶点，面积 = (→×C→+C→×B→+B→×→)∕2 ，结果是正数。

而正常轮廓线的顶点是顺时针存储，空洞轮廓线的顶点是逆时针存储。如下图所示：

如果顶点的存储顺序为 、、、、V1、V2、V3、V4、V5 。如果按存储顺序取顶点的话，应该是 V1V2 和 V2V3 和 V3V4 和 V4V5 和 V5V1 。但Recast源码计算多边形面积的时候，取的确是 V1V5 和 V2V1 和 V3V2 和 V4V3 和 V5V4 。因此因此空洞轮廓线的顶点存储虽然是逆时针，但其面积反而为负数。下图是计算多边形面积的函数。

下面是利用多边形面积检测空洞的相关代码。

• 合并空洞

如上图所示，合并空洞的步骤分为：

1) 找到空洞的左下方顶点B4。

2) 将轮廓线所有顶点与B4相连，如果连线与轮廓线、空洞都不相交，则连线构成1条对角线。

3) 选择其中长度最短的1条对角线，将空洞合并到轮廓线中。

最终轮廓线的顶点序列为A5、A6、A1、A2、A3、A4、A5、B4、B1、B2、B3、B4。

如果包含多个空洞的话，将空洞按左下方顶点排序，依次迭代将外围轮廓与空洞进行合并。

• 轮廓线三角剖分(耳切法)

耳尖的定义：

1.顶点是一个凸点

2.左右顶点相连的对角线与其它边不相交

在上图中，V1、V4、V5、V6是耳尖

将对角线最短的耳尖V1进行切割，切割后需要对左右相邻的顶点是否为耳尖重新判断，

切割后耳尖为V2、V4、V5、V6、V7。经过多次迭代后，最终形成的三角形如下图所示：

凸多边形合并

轮廓线经过三角剖分后形成了一系列凸多边形(三角形是最简单的凸多边形)。为了提升detour寻路的效率，我们需要凸多边形进行合并。

2个凸多边形必须满足下面两个条件才可以合并：

1) 必须要有公共边

2) 合并后，公共边的2个顶点是否能维持凸点

以上图举例说明，两个凸多边形合并后，其公共边的2个顶点能维持凸点的条件是：

(1) 边2在边1的右边(包括共线)。

(2) 边4在边3的右边(包括共线)。

在合并过程中，两个凸多边形的合并权重是其公共边的长度，每次都挑选合并权重最大的两个凸多边形进行合并。在上面这个图中，t1与t2可以合并，t2和t4可以合并。最终形成的效果如下图所示：

detour寻路

通过前面的体素化、构建高度场、区域划分、轮廓线生成、三角剖分、凸多边形合并，我们将场景构建成了一系列可用于寻路的凸多边形。

Detour寻路算法步骤分为：

1) 寻找离起点A和终点B距离最近的凸多边形。

2) 通过A*寻路算法找出A点到B点所经过的凸多边形序列。

3) 通过漏斗算法确认出最终的路径。

如何寻找最近的凸多边形

为了提升查找效率，我们利用所有的凸多边形构建一颗BVH树。

BVH树的结构以及查找流程

这颗BVH树的特点有：

1) 根节点的包围盒包含左右子树的包围盒。

2) 叶子节点才存储凸多边形数据。

3) 划分左右子树的时候，选择最能均匀分隔凸多边形的坐标轴。

构建BVH树的代码如下所示：

A星算法确定路径的凸多边形序列

A星算法的关键概念：

F = G + H

G是初始顶点到当前凸多边形的真实代价。

H是启发式函数，表示当前凸多边形到终点的预估代价。

OpenList

待检查的凸多边形集合，利用F值作为排序key的最小堆。

CloseList

不会再被考虑的多边形集合。

以上图为例，整个A星寻路的流程如下图所示：

在算法迭代过程中，多边形的F值如何确定？

1) 多边形的G值 = parent凸多边形的G值 + 代表parent凸多边形的顶点到parent与该多边形公共边中点的欧几里得距离。这里选择顶点代表凸多边形的规则为:parent凸多边形与其本身公共边的中点。

2) 多边形的H值 = parent与该多边形公共边中点到终点的欧几里得距离。

算法迭代过程中，顶点所用的数据结构如下所示，cost代表起点到此所用的开销，total表示F值，pidx代表parent凸多边形，flags代表该点当前是在openList还是closeList中，id代表其所属的凸多边形。

漏斗算法平滑路径

整个算法的过程可以用下面这个图来描述。

起点A不仅作为漏斗的初始顶点，也作为漏斗的初始两个端口，此后两个端口不停地向公共边的两个端点移动。

漏斗左右端点继续移动，需要满足下面2个条件

1.移动端点后的边是朝向漏斗收缩的方向。

2.移动端点后的边没有跨过另外1条边。

如果移动端点后的边是朝向漏斗收缩的方向，但会跨过另外1条边。--- 此时将另外1个端点加入路径，并将其更新为新漏斗的顶点。

漏斗算法的相关代码如下：

至此，我们就找到1条起点到终点的平滑路径。

附录(常用到的数学知识)

点到线段的最短距离

r 的几何意义就是线段AC与线段AB的比，则C点坐标的计算公式如下：

XC=XA+(XB−XA)∗r

YC=YA+(YB−YA)∗r

相关代码如下：

点到多边形的最短距离

先计算点到多边形所有边的距离，取最小值。如果点在多边形内，则取负数。可以利用射线法判断点是否在多边形内，如果射线与多边形的奇数条边相交，则表示点在多边形内部。相关代码如下：

点到三角面的距离

当投影点P1在三角形外时，距离取FLT_MAX；当投影点P1在三角形内时，距离为点P与点P1的y坐标之差的绝对值。投影点P1在三角形内需要满足的条件为：

相关代码如下：