导航
3D世界如何寻路,导航寻路RecastNavigation解析(上)
3D世界如何寻路,导航寻路RecastNavigation解析(下)
构建区域
这次主要说下分水岭算法
我们把距离场看成到水平面的距离(最高就到水平面),每次灌入一些水,做一些标记
当水达到水平线时我们已经把所有区域都做好了标记
bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,const int borderSize, const int minRegionArea, const int mergeRegionArea)
{
......
}
上次我们构建距离场,知道了最大到边界的距离maxDistance
unsigned short level = (chf.maxDistance+1) & ~1;
设置起始level,把maxDistance
变成偶数,然后每次处理2个级别的
一次处理NB_STACKS
=8个堆栈的数据
const int LOG_NB_STACKS = 3;
const int NB_STACKS = 1 << LOG_NB_STACKS;
核心2个函数expandRegions
和floodRegion
floodRegion
灌水开始,找到当前level
所有CompactSpan
灌满水,给这个区域的水分配一个新的regionId
继续往里面灌水,水位开始上升,level每次减少2
expandRegions
根据打上标记的regionId
,水开始向四周蔓延水位,标记符合level
的的CompactSpan
int sId = -1;
while (level > 0)
{
level = level >= 2 ? level-2 : 0;
sId = (sId+1) & (NB_STACKS-1);
// ctx->startTimer(RC_TIMER_DIVIDE_TO_LEVELS);
if (sId == 0)
sortCellsByLevel(level, chf, srcReg, NB_STACKS, lvlStacks, 1);
else
appendStacks(lvlStacks[sId-1], lvlStacks[sId], srcReg); // copy left overs from last level
// ctx->stopTimer(RC_TIMER_DIVIDE_TO_LEVELS);
{
rcScopedTimer timerExpand(ctx, RC_TIMER_BUILD_REGIONS_EXPAND);
// Expand current regions until no empty connected cells found.
expandRegions(expandIters, level, chf, srcReg, srcDist, lvlStacks[sId], false);
}
{
rcScopedTimer timerFloor(ctx, RC_TIMER_BUILD_REGIONS_FLOOD);
// Mark new regions with IDs.
for (int j = 0; j<lvlStacks[sId].size(); j++)
{
LevelStackEntry current = lvlStacks[sId][j];
int x = current.x;
int y = current.y;
int i = current.index;
if (i >= 0 && srcReg[i] == 0)
{
if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack))
{
if (regionId == 0xFFFF)
{
ctx->log(RC_LOG_ERROR, "rcBuildRegions: Region ID overflow");
return false;
}
regionId++;
}
}
}
}
}
// Expand current regions until no empty connected cells found.
expandRegions(expandIters*8, 0, chf, srcReg, srcDist, stack, true);
构建轮廓
标记边界
给每个CompactSpan
设置一个标记flag
,用于判断上下左右是否同一区域
如果四周都是同一区域,二进制表示1111 ,最后再取反,四周都是一样的reg
最后结果就是0,不是边界
只要有一个不属于同一区域,结果就会大于0,属于边界
// Mark boundaries.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
unsigned char res = 0;
const rcCompactSpan& s = chf.spans[i];
if (!chf.spans[i].reg || (chf.spans[i].reg & RC_BORDER_REG))
{
flags[i] = 0;
continue;
}
for (int dir = 0; dir < 4; ++dir)
{
unsigned short r = 0;
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(dir);
const int ay = y + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
r = chf.spans[ai].reg;
}
if (r == chf.spans[i].reg)
res |= (1 << dir);
}
flags[i] = res ^ 0xf; // Inverse, mark non connected edges.
}
}
}
有数字代表flag
的值,是边界
连线轮廓
static void walkContour(int x, int y, int i,
const rcCompactHeightfield& chf,
unsigned char* flags, rcIntArray& points)
{
// Choose the first non-connected edge
unsigned char dir = 0;
while ((flags[i] & (1 << dir)) == 0)
dir++;
unsigned char startDir = dir;
int starti = i;
const unsigned char area = chf.areas[i];
int iter = 0;
while (++iter < 40000)
{
if (flags[i] & (1 << dir))
{
// Choose the edge corner
bool isBorderVertex = false;
bool isAreaBorder = false;
int px = x;
int py = getCornerHeight(x, y, i, dir, chf, isBorderVertex);
int pz = y;
switch(dir)
{
case 0: pz++; break;
case 1: px++; pz++; break;
case 2: px++; break;
}
int r = 0;
const rcCompactSpan& s = chf.spans[i];
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(dir);
const int ay = y + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
r = (int)chf.spans[ai].reg;
if (area != chf.areas[ai])
isAreaBorder = true;
}
if (isBorderVertex)
r |= RC_BORDER_VERTEX;
if (isAreaBorder)
r |= RC_AREA_BORDER;
points.push(px);
points.push(py);
points.push(pz);
points.push(r);
flags[i] &= ~(1 << dir); // Remove visited edges
dir = (dir+1) & 0x3; // Rotate CW
}
else
{
int ni = -1;
const int nx = x + rcGetDirOffsetX(dir);
const int ny = y + rcGetDirOffsetY(dir);
const rcCompactSpan& s = chf.spans[i];
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const rcCompactCell& nc = chf.cells[nx+ny*chf.width];
ni = (int)nc.index + rcGetCon(s, dir);
}
if (ni == -1)
{
// Should not happen.
return;
}
x = nx;
y = ny;
i = ni;
dir = (dir+3) & 0x3; // Rotate CCW
}
if (starti == i && startDir == dir)
{
break;
}
}
}
先看2d效果
白线就是算出的轮廓,查找方向是从左上右下,dir={0,1,2,3}
,顺时针方向查找
并且会记录连接区域的reg,这里有个规则
根据dir=0
左 记录上
的reg
根据dir=1
上 记录右上
的reg
根据dir=2
右 记录右
边的reg
根据dir=3
下 记录当前
的reg
并且记录类型是RC_AREA_BORDE
和RRC_BORDER_VERTEX
,也可以二者都是
这样就可以找到哪个区域是连通的
简化轮廓
开始节点,分2中情况
轮廓有
连接其他轮廓 记录所有的连接点
轮廓无
链接其他轮廓 记录左下
和右上
2个点
遍历整个列表,每2个点组成一条线A,B,查找这条线A为起点,B为终点的所有点
找到C点,是距离AB线段最长的那条线DistancePtSeg
判断是否大于maxError*maxError
,大于则添加这个点
如果某条线长度大于maxEdgeLen*maxEdgeLen
,还去需要拆分,找到2条线中间点添加
Ok,简化完成
static void simplifyContour(rcIntArray& points, rcIntArray& simplified,
const float maxError, const int maxEdgeLen, const int buildFlags)
{
// Add initial points.
bool hasConnections = false;
for (int i = 0; i < points.size(); i += 4)
{
if ((points[i+3] & RC_CONTOUR_REG_MASK) != 0)
{
hasConnections = true;
break;
}
}
if (hasConnections)
{
// The contour has some portals to other regions.
// Add a new point to every location where the region changes.
for (int i = 0, ni = points.size()/4; i < ni; ++i)
{
int ii = (i+1) % ni;
const bool differentRegs = (points[i*4+3] & RC_CONTOUR_REG_MASK) != (points[ii*4+3] & RC_CONTOUR_REG_MASK);
const bool areaBorders = (points[i*4+3] & RC_AREA_BORDER) != (points[ii*4+3] & RC_AREA_BORDER);
if (differentRegs || areaBorders)
{
simplified.push(points[i*4+0]);
simplified.push(points[i*4+1]);
simplified.push(points[i*4+2]);
simplified.push(i);
}
}
}
if (simplified.size() == 0)
{
// If there is no connections at all,
// create some initial points for the simplification process.
// Find lower-left and upper-right vertices of the contour.
int llx = points[0];
int lly = points[1];
int llz = points[2];
int lli = 0;
int urx = points[0];
int ury = points[1];
int urz = points[2];
int uri = 0;
for (int i = 0; i < points.size(); i += 4)
{
int x = points[i+0];
int y = points[i+1];
int z = points[i+2];
if (x < llx || (x == llx && z < llz))
{
llx = x;
lly = y;
llz = z;
lli = i/4;
}
if (x > urx || (x == urx && z > urz))
{
urx = x;
ury = y;
urz = z;
uri = i/4;
}
}
simplified.push(llx);
simplified.push(lly);
simplified.push(llz);
simplified.push(lli);
simplified.push(urx);
simplified.push(ury);
simplified.push(urz);
simplified.push(uri);
}
// Add points until all raw points are within
// error tolerance to the simplified shape.
const int pn = points.size()/4;
for (int i = 0; i < simplified.size()/4; )
{
int ii = (i+1) % (simplified.size()/4);
int ax = simplified[i*4+0];
int az = simplified[i*4+2];
int ai = simplified[i*4+3];
int bx = simplified[ii*4+0];
int bz = simplified[ii*4+2];
int bi = simplified[ii*4+3];
// Find maximum deviation from the segment.
float maxd = 0;
int maxi = -1;
int ci, cinc, endi;
// Traverse the segment in lexilogical order so that the
// max deviation is calculated similarly when traversing
// opposite segments.
if (bx > ax || (bx == ax && bz > az))
{
cinc = 1;
ci = (ai+cinc) % pn;
endi = bi;
}
else
{
cinc = pn-1;
ci = (bi+cinc) % pn;
endi = ai;
rcSwap(ax, bx);
rcSwap(az, bz);
}
// Tessellate only outer edges or edges between areas.
if ((points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0 ||
(points[ci*4+3] & RC_AREA_BORDER))
{
while (ci != endi)
{
float d = distancePtSeg(points[ci*4+0], points[ci*4+2], ax, az, bx, bz);
if (d > maxd)
{
maxd = d;
maxi = ci;
}
ci = (ci+cinc) % pn;
}
}
// If the max deviation is larger than accepted error,
// add new point, else continue to next segment.
if (maxi != -1 && maxd > (maxError*maxError))
{
// Add space for the new point.
simplified.resize(simplified.size()+4);
const int n = simplified.size()/4;
for (int j = n-1; j > i; --j)
{
simplified[j*4+0] = simplified[(j-1)*4+0];
simplified[j*4+1] = simplified[(j-1)*4+1];
simplified[j*4+2] = simplified[(j-1)*4+2];
simplified[j*4+3] = simplified[(j-1)*4+3];
}
// Add the point.
simplified[(i+1)*4+0] = points[maxi*4+0];
simplified[(i+1)*4+1] = points[maxi*4+1];
simplified[(i+1)*4+2] = points[maxi*4+2];
simplified[(i+1)*4+3] = maxi;
}
else
{
++i;
}
}
// Split too long edges.
if (maxEdgeLen > 0 && (buildFlags & (RC_CONTOUR_TESS_WALL_EDGES|RC_CONTOUR_TESS_AREA_EDGES)) != 0)
{
for (int i = 0; i < simplified.size()/4; )
{
const int ii = (i+1) % (simplified.size()/4);
const int ax = simplified[i*4+0];
const int az = simplified[i*4+2];
const int ai = simplified[i*4+3];
const int bx = simplified[ii*4+0];
const int bz = simplified[ii*4+2];
const int bi = simplified[ii*4+3];
// Find maximum deviation from the segment.
int maxi = -1;
int ci = (ai+1) % pn;
// Tessellate only outer edges or edges between areas.
bool tess = false;
// Wall edges.
if ((buildFlags & RC_CONTOUR_TESS_WALL_EDGES) && (points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0)
tess = true;
// Edges between areas.
if ((buildFlags & RC_CONTOUR_TESS_AREA_EDGES) && (points[ci*4+3] & RC_AREA_BORDER))
tess = true;
if (tess)
{
int dx = bx - ax;
int dz = bz - az;
if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen)
{
// Round based on the segments in lexilogical order so that the
// max tesselation is consistent regardless in which direction
// segments are traversed.
const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
if (n > 1)
{
if (bx > ax || (bx == ax && bz > az))
maxi = (ai + n/2) % pn;
else
maxi = (ai + (n+1)/2) % pn;
}
}
}
// If the max deviation is larger than accepted error,
// add new point, else continue to next segment.
if (maxi != -1)
{
// Add space for the new point.
simplified.resize(simplified.size()+4);
const int n = simplified.size()/4;
for (int j = n-1; j > i; --j)
{
simplified[j*4+0] = simplified[(j-1)*4+0];
simplified[j*4+1] = simplified[(j-1)*4+1];
simplified[j*4+2] = simplified[(j-1)*4+2];
simplified[j*4+3] = simplified[(j-1)*4+3];
}
// Add the point.
simplified[(i+1)*4+0] = points[maxi*4+0];
simplified[(i+1)*4+1] = points[maxi*4+1];
simplified[(i+1)*4+2] = points[maxi*4+2];
simplified[(i+1)*4+3] = maxi;
}
else
{
++i;
}
}
}
for (int i = 0; i < simplified.size()/4; ++i)
{
// The edge vertex flag is take from the current raw point,
// and the neighbour region is take from the next raw point.
const int ai = (simplified[i*4+3]+1) % pn;
const int bi = simplified[i*4+3];
simplified[i*4+3] = (points[ai*4+3] & (RC_CONTOUR_REG_MASK|RC_AREA_BORDER)) | (points[bi*4+3] & RC_BORDER_VERTEX);
}
}