背景
RecastNavigation是一个开源的寻路库,UE中的寻路便是用的这个库,本文使用的UE版本是4.26。
看过RecastNavigation的同学应该知道,NavMesh是由多个Tile组成,而每个Tile又由多个Poly组成。
进行一次寻路时,对于输入起点StartPos与终点EndPos,
会先在NavMesh上找到离起点最近的StartPoly,与终点最近的EndPoly(将输入点映射到NavMesh上),只要其中一个点映射不到,则寻路直接返回失败。
而查找是有一个范围的,也就是Vector Extent参数,这个参数表示了查找范围盒子(长方体)的半径,
问题便出现在查询盒子与邻近Poly求交的过程中。
功能需求
客户端同学使用NavMesh对NPC的移动范围进行约束,不希望NPC跑到NavMesh范围外,但实际使用时发现对于NavMesh外一定距离的目标点(超过Extent),也能匹配到EndPoly,并正常寻路。
Debug时还发现,StartPoly和EndPoly是同个Poly。
观察了案发现场的NavMesh,其NavMesh形状、起点及终点大概如下图所示,
红线区域代表NavMesh,A代表起点,B代表终点,绿框代表查询盒子。
显然这个绿框没有跟NavMesh相交,那点B为什么会映射到NavMesh上。
那就只能看下是怎么对点跟Poly进行求交。
求交
- 顺着逻辑来到dtNavMeshQuery::findNearestPoly,函数功能顾名思义,
寻找center(目标点) extents(查找盒子半径)范围内最近的Poly。
dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents,
const dtQueryFilter* filter,
dtPolyRef* nearestRef, float* nearestPt,
const float* referencePt) const
{
dtAssert(m_nav);
*nearestRef = 0;
// Get nearby polygons from proximity grid.
dtPolyRef polys[128];
int polyCount = 0;
if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, 128)))
return DT_FAILURE | DT_INVALID_PARAM;
……
}
- 进入dtNavMeshQuery::queryPolygons,此处将center和extents参数构建成查询盒子,获得查询盒子覆盖的Tile范围,查询该范围内的Poly。
dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents,
const dtQueryFilter* filter,
dtPolyRef* polys, int* polyCount, const int maxPolys) const
{
dtAssert(m_nav);
float bmin[3], bmax[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
// Find tiles the query touches.
int minx, miny, maxx, maxy;
m_nav->calcTileLoc(bmin, &minx, &miny);
m_nav->calcTileLoc(bmax, &maxx, &maxy);
ReadTilesHelper TileArray;
int n = 0;
for (int y = miny; y <= maxy; ++y)
{
for (int x = minx; x <= maxx; ++x)
{
int nneis = m_nav->getTileCountAt(x,y);
const dtMeshTile** neis = (const dtMeshTile**)TileArray.PrepareArray(nneis);
m_nav->getTilesAt(x,y,neis,nneis);
for (int j = 0; j < nneis; ++j)
{
n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n);
if (n >= maxPolys)
{
*polyCount = n;
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
}
}
}
}
*polyCount = n;
return DT_SUCCESS;
}
此处的calcTileLoc函数中,也能看到NavMesh生成参数TileWidth的影响,
过大的TileWidth会导致一次比较的Poly数量太多,findNearestPoly性能变差;
当然过小的TileWith则会导致Tile数量太多,也会造成路点数量增多、findPath性能变差等问题。
- 来到dtNavMeshQuery::queryPolygonsInTile
int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
const dtQueryFilter* filter,
dtPolyRef* polys, const int maxPolys) const
{
…………
if (tile->bvTree)
{
const dtBVNode* node = &tile->bvTree[0];
const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
const float* tbmin = tile->header->bmin;
const float* tbmax = tile->header->bmax;
const float qfac = tile->header->bvQuantFactor;
// Calculate quantized box
unsigned short bmin[3], bmax[3];
// dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
// Quantize
bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
// Traverse tree
const dtPolyRef base = m_nav->getPolyRefBase(tile);
int n = 0;
while (node < end)
{
const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
const bool isLeafNode = node->i >= 0;
if (isLeafNode && overlap)
{
dtPolyRef ref = base | (dtPolyRef)node->i;
if (filter->passFilter(ref, tile, &tile->polys[node->i]) && passLinkFilter(tile, node->i))
{
if (n < maxPolys)
polys[n++] = ref;
}
}
if (overlap || isLeafNode)
node++;
else
{
const int escapeIndex = -node->i;
node += escapeIndex;
}
}
return n;
}
…………
}
这里就只摘抄关键的一段代码,
看到这可以知道,NavMesh是使用 BVTree 对 Tile 进行空间管理,
判断 Poly 与 查询盒子 是否相交的条件就是 Poly包围盒 是否与 查询盒子 相交,
是,则存到dtPolyRef* polys数组中。
到这里,就有些明白为什么那么小的查询盒子能映射到NavMesh上,
来到场景的案发现场,将组成NavMesh的Poly显示出来,果然,这个位置的Poly包围盒与查询盒子相交了,
如下图所示,蓝色线框代表Poly的包围盒。
A本来就在该Poly上,B处查询盒子又与该Poly的包围盒相交,怪不得A和B都映射到同一个Poly上。
- 回到 dtNavMeshQuery::findNearestPoly,接着queryPolygons往下看,可以看到,
将刚才统计出来的Polys数组与输入点进行距离计算,找出最近Poly。
dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents,
const dtQueryFilter* filter,
dtPolyRef* nearestRef, float* nearestPt,
const float* referencePt) const
{
dtAssert(m_nav);
*nearestRef = 0;
// Get nearby polygons from proximity grid.
dtPolyRef polys[128];
int polyCount = 0;
if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, 128)))
return DT_FAILURE | DT_INVALID_PARAM;
//@UE4 BEGIN
float referenceLocation[3];
dtVcopy(referenceLocation, referencePt ? referencePt : center);
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestDistanceSqr = FLT_MAX;
for (int i = 0; i < polyCount; ++i)
{
dtPolyRef ref = polys[i];
float closestPtPoly[3];
closestPointOnPoly(ref, referenceLocation, closestPtPoly);
const float d = dtVdistSqr(referenceLocation, closestPtPoly);
const float h = dtAbs(center[1] - closestPtPoly[1]);
//@UE4 END
if (d < nearestDistanceSqr && h < extents[1])
{
if (nearestPt)
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = ref;
}
}
if (nearestRef)
*nearestRef = nearest;
return DT_SUCCESS;
}
if (d < nearestDistanceSqr && h < extents[1]) 这句揭开了这个bug的原因:
UE仅判断Poly最近点是否在查询盒子的垂直方向中,并没有判断水平方向是否在查询盒子范围内。
(官方RecastNavigation这里更是连垂直方向都没判断,仅作包围盒相交判断。)
修复
const float h = dtAbs(center[1] - closestPtPoly[1]);
const float dx = dtAbs(center[0] - closestPtPoly[0]);
const float dz = dtAbs(center[2] - closestPtPoly[2]);
if(dx > extents[0] || h > extents[1] || dz > extents[2])
continue;
修复很简单,加上水平方向的距离判断就行了。
还得多谢这个bug,对RecastNavigation的理解更进一步,
之前只走马观花看过一遍,只知道大概是用BVTree进行相交检测,都没注意实现细节。