diff options
Diffstat (limited to 'dep')
22 files changed, 2552 insertions, 1011 deletions
diff --git a/dep/recastnavigation/Detour/DetourCommon.h b/dep/recastnavigation/Detour/DetourCommon.h index ed7c5149db9..0888614ea9b 100644 --- a/dep/recastnavigation/Detour/DetourCommon.h +++ b/dep/recastnavigation/Detour/DetourCommon.h @@ -32,6 +32,11 @@ feature to find minor members. /// @name General helper functions /// @{ +/// Used to ignore a function parameter. VS complains about unused parameters +/// and this silences the warning. +/// @param [in] _ Unused parameter +template<class T> void dtIgnoreUnused(const T&) { } + /// Swaps the values of the two parameters. /// @param[in,out] a Value A /// @param[in,out] b Value B diff --git a/dep/recastnavigation/Detour/DetourNavMesh.cpp b/dep/recastnavigation/Detour/DetourNavMesh.cpp index 49100b09816..51740509950 100644 --- a/dep/recastnavigation/Detour/DetourNavMesh.cpp +++ b/dep/recastnavigation/Detour/DetourNavMesh.cpp @@ -297,6 +297,7 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, float amin[2], amax[2]; calcSlabEndPoints(va,vb, amin,amax, side); + const float apos = getSlabCoord(va, side); // Remove links pointing to 'side' and compact the links array. float bmin[2], bmax[2]; @@ -316,6 +317,13 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, const float* vc = &tile->verts[poly->verts[j]*3]; const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; + const float bpos = getSlabCoord(vc, side); + + // Segments are not close enough. + if (dtAbs(apos-bpos) > 0.01f) + continue; + + // Check if the segments touch. calcSlabEndPoints(vc,vd, bmin,bmax, side); if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue; @@ -334,9 +342,11 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, return n; } -void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side) +void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, dtMeshTile* target) { - if (!tile) return; + if (!tile || !target) return; + + const unsigned int targetNum = decodePolyIdTile(getTileRef(target)); for (int i = 0; i < tile->header->polyCount; ++i) { @@ -345,7 +355,8 @@ void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side) unsigned int pj = DT_NULL_LINK; while (j != DT_NULL_LINK) { - if (tile->links[j].side == side) + if (tile->links[j].side != 0xff && + decodePolyIdTile(tile->links[j].ref) == targetNum) { // Revove link. unsigned int nj = tile->links[j].next; @@ -376,20 +387,25 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) dtPoly* poly = &tile->polys[i]; // Create new links. - unsigned short m = DT_EXT_LINK | (unsigned short)side; +// unsigned short m = DT_EXT_LINK | (unsigned short)side; const int nv = poly->vertCount; for (int j = 0; j < nv; ++j) { // Skip non-portal edges. - if (poly->neis[j] != m) continue; + if ((poly->neis[j] & DT_EXT_LINK) == 0) + continue; + + const int dir = (int)(poly->neis[j] & 0xff); + if (side != -1 && dir != side) + continue; // Create new links const float* va = &tile->verts[poly->verts[j]*3]; const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3]; dtPolyRef nei[4]; float neia[4*2]; - int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(side), nei,neia,4); + int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(dir), nei,neia,4); for (int k = 0; k < nnei; ++k) { unsigned int idx = allocLink(tile); @@ -398,13 +414,13 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) dtLink* link = &tile->links[idx]; link->ref = nei[k]; link->edge = (unsigned char)j; - link->side = (unsigned char)side; + link->side = (unsigned char)dir; link->next = poly->firstLink; poly->firstLink = idx; // Compress portal limits to a byte value. - if (side == 0 || side == 4) + if (dir == 0 || dir == 4) { float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]); float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]); @@ -413,7 +429,7 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side) link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f); link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f); } - else if (side == 2 || side == 6) + else if (dir == 2 || dir == 6) { float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]); float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]); @@ -434,7 +450,7 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int // Connect off-mesh links. // We are interested on links which land from target tile to this tile. - const unsigned char oppositeSide = (unsigned char)dtOppositeTile(side); + const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side); for (int i = 0; i < target->header->offMeshConCount; ++i) { @@ -443,6 +459,9 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int continue; dtPoly* targetPoly = &target->polys[targetCon->poly]; + // Skip off-mesh connections which start location could not be connected at all. + if (targetPoly->firstLink == DT_NULL_LINK) + continue; const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; @@ -476,19 +495,19 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int // Link target poly to off-mesh connection. if (targetCon->flags & DT_OFFMESH_CON_BIDIR) { - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) + unsigned int tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) { const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[idx]; + dtLink* link = &tile->links[tidx]; link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); link->edge = 0xff; - link->side = (unsigned char)side; + link->side = (unsigned char)(side == -1 ? 0xff : side); link->bmin = link->bmax = 0; // Add to linked list. link->next = landPoly->firstLink; - landPoly->firstLink = idx; + landPoly->firstLink = tidx; } } } @@ -532,13 +551,13 @@ void dtNavMesh::connectIntLinks(dtMeshTile* tile) } } -void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) +void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) { if (!tile) return; dtPolyRef base = getPolyRefBase(tile); - // Find Off-mesh connection end points. + // Base off-mesh connection start points. for (int i = 0; i < tile->header->offMeshConCount; ++i) { dtOffMeshConnection* con = &tile->offMeshCons[i]; @@ -546,72 +565,109 @@ void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; - for (int j = 0; j < 2; ++j) - { - unsigned char side = j == 0 ? 0xff : con->side; + // Find polygon to connect to. + const float* p = &con->pos[0]; // First vertex + float nearestPt[3]; + dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); + if (!ref) continue; + // findNearestPoly may return too optimistic results, further check to make sure. + if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) + continue; + // Make sure the location is on current mesh. + float* v = &tile->verts[poly->verts[0]*3]; + dtVcopy(v, nearestPt); - if (side == 0xff) - { - // Find polygon to connect to. - const float* p = &con->pos[j*3]; - float nearestPt[3]; - dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); - if (!ref) continue; - // findNearestPoly may return too optimistic results, further check to make sure. - if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) - continue; - // Make sure the location is on current mesh. - float* v = &tile->verts[poly->verts[j]*3]; - dtVcopy(v, nearestPt); - - // Link off-mesh connection to target poly. - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - dtLink* link = &tile->links[idx]; - link->ref = ref; - link->edge = (unsigned char)j; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = poly->firstLink; - poly->firstLink = idx; - } + // Link off-mesh connection to target poly. + unsigned int idx = allocLink(tile); + if (idx != DT_NULL_LINK) + { + dtLink* link = &tile->links[idx]; + link->ref = ref; + link->edge = (unsigned char)0; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = poly->firstLink; + poly->firstLink = idx; + } - // Start end-point is always connect back to off-mesh connection, - // Destination end-point only if it is bidirectional link. - if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR))) - { - // Link target poly to off-mesh connection. - unsigned int idx = allocLink(tile); - if (idx != DT_NULL_LINK) - { - const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); - dtPoly* landPoly = &tile->polys[landPolyIdx]; - dtLink* link = &tile->links[idx]; - link->ref = base | (dtPolyRef)(con->poly); - link->edge = 0xff; - link->side = 0xff; - link->bmin = link->bmax = 0; - // Add to linked list. - link->next = landPoly->firstLink; - landPoly->firstLink = idx; - } - } - - } + // Start end-point is always connect back to off-mesh connection. + unsigned int tidx = allocLink(tile); + if (tidx != DT_NULL_LINK) + { + const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); + dtPoly* landPoly = &tile->polys[landPolyIdx]; + dtLink* link = &tile->links[tidx]; + link->ref = base | (dtPolyRef)(con->poly); + link->edge = 0xff; + link->side = 0xff; + link->bmin = link->bmax = 0; + // Add to linked list. + link->next = landPoly->firstLink; + landPoly->firstLink = tidx; } } } -dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const +void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { - const dtPoly* poly = &tile->polys[ip]; + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); - float closestDistSqr = FLT_MAX; + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; + return; + } + + const unsigned int ip = (unsigned int)(poly - tile->polys); const dtPolyDetail* pd = &tile->detailMeshes[ip]; + // Clamp point to be inside the polygon. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + const int nv = poly->vertCount; + for (int i = 0; i < nv; ++i) + dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); + + dtVcopy(closest, pos); + if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + { + // Point is outside the polygon, dtClamp to nearest edge. + float dmin = FLT_MAX; + int imin = -1; + for (int i = 0; i < nv; ++i) + { + if (edged[i] < dmin) + { + dmin = edged[i]; + imin = i; + } + } + const float* va = &verts[imin*3]; + const float* vb = &verts[((imin+1)%nv)*3]; + dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; + } + + // Find height at the location. for (int j = 0; j < pd->triCount; ++j) { const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; @@ -623,17 +679,13 @@ dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned in else v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; } - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - if (d < closestDistSqr) + float h; + if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) { - dtVcopy(closest, pt); - closestDistSqr = d; + closest[1] = h; + break; } } - - return DT_SUCCESS; } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, @@ -655,13 +707,27 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, { dtPolyRef ref = polys[i]; float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); + float diff[3]; + bool posOverPoly = false; + float d = 0; + closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); + + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + dtVsub(diff, center, closestPtPoly); + if (posOverPoly) + { + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + if (d < nearestDistanceSqr) { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); + dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; } @@ -730,8 +796,11 @@ int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, co dtPolyRef base = getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { - // Calc polygon bounds. dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + // Calc polygon bounds. const float* v = &tile->verts[p->verts[0]*3]; dtVcopy(bmin, v); dtVcopy(bmax, v); @@ -773,7 +842,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, return DT_FAILURE | DT_WRONG_VERSION; // Make sure the location is free. - if (getTileAt(header->x, header->y)) + if (getTileAt(header->x, header->y, header->layer)) return DT_FAILURE; // Allocate a tile. @@ -845,6 +914,10 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, tile->bvTree = (dtBVNode*)d; d += bvtreeSize; tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; + // If there are no items in the bvtree, reset the tree pointer. + if (!bvtreeSize) + tile->bvTree = 0; + // Build links freelist tile->linksFreeList = 0; tile->links[header->maxLinkCount-1].next = DT_NULL_LINK; @@ -858,18 +931,36 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, tile->flags = flags; connectIntLinks(tile); - connectIntOffMeshLinks(tile); + baseOffMeshLinks(tile); // Create connections with neighbour tiles. + static const int MAX_NEIS = 32; + dtMeshTile* neis[MAX_NEIS]; + int nneis; + + // Connect with layers in current tile. + nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + { + if (neis[j] != tile) + { + connectExtLinks(tile, neis[j], -1); + connectExtLinks(neis[j], tile, -1); + } + connectExtOffMeshLinks(tile, neis[j], -1); + connectExtOffMeshLinks(neis[j], tile, -1); + } + + // Connect with neighbour tiles. for (int i = 0; i < 8; ++i) { - dtMeshTile* nei = getNeighbourTileAt(header->x, header->y, i); - if (nei) + nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) { - connectExtLinks(tile, nei, i); - connectExtLinks(nei, tile, dtOppositeTile(i)); - connectExtOffMeshLinks(tile, nei, i); - connectExtOffMeshLinks(nei, tile, dtOppositeTile(i)); + connectExtLinks(tile, neis[j], i); + connectExtLinks(neis[j], tile, dtOppositeTile(i)); + connectExtOffMeshLinks(tile, neis[j], i); + connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i)); } } @@ -879,55 +970,106 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, return DT_SUCCESS; } -const dtMeshTile* dtNavMesh::getTileAt(int x, int y) const +const dtMeshTile* dtNavMesh::getTileAt(const int x, const int y, const int layer) const { // Find tile based on hash. int h = computeTileHash(x,y,m_tileLutMask); dtMeshTile* tile = m_posLookup[h]; while (tile) { - if (tile->header && tile->header->x == x && tile->header->y == y) + if (tile->header && + tile->header->x == x && + tile->header->y == y && + tile->header->layer == layer) + { return tile; + } tile = tile->next; } return 0; } -dtMeshTile* dtNavMesh::getNeighbourTileAt(int x, int y, int side) const +int dtNavMesh::getNeighbourTilesAt(const int x, const int y, const int side, dtMeshTile** tiles, const int maxTiles) const { + int nx = x, ny = y; switch (side) { - case 0: x++; break; - case 1: x++; y++; break; - case 2: y++; break; - case 3: x--; y++; break; - case 4: x--; break; - case 5: x--; y--; break; - case 6: y--; break; - case 7: x++; y--; break; + case 0: nx++; break; + case 1: nx++; ny++; break; + case 2: ny++; break; + case 3: nx--; ny++; break; + case 4: nx--; break; + case 5: nx--; ny--; break; + case 6: ny--; break; + case 7: nx++; ny--; break; }; + return getTilesAt(nx, ny, tiles, maxTiles); +} + +int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const int maxTiles) const +{ + int n = 0; + // Find tile based on hash. int h = computeTileHash(x,y,m_tileLutMask); dtMeshTile* tile = m_posLookup[h]; while (tile) { - if (tile->header && tile->header->x == x && tile->header->y == y) - return tile; + if (tile->header && + tile->header->x == x && + tile->header->y == y) + { + if (n < maxTiles) + tiles[n++] = tile; + } tile = tile->next; } - return 0; + + return n; +} + +/// @par +/// +/// This function will not fail if the tiles array is too small to hold the +/// entire result set. It will simply fill the array to capacity. +int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile const** tiles, const int maxTiles) const +{ + int n = 0; + + // Find tile based on hash. + int h = computeTileHash(x,y,m_tileLutMask); + dtMeshTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->header && + tile->header->x == x && + tile->header->y == y) + { + if (n < maxTiles) + tiles[n++] = tile; + } + tile = tile->next; + } + + return n; } -dtTileRef dtNavMesh::getTileRefAt(int x, int y) const + +dtTileRef dtNavMesh::getTileRefAt(const int x, const int y, const int layer) const { // Find tile based on hash. int h = computeTileHash(x,y,m_tileLutMask); dtMeshTile* tile = m_posLookup[h]; while (tile) { - if (tile->header && tile->header->x == x && tile->header->y == y) + if (tile->header && + tile->header->x == x && + tile->header->y == y && + tile->header->layer == layer) + { return getTileRef(tile); + } tile = tile->next; } return 0; @@ -970,6 +1112,7 @@ void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const { + if (!ref) return DT_FAILURE; unsigned int salt, it, ip; decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; @@ -995,6 +1138,7 @@ void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile* bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const { + if (!ref) return false; unsigned int salt, it, ip; decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return false; @@ -1040,14 +1184,27 @@ dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSiz } // Remove connections to neighbour tiles. - for (int i = 0; i < 8; ++i) + // Create connections with neighbour tiles. + static const int MAX_NEIS = 32; + dtMeshTile* neis[MAX_NEIS]; + int nneis; + + // Connect with layers in current tile. + nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) { - dtMeshTile* nei = getNeighbourTileAt(tile->header->x,tile->header->y,i); - if (!nei) continue; - unconnectExtLinks(nei, dtOppositeTile(i)); + if (neis[j] == tile) continue; + unconnectExtLinks(neis[j], tile); } - + // Connect with neighbour tiles. + for (int i = 0; i < 8; ++i) + { + nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS); + for (int j = 0; j < nneis; ++j) + unconnectExtLinks(neis[j], tile); + } + // Reset tile. if (tile->flags & DT_TILE_FREE_DATA) { @@ -1091,7 +1248,7 @@ dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSiz dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const { if (!tile) return 0; - const unsigned int it = tile - m_tiles; + const unsigned int it = (unsigned int)(tile - m_tiles); return (dtTileRef)encodePolyId(tile->salt, it, 0); } @@ -1112,7 +1269,7 @@ dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const dtPolyRef dtNavMesh::getPolyRefBase(const dtMeshTile* tile) const { if (!tile) return 0; - const unsigned int it = tile - m_tiles; + const unsigned int it = (unsigned int)(tile - m_tiles); return encodePolyId(tile->salt, it, 0); } @@ -1216,6 +1373,9 @@ dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyR { unsigned int salt, it, ip; + if (!polyRef) + return DT_FAILURE; + // Get current polygon decodePolyId(polyRef, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; @@ -1256,6 +1416,9 @@ const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) c { unsigned int salt, it, ip; + if (!ref) + return 0; + // Get current polygon decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return 0; @@ -1276,6 +1439,7 @@ const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) c dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags) { + if (!ref) return DT_FAILURE; unsigned int salt, it, ip; decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; @@ -1292,6 +1456,7 @@ dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags) dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const { + if (!ref) return DT_FAILURE; unsigned int salt, it, ip; decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; @@ -1307,6 +1472,7 @@ dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) con dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) { + if (!ref) return DT_FAILURE; unsigned int salt, it, ip; decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; @@ -1322,6 +1488,7 @@ dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area) dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const { + if (!ref) return DT_FAILURE; unsigned int salt, it, ip; decodePolyId(ref, salt, it, ip); if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM; diff --git a/dep/recastnavigation/Detour/DetourNavMesh.h b/dep/recastnavigation/Detour/DetourNavMesh.h index 7175f8e73eb..cdd473f1aff 100644 --- a/dep/recastnavigation/Detour/DetourNavMesh.h +++ b/dep/recastnavigation/Detour/DetourNavMesh.h @@ -66,7 +66,7 @@ static const int DT_VERTS_PER_POLYGON = 6; static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V'; /// A version number used to detect compatibility of navigation tile data. -static const int DT_NAVMESH_VERSION = 6; +static const int DT_NAVMESH_VERSION = 7; /// A magic number used to detect the compatibility of navigation tile states. static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S'; @@ -112,6 +112,12 @@ enum dtStraightPathFlags DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04, ///< The vertex is the start of an off-mesh connection. }; +/// Options for dtNavMeshQuery::findStraightPath. +enum dtStraightPathOptions +{ + DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01, ///< Add a vertex at every polygon edge crossing where area changes. + DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, ///< Add a vertex at every polygon edge crossing. +}; /// Flags representing the type of a navigation mesh polygon. enum dtPolyTypes @@ -225,6 +231,7 @@ struct dtMeshHeader int version; ///< Tile data format version number. int x; ///< The x-position of the tile within the dtNavMesh tile grid. (x, y, layer) int y; ///< The y-position of the tile within the dtNavMesh tile grid. (x, y, layer) + int layer; ///< The layer of the tile within the dtNavMesh tile grid. (x, y, layer) unsigned int userId; ///< The user defined id of the tile. int polyCount; ///< The number of polygons in the tile. int vertCount; ///< The number of vertices in the tile. @@ -347,18 +354,31 @@ public: void calcTileLoc(const float* pos, int* tx, int* ty) const; /// Gets the tile at the specified grid location. - // Params: - // x,y - (in) Location of the tile to get. - // Returns: pointer to tile if tile exists or 0 tile does not exists. - const dtMeshTile* getTileAt(int x, int y) const; - - // Returns reference to tile at specified location. - // Params: - // x,y - (in) Location of the tile to get. - // Returns: reference to tile if tile exists or 0 tile does not exists. - dtTileRef getTileRefAt(int x, int y) const; + /// @param[in] x The tile's x-location. (x, y, layer) + /// @param[in] y The tile's y-location. (x, y, layer) + /// @param[in] layer The tile's layer. (x, y, layer) + /// @return The tile, or null if the tile does not exist. + const dtMeshTile* getTileAt(const int x, const int y, const int layer) const; + + /// Gets all tiles at the specified grid location. (All layers.) + /// @param[in] x The tile's x-location. (x, y) + /// @param[in] y The tile's y-location. (x, y) + /// @param[out] tiles A pointer to an array of tiles that will hold the result. + /// @param[in] maxTiles The maximum tiles the tiles parameter can hold. + /// @return The number of tiles returned in the tiles array. + int getTilesAt(const int x, const int y, + dtMeshTile const** tiles, const int maxTiles) const; - // Returns tile references of a tile based on tile pointer. + /// Gets the tile reference for the tile at specified grid location. + /// @param[in] x The tile's x-location. (x, y, layer) + /// @param[in] y The tile's y-location. (x, y, layer) + /// @param[in] layer The tile's layer. (x, y, layer) + /// @return The tile reference of the tile, or 0 if there is none. + dtTileRef getTileRefAt(int x, int y, int layer) const; + + /// Gets the tile reference for the specified tile. + /// @param[in] tile The tile. + /// @return The tile reference of the tile. dtTileRef getTileRef(const dtMeshTile* tile) const; /// Gets the tile for the specified tile reference. @@ -532,7 +552,13 @@ private: dtMeshTile* getTile(int i); /// Returns neighbour tile based on side. - dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; + int getTilesAt(const int x, const int y, + dtMeshTile** tiles, const int maxTiles) const; + + /// Returns neighbour tile based on side. + int getNeighbourTilesAt(const int x, const int y, const int side, + dtMeshTile** tiles, const int maxTiles) const; + /// Returns all polygons in neighbour tile based on portal defined by the segment. int findConnectingPolys(const float* va, const float* vb, const dtMeshTile* tile, int side, @@ -541,7 +567,7 @@ private: /// Builds internal polygons links for a tile. void connectIntLinks(dtMeshTile* tile); /// Builds internal polygons links for a tile. - void connectIntOffMeshLinks(dtMeshTile* tile); + void baseOffMeshLinks(dtMeshTile* tile); /// Builds external polygon links for a tile. void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side); @@ -549,7 +575,7 @@ private: void connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side); /// Removes external links at specified side. - void unconnectExtLinks(dtMeshTile* tile, int side); + void unconnectExtLinks(dtMeshTile* tile, dtMeshTile* target); // TODO: These methods are duplicates from dtNavMeshQuery, but are needed for off-mesh connection finding. @@ -561,8 +587,7 @@ private: dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const; /// Returns closest point on polygon. - dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const; + void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; dtNavMeshParams m_params; ///< Current initialization params. TODO: do not store this info twice. float m_orig[3]; ///< Origin of the tile (0,0) diff --git a/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp b/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp index fcac215fff4..9d8471b96a1 100644 --- a/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp +++ b/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp @@ -261,8 +261,6 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, return false; if (!params->polyCount || !params->polys) return false; - if (!params->detailMeshes || !params->detailVerts || !params->detailTris) - return false; const int nvp = params->nvp; @@ -278,10 +276,50 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, if (!offMeshConClass) return false; + // Find tight heigh bounds, used for culling out off-mesh start locations. + float hmin = FLT_MAX; + float hmax = -FLT_MAX; + + if (params->detailVerts && params->detailVertsCount) + { + for (int i = 0; i < params->detailVertsCount; ++i) + { + const float h = params->detailVerts[i*3+1]; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + else + { + for (int i = 0; i < params->vertCount; ++i) + { + const unsigned short* iv = ¶ms->verts[i*3]; + const float h = params->bmin[1] + iv[1] * params->ch; + hmin = dtMin(hmin,h); + hmax = dtMax(hmax,h); + } + } + hmin -= params->walkableClimb; + hmax += params->walkableClimb; + float bmin[3], bmax[3]; + dtVcopy(bmin, params->bmin); + dtVcopy(bmax, params->bmax); + bmin[1] = hmin; + bmax[1] = hmax; + for (int i = 0; i < params->offMeshConCount; ++i) { - offMeshConClass[i*2+0] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax); - offMeshConClass[i*2+1] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax); + const float* p0 = ¶ms->offMeshConVerts[(i*2+0)*3]; + const float* p1 = ¶ms->offMeshConVerts[(i*2+1)*3]; + offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax); + offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax); + + // Zero out off-mesh start positions which are not even potentially touching the mesh. + if (offMeshConClass[i*2+0] == 0xff) + { + if (p0[1] < bmin[1] || p0[1] > bmax[1]) + offMeshConClass[i*2+0] = 0; + } // Cound how many links should be allocated for off-mesh connections. if (offMeshConClass[i*2+0] == 0xff) @@ -307,23 +345,13 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, for (int j = 0; j < nvp; ++j) { if (p[j] == MESH_NULL_IDX) break; - int nj = j+1; - if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0; - const unsigned short* va = ¶ms->verts[p[j]*3]; - const unsigned short* vb = ¶ms->verts[p[nj]*3]; - edgeCount++; - if (params->tileSize > 0) + if (p[nvp+j] & 0x8000) { - if (va[0] == params->tileSize && vb[0] == params->tileSize) - portalCount++; // x+ - else if (va[2] == params->tileSize && vb[2] == params->tileSize) - portalCount++; // z+ - else if (va[0] == 0 && vb[0] == 0) - portalCount++; // x- - else if (va[2] == 0 && vb[2] == 0) - portalCount++; // z- + unsigned short dir = p[nvp+j] & 0xf; + if (dir != 0xf) + portalCount++; } } } @@ -332,18 +360,41 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, // Find unique detail vertices. int uniqueDetailVertCount = 0; - for (int i = 0; i < params->polyCount; ++i) + int detailTriCount = 0; + if (params->detailMeshes) { - const unsigned short* p = ¶ms->polys[i*nvp*2]; - int ndv = params->detailMeshes[i*4+1]; - int nv = 0; - for (int j = 0; j < nvp; ++j) + // Has detail mesh, count unique detail vertex count and use input detail tri count. + detailTriCount = params->detailTriCount; + for (int i = 0; i < params->polyCount; ++i) { - if (p[j] == MESH_NULL_IDX) break; - nv++; + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int ndv = params->detailMeshes[i*4+1]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + ndv -= nv; + uniqueDetailVertCount += ndv; + } + } + else + { + // No input detail mesh, build detail mesh from nav polys. + uniqueDetailVertCount = 0; // No extra detail verts. + detailTriCount = 0; + for (int i = 0; i < params->polyCount; ++i) + { + const unsigned short* p = ¶ms->polys[i*nvp*2]; + int nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + nv++; + } + detailTriCount += nv-2; } - ndv -= nv; - uniqueDetailVertCount += ndv; } // Calculate data size @@ -353,8 +404,8 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount); const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount); const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount); - const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*params->detailTriCount); - const int bvTreeSize = dtAlign4(sizeof(dtBVNode)*params->polyCount*2); + const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount); + const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0; const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount); const int dataSize = headerSize + vertsSize + polysSize + linksSize + @@ -386,6 +437,7 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, header->version = DT_NAVMESH_VERSION; header->x = params->tileX; header->y = params->tileY; + header->layer = params->tileLayer; header->userId = params->userId; header->polyCount = totPolyCount; header->vertCount = totVertCount; @@ -394,14 +446,14 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, dtVcopy(header->bmax, params->bmax); header->detailMeshCount = params->polyCount; header->detailVertCount = uniqueDetailVertCount; - header->detailTriCount = params->detailTriCount; + header->detailTriCount = detailTriCount; header->bvQuantFactor = 1.0f / params->cs; header->offMeshBase = params->polyCount; header->walkableHeight = params->walkableHeight; header->walkableRadius = params->walkableRadius; header->walkableClimb = params->walkableClimb; header->offMeshConCount = storedOffMeshConCount; - header->bvNodeCount = params->polyCount*2; + header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0; const int offMeshVertsBase = params->vertCount; const int offMeshPolyBase = params->polyCount; @@ -445,7 +497,27 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, { if (src[j] == MESH_NULL_IDX) break; p->verts[j] = src[j]; - p->neis[j] = (src[nvp+j]+1) & 0xffff; + if (src[nvp+j] & 0x8000) + { + // Border or portal edge. + unsigned short dir = src[nvp+j] & 0xf; + if (dir == 0xf) // Border + p->neis[j] = 0; + else if (dir == 0) // Portal x- + p->neis[j] = DT_EXT_LINK | 4; + else if (dir == 1) // Portal z+ + p->neis[j] = DT_EXT_LINK | 2; + else if (dir == 2) // Portal x+ + p->neis[j] = DT_EXT_LINK | 0; + else if (dir == 3) // Portal z- + p->neis[j] = DT_EXT_LINK | 6; + } + else + { + // Normal connection + p->neis[j] = src[nvp+j]+1; + } + p->vertCount++; } src += nvp*2; @@ -467,61 +539,68 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, n++; } } - - // Store portal edges. - if (params->tileSize > 0) + + // Store detail meshes and vertices. + // The nav polygon vertices are stored as the first vertices on each mesh. + // We compress the mesh data by skipping them and using the navmesh coordinates. + if (params->detailMeshes) { + unsigned short vbase = 0; for (int i = 0; i < params->polyCount; ++i) { - dtPoly* poly = &navPolys[i]; - for (int j = 0; j < poly->vertCount; ++j) + dtPolyDetail& dtl = navDMeshes[i]; + const int vb = (int)params->detailMeshes[i*4+0]; + const int ndv = (int)params->detailMeshes[i*4+1]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = (unsigned int)vbase; + dtl.vertCount = (unsigned char)(ndv-nv); + dtl.triBase = (unsigned int)params->detailMeshes[i*4+2]; + dtl.triCount = (unsigned char)params->detailMeshes[i*4+3]; + // Copy vertices except the first 'nv' verts which are equal to nav poly verts. + if (ndv-nv) { - int nj = j+1; - if (nj >= poly->vertCount) nj = 0; - - const unsigned short* va = ¶ms->verts[poly->verts[j]*3]; - const unsigned short* vb = ¶ms->verts[poly->verts[nj]*3]; - - if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+ - poly->neis[j] = DT_EXT_LINK | 0; - else if (va[2] == params->tileSize && vb[2] == params->tileSize) // z+ - poly->neis[j] = DT_EXT_LINK | 2; - else if (va[0] == 0 && vb[0] == 0) // x- - poly->neis[j] = DT_EXT_LINK | 4; - else if (va[2] == 0 && vb[2] == 0) // z- - poly->neis[j] = DT_EXT_LINK | 6; + memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); + vbase += (unsigned short)(ndv-nv); } } + // Store triangles. + memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); } - - // Store detail meshes and vertices. - // The nav polygon vertices are stored as the first vertices on each mesh. - // We compress the mesh data by skipping them and using the navmesh coordinates. - unsigned short vbase = 0; - for (int i = 0; i < params->polyCount; ++i) + else { - dtPolyDetail& dtl = navDMeshes[i]; - const int vb = (int)params->detailMeshes[i*4+0]; - const int ndv = (int)params->detailMeshes[i*4+1]; - const int nv = navPolys[i].vertCount; - dtl.vertBase = (unsigned int)vbase; - dtl.vertCount = (unsigned char)(ndv-nv); - dtl.triBase = (unsigned int)params->detailMeshes[i*4+2]; - dtl.triCount = (unsigned char)params->detailMeshes[i*4+3]; - // Copy vertices except the first 'nv' verts which are equal to nav poly verts. - if (ndv-nv) + // Create dummy detail mesh by triangulating polys. + int tbase = 0; + for (int i = 0; i < params->polyCount; ++i) { - memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv)); - vbase += (unsigned short)(ndv-nv); + dtPolyDetail& dtl = navDMeshes[i]; + const int nv = navPolys[i].vertCount; + dtl.vertBase = 0; + dtl.vertCount = 0; + dtl.triBase = (unsigned int)tbase; + dtl.triCount = (unsigned char)(nv-2); + // Triangulate polygon (local indices). + for (int j = 2; j < nv; ++j) + { + unsigned char* t = &navDTris[tbase*4]; + t[0] = 0; + t[1] = (unsigned char)(j-1); + t[2] = (unsigned char)j; + // Bit for each edge that belongs to poly boundary. + t[3] = (1<<2); + if (j == 2) t[3] |= (1<<0); + if (j == nv-1) t[3] |= (1<<4); + tbase++; + } } } - // Store triangles. - memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount); // Store and create BVtree. // TODO: take detail mesh into account! use byte per bbox extent? - createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, - nvp, params->cs, params->ch, params->polyCount*2, navBvtree); + if (params->buildBvTree) + { + createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, + nvp, params->cs, params->ch, params->polyCount*2, navBvtree); + } // Store Off-Mesh connections. n = 0; @@ -553,51 +632,14 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, return true; } -inline void swapByte(unsigned char* a, unsigned char* b) -{ - unsigned char tmp = *a; - *a = *b; - *b = tmp; -} - -inline void swapEndian(unsigned short* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+1); -} - -inline void swapEndian(short* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+1); -} - -inline void swapEndian(unsigned int* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -inline void swapEndian(int* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - -inline void swapEndian(float* v) -{ - unsigned char* x = (unsigned char*)v; - swapByte(x+0, x+3); swapByte(x+1, x+2); -} - bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) { dtMeshHeader* header = (dtMeshHeader*)data; int swappedMagic = DT_NAVMESH_MAGIC; int swappedVersion = DT_NAVMESH_VERSION; - swapEndian(&swappedMagic); - swapEndian(&swappedVersion); + dtSwapEndian(&swappedMagic); + dtSwapEndian(&swappedVersion); if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) && (header->magic != swappedMagic || header->version != swappedVersion)) @@ -605,30 +647,31 @@ bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/) return false; } - swapEndian(&header->magic); - swapEndian(&header->version); - swapEndian(&header->x); - swapEndian(&header->y); - swapEndian(&header->userId); - swapEndian(&header->polyCount); - swapEndian(&header->vertCount); - swapEndian(&header->maxLinkCount); - swapEndian(&header->detailMeshCount); - swapEndian(&header->detailVertCount); - swapEndian(&header->detailTriCount); - swapEndian(&header->bvNodeCount); - swapEndian(&header->offMeshConCount); - swapEndian(&header->offMeshBase); - swapEndian(&header->walkableHeight); - swapEndian(&header->walkableRadius); - swapEndian(&header->walkableClimb); - swapEndian(&header->bmin[0]); - swapEndian(&header->bmin[1]); - swapEndian(&header->bmin[2]); - swapEndian(&header->bmax[0]); - swapEndian(&header->bmax[1]); - swapEndian(&header->bmax[2]); - swapEndian(&header->bvQuantFactor); + dtSwapEndian(&header->magic); + dtSwapEndian(&header->version); + dtSwapEndian(&header->x); + dtSwapEndian(&header->y); + dtSwapEndian(&header->layer); + dtSwapEndian(&header->userId); + dtSwapEndian(&header->polyCount); + dtSwapEndian(&header->vertCount); + dtSwapEndian(&header->maxLinkCount); + dtSwapEndian(&header->detailMeshCount); + dtSwapEndian(&header->detailVertCount); + dtSwapEndian(&header->detailTriCount); + dtSwapEndian(&header->bvNodeCount); + dtSwapEndian(&header->offMeshConCount); + dtSwapEndian(&header->offMeshBase); + dtSwapEndian(&header->walkableHeight); + dtSwapEndian(&header->walkableRadius); + dtSwapEndian(&header->walkableClimb); + dtSwapEndian(&header->bmin[0]); + dtSwapEndian(&header->bmin[1]); + dtSwapEndian(&header->bmin[2]); + dtSwapEndian(&header->bmax[0]); + dtSwapEndian(&header->bmax[1]); + dtSwapEndian(&header->bmax[2]); + dtSwapEndian(&header->bvQuantFactor); // Freelist index and pointers are updated when tile is added, no need to swap. @@ -674,7 +717,7 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) // Vertices for (int i = 0; i < header->vertCount*3; ++i) { - swapEndian(&verts[i]); + dtSwapEndian(&verts[i]); } // Polys @@ -684,10 +727,10 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) // poly->firstLink is update when tile is added, no need to swap. for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j) { - swapEndian(&p->verts[j]); - swapEndian(&p->neis[j]); + dtSwapEndian(&p->verts[j]); + dtSwapEndian(&p->neis[j]); } - swapEndian(&p->flags); + dtSwapEndian(&p->flags); } // Links are rebuild when tile is added, no need to swap. @@ -696,14 +739,14 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) for (int i = 0; i < header->detailMeshCount; ++i) { dtPolyDetail* pd = &detailMeshes[i]; - swapEndian(&pd->vertBase); - swapEndian(&pd->triBase); + dtSwapEndian(&pd->vertBase); + dtSwapEndian(&pd->triBase); } // Detail verts for (int i = 0; i < header->detailVertCount*3; ++i) { - swapEndian(&detailVerts[i]); + dtSwapEndian(&detailVerts[i]); } // BV-tree @@ -712,10 +755,10 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) dtBVNode* node = &bvTree[i]; for (int j = 0; j < 3; ++j) { - swapEndian(&node->bmin[j]); - swapEndian(&node->bmax[j]); + dtSwapEndian(&node->bmin[j]); + dtSwapEndian(&node->bmax[j]); } - swapEndian(&node->i); + dtSwapEndian(&node->i); } // Off-mesh Connections. @@ -723,9 +766,9 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) { dtOffMeshConnection* con = &offMeshCons[i]; for (int j = 0; j < 6; ++j) - swapEndian(&con->pos[j]); - swapEndian(&con->rad); - swapEndian(&con->poly); + dtSwapEndian(&con->pos[j]); + dtSwapEndian(&con->rad); + dtSwapEndian(&con->poly); } return true; diff --git a/dep/recastnavigation/Detour/DetourNavMeshBuilder.h b/dep/recastnavigation/Detour/DetourNavMeshBuilder.h index c4a5ac07045..c80d1717630 100644 --- a/dep/recastnavigation/Detour/DetourNavMeshBuilder.h +++ b/dep/recastnavigation/Detour/DetourNavMeshBuilder.h @@ -83,6 +83,7 @@ struct dtNavMeshCreateParams unsigned int userId; ///< The user defined id of the tile. int tileX; ///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.) int tileY; ///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.) + int tileLayer; ///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.) float bmin[3]; ///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu] float bmax[3]; ///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu] @@ -95,7 +96,12 @@ struct dtNavMeshCreateParams float walkableClimb; ///< The agent maximum traversable ledge. (Up/Down) [Unit: wu] float cs; ///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu] float ch; ///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu] - int tileSize; // Tile size (width & height) (vx). + + /// True if a bounding volume tree should be built for the tile. + /// @note The BVTree is not normally needed for layered navigation meshes. + bool buildBvTree; + + /// @} }; /// Builds navigation mesh tile data from the provided tile creation data. diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp index ec0c30460a9..f1709dfd4cf 100644 --- a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp +++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp @@ -81,7 +81,7 @@ float dtQueryFilter::getCost(const float* pa, const float* pb, const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const { - return dtVdist(pa, pb) * m_areaCost[curPoly->area]; + return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; } #else inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, @@ -217,6 +217,281 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) return DT_SUCCESS; } +dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const +{ + dtAssert(m_nav); + + // Randomly pick one tile. Assume that all tiles cover roughly the same area. + const dtMeshTile* tile = 0; + float tsum = 0.0f; + for (int i = 0; i < m_nav->getMaxTiles(); i++) + { + const dtMeshTile* t = m_nav->getTile(i); + if (!t || !t->header) continue; + + // Choose random tile using reservoi sampling. + const float area = 1.0f; // Could be tile area too. + tsum += area; + const float u = frand(); + if (u*tsum <= area) + tile = t; + } + if (!tile) + return DT_FAILURE; + + // Randomly pick one polygon weighted by polygon area. + const dtPoly* poly = 0; + dtPolyRef polyRef = 0; + const dtPolyRef base = m_nav->getPolyRefBase(tile); + + float areaSum = 0.0f; + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() != DT_POLYTYPE_GROUND) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; + + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < p->vertCount; ++j) + { + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[j-1]*3]; + const float* vc = &tile->verts[p->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + poly = p; + polyRef = ref; + } + } + + if (!poly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &tile->verts[poly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < poly->vertCount; ++j) + { + v = &tile->verts[poly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus status = getPolyHeight(polyRef, pt, &h); + if (dtStatusFailed(status)) + return status; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = polyRef; + + return DT_SUCCESS; +} + +dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtMeshTile* startTile = 0; + const dtPoly* startPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly); + if (!filter->passFilter(startRef, startTile, startPoly)) + return DT_FAILURE | DT_INVALID_PARAM; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + dtVcopy(startNode->pos, centerPos); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtStatus status = DT_SUCCESS; + + const float radiusSqr = dtSqr(radius); + float areaSum = 0.0f; + + const dtMeshTile* randomTile = 0; + const dtPoly* randomPoly = 0; + dtPolyRef randomPolyRef = 0; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Place random locations on on ground. + if (bestPoly->getType() == DT_POLYTYPE_GROUND) + { + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < bestPoly->vertCount; ++j) + { + const float* va = &bestTile->verts[bestPoly->verts[0]*3]; + const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3]; + const float* vc = &bestTile->verts[bestPoly->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + randomTile = bestTile; + randomPoly = bestPoly; + randomPolyRef = bestRef; + } + } + + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); + + // Do not advance if the polygon is excluded by the filter. + if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) + continue; + + // Find edge and calc distance to the edge. + float va[3], vb[3]; + if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the circle is not touching the next polygon, skip it. + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + if (distSqr > radiusSqr) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); + + // The node is already in open list and the new result is worse, skip. + if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags = DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + if (!randomPoly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &randomTile->verts[randomPoly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < randomPoly->vertCount; ++j) + { + v = &randomTile->verts[randomPoly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus stat = getPolyHeight(randomPolyRef, pt, &h); + if (dtStatusFailed(status)) + return stat; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = randomPolyRef; + + return DT_SUCCESS; +} + + ////////////////////////////////////////////////////////////////////////////////////////// /// @par @@ -227,7 +502,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) /// /// See closestPointOnPolyBoundary() for a limited but faster option. /// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const +dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { dtAssert(m_nav); const dtMeshTile* tile = 0; @@ -236,97 +511,80 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo return DT_FAILURE | DT_INVALID_PARAM; if (!tile) return DT_FAILURE | DT_INVALID_PARAM; + + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + const float d0 = dtVdist(pos, v0); + const float d1 = dtVdist(pos, v1); + const float u = d0 / (d0+d1); + dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; + return DT_SUCCESS; + } - // Edited by TC - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - return DT_FAILURE; + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; - if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS) - return DT_FAILURE; - return DT_SUCCESS; -} + // Clamp point to be inside the polygon. + float verts[DT_VERTS_PER_POLYGON*3]; + float edged[DT_VERTS_PER_POLYGON]; + float edget[DT_VERTS_PER_POLYGON]; + const int nv = poly->vertCount; + for (int i = 0; i < nv; ++i) + dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); + + dtVcopy(closest, pos); + if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + { + // Point is outside the polygon, dtClamp to nearest edge. + float dmin = FLT_MAX; + int imin = -1; + for (int i = 0; i < nv; ++i) + { + if (edged[i] < dmin) + { + dmin = edged[i]; + imin = i; + } + } + const float* va = &verts[imin*3]; + const float* vb = &verts[((imin+1)%nv)*3]; + dtVlerp(closest, va, vb, edget[imin]); -dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, - const float* pos, float* closest) const -{ - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh. - // Test and enable. - /* - // Clamp point to be inside the polygon. - float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; - const int nv = poly->vertCount; - for (int i = 0; i < nv; ++i) - dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); - - dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = FLT_MAX; - int imin = -1; - for (int i = 0; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - } - - // Find height at the location. - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - closest[1] = h; - break; - } - } - */ - float closestDistSqr = FLT_MAX; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase + j) * 4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]] * 3]; - else - v[k] = &tile->detailVerts[(pd->vertBase + (t[k] - poly->vertCount)) * 3]; - } - - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - - if (d < closestDistSqr) - { - dtVcopy(closest, pt); - closestDistSqr = d; - } - } - - return DT_SUCCESS; + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; + } + + // Find height at the location. + for (int j = 0; j < pd->triCount; ++j) + { + const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float h; + if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) + { + closest[1] = h; + break; + } + } + + return DT_SUCCESS; } /// @par @@ -405,8 +663,8 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h { const float* v0 = &tile->verts[poly->verts[0]*3]; const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist(pos, v0); - const float d1 = dtVdist(pos, v1); + const float d0 = dtVdist2D(pos, v0); + const float d1 = dtVdist2D(pos, v1); const float u = d0 / (d0+d1); if (height) *height = v0[1] + (v1[1] - v0[1]) * u; @@ -470,9 +728,27 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten { dtPolyRef ref = polys[i]; float closestPtPoly[3]; - if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); + float diff[3]; + bool posOverPoly = false; + float d = 0; + closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); + + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + dtVsub(diff, center, closestPtPoly); + if (posOverPoly) + { + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly); + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + if (d < nearestDistanceSqr) { if (nearestPt) @@ -488,43 +764,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten return DT_SUCCESS; } -dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const -{ - dtAssert(m_nav); - - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128); - - // 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]; - const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS) - continue; - - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, dtPolyRef* polys, const int maxPolys) const @@ -592,8 +831,15 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi const dtPolyRef base = m_nav->getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { + const dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; // Calc polygon bounds. - dtPoly* p = &tile->polys[i]; const float* v = &tile->verts[p->verts[0]*3]; dtVcopy(bmin, v); dtVcopy(bmax, v); @@ -605,12 +851,8 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi } if (dtOverlapBounds(qmin,qmax, bmin,bmax)) { - const dtPolyRef ref = base | (dtPolyRef)i; - if (filter->passFilter(ref, tile, p)) - { - if (n < maxPolys) - polys[n++] = ref; - } + if (n < maxPolys) + polys[n++] = ref; } } return n; @@ -641,18 +883,23 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents m_nav->calcTileLoc(bmin, &minx, &miny); m_nav->calcTileLoc(bmax, &maxx, &maxy); + static const int MAX_NEIS = 32; + const dtMeshTile* neis[MAX_NEIS]; + int n = 0; for (int y = miny; y <= maxy; ++y) { for (int x = minx; x <= maxx; ++x) { - const dtMeshTile* tile = m_nav->getTileAt(x,y); - if (!tile) continue; - n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n); - if (n >= maxPolys) + const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS); + for (int j = 0; j < nneis; ++j) { - *polyCount = n; - return DT_SUCCESS; + n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n); + if (n >= maxPolys) + { + *polyCount = n; + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + } } } } @@ -715,6 +962,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtNode* lastBestNode = startNode; float lastBestNodeCost = startNode->total; + dtStatus status = DT_SUCCESS; + while (!m_openList->empty()) { // Remove node from open list and put it in closed list. @@ -764,7 +1013,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -817,7 +1069,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Add or update the node. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->cost = cost; neighbourNode->total = total; @@ -842,6 +1094,9 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, } } + if (lastBestNode->id != endRef) + status |= DT_PARTIAL_RESULT; + // Reverse the path. dtNode* prev = 0; dtNode* node = lastBestNode; @@ -860,13 +1115,18 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, do { path[n++] = node->id; + if (n >= maxPath) + { + status |= DT_BUFFER_TOO_SMALL; + break; + } node = m_nodePool->getNodeAtIdx(node->pidx); } - while (node && n < maxPath); + while (node); *pathCount = n; - return DT_SUCCESS; + return status; } /// @par @@ -926,7 +1186,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef return m_query.status; } -dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) +dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) { if (!dtStatusInProgress(m_query.status)) return m_query.status; @@ -952,7 +1212,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) if (bestNode->id == m_query.endRef) { m_query.lastBestNode = bestNode; - m_query.status = DT_SUCCESS; + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -965,6 +1228,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -980,6 +1245,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; + if (doneIters) + *doneIters = iter; return m_query.status; } } @@ -1003,7 +1270,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + m_query.status |= DT_OUT_OF_NODES; continue; + } // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -1056,7 +1326,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) // Add or update the node. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->cost = cost; neighbourNode->total = total; @@ -1083,7 +1353,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) // Exhausted all nodes, but could not find path. if (m_openList->empty()) - m_query.status = DT_SUCCESS; + { + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + m_query.status = DT_SUCCESS | details; + } + + if (doneIters) + *doneIters = iter; return m_query.status; } @@ -1110,6 +1386,10 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, { // Reverse the path. dtAssert(m_query.lastBestNode); + + if (m_query.lastBestNode->id != m_query.endRef) + m_query.status |= DT_PARTIAL_RESULT; + dtNode* prev = 0; dtNode* node = m_query.lastBestNode; do @@ -1126,17 +1406,24 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, do { path[n++] = node->id; + if (n >= maxPath) + { + m_query.status |= DT_BUFFER_TOO_SMALL; + break; + } node = m_nodePool->getNodeAtIdx(node->pidx); } - while (node && n < maxPath); + while (node); } + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); *pathCount = n; - return DT_SUCCESS; + return DT_SUCCESS | details; } dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, @@ -1149,7 +1436,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing return DT_FAILURE; } - if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS) + if (dtStatusFailed(m_query.status)) { // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); @@ -1177,7 +1464,9 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing if (!node) { - return DT_FAILURE; + m_query.status |= DT_PARTIAL_RESULT; + dtAssert(m_query.lastBestNode); + node = m_query.lastBestNode; } // Reverse the path. @@ -1195,24 +1484,128 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing do { path[n++] = node->id; + if (n >= maxPath) + { + m_query.status |= DT_BUFFER_TOO_SMALL; + break; + } node = m_nodePool->getNodeAtIdx(node->pidx); } - while (node && n < maxPath); + while (node); } + const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; + // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); *pathCount = n; - return DT_SUCCESS; + return DT_SUCCESS | details; } +dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath) const +{ + if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos)) + { + // The vertices are equal, update flags and poly. + if (straightPathFlags) + straightPathFlags[(*straightPathCount)-1] = flags; + if (straightPathRefs) + straightPathRefs[(*straightPathCount)-1] = ref; + } + else + { + // Append new vertex. + dtVcopy(&straightPath[(*straightPathCount)*3], pos); + if (straightPathFlags) + straightPathFlags[(*straightPathCount)] = flags; + if (straightPathRefs) + straightPathRefs[(*straightPathCount)] = ref; + (*straightPathCount)++; + // If reached end of path or there is no space to append more vertices, return. + if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath) + { + return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); + } + } + return DT_IN_PROGRESS; +} + +dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const +{ + const float* startPos = &straightPath[(*straightPathCount-1)*3]; + // Append or update last vertex + dtStatus stat = 0; + for (int i = startIdx; i < endIdx; i++) + { + // Calculate portal + const dtPolyRef from = path[i]; + const dtMeshTile* fromTile = 0; + const dtPoly* fromPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtPolyRef to = path[i+1]; + const dtMeshTile* toTile = 0; + const dtPoly* toPoly = 0; + if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) + return DT_FAILURE | DT_INVALID_PARAM; + + float left[3], right[3]; + if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) + break; + + if (options & DT_STRAIGHTPATH_AREA_CROSSINGS) + { + // Skip intersection if only area crossings are requested. + if (fromPoly->getArea() == toPoly->getArea()) + continue; + } + + // Append intersection + float s,t; + if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t)) + { + float pt[3]; + dtVlerp(pt, left,right, t); + + stat = appendVertex(pt, 0, path[i+1], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; + } + } + return DT_IN_PROGRESS; +} + +/// @par +/// +/// This method peforms what is often called 'string pulling'. +/// +/// The start position is clamped to the first polygon in the path, and the +/// end position is clamped to the last. So the start and end positions should +/// normally be within or very near the first and last polygons respectively. +/// +/// The returned polygon references represent the reference id of the polygon +/// that is entered at the associated path position. The reference id associated +/// with the end point will always be zero. This allows, for example, matching +/// off-mesh link points to their representative polygons. +/// +/// If the provided result buffers are too small for the entire result set, +/// they will be filled as far as possible from the start toward the end +/// position. +/// dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos, const dtPolyRef* path, const int pathSize, float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const + int* straightPathCount, const int maxStraightPath, const int options) const { dtAssert(m_nav); @@ -1224,29 +1617,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en if (!path[0]) return DT_FAILURE | DT_INVALID_PARAM; - int n = 0; + dtStatus stat = 0; // TODO: Should this be callers responsibility? float closestStartPos[3]; if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos))) return DT_FAILURE | DT_INVALID_PARAM; + + float closestEndPos[3]; + if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos))) + return DT_FAILURE | DT_INVALID_PARAM; // Add start point. - dtVcopy(&straightPath[n*3], closestStartPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_START; - if (straightPathRefs) - straightPathRefs[n] = path[0]; - n++; - if (n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - - float closestEndPos[3]; - if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; + stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; if (pathSize > 1) { @@ -1274,17 +1661,28 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en // Next portal. if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType))) { - if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; + // Failed to get portal points, in practice this means that path[i+1] is invalid polygon. + // Clamp the end point to path[i], and return the path so far. - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = 0; - if (straightPathRefs) - straightPathRefs[n] = path[i]; - n++; + if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos))) + { + // This should only happen when the first polygon is invalid. + return DT_FAILURE | DT_INVALID_PARAM; + } + + // Apeend portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, i, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + } + + stat = appendVertex(closestEndPos, 0, path[i], + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); - return DT_SUCCESS; + return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); } // If starting really close the portal, advance. @@ -1316,6 +1714,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } else { + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, leftIndex, portalLeft, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + dtVcopy(portalApex, portalLeft); apexIndex = leftIndex; @@ -1326,30 +1734,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; dtPolyRef ref = leftPolyRef; - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } + // Append or update vertex + stat = appendVertex(portalApex, flags, ref, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; dtVcopy(portalLeft, portalApex); dtVcopy(portalRight, portalApex); @@ -1375,6 +1765,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } else { + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, rightIndex, portalRight, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } + dtVcopy(portalApex, portalRight); apexIndex = rightIndex; @@ -1384,31 +1784,13 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; dtPolyRef ref = rightPolyRef; - - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } + + // Append or update vertex + stat = appendVertex(portalApex, flags, ref, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); + if (stat != DT_IN_PROGRESS) + return stat; dtVcopy(portalLeft, portalApex); dtVcopy(portalRight, portalApex); @@ -1422,25 +1804,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } } } + + // Append portals along the current straight path segment. + if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) + { + stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath, options); + if (stat != DT_IN_PROGRESS) + return stat; + } } + + stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, + straightPath, straightPathFlags, straightPathRefs, + straightPathCount, maxStraightPath); - // If the point already exists, remove it and add reappend the actual end location. - if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos)) - n--; - - // Add end point. - if (n < maxStraightPath) - { - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_END; - if (straightPathRefs) - straightPathRefs[n] = 0; - n++; - } - - *straightPathCount = n; - return DT_SUCCESS; + return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); } /// @par @@ -1478,6 +1858,8 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE | DT_INVALID_PARAM; + dtStatus status = DT_SUCCESS; + static const int MAX_STACK = 48; dtNode* stack[MAX_STACK]; int nstack = 0; @@ -1641,16 +2023,21 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start do { visited[n++] = node->id; + if (n >= maxVisitedSize) + { + status |= DT_BUFFER_TOO_SMALL; + break; + } node = m_tinyNodePool->getNodeAtIdx(node->pidx); } - while (node && n < maxVisitedSize); + while (node); } dtVcopy(resultPos, bestPos); *visitedCount = n; - return DT_SUCCESS; + return status; } @@ -1753,7 +2140,7 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mi { float left[3], right[3]; unsigned char fromType, toType; - if (!getPortalPoints(from, to, left,right, fromType, toType)) + if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType))) return DT_FAILURE | DT_INVALID_PARAM; mid[0] = (left[0]+right[0])*0.5f; mid[1] = (left[1]+right[1])*0.5f; @@ -1834,6 +2221,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons hitNormal[1] = 0; hitNormal[2] = 0; + dtStatus status = DT_SUCCESS; + while (curRef) { // Cast ray against current polygon. @@ -1858,7 +2247,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons // Could not hit the polygon, keep the old t and report hit. if (pathCount) *pathCount = n; - return DT_SUCCESS; + return status; } // Keep track of furthest t so far. if (tmax > *t) @@ -1867,6 +2256,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons // Store visited polygons. if (n < maxPath) path[n++] = curRef; + else + status |= DT_BUFFER_TOO_SMALL; // Ray end is completely inside the polygon. if (segMax == -1) @@ -1874,7 +2265,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons *t = FLT_MAX; if (pathCount) *pathCount = n; - return DT_SUCCESS; + return status; } // Follow neighbours. @@ -1976,7 +2367,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons if (pathCount) *pathCount = n; - return DT_SUCCESS; + return status; } // No hit, advance to neighbour polygon. @@ -1986,7 +2377,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons if (pathCount) *pathCount = n; - return DT_SUCCESS; + return status; } /// @par @@ -2045,6 +2436,8 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* startNode->flags = DT_NODE_OPEN; m_openList->push(startNode); + dtStatus status = DT_SUCCESS; + int n = 0; if (n < maxResult) { @@ -2056,6 +2449,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* resultCost[n] = 0; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } const float radiusSqr = dtSqr(radius); @@ -2111,7 +2508,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } if (neighbourNode->flags & DT_NODE_CLOSED) continue; @@ -2127,7 +2527,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* continue; neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->total = total; @@ -2147,6 +2547,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* resultCost[n] = neighbourNode->total; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } neighbourNode->flags = DT_NODE_OPEN; m_openList->push(neighbourNode); } @@ -2155,7 +2559,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* *resultCount = n; - return DT_SUCCESS; + return status; } /// @par @@ -2212,6 +2616,8 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v startNode->flags = DT_NODE_OPEN; m_openList->push(startNode); + dtStatus status = DT_SUCCESS; + int n = 0; if (n < maxResult) { @@ -2223,6 +2629,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v resultCost[n] = 0; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } while (!m_openList->empty()) { @@ -2278,7 +2688,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } if (neighbourNode->flags & DT_NODE_CLOSED) continue; @@ -2294,7 +2707,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v continue; neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->total = total; @@ -2314,6 +2727,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v resultCost[n] = neighbourNode->total; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } neighbourNode->flags = DT_NODE_OPEN; m_openList->push(neighbourNode); } @@ -2322,7 +2739,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v *resultCount = n; - return DT_SUCCESS; + return status; } /// @par @@ -2378,6 +2795,8 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* float pa[DT_VERTS_PER_POLYGON*3]; float pb[DT_VERTS_PER_POLYGON*3]; + dtStatus status = DT_SUCCESS; + int n = 0; if (n < maxResult) { @@ -2386,6 +2805,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* resultParent[n] = 0; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } while (nstack) { @@ -2499,6 +2922,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* resultParent[n] = curRef; ++n; } + else + { + status |= DT_BUFFER_TOO_SMALL; + } if (nstack < MAX_STACK) { @@ -2509,17 +2936,18 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* *resultCount = n; - return DT_SUCCESS; + return status; } struct dtSegInterval { + dtPolyRef ref; short tmin, tmax; }; static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, - const short tmin, const short tmax) + const short tmin, const short tmax, const dtPolyRef ref) { if (nints+1 > maxInts) return; // Find insertion point. @@ -2534,6 +2962,7 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, if (nints-idx) memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx)); // Store + ints[idx].ref = ref; ints[idx].tmin = tmin; ints[idx].tmax = tmax; nints++; @@ -2551,7 +2980,8 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, /// maximum segments per polygon of the source navigation mesh. /// dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segments, int* segmentCount, const int maxSegments) const + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const { dtAssert(m_nav); @@ -2567,6 +2997,10 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* dtSegInterval ints[MAX_INTERVAL]; int nints; + const bool storePortals = segmentRefs != 0; + + dtStatus status = DT_SUCCESS; + for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++) { // Skip non-solid edges. @@ -2586,54 +3020,95 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); if (filter->passFilter(link->ref, neiTile, neiPoly)) { - insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax); + insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref); } } } } } - else if (poly->neis[j]) + else { // Internal edge - const unsigned int idx = (unsigned int)(poly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx; - if (filter->passFilter(ref, tile, &tile->polys[idx])) + dtPolyRef neiRef = 0; + if (poly->neis[j]) + { + const unsigned int idx = (unsigned int)(poly->neis[j]-1); + neiRef = m_nav->getPolyRefBase(tile) | idx; + if (!filter->passFilter(neiRef, tile, &tile->polys[idx])) + neiRef = 0; + } + + // If the edge leads to another polygon and portals are not stored, skip. + if (neiRef != 0 && !storePortals) continue; + + if (n < maxSegments) + { + const float* vj = &tile->verts[poly->verts[j]*3]; + const float* vi = &tile->verts[poly->verts[i]*3]; + float* seg = &segmentVerts[n*6]; + dtVcopy(seg+0, vj); + dtVcopy(seg+3, vi); + if (segmentRefs) + segmentRefs[n] = neiRef; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } + + continue; } // Add sentinels - insertInterval(ints, nints, MAX_INTERVAL, -1, 0); - insertInterval(ints, nints, MAX_INTERVAL, 255, 256); + insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0); + insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0); - // Store segment. + // Store segments. const float* vj = &tile->verts[poly->verts[j]*3]; const float* vi = &tile->verts[poly->verts[i]*3]; for (int k = 1; k < nints; ++k) { - // Find the space inbetween the opening areas. - const int imin = ints[k-1].tmax; - const int imax = ints[k].tmin; - if (imin == imax) continue; - if (imin == 0 && imax == 255) + // Portal segment. + if (storePortals && ints[k].ref) { + const float tmin = ints[k].tmin/255.0f; + const float tmax = ints[k].tmax/255.0f; if (n < maxSegments) { - float* seg = &segments[n*6]; + float* seg = &segmentVerts[n*6]; + dtVlerp(seg+0, vj,vi, tmin); + dtVlerp(seg+3, vj,vi, tmax); + if (segmentRefs) + segmentRefs[n] = ints[k].ref; n++; - dtVcopy(seg+0, vj); - dtVcopy(seg+3, vi); + } + else + { + status |= DT_BUFFER_TOO_SMALL; } } - else + + // Wall segment. + const int imin = ints[k-1].tmax; + const int imax = ints[k].tmin; + if (imin != imax) { const float tmin = imin/255.0f; const float tmax = imax/255.0f; if (n < maxSegments) { - float* seg = &segments[n*6]; - n++; + float* seg = &segmentVerts[n*6]; dtVlerp(seg+0, vj,vi, tmin); dtVlerp(seg+3, vj,vi, tmax); + if (segmentRefs) + segmentRefs[n] = 0; + n++; + } + else + { + status |= DT_BUFFER_TOO_SMALL; } } } @@ -2641,7 +3116,7 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* *segmentCount = n; - return DT_SUCCESS; + return status; } /// @par @@ -2680,6 +3155,8 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen float radiusSqr = dtSqr(maxRadius); + dtStatus status = DT_SUCCESS; + while (!m_openList->empty()) { dtNode* bestNode = m_openList->pop(); @@ -2787,7 +3264,10 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; continue; + } if (neighbourNode->flags & DT_NODE_CLOSED) continue; @@ -2806,7 +3286,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen continue; neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->total = total; @@ -2828,7 +3308,21 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen *hitDist = sqrtf(radiusSqr); - return DT_SUCCESS; + return status; +} + +bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const +{ + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly); + // If cannot get polygon, assume it does not exists and boundary is invalid. + if (dtStatusFailed(status)) + return false; + // If cannot pass filter, assume flags has changed and boundary is invalid. + if (!filter->passFilter(ref, tile, poly)) + return false; + return true; } /// @par diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.h b/dep/recastnavigation/Detour/DetourNavMeshQuery.h index 31b567e0b50..4a5112c9eb9 100644 --- a/dep/recastnavigation/Detour/DetourNavMeshQuery.h +++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.h @@ -130,30 +130,9 @@ public: /// @returns The status flags for the query. dtStatus init(const dtNavMesh* nav, const int maxNodes); - // Finds the nearest navigation polygon around the center location. - // Params: - // center[3] - (in) The center of the search box. - // extents[3] - (in) The extents of the search box. - // filter - (in) path polygon filter. - // nearestRef - (out) Reference to the nearest polygon. - // nearestPt[3] - (out, opt) The nearest point on found polygon, null if not needed. - // Returns: Reference identifier for the polygon, or 0 if no polygons found. - dtStatus findNearestPoly(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const; - - // Returns polygons which overlap the query box. - // Params: - // center[3] - (in) the center of the search box. - // extents[3] - (in) the extents of the search box. - // filter - (in) path polygon filter. - // polys - (out) array holding the search result. - // polyCount - (out) Number of polygons in search result array. - // maxPolys - (in) The max number of polygons the polys array can hold. - dtStatus queryPolygons(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const; - + /// @name Standard Pathfinding Functions + // /@{ + /// Finds a path from the start polygon to the end polygon. /// @param[in] startRef The refrence id of the start polygon. /// @param[in] endRef The reference id of the end polygon. @@ -169,6 +148,31 @@ public: const dtQueryFilter* filter, dtPolyRef* path, int* pathCount, const int maxPath) const; + /// Finds the straight path from the start to the end position within the polygon corridor. + /// @param[in] startPos Path start position. [(x, y, z)] + /// @param[in] endPos Path end position. [(x, y, z)] + /// @param[in] path An array of polygon references that represent the path corridor. + /// @param[in] pathSize The number of polygons in the @p path array. + /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. + /// @param[out] straightPathFlags Flags describing each point. (See: #dtStraightPathFlags) [opt] + /// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt] + /// @param[out] straightPathCount The number of points in the straight path. + /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] + /// @param[in] options Query options. (see: #dtStraightPathOptions) + /// @returns The status flags for the query. + dtStatus findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options = 0) const; + + ///@} + /// @name Sliced Pathfinding Functions + /// Common use case: + /// -# Call initSlicedFindPath() to initialize the sliced path query. + /// -# Call updateSlicedFindPath() until it returns complete. + /// -# Call finalizeSlicedFindPath() to get the path. + ///@{ + /// Intializes a sliced path query. /// @param[in] startRef The refrence id of the start polygon. /// @param[in] endRef The reference id of the end polygon. @@ -181,10 +185,10 @@ public: const dtQueryFilter* filter); /// Updates an in-progress sliced path query. - // Params: - // maxIter - (in) max number of iterations to update. - // Returns: Path query state. - dtStatus updateSlicedFindPath(const int maxIter); + /// @param[in] maxIter The maximum number of iterations to perform. + /// @param[out] doneIters The actual number of iterations completed. [opt] + /// @returns The status flags for the query. + dtStatus updateSlicedFindPath(const int maxIter, int* doneIters); /// Finalizes and returns the results of a sliced path query. /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) @@ -196,8 +200,8 @@ public: /// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest /// polygon on the existing path that was visited during the search. - /// @param[out] existing An array of polygon references for the existing path. - /// @param[out] existingSize The number of polygon in the @p existing array. + /// @param[in] existing An array of polygon references for the existing path. + /// @param[in] existingSize The number of polygon in the @p existing array. /// @param[out] path An ordered list of polygon references representing the path. (Start to end.) /// [(polyRef) * @p pathCount] /// @param[out] pathCount The number of polygons returned in the @p path array. @@ -205,74 +209,11 @@ public: /// @returns The status flags for the query. dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, dtPolyRef* path, int* pathCount, const int maxPath); - - // Finds a straight path from start to end locations within the corridor - // described by the path polygons. - // Start and end locations will be clamped on the corridor. - // The returned polygon references are point to polygon which was entered when - // a path point was added. For the end point, zero will be returned. This allows - // to match for example off-mesh link points to their representative polygons. - // Params: - // startPos[3] - (in) Path start location. - // endPo[3] - (in) Path end location. - // path - (in) Array of connected polygons describing the corridor. - // pathSize - (in) Number of polygons in path array. - // straightPath - (out) Points describing the straight path. - // straightPathFlags - (out, opt) Flags describing each point type, see dtStraightPathFlags. - // straightPathRefs - (out, opt) References to polygons at point locations. - // straightPathCount - (out) Number of points in the path. - // maxStraightPath - (in) The max number of points the straight path array can hold. Must be at least 1. - dtStatus findStraightPath(const float* startPos, const float* endPos, - const dtPolyRef* path, const int pathSize, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const; - - // Moves from startPos to endPos constrained to the navmesh. - // If the endPos is reachable, the resultPos will be endPos, - // or else the resultPos will be the nearest point in navmesh. - // Note: The resulting point is not projected to the ground, use getPolyHeight() to get height. - // Note: The algorithm is optimized for small delta movement and small number of polygons. - // Params: - // startRef - (in) ref to the polygon where startPos lies. - // startPos[3] - (in) start position of the mover. - // endPos[3] - (in) desired end position of the mover. - // filter - (in) path polygon filter. - // resultPos[3] - (out) new position of the mover. - // visited - (out) array of visited polygons. - // visitedCount - (out) Number of entries in the visited array. - // maxVisitedSize - (in) max number of polygons in the visited array. - dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; - - // Casts 'walkability' ray along the navmesh surface from startPos towards the endPos. - // Params: - // startRef - (in) ref to the polygon where the start lies. - // startPos[3] - (in) start position of the query. - // endPos[3] - (in) end position of the query. - // t - (out) hit parameter along the segment, FLT_MAX if no hit. - // hitNormal[3] - (out) normal of the nearest hit. - // filter - (in) path polygon filter. - // path - (out,opt) visited path polygons. - // pathCount - (out,opt) Number of polygons visited. - // maxPath - (in) max number of polygons in the path array. - dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const; - - // Returns distance to nearest wall from the specified location. - // Params: - // startRef - (in) ref to the polygon where the center lies. - // centerPos[3] - (in) center if the query circle. - // maxRadius - (in) max search radius. - // filter - (in) path polygon filter. - // hitDist - (out) distance to nearest wall from the test location. - // hitPos[3] - (out) location of the nearest hit. - // hitNormal[3] - (out) normal of the nearest hit. - dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const; - + + ///@} + /// @name Dijkstra Search Functions + /// @{ + /// Finds the polygons along the navigation graph that touch the specified circle. /// @param[in] startRef The reference id of the polygon where the search starts. /// @param[in] centerPos The center of the search circle. [(x, y, z)] @@ -308,6 +249,33 @@ public: dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, int* resultCount, const int maxResult) const; + /// @} + /// @name Local Query Functions + ///@{ + + /// Finds the polygon nearest to the specified center point. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] extents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] nearestRef The reference id of the nearest polygon. + /// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findNearestPoly(const float* center, const float* extents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt) const; + + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] extents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] polys The reference ids of the polygons that overlap the query box. + /// @param[out] polyCount The number of polygons in the search result. + /// @param[in] maxPolys The maximum number of polygons the search result can hold. + /// @returns The status flags for the query. + dtStatus queryPolygons(const float* center, const float* extents, + const dtQueryFilter* filter, + dtPolyRef* polys, int* polyCount, const int maxPolys) const; + /// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position. /// @param[in] startRef The reference id of the polygon where the search starts. /// @param[in] centerPos The center of the query circle. [(x, y, z)] @@ -323,23 +291,96 @@ public: const dtQueryFilter* filter, dtPolyRef* resultRef, dtPolyRef* resultParent, int* resultCount, const int maxResult) const; + + /// Moves from the start to the end position constrained to the navigation mesh. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)] + /// @param[in] endPos The desired end position of the mover. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] resultPos The result position of the mover. [(x, y, z)] + /// @param[out] visited The reference ids of the polygons visited during the move. + /// @param[out] visitedCount The number of polygons visited during the move. + /// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold. + /// @returns The status flags for the query. + dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const; + + /// Casts a 'walkability' ray along the surface of the navigation mesh from + /// the start position toward the end position. + /// @param[in] startRef The reference id of the start polygon. + /// @param[in] startPos A position within the start polygon representing + /// the start of the ray. [(x, y, z)] + /// @param[in] endPos The position to cast the ray toward. [(x, y, z)] + /// @param[out] t The hit parameter. (FLT_MAX if no wall hit.) + /// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] path The reference ids of the visited polygons. [opt] + /// @param[out] pathCount The number of visited polygons. [opt] + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. + /// @returns The status flags for the query. + dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, + const dtQueryFilter* filter, + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const; + + /// Finds the distance from the specified position to the nearest polygon wall. + /// @param[in] startRef The reference id of the polygon containing @p centerPos. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] maxRadius The radius of the search circle. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] hitDist The distance to the nearest wall from @p centerPos. + /// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)] + /// @param[out] hitNormal The normalized ray formed from the wall point to the + /// source point. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, + float* hitDist, float* hitPos, float* hitNormal) const; /// Returns the segments for the specified polygon, optionally including portals. - // Params: - // ref - (in) ref to the polygon. - // filter - (in) path polygon filter. - // segments[6*maxSegments] - (out) wall segments (2 endpoints per segment). - // segmentCount - (out) number of wall segments. - // maxSegments - (in) max number of segments that can be stored in 'segments'. + /// @param[in] ref The reference id of the polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount] + /// @param[out] segmentRefs The reference ids of each segment's neighbor polygon. + /// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] + /// @param[out] segmentCount The number of segments returned. + /// @param[in] maxSegments The maximum number of segments the result arrays can hold. + /// @returns The status flags for the query. dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segments, int* segmentCount, const int maxSegments) const; + float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, + const int maxSegments) const; + + /// Returns random location on navmesh. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. + /// @returns The status flags for the query. + dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// Returns random location on navmesh within the reach of specified location. + /// Polygons are chosen weighted by area. The search runs in linear related to number of polygon. + /// The location is not exactly constrained by the circle, but it limits the visited polygons. + /// @param[in] startRef The reference id of the polygon where the search starts. + /// @param[in] centerPos The center of the search circle. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] frand Function returning a random number [0..1). + /// @param[out] randomRef The reference id of the random location. + /// @param[out] randomPt The random location. [(x, y, z)] + /// @returns The status flags for the query. + dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; /// Finds the closest point on the specified polygon. /// @param[in] ref The reference id of the polygon. /// @param[in] pos The position to check. [(x, y, z)] /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] posOverPoly True of the position is over the polygon. /// @returns The status flags for the query. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const; + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; /// Returns a point on the boundary closest to the source point if the source point is outside the /// polygon's xz-bounds. @@ -349,15 +390,6 @@ public: /// @returns The status flags for the query. dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const; - // Returns start and end location of an off-mesh link polygon. - // Params: - // prevRef - (in) ref to the polygon before the link (used to select direction). - // polyRef - (in) ref to the off-mesh link polygon. - // startPos[3] - (out) start point of the link. - // endPos[3] - (out) end point of the link. - // Returns: true if link is found. - dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const; - /// Gets the height of the polygon at the provided position using the height detail. (Most accurate.) /// @param[in] ref The reference id of the polygon. /// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)] @@ -365,6 +397,15 @@ public: /// @returns The status flags for the query. dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const; + /// @} + /// @name Miscellaneous Functions + /// @{ + + /// Returns true if the polygon reference is valid and passes the filter restrictions. + /// @param[in] ref The polygon reference to check. + /// @param[in] filter The filter to apply. + bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const; + /// Returns true if the polygon reference is in the closed list. /// @param[in] ref The reference id of the polygon to check. /// @returns True if the polygon is in closed list. @@ -374,6 +415,12 @@ public: /// @returns The node pool. class dtNodePool* getNodePool() const { return m_nodePool; } + /// Gets the navigation mesh the query object is using. + /// @return The navigation mesh the query object is using. + const dtNavMesh* getAttachedNavMesh() const { return m_nav; } + + /// @} + private: /// Returns neighbour tile based on side. @@ -382,12 +429,7 @@ private: /// Queries polygons within a tile. int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, dtPolyRef* polys, const int maxPolys) const; - /// Find nearest polygon within a tile. - dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const; - /// Returns closest point on polygon. - dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; - + /// Returns portal points between two polygons. dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, unsigned char& fromType, unsigned char& toType) const; @@ -401,6 +443,16 @@ private: dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, float* mid) const; + // Appends vertex to a straight path + dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath) const; + + // Appends intermediate portal points to a straight path. + dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, + float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, + int* straightPathCount, const int maxStraightPath, const int options) const; + const dtNavMesh* m_nav; ///< Pointer to navmesh data. struct dtQueryData diff --git a/dep/recastnavigation/Detour/DetourNode.cpp b/dep/recastnavigation/Detour/DetourNode.cpp index 03c6e370aac..4c8215e20d0 100644 --- a/dep/recastnavigation/Detour/DetourNode.cpp +++ b/dep/recastnavigation/Detour/DetourNode.cpp @@ -47,15 +47,15 @@ dtNodePool::dtNodePool(int maxNodes, int hashSize) : dtAssert(m_maxNodes > 0); m_nodes = (dtNode*)dtAlloc(sizeof(dtNode)*m_maxNodes, DT_ALLOC_PERM); - m_next = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_maxNodes, DT_ALLOC_PERM); - m_first = (unsigned short*)dtAlloc(sizeof(unsigned short)*hashSize, DT_ALLOC_PERM); + m_next = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*m_maxNodes, DT_ALLOC_PERM); + m_first = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*hashSize, DT_ALLOC_PERM); dtAssert(m_nodes); dtAssert(m_next); dtAssert(m_first); - memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); - memset(m_next, 0xff, sizeof(unsigned short)*m_maxNodes); + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); + memset(m_next, 0xff, sizeof(dtNodeIndex)*m_maxNodes); } dtNodePool::~dtNodePool() @@ -67,14 +67,14 @@ dtNodePool::~dtNodePool() void dtNodePool::clear() { - memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); + memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize); m_nodeCount = 0; } dtNode* dtNodePool::findNode(dtPolyRef id) { unsigned int bucket = dtHashRef(id) & (m_hashSize-1); - unsigned short i = m_first[bucket]; + dtNodeIndex i = m_first[bucket]; while (i != DT_NULL_IDX) { if (m_nodes[i].id == id) @@ -87,7 +87,7 @@ dtNode* dtNodePool::findNode(dtPolyRef id) dtNode* dtNodePool::getNode(dtPolyRef id) { unsigned int bucket = dtHashRef(id) & (m_hashSize-1); - unsigned short i = m_first[bucket]; + dtNodeIndex i = m_first[bucket]; dtNode* node = 0; while (i != DT_NULL_IDX) { @@ -99,7 +99,7 @@ dtNode* dtNodePool::getNode(dtPolyRef id) if (m_nodeCount >= m_maxNodes) return 0; - i = (unsigned short)m_nodeCount; + i = (dtNodeIndex)m_nodeCount; m_nodeCount++; // Init node diff --git a/dep/recastnavigation/Detour/DetourNode.h b/dep/recastnavigation/Detour/DetourNode.h index aec26d2fc57..b68c922d038 100644 --- a/dep/recastnavigation/Detour/DetourNode.h +++ b/dep/recastnavigation/Detour/DetourNode.h @@ -27,7 +27,8 @@ enum dtNodeFlags DT_NODE_CLOSED = 0x02, }; -static const unsigned short DT_NULL_IDX = 0xffff; +typedef unsigned short dtNodeIndex; +static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0; struct dtNode { @@ -72,21 +73,21 @@ public: { return sizeof(*this) + sizeof(dtNode)*m_maxNodes + - sizeof(unsigned short)*m_maxNodes + - sizeof(unsigned short)*m_hashSize; + sizeof(dtNodeIndex)*m_maxNodes + + sizeof(dtNodeIndex)*m_hashSize; } inline int getMaxNodes() const { return m_maxNodes; } inline int getHashSize() const { return m_hashSize; } - inline unsigned short getFirst(int bucket) const { return m_first[bucket]; } - inline unsigned short getNext(int i) const { return m_next[i]; } + inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; } + inline dtNodeIndex getNext(int i) const { return m_next[i]; } private: dtNode* m_nodes; - unsigned short* m_first; - unsigned short* m_next; + dtNodeIndex* m_first; + dtNodeIndex* m_next; const int m_maxNodes; const int m_hashSize; int m_nodeCount; diff --git a/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp b/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp index a255c9b3fd1..d3f90b7ab17 100644 --- a/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp +++ b/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp @@ -25,6 +25,7 @@ #include <float.h> #include <new> +static const float DT_PI = 3.14159265f; static int sweepCircleCircle(const float* c0, const float r0, const float* v, const float* c1, const float r1, @@ -206,12 +207,6 @@ void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr) dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() : - m_velBias(0.0f), - m_weightDesVel(0.0f), - m_weightCurVel(0.0f), - m_weightSide(0.0f), - m_weightToi(0.0f), - m_horizTime(0.0f), m_maxCircles(0), m_circles(0), m_ncircles(0), @@ -318,11 +313,11 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel) float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs, const float* pos, const float rad, - const float vmax, const float* vel, const float* dvel, + const float* vel, const float* dvel, dtObstacleAvoidanceDebugData* debug) { // Find min time of impact and exit amongst all obstacles. - float tmin = m_horizTime; + float tmin = m_params.horizTime; float side = 0; int nside = 0; @@ -395,11 +390,10 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs if (nside) side /= nside; - const float ivmax = 1.0f / vmax; - const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax); - const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax); - const float spen = m_weightSide * side; - const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime)); + const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax); + const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax); + const float spen = m_params.weightSide * side; + const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime)); const float penalty = vpen + vcpen + spen + tpen; @@ -410,28 +404,34 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs return penalty; } -void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, - float* nvel, const int gsize, - dtObstacleAvoidanceDebugData* debug) +int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const dtObstacleAvoidanceParams* params, + dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); + memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams)); + m_invHorizTime = 1.0f / m_params.horizTime; + m_vmax = vmax; + m_invVmax = 1.0f / vmax; + dtVset(nvel, 0,0,0); if (debug) debug->reset(); - const float cvx = dvel[0] * m_velBias; - const float cvz = dvel[2] * m_velBias; - const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1); - const float half = (gsize-1)*cs*0.5f; + const float cvx = dvel[0] * m_params.velBias; + const float cvz = dvel[2] * m_params.velBias; + const float cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1); + const float half = (m_params.gridSize-1)*cs*0.5f; float minPenalty = FLT_MAX; + int ns = 0; - for (int y = 0; y < gsize; ++y) + for (int y = 0; y < m_params.gridSize; ++y) { - for (int x = 0; x < gsize; ++x) + for (int x = 0; x < m_params.gridSize; ++x) { float vcand[3]; vcand[0] = cvx + x*cs - half; @@ -440,7 +440,8 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue; - const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug); + const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug); + ns++; if (penalty < minPenalty) { minPenalty = penalty; @@ -448,31 +449,38 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float } } } + + return ns; } -static const float DT_PI = 3.14159265f; - -void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug) +int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const dtObstacleAvoidanceParams* params, + dtObstacleAvoidanceDebugData* debug) { prepare(pos, dvel); + memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams)); + m_invHorizTime = 1.0f / m_params.horizTime; + m_vmax = vmax; + m_invVmax = 1.0f / vmax; + dtVset(nvel, 0,0,0); if (debug) debug->reset(); - + // Build sampling pattern aligned to desired velocity. - static const int MAX_PATTERN_DIVS = 32; - static const int MAX_PATTERN_RINGS = 4; - float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2]; + float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2]; int npat = 0; - const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS); - const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS); + const int ndivs = (int)m_params.adaptiveDivs; + const int nrings= (int)m_params.adaptiveRings; + const int depth = (int)m_params.adaptiveDepth; + + const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS); + const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS); const float da = (1.0f/nd) * DT_PI*2; const float dang = atan2f(dvel[2], dvel[0]); @@ -483,21 +491,22 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl for (int j = 0; j < nr; ++j) { - const float rad = (float)(nr-j)/(float)nr; + const float r = (float)(nr-j)/(float)nr; float a = dang + (j&1)*0.5f*da; for (int i = 0; i < nd; ++i) { - pat[npat*2+0] = cosf(a)*rad; - pat[npat*2+1] = sinf(a)*rad; + pat[npat*2+0] = cosf(a)*r; + pat[npat*2+1] = sinf(a)*r; npat++; a += da; } } // Start sampling. - float cr = vmax * (1.0f-m_velBias); + float cr = vmax * (1.0f - m_params.velBias); float res[3]; - dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias); + dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias); + int ns = 0; for (int k = 0; k < depth; ++k) { @@ -514,7 +523,8 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; - const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug); + const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug); + ns++; if (penalty < minPenalty) { minPenalty = penalty; @@ -528,5 +538,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl } dtVcopy(nvel, res); + + return ns; } diff --git a/dep/recastnavigation/Detour/DetourObstacleAvoidance.h b/dep/recastnavigation/Detour/DetourObstacleAvoidance.h index 4a7187a7998..8ff6211e867 100644 --- a/dep/recastnavigation/Detour/DetourObstacleAvoidance.h +++ b/dep/recastnavigation/Detour/DetourObstacleAvoidance.h @@ -21,21 +21,19 @@ struct dtObstacleCircle { - float p[3]; // Position of the obstacle - float vel[3]; // Velocity of the obstacle - float dvel[3]; // Velocity of the obstacle - float rad; // Radius of the obstacle - float dp[3], np[3]; // Use for side selection during sampling. + float p[3]; ///< Position of the obstacle + float vel[3]; ///< Velocity of the obstacle + float dvel[3]; ///< Velocity of the obstacle + float rad; ///< Radius of the obstacle + float dp[3], np[3]; ///< Use for side selection during sampling. }; struct dtObstacleSegment { - float p[3], q[3]; // End points of the obstacle segment + float p[3], q[3]; ///< End points of the obstacle segment bool touch; }; -static const int RVO_SAMPLE_RAD = 15; -static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100; class dtObstacleAvoidanceDebugData { @@ -75,6 +73,23 @@ dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData(); void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr); +static const int DT_MAX_PATTERN_DIVS = 32; ///< Max numver of adaptive divs. +static const int DT_MAX_PATTERN_RINGS = 4; ///< Max number of adaptive rings. + +struct dtObstacleAvoidanceParams +{ + float velBias; + float weightDesVel; + float weightCurVel; + float weightSide; + float weightToi; + float horizTime; + unsigned char gridSize; ///< grid + unsigned char adaptiveDivs; ///< adaptive + unsigned char adaptiveRings; ///< adaptive + unsigned char adaptiveDepth; ///< adaptive +}; + class dtObstacleAvoidanceQuery { public: @@ -90,22 +105,15 @@ public: void addSegment(const float* p, const float* q); - inline void setVelocitySelectionBias(float v) { m_velBias = v; } - inline void setDesiredVelocityWeight(float w) { m_weightDesVel = w; } - inline void setCurrentVelocityWeight(float w) { m_weightCurVel = w; } - inline void setPreferredSideWeight(float w) { m_weightSide = w; } - inline void setCollisionTimeWeight(float w) { m_weightToi = w; } - inline void setTimeHorizon(float t) { m_horizTime = t; } - - void sampleVelocityGrid(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int gsize, - dtObstacleAvoidanceDebugData* debug = 0); - - void sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, - const float* vel, const float* dvel, float* nvel, - const int ndivs, const int nrings, const int depth, - dtObstacleAvoidanceDebugData* debug = 0); + int sampleVelocityGrid(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const dtObstacleAvoidanceParams* params, + dtObstacleAvoidanceDebugData* debug = 0); + + int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, + const float* vel, const float* dvel, float* nvel, + const dtObstacleAvoidanceParams* params, + dtObstacleAvoidanceDebugData* debug = 0); inline int getObstacleCircleCount() const { return m_ncircles; } const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; } @@ -119,19 +127,17 @@ private: float processSample(const float* vcand, const float cs, const float* pos, const float rad, - const float vmax, const float* vel, const float* dvel, + const float* vel, const float* dvel, dtObstacleAvoidanceDebugData* debug); dtObstacleCircle* insertCircle(const float dist); dtObstacleSegment* insertSegment(const float dist); - float m_velBias; - float m_weightDesVel; - float m_weightCurVel; - float m_weightSide; - float m_weightToi; - float m_horizTime; - + dtObstacleAvoidanceParams m_params; + float m_invHorizTime; + float m_vmax; + float m_invVmax; + int m_maxCircles; dtObstacleCircle* m_circles; int m_ncircles; @@ -145,4 +151,4 @@ dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery(); void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr); -#endif // DETOUROBSTACLEAVOIDANCE_H
\ No newline at end of file +#endif // DETOUROBSTACLEAVOIDANCE_H diff --git a/dep/recastnavigation/Recast/CMakeLists.txt b/dep/recastnavigation/Recast/CMakeLists.txt index e5e30294238..10028f8535f 100644 --- a/dep/recastnavigation/Recast/CMakeLists.txt +++ b/dep/recastnavigation/Recast/CMakeLists.txt @@ -14,6 +14,7 @@ set(Recast_STAT_SRCS RecastArea.cpp RecastContour.cpp RecastFilter.cpp + RecastLayers.cpp RecastMesh.cpp RecastMeshDetail.cpp RecastRasterization.cpp diff --git a/dep/recastnavigation/Recast/Recast.cpp b/dep/recastnavigation/Recast/Recast.cpp index 922c9163c13..b9d86036c3f 100644 --- a/dep/recastnavigation/Recast/Recast.cpp +++ b/dep/recastnavigation/Recast/Recast.cpp @@ -109,6 +109,28 @@ void rcFreeCompactHeightfield(rcCompactHeightfield* chf) rcFree(chf); } + +rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet() +{ + rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM); + memset(lset, 0, sizeof(rcHeightfieldLayerSet)); + return lset; +} + +void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset) +{ + if (!lset) return; + for (int i = 0; i < lset->nlayers; ++i) + { + rcFree(lset->layers[i].heights); + rcFree(lset->layers[i].areas); + rcFree(lset->layers[i].cons); + } + rcFree(lset->layers); + rcFree(lset); +} + + rcContourSet* rcAllocContourSet() { rcContourSet* cset = (rcContourSet*)rcAlloc(sizeof(rcContourSet), RC_ALLOC_PERM); @@ -186,12 +208,11 @@ void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* /// See the #rcConfig documentation for more information on the configuration parameters. /// /// @see rcAllocHeightfield, rcHeightfield -bool rcCreateHeightfield(rcContext* /*ctx*/, rcHeightfield& hf, int width, int height, +bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height, const float* bmin, const float* bmax, float cs, float ch) { - // TODO: VC complains about unref formal variable, figure out a way to handle this better. -// rcAssert(ctx); + rcIgnoreUnused(ctx); hf.width = width; hf.height = height; @@ -223,13 +244,12 @@ static void calcTriNormal(const float* v0, const float* v1, const float* v2, flo /// See the #rcConfig documentation for more information on the configuration parameters. /// /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles -void rcMarkWalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle, +void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const float* verts, int /*nv*/, const int* tris, int nt, unsigned char* areas) { - // TODO: VC complains about unref formal variable, figure out a way to handle this better. -// rcAssert(ctx); + rcIgnoreUnused(ctx); const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI); @@ -253,13 +273,12 @@ void rcMarkWalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle, /// See the #rcConfig documentation for more information on the configuration parameters. /// /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles -void rcClearUnwalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle, +void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const float* verts, int /*nv*/, const int* tris, int nt, unsigned char* areas) { - // TODO: VC complains about unref formal variable, figure out a way to handle this better. -// rcAssert(ctx); + rcIgnoreUnused(ctx); const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI); @@ -275,10 +294,9 @@ void rcClearUnwalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAng } } -int rcGetHeightFieldSpanCount(rcContext* /*ctx*/, rcHeightfield& hf) +int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf) { - // TODO: VC complains about unref formal variable, figure out a way to handle this better. -// rcAssert(ctx); + rcIgnoreUnused(ctx); const int w = hf.width; const int h = hf.height; @@ -417,13 +435,13 @@ bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const i if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb) { // Mark direction as walkable. - const int idx = k - (int)nc.index; - if (idx < 0 || idx > MAX_LAYERS) + const int lidx = k - (int)nc.index; + if (lidx < 0 || lidx > MAX_LAYERS) { - tooHighNeighbour = rcMax(tooHighNeighbour, idx); + tooHighNeighbour = rcMax(tooHighNeighbour, lidx); continue; } - rcSetCon(s, dir, idx); + rcSetCon(s, dir, lidx); break; } } diff --git a/dep/recastnavigation/Recast/Recast.h b/dep/recastnavigation/Recast/Recast.h index 57e98ea444a..3f4ae96d1c9 100644 --- a/dep/recastnavigation/Recast/Recast.h +++ b/dep/recastnavigation/Recast/Recast.h @@ -65,6 +65,8 @@ enum rcTimerLabel RC_TIMER_ERODE_AREA, /// The time to mark a box area. (See: #rcMarkBoxArea) RC_TIMER_MARK_BOX_AREA, + /// The time to mark a cylinder area. (See: #rcMarkCylinderArea) + RC_TIMER_MARK_CYLINDER_AREA, /// The time to mark a convex polygon area. (See: #rcMarkConvexPolyArea) RC_TIMER_MARK_CONVEXPOLY_AREA, /// The total time to build the distance field. (See: #rcBuildDistanceField) @@ -83,6 +85,8 @@ enum rcTimerLabel RC_TIMER_BUILD_REGIONS_FLOOD, /// The time to filter out small regions. (See: #rcBuildRegions, #rcBuildRegionsMonotone) RC_TIMER_BUILD_REGIONS_FILTER, + /// The time to build heightfield layers. (See: #rcBuildHeightfieldLayers) + RC_TIMER_BUILD_LAYERS, /// The time to build the polygon mesh detail. (See: #rcBuildPolyMeshDetail) RC_TIMER_BUILD_POLYMESHDETAIL, /// The time to merge polygon mesh details. (See: #rcMergePolyMeshDetails) @@ -215,7 +219,7 @@ struct rcConfig int maxEdgeLen; /// The maximum distance a simplfied contour's border edges should deviate - /// the original raw contour. [Limit: >=0] [Units: wu] + /// the original raw contour. [Limit: >=0] [Units: vx] float maxSimplificationError; /// The minimum number of cells allowed to form isolated island areas. [Limit: >=0] [Units: vx] @@ -280,9 +284,6 @@ struct rcHeightfield rcSpan* freelist; ///< The next free span. }; -rcHeightfield* rcAllocHeightfield(); -void rcFreeHeightField(rcHeightfield* hf); - /// Provides information on the content of a cell column in a compact heightfield. struct rcCompactCell { @@ -308,6 +309,7 @@ struct rcCompactHeightfield int spanCount; ///< The number of spans in the heightfield. int walkableHeight; ///< The walkable height used during the build of the field. (See: rcConfig::walkableHeight) int walkableClimb; ///< The walkable climb used during the build of the field. (See: rcConfig::walkableClimb) + int borderSize; ///< The AABB border size used during the build of the field. (See: rcConfig::borderSize) unsigned short maxDistance; ///< The maximum distance value of any span within the field. unsigned short maxRegions; ///< The maximum region id of any span within the field. float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] @@ -320,9 +322,35 @@ struct rcCompactHeightfield unsigned char* areas; ///< Array containing area id data. [Size: #spanCount] }; -rcCompactHeightfield* rcAllocCompactHeightfield(); -void rcFreeCompactHeightfield(rcCompactHeightfield* chf); +/// Represents a heightfield layer within a layer set. +/// @see rcHeightfieldLayerSet +struct rcHeightfieldLayer +{ + float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] + float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] + float cs; ///< The size of each cell. (On the xz-plane.) + float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + int width; ///< The width of the heightfield. (Along the x-axis in cell units.) + int height; ///< The height of the heightfield. (Along the z-axis in cell units.) + int minx; ///< The minimum x-bounds of usable data. + int maxx; ///< The maximum x-bounds of usable data. + int miny; ///< The minimum y-bounds of usable data. (Along the z-axis.) + int maxy; ///< The maximum y-bounds of usable data. (Along the z-axis.) + int hmin; ///< The minimum height bounds of usable data. (Along the y-axis.) + int hmax; ///< The maximum height bounds of usable data. (Along the y-axis.) + unsigned char* heights; ///< The heightfield. [Size: (width - borderSize*2) * (h - borderSize*2)] + unsigned char* areas; ///< Area ids. [Size: Same as #heights] + unsigned char* cons; ///< Packed neighbor connection information. [Size: Same as #heights] +}; +/// Represents a set of heightfield layers. +/// @ingroup recast +/// @see rcAllocHeightfieldLayerSet, rcFreeHeightfieldLayerSet +struct rcHeightfieldLayerSet +{ + rcHeightfieldLayer* layers; ///< The layers in the set. [Size: #nlayers] + int nlayers; ///< The number of layers in the set. +}; /// Represents a simple, non-overlapping contour in field space. struct rcContour @@ -345,12 +373,11 @@ struct rcContourSet float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] float cs; ///< The size of each cell. (On the xz-plane.) float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + int width; ///< The width of the set. (Along the x-axis in cell units.) + int height; ///< The height of the set. (Along the z-axis in cell units.) + int borderSize; ///< The AABB border size used to generate the source data from which the contours were derived. }; -rcContourSet* rcAllocContourSet(); -void rcFreeContourSet(rcContourSet* cset); - - /// Represents a polygon mesh suitable for use in building a navigation mesh. /// @ingroup recast struct rcPolyMesh @@ -368,12 +395,9 @@ struct rcPolyMesh float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] float cs; ///< The size of each cell. (On the xz-plane.) float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + int borderSize; ///< The AABB border size used to generate the source data from which the mesh was derived. }; -rcPolyMesh* rcAllocPolyMesh(); -void rcFreePolyMesh(rcPolyMesh* pmesh); - - /// Contains triangle meshes that represent detailed height data associated /// with the polygons in its associated polygon mesh object. /// @ingroup recast @@ -387,6 +411,71 @@ struct rcPolyMeshDetail int ntris; ///< The number of triangles in #tris. }; +/// @name Allocation Functions +/// Functions used to allocate and de-allocate Recast objects. +/// @see rcAllocSetCustom +/// @{ + +/// Allocates a heightfield object using the Recast allocator. +/// @return A heightfield that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcCreateHeightfield, rcFreeHeightField +rcHeightfield* rcAllocHeightfield(); + +/// Frees the specified heightfield object using the Recast allocator. +/// @param[in] hf A heightfield allocated using #rcAllocHeightfield +/// @ingroup recast +/// @see rcAllocHeightfield +void rcFreeHeightField(rcHeightfield* hf); + +/// Allocates a compact heightfield object using the Recast allocator. +/// @return A compact heightfield that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildCompactHeightfield, rcFreeCompactHeightfield +rcCompactHeightfield* rcAllocCompactHeightfield(); + +/// Frees the specified compact heightfield object using the Recast allocator. +/// @param[in] chf A compact heightfield allocated using #rcAllocCompactHeightfield +/// @ingroup recast +/// @see rcAllocCompactHeightfield +void rcFreeCompactHeightfield(rcCompactHeightfield* chf); + +/// Allocates a heightfield layer set using the Recast allocator. +/// @return A heightfield layer set that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildHeightfieldLayers, rcFreeHeightfieldLayerSet +rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet(); + +/// Frees the specified heightfield layer set using the Recast allocator. +/// @param[in] lset A heightfield layer set allocated using #rcAllocHeightfieldLayerSet +/// @ingroup recast +/// @see rcAllocHeightfieldLayerSet +void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset); + +/// Allocates a contour set object using the Recast allocator. +/// @return A contour set that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildContours, rcFreeContourSet +rcContourSet* rcAllocContourSet(); + +/// Frees the specified contour set using the Recast allocator. +/// @param[in] cset A contour set allocated using #rcAllocContourSet +/// @ingroup recast +/// @see rcAllocContourSet +void rcFreeContourSet(rcContourSet* cset); + +/// Allocates a polygon mesh object using the Recast allocator. +/// @return A polygon mesh that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildPolyMesh, rcFreePolyMesh +rcPolyMesh* rcAllocPolyMesh(); + +/// Frees the specified polygon mesh using the Recast allocator. +/// @param[in] pmesh A polygon mesh allocated using #rcAllocPolyMesh +/// @ingroup recast +/// @see rcAllocPolyMesh +void rcFreePolyMesh(rcPolyMesh* pmesh); + /// Allocates a detail mesh object using the Recast allocator. /// @return A detail mesh that is ready for initialization, or null on failure. /// @ingroup recast @@ -457,35 +546,14 @@ static const unsigned char RC_WALKABLE_AREA = 63; /// to another span. (Has no neighbor.) static const int RC_NOT_CONNECTED = 0x3f; -// Compact span neighbour helpers. -inline void rcSetCon(rcCompactSpan& s, int dir, int i) -{ - const unsigned int shift = (unsigned int)dir*6; - unsigned int con = s.con; - s.con = (con & ~(0x3f << shift)) | (((unsigned int)i & 0x3f) << shift); -} - -inline int rcGetCon(const rcCompactSpan& s, int dir) -{ - const unsigned int shift = (unsigned int)dir*6; - return (s.con >> shift) & 0x3f; -} - -inline int rcGetDirOffsetX(int dir) -{ - const int offset[4] = { -1, 0, 1, 0, }; - return offset[dir&0x03]; -} - -inline int rcGetDirOffsetY(int dir) -{ - const int offset[4] = { 0, 1, 0, -1 }; - return offset[dir&0x03]; -} - /// @name General helper functions /// @{ +/// Used to ignore a function parameter. VS complains about unused parameters +/// and this silences the warning. +/// @param [in] _ Unused parameter +template<class T> void rcIgnoreUnused(const T&) { } + /// Swaps the values of the two parameters. /// @param[in,out] a Value A /// @param[in,out] b Value B @@ -647,13 +715,6 @@ inline void rcVnormalize(float* v) v[2] *= d; } -inline bool rcVequal(const float* p0, const float* p1) -{ - static const float thr = rcSqr(1.0f/16384.0f); - const float d = rcVdistSqr(p0, p1); - return d < thr; -} - /// @} /// @name Heightfield Functions /// @see rcHeightfield @@ -718,18 +779,20 @@ void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const int* tris, int nt, unsigned char* areas); /// Adds a span to the specified heightfield. -// The span addition can set to favor flags. If the span is merged to -// another span and the new smax is within 'flagMergeThr' units away -// from the existing span the span flags are merged and stored. -// Params: -// solid - (in) heightfield where the spans is added to -// x,y - (in) location on the heightfield where the span is added -// smin,smax - (in) spans min/max height -// flags - (in) span flags (zero or WALKABLE) -// flagMergeThr - (in) merge threshold. -void rcAddSpan(rcContext* ctx, rcHeightfield& solid, const int x, const int y, +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] hf An initialized heightfield. +/// @param[in] x The width index where the span is to be added. +/// [Limits: 0 <= value < rcHeightfield::width] +/// @param[in] y The height index where the span is to be added. +/// [Limits: 0 <= value < rcHeightfield::height] +/// @param[in] smin The minimum height of the span. [Limit: < @p smax] [Units: vx] +/// @param[in] smax The maximum height of the span. [Limit: <= #RC_SPAN_MAX_HEIGHT] [Units: vx] +/// @param[in] area The area id of the span. [Limit: <= #RC_WALKABLE_AREA) +/// @param[in] flagMergeThr The merge theshold. [Limit: >= 0] [Units: vx] +void rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y, const unsigned short smin, const unsigned short smax, - const unsigned short area, const int flagMergeThr); + const unsigned char area, const int flagMergeThr); /// Rasterizes a triangle into the specified heightfield. /// @ingroup recast @@ -877,6 +940,28 @@ void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, const float hmin, const float hmax, unsigned char areaId, rcCompactHeightfield& chf); +/// Helper function to offset voncex polygons for rcMarkConvexPolyArea. +/// @ingroup recast +/// @param[in] verts The vertices of the polygon [Form: (x, y, z) * @p nverts] +/// @param[in] nverts The number of vertices in the polygon. +/// @param[out] outVerts The offset vertices (should hold up to 2 * @p nverts) [Form: (x, y, z) * return value] +/// @param[in] maxOutVerts The max number of vertices that can be stored to @p outVerts. +/// @returns Number of vertices in the offset polygon or 0 if too few vertices in @p outVerts. +int rcOffsetPoly(const float* verts, const int nverts, const float offset, + float* outVerts, const int maxOutVerts); + +/// Applies the area id to all spans within the specified cylinder. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] pos The center of the base of the cylinder. [Form: (x, y, z)] +/// @param[in] r The radius of the cylinder. +/// @param[in] h The height of the cylinder. +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] chf A populated compact heightfield. +void rcMarkCylinderArea(rcContext* ctx, const float* pos, + const float r, const float h, unsigned char areaId, + rcCompactHeightfield& chf); + /// Builds the distance field for the specified compact heightfield. /// @ingroup recast /// @param[in,out] ctx The build context to use during the operation. @@ -912,6 +997,68 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf, const int borderSize, const int minRegionArea, const int mergeRegionArea); + +/// Sets the neighbor connection data for the specified direction. +/// @param[in] s The span to update. +/// @param[in] dir The direction to set. [Limits: 0 <= value < 4] +/// @param[in] i The index of the neighbor span. +inline void rcSetCon(rcCompactSpan& s, int dir, int i) +{ + const unsigned int shift = (unsigned int)dir*6; + unsigned int con = s.con; + s.con = (con & ~(0x3f << shift)) | (((unsigned int)i & 0x3f) << shift); +} + +/// Gets neighbor connection data for the specified direction. +/// @param[in] s The span to check. +/// @param[in] dir The direction to check. [Limits: 0 <= value < 4] +/// @return The neighbor connection data for the specified direction, +/// or #RC_NOT_CONNECTED if there is no connection. +inline int rcGetCon(const rcCompactSpan& s, int dir) +{ + const unsigned int shift = (unsigned int)dir*6; + return (s.con >> shift) & 0x3f; +} + +/// Gets the standard width (x-axis) offset for the specified direction. +/// @param[in] dir The direction. [Limits: 0 <= value < 4] +/// @return The width offset to apply to the current cell position to move +/// in the direction. +inline int rcGetDirOffsetX(int dir) +{ + const int offset[4] = { -1, 0, 1, 0, }; + return offset[dir&0x03]; +} + +/// Gets the standard height (z-axis) offset for the specified direction. +/// @param[in] dir The direction. [Limits: 0 <= value < 4] +/// @return The height offset to apply to the current cell position to move +/// in the direction. +inline int rcGetDirOffsetY(int dir) +{ + const int offset[4] = { 0, 1, 0, -1 }; + return offset[dir&0x03]; +} + +/// @} +/// @name Layer, Contour, Polymesh, and Detail Mesh Functions +/// @see rcHeightfieldLayer, rcContourSet, rcPolyMesh, rcPolyMeshDetail +/// @{ + +/// Builds a layer set from the specified compact heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] chf A fully built compact heightfield. +/// @param[in] borderSize The size of the non-navigable border around the heightfield. [Limit: >=0] +/// [Units: vx] +/// @param[in] walkableHeight Minimum floor to 'ceiling' height that will still allow the floor area +/// to be considered walkable. [Limit: >= 3] [Units: vx] +/// @param[out] lset The resulting layer set. (Must be pre-allocated.) +/// @returns True if the operation completed successfully. +bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int walkableHeight, + rcHeightfieldLayerSet& lset); + /// Builds a contour set from the region outlines in the provided compact heightfield. /// @ingroup recast /// @param[in,out] ctx The build context to use during the operation. @@ -927,13 +1074,15 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, const float maxError, const int maxEdgeLen, rcContourSet& cset, const int flags = RC_CONTOUR_TESS_WALL_EDGES); -// Builds connected convex polygon mesh from contour polygons. -// Params: -// cset - (in) contour set. -// nvp - (in) maximum number of vertices per polygon. -// mesh - (out) poly mesh. -// Returns false if operation ran out of memory. -bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& mesh); +/// Builds a polygon mesh from the provided contours. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] cset A fully built contour set. +/// @param[in] nvp The maximum number of vertices allowed for polygons generated during the +/// contour to polygon conversion process. [Limit: >= 3] +/// @param[out] mesh The resulting polygon mesh. (Must be re-allocated.) +/// @returns True if the operation completed successfully. +bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh); /// Merges multiple polygon meshes into a single mesh. /// @ingroup recast @@ -958,6 +1107,14 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa const float sampleDist, const float sampleMaxError, rcPolyMeshDetail& dmesh); +/// Copies the poly mesh data from src to dst. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] src The source mesh to copy from. +/// @param[out] dst The resulting detail mesh. (Must be pre-allocated, must be empty mesh.) +/// @returns True if the operation completed successfully. +bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst); + /// Merges multiple detail meshes into a single detail mesh. /// @ingroup recast /// @param[in,out] ctx The build context to use during the operation. diff --git a/dep/recastnavigation/Recast/RecastArea.cpp b/dep/recastnavigation/Recast/RecastArea.cpp index 4e7b79d301d..1a338cd9b8c 100644 --- a/dep/recastnavigation/Recast/RecastArea.cpp +++ b/dep/recastnavigation/Recast/RecastArea.cpp @@ -61,14 +61,26 @@ bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf) const rcCompactCell& c = chf.cells[x+y*w]; for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) { - if (chf.areas[i] != RC_NULL_AREA) + if (chf.areas[i] == RC_NULL_AREA) + { + dist[i] = 0; + } + else { const rcCompactSpan& s = chf.spans[i]; int nc = 0; for (int dir = 0; dir < 4; ++dir) { if (rcGetCon(s, dir) != RC_NOT_CONNECTED) - nc++; + { + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir); + if (chf.areas[nidx] != RC_NULL_AREA) + { + nc++; + } + } } // At least one missing neighbour. if (nc != 4) @@ -339,7 +351,8 @@ void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigne rcCompactSpan& s = chf.spans[i]; if ((int)s.y >= miny && (int)s.y <= maxy) { - chf.areas[i] = areaId; + if (chf.areas[i] != RC_NULL_AREA) + chf.areas[i] = areaId; } } } @@ -418,6 +431,8 @@ void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) { rcCompactSpan& s = chf.spans[i]; + if (chf.areas[i] == RC_NULL_AREA) + continue; if ((int)s.y >= miny && (int)s.y <= maxy) { float p[3]; @@ -436,3 +451,152 @@ void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, ctx->stopTimer(RC_TIMER_MARK_CONVEXPOLY_AREA); } + +int rcOffsetPoly(const float* verts, const int nverts, const float offset, + float* outVerts, const int maxOutVerts) +{ + const float MITER_LIMIT = 1.20f; + + int n = 0; + + for (int i = 0; i < nverts; i++) + { + const int a = (i+nverts-1) % nverts; + const int b = i; + const int c = (i+1) % nverts; + const float* va = &verts[a*3]; + const float* vb = &verts[b*3]; + const float* vc = &verts[c*3]; + float dx0 = vb[0] - va[0]; + float dy0 = vb[2] - va[2]; + float d0 = dx0*dx0 + dy0*dy0; + if (d0 > 1e-6f) + { + d0 = 1.0f/rcSqrt(d0); + dx0 *= d0; + dy0 *= d0; + } + float dx1 = vc[0] - vb[0]; + float dy1 = vc[2] - vb[2]; + float d1 = dx1*dx1 + dy1*dy1; + if (d1 > 1e-6f) + { + d1 = 1.0f/rcSqrt(d1); + dx1 *= d1; + dy1 *= d1; + } + const float dlx0 = -dy0; + const float dly0 = dx0; + const float dlx1 = -dy1; + const float dly1 = dx1; + float cross = dx1*dy0 - dx0*dy1; + float dmx = (dlx0 + dlx1) * 0.5f; + float dmy = (dly0 + dly1) * 0.5f; + float dmr2 = dmx*dmx + dmy*dmy; + bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f; + if (dmr2 > 1e-6f) + { + const float scale = 1.0f / dmr2; + dmx *= scale; + dmy *= scale; + } + + if (bevel && cross < 0.0f) + { + if (n+2 >= maxOutVerts) + return 0; + float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f; + outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset; + outVerts[n*3+1] = vb[1]; + outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset; + n++; + outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset; + outVerts[n*3+1] = vb[1]; + outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset; + n++; + } + else + { + if (n+1 >= maxOutVerts) + return 0; + outVerts[n*3+0] = vb[0] - dmx*offset; + outVerts[n*3+1] = vb[1]; + outVerts[n*3+2] = vb[2] - dmy*offset; + n++; + } + } + + return n; +} + + +/// @par +/// +/// The value of spacial parameters are in world units. +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea +void rcMarkCylinderArea(rcContext* ctx, const float* pos, + const float r, const float h, unsigned char areaId, + rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + ctx->startTimer(RC_TIMER_MARK_CYLINDER_AREA); + + float bmin[3], bmax[3]; + bmin[0] = pos[0] - r; + bmin[1] = pos[1]; + bmin[2] = pos[2] - r; + bmax[0] = pos[0] + r; + bmax[1] = pos[1] + h; + bmax[2] = pos[2] + r; + const float r2 = r*r; + + int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); + int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); + int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); + int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); + int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); + int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); + + if (maxx < 0) return; + if (minx >= chf.width) return; + if (maxz < 0) return; + if (minz >= chf.height) return; + + if (minx < 0) minx = 0; + if (maxx >= chf.width) maxx = chf.width-1; + if (minz < 0) minz = 0; + if (maxz >= chf.height) maxz = chf.height-1; + + + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+z*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + + if (chf.areas[i] == RC_NULL_AREA) + continue; + + if ((int)s.y >= miny && (int)s.y <= maxy) + { + const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; + const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; + const float dx = sx - pos[0]; + const float dz = sz - pos[2]; + + if (dx*dx + dz*dz < r2) + { + chf.areas[i] = areaId; + } + } + } + } + } + + ctx->stopTimer(RC_TIMER_MARK_CYLINDER_AREA); +} diff --git a/dep/recastnavigation/Recast/RecastContour.cpp b/dep/recastnavigation/Recast/RecastContour.cpp index 8500e97f08d..5c324bcedfe 100644 --- a/dep/recastnavigation/Recast/RecastContour.cpp +++ b/dep/recastnavigation/Recast/RecastContour.cpp @@ -420,15 +420,13 @@ static void simplifyContour(rcIntArray& points, rcIntArray& simplified, // Round based on the segments in lexilogical order so that the // max tesselation is consistent regardles in which direction // segments are traversed. - if (bx > ax || (bx == ax && bz > az)) + const int n = bi < ai ? (bi+pn - ai) : (bi - ai); + if (n > 1) { - const int n = bi < ai ? (bi+pn - ai) : (bi - ai); - maxi = (ai + n/2) % pn; - } - else - { - const int n = bi < ai ? (bi+pn - ai) : (bi - ai); - maxi = (ai + (n+1)/2) % pn; + if (bx > ax || (bx == ax && bz > az)) + maxi = (ai + n/2) % pn; + else + maxi = (ai + (n+1)/2) % pn; } } } @@ -466,7 +464,7 @@ static void simplifyContour(rcIntArray& points, rcIntArray& simplified, // 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) | (points[bi*4+3] & RC_BORDER_VERTEX); + simplified[i*4+3] = (points[ai*4+3] & (RC_CONTOUR_REG_MASK|RC_AREA_BORDER)) | (points[bi*4+3] & RC_BORDER_VERTEX); } } @@ -613,13 +611,26 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, const int w = chf.width; const int h = chf.height; + const int borderSize = chf.borderSize; ctx->startTimer(RC_TIMER_BUILD_CONTOURS); rcVcopy(cset.bmin, chf.bmin); rcVcopy(cset.bmax, chf.bmax); + if (borderSize > 0) + { + // If the heightfield was build with bordersize, remove the offset. + const float pad = borderSize*chf.cs; + cset.bmin[0] += pad; + cset.bmin[2] += pad; + cset.bmax[0] -= pad; + cset.bmax[2] -= pad; + } cset.cs = chf.cs; cset.ch = chf.ch; + cset.width = chf.width - chf.borderSize*2; + cset.height = chf.height - chf.borderSize*2; + cset.borderSize = chf.borderSize; int maxContours = rcMax((int)chf.maxRegions, 8); cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM); @@ -671,8 +682,6 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE); - ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY); - rcIntArray verts(256); rcIntArray simplified(64); @@ -695,10 +704,17 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, verts.resize(0); simplified.resize(0); + + ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE); walkContour(x, y, i, chf, flags, verts); + ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE); + + ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY); simplifyContour(verts, simplified, maxError, maxEdgeLen, buildFlags); removeDegenerateSegments(simplified); + ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY); + // Store region->contour remap info. // Create contour. if (simplified.size()/4 >= 3) @@ -733,6 +749,16 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, return false; } memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4); + if (borderSize > 0) + { + // If the heightfield was build with bordersize, remove the offset. + for (int j = 0; j < cont->nverts; ++j) + { + int* v = &cont->verts[j*4]; + v[0] -= borderSize; + v[2] -= borderSize; + } + } cont->nrverts = verts.size()/4; cont->rverts = (int*)rcAlloc(sizeof(int)*cont->nrverts*4, RC_ALLOC_PERM); @@ -742,6 +768,16 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, return false; } memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4); + if (borderSize > 0) + { + // If the heightfield was build with bordersize, remove the offset. + for (int j = 0; j < cont->nrverts; ++j) + { + int* v = &cont->rverts[j*4]; + v[0] -= borderSize; + v[2] -= borderSize; + } + } /* cont->cx = cont->cy = cont->cz = 0; for (int i = 0; i < cont->nverts; ++i) @@ -809,8 +845,6 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, } } - ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY); - ctx->stopTimer(RC_TIMER_BUILD_CONTOURS); return true; diff --git a/dep/recastnavigation/Recast/RecastFilter.cpp b/dep/recastnavigation/Recast/RecastFilter.cpp index 42d9f2c755b..bf985c362c9 100644 --- a/dep/recastnavigation/Recast/RecastFilter.cpp +++ b/dep/recastnavigation/Recast/RecastFilter.cpp @@ -48,6 +48,7 @@ void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb { rcSpan* ps = 0; bool previousWalkable = false; + unsigned char previousArea = RC_NULL_AREA; for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next) { @@ -57,11 +58,12 @@ void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb if (!walkable && previousWalkable) { if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb) - s->area = RC_NULL_AREA; + s->area = previousArea; } // Copy walkable flag so that it cannot propagate // past multiple non-walkable objects. previousWalkable = walkable; + previousArea = s->area; } } } diff --git a/dep/recastnavigation/Recast/RecastLayers.cpp b/dep/recastnavigation/Recast/RecastLayers.cpp index 5ea6cb79d16..204f72e8cb2 100644 --- a/dep/recastnavigation/Recast/RecastLayers.cpp +++ b/dep/recastnavigation/Recast/RecastLayers.cpp @@ -325,7 +325,7 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, continue; // Skip if the height range would become too large. const int ymin = rcMin(root.ymin, regn.ymin); - const int ymax = rcMax(root.ymax, regn.ymax); // Edited by TC + const int ymax = rcMax(root.ymax, regn.ymax); if ((ymax - ymin) >= 255) continue; @@ -373,7 +373,7 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, continue; // Skip if the height range would become too large. const int ymin = rcMin(ri.ymin, rj.ymin); - const int ymax = rcMax(ri.ymax, rj.ymax); // Edited by TC + const int ymax = rcMax(ri.ymax, rj.ymax); if ((ymax - ymin) >= 255) continue; diff --git a/dep/recastnavigation/Recast/RecastMesh.cpp b/dep/recastnavigation/Recast/RecastMesh.cpp index 088b67aafd3..8af609b79fb 100644 --- a/dep/recastnavigation/Recast/RecastMesh.cpp +++ b/dep/recastnavigation/Recast/RecastMesh.cpp @@ -59,6 +59,7 @@ static bool buildMeshAdjacency(unsigned short* polys, const int npolys, unsigned short* t = &polys[i*vertsPerPoly*2]; for (int j = 0; j < vertsPerPoly; ++j) { + if (t[j] == RC_MESH_NULL_IDX) break; unsigned short v0 = t[j]; unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1]; if (v0 < v1) @@ -83,6 +84,7 @@ static bool buildMeshAdjacency(unsigned short* polys, const int npolys, unsigned short* t = &polys[i*vertsPerPoly*2]; for (int j = 0; j < vertsPerPoly; ++j) { + if (t[j] == RC_MESH_NULL_IDX) break; unsigned short v0 = t[j]; unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1]; if (v0 > v1) @@ -195,7 +197,7 @@ inline bool collinear(const int* a, const int* b, const int* c) // Returns true iff ab properly intersects cd: they share // a point interior to both segments. The properness of the // intersection is ensured by using strict leftness. -bool intersectProp(const int* a, const int* b, const int* c, const int* d) +static bool intersectProp(const int* a, const int* b, const int* c, const int* d) { // Eliminate improper cases. if (collinear(a,b,c) || collinear(a,b,d) || @@ -548,9 +550,9 @@ static bool canRemoveVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned sho // Check if the edge exists bool exists = false; - for (int k = 0; k < nedges; ++k) + for (int m = 0; m < nedges; ++m) { - int* e = &edges[k*3]; + int* e = &edges[m*3]; if (e[1] == b) { // Exists, increment vertex share count. @@ -659,7 +661,8 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short } // Remove the polygon. unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2]; - memcpy(p,p2,sizeof(unsigned short)*nvp); + if (p != p2) + memcpy(p,p2,sizeof(unsigned short)*nvp); memset(p+nvp,0xff,sizeof(unsigned short)*nvp); mesh.regs[i] = mesh.regs[mesh.npolys-1]; mesh.areas[i] = mesh.areas[mesh.npolys-1]; @@ -859,7 +862,9 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short unsigned short* pa = &polys[bestPa*nvp]; unsigned short* pb = &polys[bestPb*nvp]; mergePolys(pa, pb, bestEa, bestEb, tmpPoly, nvp); - memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp); + unsigned short* last = &polys[(npolys-1)*nvp]; + if (pb != last) + memcpy(pb, last, sizeof(unsigned short)*nvp); pregs[bestPb] = pregs[npolys-1]; pareas[bestPb] = pareas[npolys-1]; npolys--; @@ -893,8 +898,13 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short return true; } - -bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& mesh) +/// @par +/// +/// @note If the mesh data is to be used to construct a Detour navigation mesh, then the upper +/// limit must be retricted to <= #DT_VERTS_PER_POLYGON. +/// +/// @see rcAllocPolyMesh, rcContourSet, rcPolyMesh, rcConfig +bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh) { rcAssert(ctx); @@ -904,6 +914,7 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me rcVcopy(mesh.bmax, cset.bmax); mesh.cs = cset.cs; mesh.ch = cset.ch; + mesh.borderSize = cset.borderSize; int maxVertices = 0; int maxTris = 0; @@ -926,7 +937,7 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me rcScopedDelete<unsigned char> vflags = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxVertices, RC_ALLOC_TEMP); if (!vflags) { - ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'vflags' (%d).", maxVertices); return false; } memset(vflags, 0, maxVertices); @@ -937,7 +948,7 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); return false; } - mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris*nvp*2*2, RC_ALLOC_PERM); + mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris*nvp*2, RC_ALLOC_PERM); if (!mesh.polys) { ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2); @@ -1097,7 +1108,9 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me unsigned short* pa = &polys[bestPa*nvp]; unsigned short* pb = &polys[bestPb*nvp]; mergePolys(pa, pb, bestEa, bestEb, tmpPoly, nvp); - memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp); + unsigned short* lastPoly = &polys[(npolys-1)*nvp]; + if (pb != lastPoly) + memcpy(pb, lastPoly, sizeof(unsigned short)*nvp); npolys--; } else @@ -1155,6 +1168,37 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed."); return false; } + + // Find portal edges + if (mesh.borderSize > 0) + { + const int w = cset.width; + const int h = cset.height; + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + // Skip connected edges. + if (p[nvp+j] != RC_MESH_NULL_IDX) + continue; + int nj = j+1; + if (nj >= nvp || p[nj] == RC_MESH_NULL_IDX) nj = 0; + const unsigned short* va = &mesh.verts[p[j]*3]; + const unsigned short* vb = &mesh.verts[p[nj]*3]; + + if ((int)va[0] == 0 && (int)vb[0] == 0) + p[nvp+j] = 0x8000 | 0; + else if ((int)va[2] == h && (int)vb[2] == h) + p[nvp+j] = 0x8000 | 1; + else if ((int)va[0] == w && (int)vb[0] == w) + p[nvp+j] = 0x8000 | 2; + else if ((int)va[2] == 0 && (int)vb[2] == 0) + p[nvp+j] = 0x8000 | 3; + } + } + } // Just allocate the mesh flags array. The user is resposible to fill it. mesh.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*mesh.npolys, RC_ALLOC_PERM); @@ -1167,11 +1211,11 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me if (mesh.nverts > 0xffff) { - ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff); + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff); } if (mesh.npolys > 0xffff) { - ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff); + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff); } ctx->stopTimer(RC_TIMER_BUILD_POLYMESH); @@ -1271,7 +1315,7 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh); return false; } - memset(nextVert, 0, sizeof(int)*maxVerts); + memset(vremap, 0, sizeof(unsigned short)*maxVertsPerMesh); for (int i = 0; i < nmeshes; ++i) { @@ -1280,6 +1324,12 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f); const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f); + bool isMinX = (ox == 0); + bool isMinZ = (oz == 0); + bool isMaxX = ((unsigned short)floorf((mesh.bmax[0] - pmesh->bmax[0]) / mesh.cs + 0.5f)) == 0; + bool isMaxZ = ((unsigned short)floorf((mesh.bmax[2] - pmesh->bmax[2]) / mesh.cs + 0.5f)) == 0; + bool isOnBorder = (isMinX || isMinZ || isMaxX || isMaxZ); + for (int j = 0; j < pmesh->nverts; ++j) { unsigned short* v = &pmesh->verts[j*3]; @@ -1300,6 +1350,36 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r if (src[k] == RC_MESH_NULL_IDX) break; tgt[k] = vremap[src[k]]; } + + if (isOnBorder) + { + for (int k = mesh.nvp; k < mesh.nvp * 2; ++k) + { + if (src[k] & 0x8000 && src[k] != 0xffff) + { + unsigned short dir = src[k] & 0xf; + switch (dir) + { + case 0: // Portal x- + if (isMinX) + tgt[k] = src[k]; + break; + case 1: // Portal z+ + if (isMaxZ) + tgt[k] = src[k]; + break; + case 2: // Portal x+ + if (isMaxX) + tgt[k] = src[k]; + break; + case 3: // Portal z- + if (isMinZ) + tgt[k] = src[k]; + break; + } + } + } + } } } @@ -1323,3 +1403,67 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r return true; } + +bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst) +{ + rcAssert(ctx); + + // Destination must be empty. + rcAssert(dst.verts == 0); + rcAssert(dst.polys == 0); + rcAssert(dst.regs == 0); + rcAssert(dst.areas == 0); + rcAssert(dst.flags == 0); + + dst.nverts = src.nverts; + dst.npolys = src.npolys; + dst.maxpolys = src.npolys; + dst.nvp = src.nvp; + rcVcopy(dst.bmin, src.bmin); + rcVcopy(dst.bmax, src.bmax); + dst.cs = src.cs; + dst.ch = src.ch; + dst.borderSize = src.borderSize; + + dst.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.nverts*3, RC_ALLOC_PERM); + if (!dst.verts) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.verts' (%d).", src.nverts*3); + return false; + } + memcpy(dst.verts, src.verts, sizeof(unsigned short)*src.nverts*3); + + dst.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys*2*src.nvp, RC_ALLOC_PERM); + if (!dst.polys) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.polys' (%d).", src.npolys*2*src.nvp); + return false; + } + memcpy(dst.polys, src.polys, sizeof(unsigned short)*src.npolys*2*src.nvp); + + dst.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM); + if (!dst.regs) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.regs' (%d).", src.npolys); + return false; + } + memcpy(dst.regs, src.regs, sizeof(unsigned short)*src.npolys); + + dst.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*src.npolys, RC_ALLOC_PERM); + if (!dst.areas) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.areas' (%d).", src.npolys); + return false; + } + memcpy(dst.areas, src.areas, sizeof(unsigned char)*src.npolys); + + dst.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM); + if (!dst.flags) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.flags' (%d).", src.npolys); + return false; + } + memcpy(dst.flags, src.flags, sizeof(unsigned char)*src.npolys); + + return true; +} diff --git a/dep/recastnavigation/Recast/RecastMeshDetail.cpp b/dep/recastnavigation/Recast/RecastMeshDetail.cpp index 77a085c5c2b..8325b883707 100644 --- a/dep/recastnavigation/Recast/RecastMeshDetail.cpp +++ b/dep/recastnavigation/Recast/RecastMeshDetail.cpp @@ -200,8 +200,8 @@ static unsigned short getHeight(const float fx, const float fy, const float fz, { int ix = (int)floorf(fx*ics + 0.01f); int iz = (int)floorf(fz*ics + 0.01f); - ix = rcClamp(ix-hp.xmin, 0, hp.width); - iz = rcClamp(iz-hp.ymin, 0, hp.height); + ix = rcClamp(ix-hp.xmin, 0, hp.width - 1); + iz = rcClamp(iz-hp.ymin, 0, hp.height - 1); unsigned short h = hp.data[ix+iz*hp.width]; if (h == RC_UNSET_HEIGHT) { @@ -267,11 +267,11 @@ static int addEdge(rcContext* ctx, int* edges, int& nedges, const int maxEdges, int e = findEdge(edges, nedges, s, t); if (e == UNDEF) { - int* e = &edges[nedges*4]; - e[0] = s; - e[1] = t; - e[2] = l; - e[3] = r; + int* edge = &edges[nedges*4]; + edge[0] = s; + edge[1] = t; + edge[2] = l; + edge[3] = r; return nedges++; } else @@ -583,10 +583,10 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, int maxi = -1; for (int m = a+1; m < b; ++m) { - float d = distancePtSeg(&edge[m*3],va,vb); - if (d > maxd) + float dev = distancePtSeg(&edge[m*3],va,vb); + if (dev > maxd) { - maxd = d; + maxd = dev; maxi = m; } } @@ -741,9 +741,10 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, return true; } -static void getHeightData(const rcCompactHeightfield& chf, + +static void getHeightDataSeedsFromVertices(const rcCompactHeightfield& chf, const unsigned short* poly, const int npoly, - const unsigned short* verts, + const unsigned short* verts, const int bs, rcHeightPatch& hp, rcIntArray& stack) { // Floodfill the heightfield to get 2D height data, @@ -775,7 +776,7 @@ static void getHeightData(const rcCompactHeightfield& chf, az < hp.ymin || az >= hp.ymin+hp.height) continue; - const rcCompactCell& c = chf.cells[ax+az*chf.width]; + const rcCompactCell& c = chf.cells[(ax+bs)+(az+bs)*chf.width]; for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) { const rcCompactSpan& s = chf.spans[i]; @@ -847,7 +848,7 @@ static void getHeightData(const rcCompactHeightfield& chf, if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0) continue; - const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir); + const int ai = (int)chf.cells[(ax+bs)+(ay+bs)*chf.width].index + rcGetCon(cs, dir); int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width; hp.data[idx] = 1; @@ -869,8 +870,83 @@ static void getHeightData(const rcCompactHeightfield& chf, int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width; const rcCompactSpan& cs = chf.spans[ci]; hp.data[idx] = cs.y; + + // getHeightData seeds are given in coordinates with borders + stack[i+0] += bs; + stack[i+1] += bs; } +} + + + +static void getHeightData(const rcCompactHeightfield& chf, + const unsigned short* poly, const int npoly, + const unsigned short* verts, const int bs, + rcHeightPatch& hp, rcIntArray& stack, + int region) +{ + // Note: Reads to the compact heightfield are offset by border size (bs) + // since border size offset is already removed from the polymesh vertices. + + stack.resize(0); + memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + + bool empty = true; + + // Copy the height from the same region, and mark region borders + // as seed points to fill the rest. + for (int hy = 0; hy < hp.height; hy++) + { + int y = hp.ymin + hy + bs; + for (int hx = 0; hx < hp.width; hx++) + { + int x = hp.xmin + hx + bs; + const rcCompactCell& c = chf.cells[x+y*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + if (s.reg == region) + { + // Store height + hp.data[hx + hy*hp.width] = s.y; + empty = false; + + // If any of the neighbours is not in same region, + // add the current location as flood fill start + bool border = false; + for (int dir = 0; dir < 4; ++dir) + { + 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); + const rcCompactSpan& as = chf.spans[ai]; + if (as.reg != region) + { + border = true; + break; + } + } + } + if (border) + { + stack.push(x); + stack.push(y); + stack.push(i); + } + break; + } + } + } + } + + // if the polygon does not contian any points from the current region (rare, but happens) + // then use the cells closest to the polygon vertices as seeds to fill the height field + if (empty) + getHeightDataSeedsFromVertices(chf, poly, npoly, verts, bs, hp, stack); + static const int RETRACT_SIZE = 256; int head = 0; @@ -895,26 +971,25 @@ static void getHeightData(const rcCompactHeightfield& chf, const int ax = cx + rcGetDirOffsetX(dir); const int ay = cy + rcGetDirOffsetY(dir); + const int hx = ax - hp.xmin - bs; + const int hy = ay - hp.ymin - bs; - if (ax < hp.xmin || ax >= (hp.xmin+hp.width) || - ay < hp.ymin || ay >= (hp.ymin+hp.height)) + if (hx < 0 || hx >= hp.width || hy < 0 || hy >= hp.height) continue; - if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != RC_UNSET_HEIGHT) + if (hp.data[hx + hy*hp.width] != RC_UNSET_HEIGHT) continue; - const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir); - + const int ai = (int)chf.cells[ax + ay*chf.width].index + rcGetCon(cs, dir); const rcCompactSpan& as = chf.spans[ai]; - int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width; - hp.data[idx] = as.y; + + hp.data[hx + hy*hp.width] = as.y; stack.push(ax); stack.push(ay); stack.push(ai); } } - } static unsigned char getEdgeFlags(const float* va, const float* vb, @@ -961,6 +1036,7 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa const float cs = mesh.cs; const float ch = mesh.ch; const float* orig = mesh.bmin; + const int borderSize = mesh.borderSize; rcIntArray edges(64); rcIntArray tris(512); @@ -1071,7 +1147,7 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa hp.ymin = bounds[i*4+2]; hp.width = bounds[i*4+1]-bounds[i*4+0]; hp.height = bounds[i*4+3]-bounds[i*4+2]; - getHeightData(chf, p, npoly, mesh.verts, hp, stack); + getHeightData(chf, p, npoly, mesh.verts, borderSize, hp, stack, mesh.regs[i]); // Build detail mesh. int nverts = 0; @@ -1241,4 +1317,3 @@ bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int return true; } - diff --git a/dep/recastnavigation/Recast/RecastRasterization.cpp b/dep/recastnavigation/Recast/RecastRasterization.cpp index d2bb7c98f18..45a7d35bf3e 100644 --- a/dep/recastnavigation/Recast/RecastRasterization.cpp +++ b/dep/recastnavigation/Recast/RecastRasterization.cpp @@ -95,7 +95,7 @@ static void addSpan(rcHeightfield& hf, const int x, const int y, s->area = area; s->next = 0; - // Empty cell, add he first span. + // Empty cell, add the first span. if (!hf.spans[idx]) { hf.spans[idx] = s; @@ -169,36 +169,64 @@ void rcAddSpan(rcContext* /*ctx*/, rcHeightfield& hf, const int x, const int y, addSpan(hf, x,y, smin, smax, area, flagMergeThr); } -static int clipPoly(const float* in, int n, float* out, float pnx, float pnz, float pd) +// divides a convex polygons into two convex polygons on both sides of a line +static void dividePoly(const float* in, int nin, + float* out1, int* nout1, + float* out2, int* nout2, + float x, int axis) { float d[12]; - for (int i = 0; i < n; ++i) - d[i] = pnx*in[i*3+0] + pnz*in[i*3+2] + pd; - - int m = 0; - for (int i = 0, j = n-1; i < n; j=i, ++i) + for (int i = 0; i < nin; ++i) + d[i] = x - in[i*3+axis]; + + int m = 0, n = 0; + for (int i = 0, j = nin-1; i < nin; j=i, ++i) { bool ina = d[j] >= 0; bool inb = d[i] >= 0; if (ina != inb) { float s = d[j] / (d[j] - d[i]); - out[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s; - out[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s; - out[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s; + out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s; + out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s; + out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s; + rcVcopy(out2 + n*3, out1 + m*3); m++; + n++; + // add the i'th point to the right polygon. Do NOT add points that are on the dividing line + // since these were already added above + if (d[i] > 0) + { + rcVcopy(out1 + m*3, in + i*3); + m++; + } + else if (d[i] < 0) + { + rcVcopy(out2 + n*3, in + i*3); + n++; + } } - if (inb) + else // same side { - out[m*3+0] = in[i*3+0]; - out[m*3+1] = in[i*3+1]; - out[m*3+2] = in[i*3+2]; - m++; + // add the i'th point to the right polygon. Addition is done even for points on the dividing line + if (d[i] >= 0) + { + rcVcopy(out1 + m*3, in + i*3); + m++; + if (d[i] != 0) + continue; + } + rcVcopy(out2 + n*3, in + i*3); + n++; } } - return m; + + *nout1 = m; + *nout2 = n; } + + static void rasterizeTri(const float* v0, const float* v1, const float* v2, const unsigned char area, rcHeightfield& hf, const float* bmin, const float* bmax, @@ -222,48 +250,57 @@ static void rasterizeTri(const float* v0, const float* v1, const float* v2, if (!overlapBounds(bmin, bmax, tmin, tmax)) return; - // Calculate the footpring of the triangle on the grid. - int x0 = (int)((tmin[0] - bmin[0])*ics); + // Calculate the footprint of the triangle on the grid's y-axis int y0 = (int)((tmin[2] - bmin[2])*ics); - int x1 = (int)((tmax[0] - bmin[0])*ics); int y1 = (int)((tmax[2] - bmin[2])*ics); - x0 = rcClamp(x0, 0, w-1); y0 = rcClamp(y0, 0, h-1); - x1 = rcClamp(x1, 0, w-1); y1 = rcClamp(y1, 0, h-1); // Clip the triangle into all grid cells it touches. - float in[7*3], out[7*3], inrow[7*3]; + float buf[7*3*4]; + float *in = buf, *inrow = buf+7*3, *p1 = inrow+7*3, *p2 = p1+7*3; + + rcVcopy(&in[0], v0); + rcVcopy(&in[1*3], v1); + rcVcopy(&in[2*3], v2); + int nvrow, nvIn = 3; for (int y = y0; y <= y1; ++y) { - // Clip polygon to row. - rcVcopy(&in[0], v0); - rcVcopy(&in[1*3], v1); - rcVcopy(&in[2*3], v2); - int nvrow = 3; + // Clip polygon to row. Store the remaining polygon as well const float cz = bmin[2] + y*cs; - nvrow = clipPoly(in, nvrow, out, 0, 1, -cz); - if (nvrow < 3) continue; - nvrow = clipPoly(out, nvrow, inrow, 0, -1, cz+cs); + dividePoly(in, nvIn, inrow, &nvrow, p1, &nvIn, cz+cs, 2); + rcSwap(in, p1); if (nvrow < 3) continue; + // find the horizontal bounds in the row + float minX = inrow[0], maxX = inrow[0]; + for (int i=1; i<nvrow; ++i) + { + if (minX > inrow[i*3]) minX = inrow[i*3]; + if (maxX < inrow[i*3]) maxX = inrow[i*3]; + } + int x0 = (int)((minX - bmin[0])*ics); + int x1 = (int)((maxX - bmin[0])*ics); + x0 = rcClamp(x0, 0, w-1); + x1 = rcClamp(x1, 0, w-1); + + int nv, nv2 = nvrow; + for (int x = x0; x <= x1; ++x) { - // Clip polygon to column. - int nv = nvrow; + // Clip polygon to column. store the remaining polygon as well const float cx = bmin[0] + x*cs; - nv = clipPoly(inrow, nv, out, 1, 0, -cx); - if (nv < 3) continue; - nv = clipPoly(out, nv, in, -1, 0, cx+cs); + dividePoly(inrow, nv2, p1, &nv, p2, &nv2, cx+cs, 0); + rcSwap(inrow, p2); if (nv < 3) continue; // Calculate min and max of the span. - float smin = in[1], smax = in[1]; + float smin = p1[1], smax = p1[1]; for (int i = 1; i < nv; ++i) { - smin = rcMin(smin, in[i*3+1]); - smax = rcMax(smax, in[i*3+1]); + smin = rcMin(smin, p1[i*3+1]); + smax = rcMax(smax, p1[i*3+1]); } smin -= bmin[1]; smax -= bmin[1]; diff --git a/dep/recastnavigation/Recast/RecastRegion.cpp b/dep/recastnavigation/Recast/RecastRegion.cpp index 2da99abb41b..589fac29203 100644 --- a/dep/recastnavigation/Recast/RecastRegion.cpp +++ b/dep/recastnavigation/Recast/RecastRegion.cpp @@ -283,8 +283,13 @@ static bool floodRegion(int x, int y, int i, if (chf.areas[ai] != area) continue; unsigned short nr = srcReg[ai]; + if (nr & RC_BORDER_REG) // Do not take borders into account. + continue; if (nr != 0 && nr != r) + { ar = nr; + break; + } const rcCompactSpan& as = chf.spans[ai]; @@ -296,9 +301,12 @@ static bool floodRegion(int x, int y, int i, const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2); if (chf.areas[ai2] != area) continue; - unsigned short nr = srcReg[ai2]; - if (nr != 0 && nr != r) - ar = nr; + unsigned short nr2 = srcReg[ai2]; + if (nr2 != 0 && nr2 != r) + { + ar = nr2; + break; + } } } } @@ -319,16 +327,13 @@ static bool floodRegion(int x, int y, int i, const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir); if (chf.areas[ai] != area) continue; - if (chf.dist[ai] >= lev) + if (chf.dist[ai] >= lev && srcReg[ai] == 0) { - if (srcReg[ai] == 0) - { - srcReg[ai] = r; - srcDist[ai] = 0; - stack.push(ax); - stack.push(ay); - stack.push(ai); - } + srcReg[ai] = r; + srcDist[ai] = 0; + stack.push(ax); + stack.push(ay); + stack.push(ai); } } } @@ -341,30 +346,44 @@ static unsigned short* expandRegions(int maxIter, unsigned short level, rcCompactHeightfield& chf, unsigned short* srcReg, unsigned short* srcDist, unsigned short* dstReg, unsigned short* dstDist, - rcIntArray& stack) + rcIntArray& stack, + bool fillStack) { const int w = chf.width; const int h = chf.height; - // Find cells revealed by the raised level. - stack.resize(0); - for (int y = 0; y < h; ++y) + if (fillStack) { - for (int x = 0; x < w; ++x) + // Find cells revealed by the raised level. + stack.resize(0); + for (int y = 0; y < h; ++y) { - const rcCompactCell& c = chf.cells[x+y*w]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + for (int x = 0; x < w; ++x) { - if (chf.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA) + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) { - stack.push(x); - stack.push(y); - stack.push(i); + if (chf.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA) + { + stack.push(x); + stack.push(y); + stack.push(i); + } } } } } - + else // use cells in the input stack + { + // mark all cells which already have a region + for (int j=0; j<stack.size(); j+=3) + { + int i = stack[j+2]; + if (srcReg[i] != 0) + stack[j+2] = -1; + } + } + int iter = 0; while (stack.size() > 0) { @@ -435,6 +454,61 @@ static unsigned short* expandRegions(int maxIter, unsigned short level, } + +static void sortCellsByLevel(unsigned short startLevel, + rcCompactHeightfield& chf, + unsigned short* srcReg, + unsigned int nbStacks, rcIntArray* stacks, + unsigned short loglevelsPerStack) // the levels per stack (2 in our case) as a bit shift +{ + const int w = chf.width; + const int h = chf.height; + startLevel = startLevel >> loglevelsPerStack; + + for (unsigned int j=0; j<nbStacks; ++j) + stacks[j].resize(0); + + // put all cells in the level range into the appropriate stacks + 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) + { + if (chf.areas[i] == RC_NULL_AREA || srcReg[i] != 0) + continue; + + int level = chf.dist[i] >> loglevelsPerStack; + int sId = startLevel - level; + if (sId >= (int)nbStacks) + continue; + if (sId < 0) + sId = 0; + + stacks[sId].push(x); + stacks[sId].push(y); + stacks[sId].push(i); + } + } + } +} + + +static void appendStacks(rcIntArray& srcStack, rcIntArray& dstStack, + unsigned short* srcReg) +{ + for (int j=0; j<srcStack.size(); j+=3) + { + int i = srcStack[j+2]; + if ((i < 0) || (srcReg[i] != 0)) + continue; + dstStack.push(srcStack[j]); + dstStack.push(srcStack[j+1]); + dstStack.push(srcStack[j+2]); + } +} + struct rcRegion { inline rcRegion(unsigned short i) : @@ -679,17 +753,17 @@ static void walkContour(int x, int y, int i, int dir, // Remove adjacent duplicates. if (cont.size() > 1) { - for (int i = 0; i < cont.size(); ) + for (int j = 0; j < cont.size(); ) { - int ni = (i+1) % cont.size(); - if (cont[i] == cont[ni]) + int nj = (j+1) % cont.size(); + if (cont[j] == cont[nj]) { - for (int j = i; j < cont.size()-1; ++j) - cont[j] = cont[j+1]; + for (int k = j; k < cont.size()-1; ++k) + cont[k] = cont[k+1]; cont.pop(); } else - ++i; + ++j; } } } @@ -807,14 +881,14 @@ static bool filterSmallRegions(rcContext* ctx, int minRegionArea, int mergeRegio connectsToBorder = true; continue; } - rcRegion& nreg = regions[creg.connections[j]]; - if (nreg.visited) + rcRegion& neireg = regions[creg.connections[j]]; + if (neireg.visited) continue; - if (nreg.id == 0 || (nreg.id & RC_BORDER_REG)) + if (neireg.id == 0 || (neireg.id & RC_BORDER_REG)) continue; // Visit - stack.push(nreg.id); - nreg.visited = true; + stack.push(neireg.id); + neireg.visited = true; } } @@ -1087,6 +1161,8 @@ bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf, paintRectRegion(w-bw, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; paintRectRegion(0, w, 0, bh, id|RC_BORDER_REG, chf, srcReg); id++; paintRectRegion(0, w, h-bh, h, id|RC_BORDER_REG, chf, srcReg); id++; + + chf.borderSize = borderSize; } rcIntArray prev(256); @@ -1235,7 +1311,13 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, } ctx->startTimer(RC_TIMER_BUILD_REGIONS_WATERSHED); - + + const int LOG_NB_STACKS = 3; + const int NB_STACKS = 1 << LOG_NB_STACKS; + rcIntArray lvlStacks[NB_STACKS]; + for (int i=0; i<NB_STACKS; ++i) + lvlStacks[i].resize(1024); + rcIntArray stack(1024); rcIntArray visited(1024); @@ -1256,20 +1338,39 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, // const int expandIters = 4 + walkableRadius * 2; const int expandIters = 8; - // Mark border regions. - paintRectRegion(0, borderSize, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; - paintRectRegion(w-borderSize, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; - paintRectRegion(0, w, 0, borderSize, regionId|RC_BORDER_REG, chf, srcReg); regionId++; - paintRectRegion(0, w, h-borderSize, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + if (borderSize > 0) + { + // Make sure border will not overflow. + const int bw = rcMin(w, borderSize); + const int bh = rcMin(h, borderSize); + // Paint regions + paintRectRegion(0, bw, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(w-bw, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(0, w, 0, bh, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(0, w, h-bh, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + + chf.borderSize = borderSize; + } + 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); + ctx->startTimer(RC_TIMER_BUILD_REGIONS_EXPAND); // Expand current regions until no empty connected cells found. - if (expandRegions(expandIters, level, chf, srcReg, srcDist, dstReg, dstDist, stack) != srcReg) + if (expandRegions(expandIters, level, chf, srcReg, srcDist, dstReg, dstDist, lvlStacks[sId], false) != srcReg) { rcSwap(srcReg, dstReg); rcSwap(srcDist, dstDist); @@ -1280,18 +1381,15 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, ctx->startTimer(RC_TIMER_BUILD_REGIONS_FLOOD); // Mark new regions with IDs. - for (int y = 0; y < h; ++y) + for (int j=0; j<lvlStacks[sId].size(); j+=3) { - for (int x = 0; x < w; ++x) + int x = lvlStacks[sId][j]; + int y = lvlStacks[sId][j+1]; + int i = lvlStacks[sId][j+2]; + if (i >= 0 && srcReg[i] == 0) { - const rcCompactCell& c = chf.cells[x+y*w]; - for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) - { - if (chf.dist[i] < level || srcReg[i] != 0 || chf.areas[i] == RC_NULL_AREA) - continue; - if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack)) - regionId++; - } + if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack)) + regionId++; } } @@ -1299,7 +1397,7 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, } // Expand current regions until no empty connected cells found. - if (expandRegions(expandIters*8, 0, chf, srcReg, srcDist, dstReg, dstDist, stack) != srcReg) + if (expandRegions(expandIters*8, 0, chf, srcReg, srcDist, dstReg, dstDist, stack, true) != srcReg) { rcSwap(srcReg, dstReg); rcSwap(srcDist, dstDist); |