[unity3d]recast navigation navmesh 导航网格 寻路算法 源码分析

recast navigation navmesh导航网格算法源码分析

Author:  林绍川

recast navigation navmesh是unity3d ue4内置的寻路算法

本文为了方便,引用了一些网上的相关图片

图片出处:Recast源码解析（二）：NavMesh导航网格生成原理（上） - 程序员大本营 特此致谢

1 加载.obj文件

InputGeom::load--->InputGeom::loadMesh负责加载所有三角形

取obj文件中顶点数据,和顶点索引数据

1.1 obj的格式:

v  6.5 0.07999992 20.5

...(每行以v开头,连着三个浮点数,为一个顶点坐标)

f 1 2 3

...(每行以f开头,连着三个整数,为顶点索引)

1.2 三角形空间分割

rcCreateChunkyTriMesh,这个函数为之后建立高度场时,能快速索引出对应的三角形

按如下步骤:

1 此函数会为加载的三角形计算aabb盒(其实是xz坐标的包围矩形)

2 以aabb哪个轴长(x轴方向长),就按哪个轴进行排序,然按三角形数量1分为2

3 如此循环,直到划分的节点里三角形个数小于某个阀值(256)

2 建立高度场

结束加载obj,就要开始建立高度场

1 Recast navmesh是将场景中的三角形光栅光,形成高度场体素,

2 然后根据高度场计算layer,还生成导航多边形数据

为何不直接利用obj中的三角形数据,原因可能如下:

1 obj中的三角形数据可能细节更多,而导航寻路可以适当简化多边形细节,提高效率

2 寻路有对象宽度限制,体素化利于处理

3 obj中没有包含layer信息,需要处理

layer是recast navmesh里的一个概念,是导入obj里,一些独立,不连通的区域,或者不同的层,彼此之间寻路上不能互通,

或者通过port互通

此处以 Sample_TempObstacles::handleBuild为例,说明这一过程

这个sample支持动态创建障碍物,具体的处理方向如下步骤:

1 将高度场数据划分成tile,

2 障碍物改变与其相交的高度场体素,将处于障碍物(包围盒/或者圆柱体)中的体素标记为不可走

3 然后将受影响的tile重新计算,局部重新生成导航多边形数据

2.1 rasterizeTileLayers光栅化tile内三角形

光栅化场景中的三角形,即用一个正方形单元(xz平面),对场景中三角形数据进行采样高度数据,此处借用网上的一个图,形象表示如下:

图1

对图中的每个小方格,以下会以span这一称呼来代替,

一个span是xz大小固定的单元格,在y方向不固定

(根据光栅光时,截取到的多边形高范围,记录ymax ymin)

2.2 划分tile

Sample_TempObstacles为了动态重新开销减少,需要划分tile,

即将整个场景算出一个aabb,在xz平面上按固定大小分tile

每个tile内包含一个span体素数组

即tile是一个块, span则是一个最小的体素点,

二维的span组成一个tile

二维的tile组成一个场景

2.3 rasterizeTileLayers的参数 rcConfig

这个参数部分重要成员变量含义:

cs:

体素化的单位xz的量化(每个span在xz平面的长宽,为一个正方向)

ch:

 y方向的量化单位

walkableSlopeAngle:

可攀爬的角度(0-90度),用来判断一个span是否可以行走(太斜了,就不能行走)

walkableHeight:

寻路单位的高度,高度不够,不能走, 例如:一个身高两米的人,不能通过只有1米高的门框之类的

walkableClimb:

同上,高度差小于此值,认为两个span是可以行走的

walkableHeight walkableClimb会被ch量化,例如:

ch=0.5  walkableHeight=2.0  那么 walkableHeight = walkableHeight / ch,

最终walkableHeight =4;

walkableRadius:

可行走的宽度,即一个太窄,狭小的地方认为不能走

walkableRadius会被cs量化

例如:

cs = 0.5  walkableRadius = 2.0

walkableRadius = walkableRadius / cs 即walkableRadius = 4

borderSize:

为每个tile多准备一些span数据,相当于边框,值会设成比walkableRadius大,避免把一些span错误置成不可行走

2.4 rcGetChunksOverlappingRect

此函数比较简单,计算当前tile范围,将tile包含的三角形找出来

2.5 rcMarkWalkableTriangles

依据walkableSlopeAngle,根据法线,判断哪些三角形不能走

2.6 rcRasterizeTriangles

对每个三角形调用rasterizeTri光栅化(或者说体素化),

形成一个高度场,(x,z)单元格,一个格子对应一个链表,链表里的数据按高度从小到大排列

2.7 rasterizeTri

对单个三角形进行光栅化(见图1)

在xz平面,对三角形进行分割,

循环调用dividePoly,先平行x轴模向切割, 再平行z轴纵切

2.7.1 dividePoly

分割多边形,以沿z轴平行分割为例,有如下 图2 图3两种情况

 图2

 图3

假设蓝色的那条边是要被分割的线,该函数会计算线段两个端点A,B的z坐标与分割线的z坐标差值

如果是图2的情况:

1 如果处理的点是A记入上半个多边形顶点,是B记入下半个

(根据d[i]的符号判断点是在A位置,还是在B位置)

2 交点分别计下上下两个多边形顶点

如果是图3的情况:

点A记入上半个多边形顶点(或者下半个多边形顶点,根据d[i]的符号)

代码片段,记算d[i]

体素化分割截取形象表示如下网图

三角形被x轴z轴的平行线切割后形成的多边形,取该多边形在y轴的最大值与最小值,生成span,

即上面所提到的图1,再次贴如下:

所对应的代码:

2.7.2 addSpan

这个函数会做一个处理,所有的span链表都是按高度从低到高排序的, span有多个邻居时,取高度最高的,

原因如下图:

2.7.3 最终高度场

到此,完成了每个tile的高度场建立

参考网上找到的示意图,高度场形象表示如下:

每个span xz长度固定, y方向包含一个最大值最小值, 同一个xz坐标会有多个span,

同一个xz坐标处的所有span形成一个链表,并且按高度排序(从小到大)

2.7.4  rcFilterLowHangingWalkableObstacles

根据同一个xz坐标不同span之间,如果它们的距离不walkableClimb,并且下面的span可走,则把上面的span也标记成可走, 即处理台阶楼梯之类的情形

2.7.5  rcFilterLedgeSpans

和周围的span比较,"空洞"的高度差是否大于阀值,不大于阀值,说明不够高,

4个方向都不满足,该span也会被判断定为不能走,

例如:该span周围被墙包围,

如图所示的红线部分,rcFilterLedgeSpans这个函数,判断空洞交叠部分(红线长度)是否大开于路单位的高度walkableHeight

2.7.6  rcFilterWalkableLowHeightSpans

判断当前格子所形成的空间,是否足够高(>walkableHeight),不够高也判断为不可走

如下图所示,同一个xz坐标上,一系列的span,它们之间的高度差要足够

致此,完成了所有span能不能行走的标记

2.8 rcBuildCompactHeightfield

与周围的span进行比较,是否高度足够,并且高度差是可以走的,形成连接信息

即每个span会记录与周围前后左右四个邻是否连通

如果当前坐标为xz邻居方向dir值

0:表示x-1的span

1:表示z+1的span

2:表示x+1的span

3:表示z-1的span

Recast navmesh邻居定义图示如下:

2.9 rcErodeWalkableArea

Erode的中文含义为侵蚀,即将障碍物周围可行走区域按radius值适当扩散不可行走区域,目的是寻路目标贴墙走时,留出一定的宽度

在不可行走的区域周围,标记格子g(格子的邻居存在不可行走区域)

格子g与障碍的距离dis标记为0(dis默认为0xff)

继续处理,标记出所有格子与障碍物的距离,根据格子的可连通邻居dis值

根据dis值与寻路物体宽度值radius比较,小则将该区域也标记成不可行走

如下图所示,可行走区域要收缩一些,为寻路单位留出walkableRadius

2.10 rcMarkConvexPolyArea

这个是动态障碍用的,把一些span标记成不可行走

函数很简单,不影响对主要代码的理解,不详细说明了

2.11 rcBuildHeightfieldLayers

完成了高度场"扁平化",

每个layer

rcHeightfieldLayer::area： 保存是否可行走

rcHeightfieldLayer::heights：高度

rcHeightfieldLayer::cons：连通性(低4bit)+是否为layer之间的通道(高4bit)

以下说明该函数的执行过程：

2.11.1 找出连通区域

互相连接的span会形成一个区域,形成区域的规则如下图所示: (*每个代表一个span)

对应的部分代码片段截图

2.11.2 相邻的连通区域合并成一个区域

2.11.2.1 根据区域内span的邻居的区域id,为每个区域添加邻居

  部分代码片段如下

2.11.2.2 同时根据span的链表,可以查当该span同一个xz坐标处的其它layer,记录下

  形象图示如下

对应的代码片段

2.11.2.3 把所有互相连接的连通区域的打上同一个layerId, 排除掉自身的"不同层" 邻居的"不同层"

即:区域之间根据邻接关系互联的时候, 不同的层不能是同一个区域, 邻居的不同层也不能进入同一个区域

这样处理是为了扁平化高度场,区分出不同的层,

将3d的场景,转化成不同的层级的2d场

同时对于不连接的区域, 为了减少区域数量,对于高度差小于阀值的区域之间,也会进入合并处理

(不理解可以跳过此段代码处理,对整个逻辑理解不影响,是一个优化操作)

该优化部分代码片段如下:

2.11.2.4 扁平化的高度场数据保存

见2.11处

每个layer保存

rcHeightfieldLayer::area： 保存是否可行走

rcHeightfieldLayer::heights：高度

rcHeightfieldLayer::cons：连通性(低4bit)+是否为layer之间的通道(高4bit)

3 buildNavMeshTilesAt从压缩的高度场中,重建导航数据

大致过程如下:

1 扁平化高度场数据会被压缩,解压高度场数据

2 重建导航数据是逐个tile进行,

3  tile先按区域进行边缘检测,得到多边形

4 将多边形分解成三角形

5 将三角形进行适当合并(保证合并后的多边形是凸的)

6 记录tile多边形邻接信息

7  tile之间检测多边形边交叠情况,记录连接信息

下面进行详细说明

3.1 标记动态障碍

dtMarkCylinderArea

dtMarkBoxArea

dtMarkBoxArea

动态障碍物标记,改变与障碍物相交的area(可以理解为前文提到的span)标记为不可行走
对代码理解不影响,可以跳过

3.2 dtBuildTileCacheRegions 再次生成区域数据

部分逻辑参考 2.11 rcBuildHeightfieldLayers

1 将已经扁平化的高度场数据重新生成区域,

2 相信的区域组合成一个区域

原因:因为动态障碍支持,不能从解压后的数据直接取结果

canMerge函数比较难以理解,做个简要说明:

1 oldRegId邻居中有一个regId为newRegId,可以合并

2  count=0:表时newRegId并不在oldRegId邻居

3  count>=2:说明oldRegId已与别的区域合并,不能再newRegId合并,否则会导致合并好的区域又分裂了

4 这样处理,保证合并区域不断变大

可以自行画三个相邻的区域,按代码来理解canMerge的含义

因为数据已经扁平化,这里就没有扁平化处理了

3.3 dtBuildTileCacheContours 轮廓处理

dtTileCacheContour::verts:记录的轮廓 (即多边形的顶点)

dtTileCacheContour::reg:区域id

dtTileCacheContour::area可行走标识

多边形的点的顺时针记录的

以下进行详细说明

3.3.1 walkContour

遍历tile中所有xz坐标,

从每个合法的点area(即span)查找边缘

每个点与周围4个邻点,如果所处的reg不一样,则认为是边界点

4个邻点标号如下图所示,从3号位开始比对

#表示其它区域的点 1.2.3.4表示当前要查找的轮廓边缘点, 如果出现x(或z)坐标相同的点,则会把中间的点舍弃

以上图为例从1点开始

1. 点1的三号位,即点1的下方提#所以点1是边缘点

2. 按代码逻辑,x,z不变,然后顺时针旋转,即dir=0,代码片段如下,可以看出当处理3号位时,只改变dir

 <代码片段1>

3. 此时判断点1与点2,发现它们是同一个区域的,发现它们是同一区域,此时会把dir逆时针转回去

  即dir=0,同时x,z坐标变成点2,代码片段如下

4. 点2的下方是#,说明点2是边缘点,dir这时又顺时针转,即dir=0,同时x,z还是点2的,见<代码片段1>的逻辑

5. 点2的左边(dir=0)是不同区域,将z坐标加1(见<代码片段1>),到点3,同时dir=1,

    因为左下角是不同区域点3也算边缘点

6. 点3的上方(dir=1)是同一个区域,把xz坐标切到点4,并逆时针处理dir=0

7. 点4的左边(dir=1)是同不区域,点4是边缘点

8. 就这样不断地旋,切坐标,把轮廓找出来,当回到点1 时,说明找到了完整的轮廓,判断代码如下

  

9. 可以看出点是按顺时针存储的

以下4图片形象说明了这一过程:

3.3.2 simplifyContour简化轮廓

将轮廓分段,如果该段内的点偏离段小于阀值,合并这些点

采用Douglas-Peucker算法

1 按顺序存储的一系列顶点,将首尾两点(记为V0和Vn)连成线段

2 计算中间各点与该线段的距离,如果距离小于某个阀值忽略,大于阀值的记录下来,这个点我们记录Vmaxd

3  Vmaxd与V0 Vn分别组成两条线段 V0-Vmaxd  Vmaxd-Vn,分别对这两条线段重复第2步骤,

4 得到一系列线段,并且没有中间点与对应线段距离大于阀值,结束处理过程

形象图示如下: 此图出处 Douglas-Peucker算法 - qingsun_ny - 博客园

3.3.3  getCornerHeight

如图所示的蓝点,如果轮廓中有这样的占,要去除,是一种容错机制,

不好理解可以跳过,不影响后面的代码理解

3.3.4 工具函数介绍

distancePtSeg 计算点到线段的距离,根据向量点乘算出垂足坐标,再计算点与垂足的距离即可

代码片段截取如下:

3.4 dtBuildTileCachePolyMesh 生成邻接多边形

navmesh导航算法需要确保多边形是凸的,此函数负责生成凸多边形,并生成邻接信息

因为高度场已经扁平化,所以这个函数的操作都是在xz平面上行进的

1 先把之前得到的多边形轮廓(有可能是凹的),分割成三角形

2 再把三角形组合成多边形(确保是凸的)

3 处理多边形之间的邻接关系

以下详细说胆此函数

3.4.1 triangulate

将多边形(有可能是凹的)分割成三角形

diagonal

bool diagonal(int i, int j, int n, const unsigned char* verts, const unsigned short* indices)

i和j分别为多边形的顶点(索引),此函数判断顶点i 顶点j的连线能否作为多边形的对角线

用图表示如下:

diagonal调用了一些工具函数,以下做一些简要说明

工具函数 leftOn  left  area2  uleft

这四个函数大同小异

利用向量的叉积(即两个向量的夹角sin值),判断两个向量的方向关系

以leftOn为例向量V1 =b-a  V2=c-a

V1 V2的叉积:

=0时 表示夹角为零(用于共线判断)

>0时 表示V1转到V2是顺时针,如下图:

<0时 表示V1转到V2是逆时针, 此时V1 V2与上图相反

leftOn代码片段截取如下:

返回0:表示 a b c 三点共线

返回<0 表示 c 在向量ab的左边

返回>0 表示 c 在向量ab的右边

用右手,绕序abc,

大拇指朝上: c在ab右

大拇指朝下: c在ab左

工具函数inCone

此函数利用工具函数leftOn left保证一系列4个点是凸的

代码片段如下:

工具函数diagonalie

此函数排除与多边线相交的对角线,如下图

工具函数intersect

此函数判断两个线段是否相交

bool intersectProp(const unsigned char* a, const unsigned char* b, const unsigned char* c, const unsigned char* d)

这个函数利用left工具函数判断

1 点a和点b在线段cd的两边

2 点c和点d在线段ab的两边

如果满足1,2这两个条件,则说明线相交

bool between(const unsigned char* a, const unsigned char* b, const unsigned char* c)

利用area2判断是否共线,即叉积为0

在共线的情况下,判断端点是否交叠,交叠才能算相交

如果下图所示,要排除红蓝这种共线的方式,因为没有交叠

致此,triangulate函数中找合法对角线相关的函数大部分介绍完毕

切割三角形

根据合法的对象线,从多边形中切割出三角形

每次循环找出符合条件的对角线(程序中是长度最小)

根据对角线,把对应的顶点分离出去,形成一个三角形, 剩余的点形成新的多边形

多边形改变了,对受影响的点重新调用diagonal,计算对角线的合法性

如下图所示,沿着绿色虚线,切出三角形,

反复循环之后,多边形变成三角形,如下

致此,完成多边形三角化

3.4.2 合并三角形为凸多边形

好像不合并也行,三角形一定是凸的,合并后数据更少,执行效率更优

getPolyMergeValue

判断两个多边形是否有共同的边,

并且保证合并后是凸的,

返回值是公共边的长度

利用uleft来判断合并后是否是凸的,公共边的两个端点处都要判断,如下图:

代码片段如下:

mergePolys

合并多点边形,这个函数比较简单,复制数据,保证点的绕序,代码与相应的注释说明如下:

3.4.3  buildMeshAdjacency

生成多边形邻接信息

1 首先将所有的边,按顶点索引为key建立类哈希表

2 对具有相同顶点的边进行判断,是否为公共边

3 边连接

4  port连接

简单说一下有port连接,其实和普通的边连接是一样的

如下图,  根据扁平化规则, 黑色线处(水平的黑色和斜的黑色)的span和蓝色线处的span会被划分至不同layer

可回看addSpan  假设黑色斜的那一部分,在蓝色处形成投影,

投影边缘的那些点(假设是蓝色面的边线),

如果蓝色面周围有其它span邻居(假设是图中的红色,并且红色与黑色layer是同一个layer)

蓝色面的边缘span会变成port,并形成port连接

即port和 普通边连接可以认为不共存

部分代码片段如下:

如果是边连接,存放对应多边形的数组下标

如果是port,则要额外打个0x8000的标记,存port的方向(0,1,2,3)

这些数据会dtCreateNavMeshData里做转换

Port(0,1,2,3,4)会转成 4,2,0,6

3.4.4 dtNavMesh::addTile

将数据添加进tile,这一步要处理tile之间的连接

即不同的tile,要处理连缘处的多边形的连通情况,便于tile之间导航寻路

如果只是想了解Sample_SoloMesh的业务逻辑,可以跳过本段

connectIntLinks

这个函数负责生成tile之内多边形邻居信息生成(主要是相邻多边形的公共边)

findConnectingPolys

1 此函数负责查找tile之间的多边形连接情况

这种情况,记录的不是公共边,而是多边形相互靠近的两条边,它们的交叠部分记录下来

2 此函数也负责tile之间,不同层之间的连通情况,即因为port产生的连接

  代码片段如下:

Port可以认为是高一层的layer在低上层的layer上投影的边缘的那些点

记录相邻边交叠部分的代码

致此,导航需要多边形邻接信息生成完毕,

之后就是利用这些信息进行寻路

4 寻路

4.1 dtNavMeshQuery::findPath   网格寻路

此函数没有太多难懂的地方

按网格邻接查找网格路径,类A*算法,按多边形中心点的距离估算cost

m_openList:是一个堆(极小堆),每个弹出一个cost最优的节点进行循环迭代

4.2 dtNavMeshQuery::findStraightPath  寻找拐点

得到一系列相邻的多边形路径, 如何穿过这些多边形, 需要此函数处理,即需要知道要在哪些地方拐弯

4.2.1 寻路初始

 如下图所示,

从红点出发,要走到蓝点

用红色标出每个多边形的邻接边:

4.2.2 射线包络

对第一条邻边,从起始的红点,与邻接边的端点,发出一条射线(图中的紫线)

对第二条邻边也做同样的两条射线(图中的黄线)

两条紫射线为一组,两条黄射线为一组,取它们的交集,

以下图为例,交集显示是两条黄射线包络的那部分,

如下图,处理到第三条邻边时的最小包络射线

4.2.3 拐点出现的情况

对之后的每条邻边重复上面的循环,直到出现没有交集的情况

检测到拐点(粉红的那个点),

如下图,此例在红色邻边的两个端点,此时与紫色的包络线已经没有交集了

4.2.4 寻路结束

然后以粉色点为新的起点,重复以上过程,就可以找到一条在多边形内部的路径,

即最终的路径会是从红点走到粉点,再从粉点走到蓝色

4.2.5 结束语

致此,完成了导航网格生成与寻路功能说明

====================== 分割线 ============================

4.3 一些工具函数简介

寻路过程,使用了一些工具函数,以下做一个简要说明

dtDistancePtPolyEdgesSqr / dtPointInPolygon

利用射线与多边形的交点是奇偶判断是否在多边形内,

看代码射线应该是平行x轴往x轴正方向

代码片段如下:

dtDistancePtSegSqr2D

求点到线段的距离平方

dtTriArea2D

recast里有很多函数功能相似,此函数参考 leftOn

利用差乘判断线段与点的位置关系

getPortalPoints

获多边形邻边的两个端点(如果是tile间的连接或者port,取出交叠的首尾)

dtClosestHeightPointTriangle

利用射线(特殊射线,垂与xz平面的)与三角形的交点计算方法,求出射线与三角形的交点

三角形(p1 p2 p3)内部的顶点可以看到是

向量v1 = p3 - p1;

向量v2 = p2 - p1;

内部点 px = p1 + u*v1 + v*v2;

等同于: px = p1*(1-u-v) + p3*u + p2*v;

另外 px = O + d*t   O:射线起点 d:射线方向 t:射线"长度"

O + d*t = p1*(1-u-v) + p3*u + p2*v; 这样可以得到一个三元一次方程组 未知数为(t u v)

根据克莱默法则解这个方组即可t u v   代入px = O + d*t即可得到对应的px坐标

同时,这个函数dtClosestHeightPointTriangle的射线是特殊的,垂直于xz平面