diff --git a/Engine/lib/recast/DebugUtils/Include/DebugDraw.h b/Engine/lib/recast/DebugUtils/Include/DebugDraw.h index 0b8c59352..00b544d1c 100644 --- a/Engine/lib/recast/DebugUtils/Include/DebugDraw.h +++ b/Engine/lib/recast/DebugUtils/Include/DebugDraw.h @@ -66,6 +66,9 @@ struct duDebugDraw /// End drawing primitives. virtual void end() = 0; + + /// Compute a color for given area. + virtual unsigned int areaToCol(unsigned int area); }; inline unsigned int duRGBA(int r, int g, int b, int a) diff --git a/Engine/lib/recast/DebugUtils/Source/DebugDraw.cpp b/Engine/lib/recast/DebugUtils/Source/DebugDraw.cpp index a009def36..d0179bca2 100644 --- a/Engine/lib/recast/DebugUtils/Source/DebugDraw.cpp +++ b/Engine/lib/recast/DebugUtils/Source/DebugDraw.cpp @@ -20,13 +20,26 @@ #include #include "DebugDraw.h" #include "DetourMath.h" +#include "DetourNavMesh.h" duDebugDraw::~duDebugDraw() { // Empty } - + +unsigned int duDebugDraw::areaToCol(unsigned int area) +{ + if (area == 0) + { + // Treat zero area type as default. + return duRGBA(0, 192, 255, 255); + } + else + { + return duIntToCol(area, 255); + } +} inline int bit(int a, int b) { diff --git a/Engine/lib/recast/DebugUtils/Source/DetourDebugDraw.cpp b/Engine/lib/recast/DebugUtils/Source/DetourDebugDraw.cpp index e2db1078f..dd4bad3fd 100644 --- a/Engine/lib/recast/DebugUtils/Source/DetourDebugDraw.cpp +++ b/Engine/lib/recast/DebugUtils/Source/DetourDebugDraw.cpp @@ -121,6 +121,7 @@ static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMesh dtPolyRef base = mesh.getPolyRefBase(tile); int tileNum = mesh.decodePolyIdTile(base); + const unsigned int tileColor = duIntToCol(tileNum, 128); dd->depthMask(false); @@ -139,16 +140,9 @@ static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMesh else { if (flags & DU_DRAWNAVMESH_COLOR_TILES) - { - col = duIntToCol(tileNum, 128); - } + col = tileColor; else - { - if (p->getArea() == 0) // Treat zero area type as default. - col = duRGBA(0,192,255,64); - else - col = duIntToCol(p->getArea(), 64); - } + col = duTransCol(dd->areaToCol(p->getArea()), 64); } for (int j = 0; j < pd->triCount; ++j) @@ -184,8 +178,8 @@ static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMesh if (query && query->isInClosedList(base | (dtPolyRef)i)) col = duRGBA(255,196,0,220); else - col = duDarkenCol(duIntToCol(p->getArea(), 220)); - + col = duDarkenCol(duTransCol(dd->areaToCol(p->getArea()), 220)); + const dtOffMeshConnection* con = &tile->offMeshCons[i - tile->header->offMeshBase]; const float* va = &tile->verts[p->verts[0]*3]; const float* vb = &tile->verts[p->verts[1]*3]; @@ -451,7 +445,7 @@ void duDebugDrawNavMeshPoly(duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef re dd->depthMask(false); - const unsigned int c = (col & 0x00ffffff) | (64 << 24); + const unsigned int c = duTransCol(col, 64); const unsigned int ip = (unsigned int)(poly - tile->polys); if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) @@ -462,7 +456,7 @@ void duDebugDrawNavMeshPoly(duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef re // Connection arc. duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f, - (con->flags & 1) ? 0.6f : 0, 0.6f, c); + (con->flags & 1) ? 0.6f : 0.0f, 0.6f, c); dd->end(); } @@ -559,15 +553,15 @@ void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLay const int lidx = x+y*w; const int lh = (int)layer.heights[lidx]; if (lh == 0xff) continue; + const unsigned char area = layer.areas[lidx]; - unsigned int col; if (area == 63) col = duLerpCol(color, duRGBA(0,192,255,64), 32); else if (area == 0) col = duLerpCol(color, duRGBA(0,0,0,64), 32); else - col = duLerpCol(color, duIntToCol(area, 255), 32); + col = duLerpCol(color, dd->areaToCol(area), 32); const float fx = bmin[0] + x*cs; const float fy = bmin[1] + (lh+1)*ch; @@ -743,14 +737,15 @@ void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyM for (int i = 0; i < lmesh.npolys; ++i) { const unsigned short* p = &lmesh.polys[i*nvp*2]; + const unsigned char area = lmesh.areas[i]; unsigned int color; - if (lmesh.areas[i] == DT_TILECACHE_WALKABLE_AREA) + if (area == DT_TILECACHE_WALKABLE_AREA) color = duRGBA(0,192,255,64); - else if (lmesh.areas[i] == DT_TILECACHE_NULL_AREA) + else if (area == DT_TILECACHE_NULL_AREA) color = duRGBA(0,0,0,64); else - color = duIntToCol(lmesh.areas[i], 255); + color = dd->areaToCol(area); unsigned short vi[3]; for (int j = 2; j < nvp; ++j) diff --git a/Engine/lib/recast/DebugUtils/Source/RecastDebugDraw.cpp b/Engine/lib/recast/DebugUtils/Source/RecastDebugDraw.cpp index 82050bde0..c1a73a168 100644 --- a/Engine/lib/recast/DebugUtils/Source/RecastDebugDraw.cpp +++ b/Engine/lib/recast/DebugUtils/Source/RecastDebugDraw.cpp @@ -198,7 +198,7 @@ void duDebugDrawHeightfieldWalkable(duDebugDraw* dd, const rcHeightfield& hf) else if (s->area == RC_NULL_AREA) fcol[0] = duRGBA(64,64,64,255); else - fcol[0] = duMultCol(duIntToCol(s->area, 255), 200); + fcol[0] = duMultCol(dd->areaToCol(s->area), 200); duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol); s = s->next; @@ -230,13 +230,14 @@ void duDebugDrawCompactHeightfieldSolid(duDebugDraw* dd, const rcCompactHeightfi { const rcCompactSpan& s = chf.spans[i]; + const unsigned char area = chf.areas[i]; unsigned int color; - if (chf.areas[i] == RC_WALKABLE_AREA) + if (area == RC_WALKABLE_AREA) color = duRGBA(0,192,255,64); - else if (chf.areas[i] == RC_NULL_AREA) + else if (area == RC_NULL_AREA) color = duRGBA(0,0,0,64); else - color = duIntToCol(chf.areas[i], 255); + color = dd->areaToCol(area); const float fy = chf.bmin[1] + (s.y+1)*ch; dd->vertex(fx, fy, fz, color); @@ -403,7 +404,7 @@ void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLaye else if (area == RC_NULL_AREA) col = duLerpCol(color, duRGBA(0,0,0,64), 32); else - col = duLerpCol(color, duIntToCol(area, 255), 32); + col = duLerpCol(color, dd->areaToCol(area), 32); const float fx = layer.bmin[0] + x*cs; const float fy = layer.bmin[1] + (lh+1)*ch; @@ -866,14 +867,15 @@ void duDebugDrawPolyMesh(duDebugDraw* dd, const struct rcPolyMesh& mesh) for (int i = 0; i < mesh.npolys; ++i) { const unsigned short* p = &mesh.polys[i*nvp*2]; + const unsigned char area = mesh.areas[i]; unsigned int color; - if (mesh.areas[i] == RC_WALKABLE_AREA) + if (area == RC_WALKABLE_AREA) color = duRGBA(0,192,255,64); - else if (mesh.areas[i] == RC_NULL_AREA) + else if (area == RC_NULL_AREA) color = duRGBA(0,0,0,64); else - color = duIntToCol(mesh.areas[i], 255); + color = dd->areaToCol(area); unsigned short vi[3]; for (int j = 2; j < nvp; ++j) diff --git a/Engine/lib/recast/Detour/Include/DetourAssert.h b/Engine/lib/recast/Detour/Include/DetourAssert.h index 3cf652288..e05fd66fa 100644 --- a/Engine/lib/recast/Detour/Include/DetourAssert.h +++ b/Engine/lib/recast/Detour/Include/DetourAssert.h @@ -23,11 +23,34 @@ // Feel free to change the file and include your own implementation instead. #ifdef NDEBUG + // From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ # define dtAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) + #else + +/// An assertion failure function. +// @param[in] expression asserted expression. +// @param[in] file Filename of the failed assertion. +// @param[in] line Line number of the failed assertion. +/// @see dtAssertFailSetCustom +typedef void (dtAssertFailFunc)(const char* expression, const char* file, int line); + +/// Sets the base custom assertion failure function to be used by Detour. +/// @param[in] assertFailFunc The function to be invoked in case of failure of #dtAssert +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc); + +/// Gets the base custom assertion failure function to be used by Detour. +dtAssertFailFunc* dtAssertFailGetCustom(); + # include -# define dtAssert assert +# define dtAssert(expression) \ + { \ + dtAssertFailFunc* failFunc = dtAssertFailGetCustom(); \ + if(failFunc == NULL) { assert(expression); } \ + else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \ + } + #endif #endif // DETOURASSERT_H diff --git a/Engine/lib/recast/Detour/Include/DetourCommon.h b/Engine/lib/recast/Detour/Include/DetourCommon.h index 2afba0d78..739858cd9 100644 --- a/Engine/lib/recast/Detour/Include/DetourCommon.h +++ b/Engine/lib/recast/Detour/Include/DetourCommon.h @@ -20,6 +20,7 @@ #define DETOURCOMMON_H #include "DetourMath.h" +#include /** @defgroup detour Detour @@ -482,6 +483,23 @@ inline void dtSwapEndian(float* v) void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, const float s, const float t, float* out); +template +TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(const unsigned char*& buffer, const size_t distanceToAdvance) +{ + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; +} + +template +TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(unsigned char*& buffer, const size_t distanceToAdvance) +{ + TypeToRetrieveAs* returnPointer = reinterpret_cast(buffer); + buffer += distanceToAdvance; + return returnPointer; +} + + /// @} #endif // DETOURCOMMON_H diff --git a/Engine/lib/recast/Detour/Include/DetourNavMesh.h b/Engine/lib/recast/Detour/Include/DetourNavMesh.h index 8ecd57e46..02ee5e78c 100644 --- a/Engine/lib/recast/Detour/Include/DetourNavMesh.h +++ b/Engine/lib/recast/Detour/Include/DetourNavMesh.h @@ -635,7 +635,7 @@ private: dtPolyRef* polys, const int maxPolys) const; /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, - const float* extents, float* nearestPt) const; + const float* halfExtents, float* nearestPt) const; /// Returns closest point on polygon. void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; diff --git a/Engine/lib/recast/Detour/Include/DetourNavMeshBuilder.h b/Engine/lib/recast/Detour/Include/DetourNavMeshBuilder.h index c80d17176..9425a7a78 100644 --- a/Engine/lib/recast/Detour/Include/DetourNavMeshBuilder.h +++ b/Engine/lib/recast/Detour/Include/DetourNavMeshBuilder.h @@ -145,4 +145,5 @@ function. @see dtCreateNavMeshData -*/ \ No newline at end of file +*/ + diff --git a/Engine/lib/recast/Detour/Include/DetourNavMeshQuery.h b/Engine/lib/recast/Detour/Include/DetourNavMeshQuery.h index ad425fb0c..1c23e4857 100644 --- a/Engine/lib/recast/Detour/Include/DetourNavMeshQuery.h +++ b/Engine/lib/recast/Detour/Include/DetourNavMeshQuery.h @@ -148,7 +148,18 @@ struct dtRaycastHit float pathCost; }; +/// Provides custom polygon query behavior. +/// Used by dtNavMeshQuery::queryPolygons. +/// @ingroup detour +class dtPolyQuery +{ +public: + virtual ~dtPolyQuery() { } + /// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons. + /// This can be called multiple times for a single query. + virtual void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) = 0; +}; /// Provides the ability to perform pathfinding related queries against /// a navigation mesh. @@ -182,7 +193,7 @@ public: const float* startPos, const float* endPos, 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)] @@ -285,33 +296,55 @@ public: dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, int* resultCount, const int maxResult) const; + /// Gets a path from the explored nodes in the previous search. + /// @param[in] endRef The reference id of the end polygon. + /// @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. + /// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 0] + /// @returns The status flags. Returns DT_FAILURE | DT_INVALID_PARAM if any parameter is wrong, or if + /// @p endRef was not explored in the previous search. Returns DT_SUCCESS | DT_BUFFER_TOO_SMALL + /// if @p path cannot contain the entire path. In this case it is filled to capacity with a partial path. + /// Otherwise returns DT_SUCCESS. + /// @remarks The result of this function depends on the state of the query object. For that reason it should only + /// be used immediately after one of the two Dijkstra searches, findPolysAroundCircle or findPolysAroundShape. + dtStatus getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) 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] halfExtents 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, + dtStatus findNearestPoly(const float* center, const float* halfExtents, 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] halfExtents 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, + dtStatus queryPolygons(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* polys, int* polyCount, const int maxPolys) const; + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[in] query The query. Polygons found will be batched together and passed to this query. + dtStatus queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, dtPolyQuery* query) 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)] @@ -479,12 +512,9 @@ private: dtNavMeshQuery(const dtNavMeshQuery&); dtNavMeshQuery& operator=(const dtNavMeshQuery&); - /// Returns neighbour tile based on side. - dtMeshTile* getNeighbourTileAt(int x, int y, int side) const; - /// 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; + void queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + const dtQueryFilter* filter, dtPolyQuery* query) const; /// Returns portal points between two polygons. dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, @@ -508,6 +538,9 @@ private: 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; + + // Gets the path leading to the specified end node. + dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const; const dtNavMesh* m_nav; ///< Pointer to navmesh data. diff --git a/Engine/lib/recast/Detour/Source/DetourAssert.cpp b/Engine/lib/recast/Detour/Source/DetourAssert.cpp new file mode 100644 index 000000000..5e019e0cf --- /dev/null +++ b/Engine/lib/recast/Detour/Source/DetourAssert.cpp @@ -0,0 +1,35 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "DetourAssert.h" + +#ifndef NDEBUG + +static dtAssertFailFunc* sAssertFailFunc = 0; + +void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc) +{ + sAssertFailFunc = assertFailFunc; +} + +dtAssertFailFunc* dtAssertFailGetCustom() +{ + return sAssertFailFunc; +} + +#endif diff --git a/Engine/lib/recast/Detour/Source/DetourCommon.cpp b/Engine/lib/recast/Detour/Source/DetourCommon.cpp index 26fe65c17..41d0d7bd3 100644 --- a/Engine/lib/recast/Detour/Source/DetourCommon.cpp +++ b/Engine/lib/recast/Detour/Source/DetourCommon.cpp @@ -342,8 +342,8 @@ void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, // Find sub triangle weighted by area. const float thr = s*areasum; float acc = 0.0f; - float u = 0.0f; - int tri = 0; + float u = 1.0f; + int tri = npts - 1; for (int i = 2; i < npts; i++) { const float dacc = areas[i]; if (thr >= acc && thr < (acc+dacc)) diff --git a/Engine/lib/recast/Detour/Source/DetourNavMesh.cpp b/Engine/lib/recast/Detour/Source/DetourNavMesh.cpp index bb6c9e4b7..b81a2567b 100644 --- a/Engine/lib/recast/Detour/Source/DetourNavMesh.cpp +++ b/Engine/lib/recast/Detour/Source/DetourNavMesh.cpp @@ -304,7 +304,7 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb, if (!tile) return 0; float amin[2], amax[2]; - calcSlabEndPoints(va,vb, amin,amax, side); + calcSlabEndPoints(va, vb, amin, amax, side); const float apos = getSlabCoord(va, side); // Remove links pointing to 'side' and compact the links array. @@ -470,12 +470,12 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int if (targetPoly->firstLink == DT_NULL_LINK) continue; - const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; + const float halfExtents[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; // Find polygon to connect to. const float* p = &targetCon->pos[3]; float nearestPt[3]; - dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); + dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt); if (!ref) continue; // findNearestPoly may return too optimistic results, further check to make sure. @@ -570,12 +570,12 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) dtOffMeshConnection* con = &tile->offMeshCons[i]; dtPoly* poly = &tile->polys[con->poly]; - const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; + const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad }; // Find polygon to connect to. const float* p = &con->pos[0]; // First vertex float nearestPt[3]; - dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); + dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, 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)) @@ -651,9 +651,9 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close 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) + float dmin = edged[0]; + int imin = 0; + for (int i = 1; i < nv; ++i) { if (edged[i] < dmin) { @@ -687,7 +687,7 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; } float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) + if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h)) { closest[1] = h; break; @@ -696,12 +696,12 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, - const float* center, const float* extents, + const float* center, const float* halfExtents, float* nearestPt) const { float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); // Get nearby polygons from proximity grid. dtPolyRef polys[128]; @@ -917,14 +917,14 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount); unsigned char* d = data + headerSize; - tile->verts = (float*)d; d += vertsSize; - tile->polys = (dtPoly*)d; d += polysSize; - tile->links = (dtLink*)d; d += linksSize; - tile->detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize; - tile->detailVerts = (float*)d; d += detailVertsSize; - tile->detailTris = (unsigned char*)d; d += detailTrisSize; - tile->bvTree = (dtBVNode*)d; d += bvtreeSize; - tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; + tile->verts = dtGetThenAdvanceBufferPointer(d, vertsSize); + tile->polys = dtGetThenAdvanceBufferPointer(d, polysSize); + tile->links = dtGetThenAdvanceBufferPointer(d, linksSize); + tile->detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + tile->detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + tile->detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + tile->bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); + tile->offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); // If there are no items in the bvtree, reset the tree pointer. if (!bvtreeSize) @@ -943,7 +943,10 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, tile->flags = flags; connectIntLinks(tile); + + // Base off-mesh connections to their starting polygons and connect connections inside the tile. baseOffMeshLinks(tile); + connectExtOffMeshLinks(tile, tile, -1); // Create connections with neighbour tiles. static const int MAX_NEIS = 32; @@ -954,11 +957,11 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, 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); - } + if (neis[j] == tile) + continue; + + connectExtLinks(tile, neis[j], -1); + connectExtLinks(neis[j], tile, -1); connectExtOffMeshLinks(tile, neis[j], -1); connectExtOffMeshLinks(neis[j], tile, -1); } @@ -1322,8 +1325,8 @@ dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, if (maxDataSize < sizeReq) return DT_FAILURE | DT_BUFFER_TOO_SMALL; - dtTileState* tileState = (dtTileState*)data; data += dtAlign4(sizeof(dtTileState)); - dtPolyState* polyStates = (dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); + dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); + dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount)); // Store tile state. tileState->magic = DT_NAVMESH_STATE_MAGIC; @@ -1354,8 +1357,8 @@ dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data if (maxDataSize < sizeReq) return DT_FAILURE | DT_INVALID_PARAM; - const dtTileState* tileState = (const dtTileState*)data; data += dtAlign4(sizeof(dtTileState)); - const dtPolyState* polyStates = (const dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount); + const dtTileState* tileState = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtTileState))); + const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount)); // Check that the restore is possible. if (tileState->magic != DT_NAVMESH_STATE_MAGIC) diff --git a/Engine/lib/recast/Detour/Source/DetourNavMeshBuilder.cpp b/Engine/lib/recast/Detour/Source/DetourNavMeshBuilder.cpp index 1bf271bed..e93a97629 100644 --- a/Engine/lib/recast/Detour/Source/DetourNavMeshBuilder.cpp +++ b/Engine/lib/recast/Detour/Source/DetourNavMeshBuilder.cpp @@ -106,7 +106,6 @@ inline int longestAxis(unsigned short x, unsigned short y, unsigned short z) if (z > maxVal) { axis = 2; - maxVal = z; } return axis; } @@ -169,45 +168,72 @@ static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNod } } -static int createBVTree(const unsigned short* verts, const int /*nverts*/, - const unsigned short* polys, const int npolys, const int nvp, - const float cs, const float ch, - const int /*nnodes*/, dtBVNode* nodes) +static int createBVTree(dtNavMeshCreateParams* params, dtBVNode* nodes, int /*nnodes*/) { // Build tree - BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*npolys, DT_ALLOC_TEMP); - for (int i = 0; i < npolys; i++) + float quantFactor = 1 / params->cs; + BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*params->polyCount, DT_ALLOC_TEMP); + for (int i = 0; i < params->polyCount; i++) { BVItem& it = items[i]; it.i = i; - // Calc polygon bounds. - const unsigned short* p = &polys[i*nvp*2]; - it.bmin[0] = it.bmax[0] = verts[p[0]*3+0]; - it.bmin[1] = it.bmax[1] = verts[p[0]*3+1]; - it.bmin[2] = it.bmax[2] = verts[p[0]*3+2]; - - for (int j = 1; j < nvp; ++j) + // Calc polygon bounds. Use detail meshes if available. + if (params->detailMeshes) { - if (p[j] == MESH_NULL_IDX) break; - unsigned short x = verts[p[j]*3+0]; - unsigned short y = verts[p[j]*3+1]; - unsigned short z = verts[p[j]*3+2]; - - if (x < it.bmin[0]) it.bmin[0] = x; - if (y < it.bmin[1]) it.bmin[1] = y; - if (z < it.bmin[2]) it.bmin[2] = z; - - if (x > it.bmax[0]) it.bmax[0] = x; - if (y > it.bmax[1]) it.bmax[1] = y; - if (z > it.bmax[2]) it.bmax[2] = z; + int vb = (int)params->detailMeshes[i*4+0]; + int ndv = (int)params->detailMeshes[i*4+1]; + float bmin[3]; + float bmax[3]; + + const float* dv = ¶ms->detailVerts[vb*3]; + dtVcopy(bmin, dv); + dtVcopy(bmax, dv); + + for (int j = 1; j < ndv; j++) + { + dtVmin(bmin, &dv[j * 3]); + dtVmax(bmax, &dv[j * 3]); + } + + // BV-tree uses cs for all dimensions + it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0])*quantFactor), 0, 0xffff); + it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1])*quantFactor), 0, 0xffff); + it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2])*quantFactor), 0, 0xffff); + + it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0])*quantFactor), 0, 0xffff); + it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1])*quantFactor), 0, 0xffff); + it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2])*quantFactor), 0, 0xffff); + } + else + { + const unsigned short* p = ¶ms->polys[i*params->nvp * 2]; + it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0]; + it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1]; + it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2]; + + for (int j = 1; j < params->nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + unsigned short x = params->verts[p[j] * 3 + 0]; + unsigned short y = params->verts[p[j] * 3 + 1]; + unsigned short z = params->verts[p[j] * 3 + 2]; + + if (x < it.bmin[0]) it.bmin[0] = x; + if (y < it.bmin[1]) it.bmin[1] = y; + if (z < it.bmin[2]) it.bmin[2] = z; + + if (x > it.bmax[0]) it.bmax[0] = x; + if (y > it.bmax[1]) it.bmax[1] = y; + if (z > it.bmax[2]) it.bmax[2] = z; + } + // Remap y + it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs); + it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs); } - // Remap y - it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1]*ch/cs); - it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1]*ch/cs); } int curNode = 0; - subdivide(items, npolys, 0, npolys, curNode, nodes); + subdivide(items, params->polyCount, 0, params->polyCount, curNode, nodes); dtFree(items); @@ -421,15 +447,16 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, memset(data, 0, dataSize); unsigned char* d = data; - dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize; - float* navVerts = (float*)d; d += vertsSize; - dtPoly* navPolys = (dtPoly*)d; d += polysSize; - d += linksSize; - dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize; - float* navDVerts = (float*)d; d += detailVertsSize; - unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize; - dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize; - dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize; + + dtMeshHeader* header = dtGetThenAdvanceBufferPointer(d, headerSize); + float* navVerts = dtGetThenAdvanceBufferPointer(d, vertsSize); + dtPoly* navPolys = dtGetThenAdvanceBufferPointer(d, polysSize); + d += linksSize; // Ignore links; just leave enough space for them. They'll be created on load. + dtPolyDetail* navDMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + float* navDVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + unsigned char* navDTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + dtBVNode* navBvtree = dtGetThenAdvanceBufferPointer(d, bvTreeSize); + dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshConsSize); // Store header @@ -595,11 +622,9 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, } // Store and create BVtree. - // TODO: take detail mesh into account! use byte per bbox extent? if (params->buildBvTree) { - createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, - nvp, params->cs, params->ch, params->polyCount*2, navBvtree); + createBVTree(params, navBvtree, 2*params->polyCount); } // Store Off-Mesh connections. @@ -705,14 +730,16 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/) const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount); unsigned char* d = data + headerSize; - float* verts = (float*)d; d += vertsSize; - dtPoly* polys = (dtPoly*)d; d += polysSize; - /*dtLink* links = (dtLink*)d;*/ d += linksSize; - dtPolyDetail* detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize; - float* detailVerts = (float*)d; d += detailVertsSize; - /*unsigned char* detailTris = (unsigned char*)d;*/ d += detailTrisSize; - dtBVNode* bvTree = (dtBVNode*)d; d += bvtreeSize; - dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize; + float* verts = dtGetThenAdvanceBufferPointer(d, vertsSize); + dtPoly* polys = dtGetThenAdvanceBufferPointer(d, polysSize); + d += linksSize; // Ignore links; they technically should be endian-swapped but all their data is overwritten on load anyway. + //dtLink* links = dtGetThenAdvanceBufferPointer(d, linksSize); + dtPolyDetail* detailMeshes = dtGetThenAdvanceBufferPointer(d, detailMeshesSize); + float* detailVerts = dtGetThenAdvanceBufferPointer(d, detailVertsSize); + d += detailTrisSize; // Ignore detail tris; single bytes can't be endian-swapped. + //unsigned char* detailTris = dtGetThenAdvanceBufferPointer(d, detailTrisSize); + dtBVNode* bvTree = dtGetThenAdvanceBufferPointer(d, bvtreeSize); + dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer(d, offMeshLinksSize); // Vertices for (int i = 0; i < header->vertCount*3; ++i) diff --git a/Engine/lib/recast/Detour/Source/DetourNavMeshQuery.cpp b/Engine/lib/recast/Detour/Source/DetourNavMeshQuery.cpp index 87706c1de..90999f2f6 100644 --- a/Engine/lib/recast/Detour/Source/DetourNavMeshQuery.cpp +++ b/Engine/lib/recast/Detour/Source/DetourNavMeshQuery.cpp @@ -542,9 +542,9 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo 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) + float dmin = edged[0]; + int imin = 0; + for (int i = 1; i < nv; ++i) { if (edged[i] < dmin) { @@ -578,7 +578,7 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; } float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) + if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h)) { closest[1] = h; break; @@ -628,9 +628,9 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* else { // Point is outside the polygon, dtClamp to nearest edge. - float dmin = FLT_MAX; - int imin = -1; - for (int i = 0; i < nv; ++i) + float dmin = edged[0]; + int imin = 0; + for (int i = 1; i < nv; ++i) { if (edged[i] < dmin) { @@ -699,16 +699,67 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h return DT_FAILURE | DT_INVALID_PARAM; } +class dtFindNearestPolyQuery : public dtPolyQuery +{ + const dtNavMeshQuery* m_query; + const float* m_center; + float m_nearestDistanceSqr; + dtPolyRef m_nearestRef; + float m_nearestPoint[3]; + +public: + dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center) + : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint() + { + } + + dtPolyRef nearestRef() const { return m_nearestRef; } + const float* nearestPoint() const { return m_nearestPoint; } + + void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) + { + dtIgnoreUnused(polys); + + for (int i = 0; i < count; ++i) + { + dtPolyRef ref = refs[i]; + float closestPtPoly[3]; + float diff[3]; + bool posOverPoly = false; + float d; + m_query->closestPointOnPoly(ref, m_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, m_center, closestPtPoly); + if (posOverPoly) + { + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); + } + + if (d < m_nearestDistanceSqr) + { + dtVcopy(m_nearestPoint, closestPtPoly); + + m_nearestDistanceSqr = d; + m_nearestRef = ref; + } + } + } +}; + /// @par /// /// @note If the search box does not intersect any polygons the search will /// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check /// @p nearestRef before using @p nearestPt. /// -/// @warning This function is not suitable for large area searches. If the search -/// extents overlaps more than MAX_SEARCH (128) polygons it may return an invalid result. -/// -dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents, +dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* nearestRef, float* nearestPt) const { @@ -717,70 +768,29 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten if (!nearestRef) return DT_FAILURE | DT_INVALID_PARAM; - // Get nearby polygons from proximity grid. - const int MAX_SEARCH = 128; - dtPolyRef polys[MAX_SEARCH]; - int polyCount = 0; - if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, MAX_SEARCH))) - return DT_FAILURE | DT_INVALID_PARAM; - - *nearestRef = 0; + dtFindNearestPolyQuery query(this, center); - if (polyCount == 0) - return DT_SUCCESS; - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestPoint[3]; + dtStatus status = queryPolygons(center, halfExtents, filter, &query); + if (dtStatusFailed(status)) + return status; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - 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) - { - dtVcopy(nearestPoint, closestPtPoly); - - nearestDistanceSqr = d; - nearest = ref; - } - } - - *nearestRef = nearest; - - if (nearestPt) - dtVcopy(nearestPt, nearestPoint); + *nearestRef = query.nearestRef(); + // Only override nearestPt if we actually found a poly so the nearest point + // is valid. + if (nearestPt && *nearestRef) + dtVcopy(nearestPt, query.nearestPoint()); return DT_SUCCESS; } -int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - const dtQueryFilter* filter, - dtPolyRef* polys, const int maxPolys) const +void dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, + const dtQueryFilter* filter, dtPolyQuery* query) const { dtAssert(m_nav); + static const int batchSize = 32; + dtPolyRef polyRefs[batchSize]; + dtPoly* polys[batchSize]; + int n = 0; if (tile->bvTree) { @@ -789,7 +799,7 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi const float* tbmin = tile->header->bmin; const float* tbmax = tile->header->bmax; const float qfac = tile->header->bvQuantFactor; - + // Calculate quantized box unsigned short bmin[3], bmax[3]; // dtClamp query box to world box. @@ -806,25 +816,34 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; - + // Traverse tree const dtPolyRef base = m_nav->getPolyRefBase(tile); - int n = 0; while (node < end) { const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); const bool isLeafNode = node->i >= 0; - + if (isLeafNode && overlap) { dtPolyRef ref = base | (dtPolyRef)node->i; if (filter->passFilter(ref, tile, &tile->polys[node->i])) { - if (n < maxPolys) - polys[n++] = ref; + polyRefs[n] = ref; + polys[n] = &tile->polys[node->i]; + + if (n == batchSize - 1) + { + query->process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } } } - + if (overlap || isLeafNode) node++; else @@ -833,17 +852,14 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi node += escapeIndex; } } - - return n; } else { float bmin[3], bmax[3]; - int n = 0; const dtPolyRef base = m_nav->getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { - const dtPoly* p = &tile->polys[i]; + dtPoly* p = &tile->polys[i]; // Do not return off-mesh connection polygons. if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; @@ -861,16 +877,63 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi dtVmin(bmin, v); dtVmax(bmax, v); } - if (dtOverlapBounds(qmin,qmax, bmin,bmax)) + if (dtOverlapBounds(qmin, qmax, bmin, bmax)) { - if (n < maxPolys) - polys[n++] = ref; + polyRefs[n] = ref; + polys[n] = p; + + if (n == batchSize - 1) + { + query->process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } } } - return n; } + + // Process the last polygons that didn't make a full batch. + if (n > 0) + query->process(tile, polys, polyRefs, n); } +class dtCollectPolysQuery : public dtPolyQuery +{ + dtPolyRef* m_polys; + const int m_maxPolys; + int m_numCollected; + bool m_overflow; + +public: + dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys) + : m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false) + { + } + + int numCollected() const { return m_numCollected; } + bool overflowed() const { return m_overflow; } + + void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) + { + dtIgnoreUnused(tile); + dtIgnoreUnused(polys); + + int numLeft = m_maxPolys - m_numCollected; + int toCopy = count; + if (toCopy > numLeft) + { + m_overflow = true; + toCopy = numLeft; + } + + memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef)); + m_numCollected += toCopy; + } +}; + /// @par /// /// If no polygons are found, the function will return #DT_SUCCESS with a @@ -880,15 +943,41 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi /// be filled to capacity. The method of choosing which polygons from the /// full set are included in the partial result set is undefined. /// -dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents, +dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* polys, int* polyCount, const int maxPolys) const +{ + if (!polys || !polyCount || maxPolys < 0) + return DT_FAILURE | DT_INVALID_PARAM; + + dtCollectPolysQuery collector(polys, maxPolys); + + dtStatus status = queryPolygons(center, halfExtents, filter, &collector); + if (dtStatusFailed(status)) + return status; + + *polyCount = collector.numCollected(); + return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS; +} + +/// @par +/// +/// The query will be invoked with batches of polygons. Polygons passed +/// to the query have bounding boxes that overlap with the center and halfExtents +/// passed to this function. The dtPolyQuery::process function is invoked multiple +/// times until all overlapping polygons have been processed. +/// +dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, + const dtQueryFilter* filter, dtPolyQuery* query) const { dtAssert(m_nav); - + + if (!center || !halfExtents || !filter || !query) + return DT_FAILURE | DT_INVALID_PARAM; + float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); // Find tiles the query touches. int minx, miny, maxx, maxy; @@ -898,7 +987,6 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents 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) @@ -906,16 +994,10 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS); for (int j = 0; j < nneis; ++j) { - n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n); - if (n >= maxPolys) - { - *polyCount = n; - return DT_SUCCESS | DT_BUFFER_TOO_SMALL; - } + queryPolygonsInTile(neis[j], bmin, bmax, filter, query); } } } - *polyCount = n; return DT_SUCCESS; } @@ -940,18 +1022,14 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtAssert(m_nodePool); dtAssert(m_openList); - *pathCount = 0; - - if (!startRef || !endRef) - return DT_FAILURE | DT_INVALID_PARAM; - - if (!maxPath) - return DT_FAILURE | DT_INVALID_PARAM; + if (pathCount) + *pathCount = 0; // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) + if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || + !startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount) return DT_FAILURE | DT_INVALID_PARAM; - + if (startRef == endRef) { path[0] = startRef; @@ -974,7 +1052,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtNode* lastBestNode = startNode; float lastBestNodeCost = startNode->total; - dtStatus status = DT_SUCCESS; + bool outOfNodes = false; while (!m_openList->empty()) { @@ -1032,7 +1110,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); if (!neighbourNode) { - status |= DT_OUT_OF_NODES; + outOfNodes = true; continue; } @@ -1111,42 +1189,59 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, } } } - + + dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath); + if (lastBestNode->id != endRef) status |= DT_PARTIAL_RESULT; - - // Reverse the path. - dtNode* prev = 0; - dtNode* node = lastBestNode; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - int n = 0; - do - { - path[n++] = node->id; - if (n >= maxPath) - { - status |= DT_BUFFER_TOO_SMALL; - break; - } - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node); - - *pathCount = n; + + if (outOfNodes) + status |= DT_OUT_OF_NODES; return status; } +dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const +{ + // Find the length of the entire path. + dtNode* curNode = endNode; + int length = 0; + do + { + length++; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } while (curNode); + + // If the path cannot be fully stored then advance to the last node we will be able to store. + curNode = endNode; + int writeCount; + for (writeCount = length; writeCount > maxPath; writeCount--) + { + dtAssert(curNode); + + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } + + // Write path + for (int i = writeCount - 1; i >= 0; i--) + { + dtAssert(curNode); + + path[i] = curNode->id; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); + } + + dtAssert(!curNode); + + *pathCount = dtMin(length, maxPath); + + if (length > maxPath) + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + + return DT_SUCCESS; +} + + /// @par /// /// @warning Calling any non-slice methods before calling finalizeSlicedFindPath() @@ -1639,10 +1734,17 @@ dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flag 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) + + // If there is no space to append more vertices, return. + if ((*straightPathCount) >= maxStraightPath) { - return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); + return DT_SUCCESS | DT_BUFFER_TOO_SMALL; + } + + // If reached end of path, return. + if (flags == DT_STRAIGHTPATH_END) + { + return DT_SUCCESS; } } return DT_IN_PROGRESS; @@ -1767,10 +1869,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en for (int i = 0; i < pathSize; ++i) { float left[3], right[3]; - unsigned char fromType, toType; + unsigned char toType; if (i+1 < pathSize) { + unsigned char fromType; // fromType is ignored. + // Next portal. if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType))) { @@ -1786,12 +1890,14 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en // Apeend portals along the current straight path segment. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) { - stat = appendPortals(apexIndex, i, closestEndPos, path, + // Ignore status return value as we're just about to return anyway. + appendPortals(apexIndex, i, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath, options); } - stat = appendVertex(closestEndPos, 0, path[i], + // Ignore status return value as we're just about to return anyway. + appendVertex(closestEndPos, 0, path[i], straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); @@ -1812,7 +1918,7 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en dtVcopy(left, closestEndPos); dtVcopy(right, closestEndPos); - fromType = toType = DT_POLYTYPE_GROUND; + toType = DT_POLYTYPE_GROUND; } // Right vertex. @@ -1929,7 +2035,8 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en } } - stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, + // Ignore status return value as we're just about to return anyway. + appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); @@ -2400,10 +2507,10 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons const dtMeshTile* prevTile, *tile, *nextTile; const dtPoly* prevPoly, *poly, *nextPoly; - dtPolyRef curRef, nextRef; + dtPolyRef curRef; // The API input has been checked already, skip checking internal data. - nextRef = curRef = startRef; + curRef = startRef; tile = 0; poly = 0; m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); @@ -2458,7 +2565,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons } // Follow neighbours. - nextRef = 0; + dtPolyRef nextRef = 0; for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) { @@ -2649,20 +2756,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* dtStatus status = DT_SUCCESS; int n = 0; - if (n < maxResult) - { - if (resultRef) - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - if (resultCost) - resultCost[n] = 0; - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } const float radiusSqr = dtSqr(radius); @@ -2687,6 +2780,21 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; if (parentRef) m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + if (n < maxResult) + { + if (resultRef) + resultRef[n] = bestRef; + if (resultParent) + resultParent[n] = parentRef; + if (resultCost) + resultCost[n] = bestNode->total; + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { @@ -2730,14 +2838,19 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* if (neighbourNode->flags == 0) dtVlerp(neighbourNode->pos, va, vb, 0.5f); - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); + float cost = filter->getCost( + bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + + const float total = bestNode->total + cost; // 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; @@ -2747,20 +2860,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* } else { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = neighbourNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id; - if (resultCost) - resultCost[n] = neighbourNode->total; - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } neighbourNode->flags = DT_NODE_OPEN; m_openList->push(neighbourNode); } @@ -2829,20 +2928,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v dtStatus status = DT_SUCCESS; int n = 0; - if (n < maxResult) - { - if (resultRef) - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - if (resultCost) - resultCost[n] = 0; - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } while (!m_openList->empty()) { @@ -2865,6 +2950,22 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; if (parentRef) m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + if (n < maxResult) + { + if (resultRef) + resultRef[n] = bestRef; + if (resultParent) + resultParent[n] = parentRef; + if (resultCost) + resultCost[n] = bestNode->total; + + ++n; + } + else + { + status |= DT_BUFFER_TOO_SMALL; + } for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { @@ -2910,14 +3011,19 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v if (neighbourNode->flags == 0) dtVlerp(neighbourNode->pos, va, vb, 0.5f); - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); + float cost = filter->getCost( + bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + + const float total = bestNode->total + cost; // 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; @@ -2927,20 +3033,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v } else { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = neighbourNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id; - if (resultCost) - resultCost[n] = neighbourNode->total; - ++n; - } - else - { - status |= DT_BUFFER_TOO_SMALL; - } neighbourNode->flags = DT_NODE_OPEN; m_openList->push(neighbourNode); } @@ -2952,6 +3044,21 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v return status; } +dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const +{ + if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0) + return DT_FAILURE | DT_INVALID_PARAM; + + *pathCount = 0; + + dtNode* endNode; + if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 || + (endNode->flags & DT_NODE_CLOSED) == 0) + return DT_FAILURE | DT_INVALID_PARAM; + + return getPathToNode(endNode, path, pathCount, maxPath); +} + /// @par /// /// This method is optimized for a small search radius and small number of result diff --git a/Engine/lib/recast/DetourCrowd/Include/DetourCrowd.h b/Engine/lib/recast/DetourCrowd/Include/DetourCrowd.h index 2a2003b16..952050878 100644 --- a/Engine/lib/recast/DetourCrowd/Include/DetourCrowd.h +++ b/Engine/lib/recast/DetourCrowd/Include/DetourCrowd.h @@ -217,7 +217,7 @@ class dtCrowd dtPolyRef* m_pathResult; int m_maxPathResult; - float m_ext[3]; + float m_agentPlacementHalfExtents[3]; dtQueryFilter m_filters[DT_CROWD_MAX_QUERY_FILTER_TYPE]; @@ -325,9 +325,13 @@ public: /// @return The filter used by the crowd. inline dtQueryFilter* getEditableFilter(const int i) { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; } - /// Gets the search extents [(x, y, z)] used by the crowd for query operations. - /// @return The search extents used by the crowd. [(x, y, z)] - const float* getQueryExtents() const { return m_ext; } + /// Gets the search halfExtents [(x, y, z)] used by the crowd for query operations. + /// @return The search halfExtents used by the crowd. [(x, y, z)] + const float* getQueryHalfExtents() const { return m_agentPlacementHalfExtents; } + + /// Same as getQueryHalfExtents. Left to maintain backwards compatibility. + /// @return The search halfExtents used by the crowd. [(x, y, z)] + const float* getQueryExtents() const { return m_agentPlacementHalfExtents; } /// Gets the velocity sample count. /// @return The velocity sample count. @@ -453,3 +457,4 @@ A higher value will result in agents trying to stay farther away from each other the cost of more difficult steering in tight spaces. */ + diff --git a/Engine/lib/recast/DetourCrowd/Source/DetourCrowd.cpp b/Engine/lib/recast/DetourCrowd/Source/DetourCrowd.cpp index f9f436077..1e76e40ce 100644 --- a/Engine/lib/recast/DetourCrowd/Source/DetourCrowd.cpp +++ b/Engine/lib/recast/DetourCrowd/Source/DetourCrowd.cpp @@ -386,7 +386,8 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n m_maxAgents = maxAgents; m_maxAgentRadius = maxAgentRadius; - dtVset(m_ext, m_maxAgentRadius*2.0f,m_maxAgentRadius*1.5f,m_maxAgentRadius*2.0f); + // Larger than agent radius because it is also used for agent recovery. + dtVset(m_agentPlacementHalfExtents, m_maxAgentRadius*2.0f, m_maxAgentRadius*1.5f, m_maxAgentRadius*2.0f); m_grid = dtAllocProximityGrid(); if (!m_grid) @@ -531,7 +532,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params) float nearest[3]; dtPolyRef ref = 0; dtVcopy(nearest, pos); - dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest); + dtStatus status = m_navquery->findNearestPoly(pos, m_agentPlacementHalfExtents, &m_filters[ag->params.queryFilterType], &ref, nearest); if (dtStatusFailed(status)) { dtVcopy(nearest, pos); @@ -965,7 +966,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float nearest[3]; dtVcopy(nearest, agentPos); agentRef = 0; - m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->params.queryFilterType], &agentRef, nearest); + m_navquery->findNearestPoly(ag->npos, m_agentPlacementHalfExtents, &m_filters[ag->params.queryFilterType], &agentRef, nearest); dtVcopy(agentPos, nearest); if (!agentRef) @@ -1001,7 +1002,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float nearest[3]; dtVcopy(nearest, ag->targetPos); ag->targetRef = 0; - m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest); + m_navquery->findNearestPoly(ag->targetPos, m_agentPlacementHalfExtents, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest); dtVcopy(ag->targetPos, nearest); replan = true; } diff --git a/Engine/lib/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp b/Engine/lib/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp index 8466c813a..94f7df6ad 100644 --- a/Engine/lib/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp +++ b/Engine/lib/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp @@ -207,6 +207,9 @@ void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr) dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() : + m_invHorizTime(0), + m_vmax(0), + m_invVmax(0), m_maxCircles(0), m_circles(0), m_ncircles(0), diff --git a/Engine/lib/recast/DetourCrowd/Source/DetourPathCorridor.cpp b/Engine/lib/recast/DetourCrowd/Source/DetourPathCorridor.cpp index 54a2ab8b8..e54d4637d 100644 --- a/Engine/lib/recast/DetourCrowd/Source/DetourPathCorridor.cpp +++ b/Engine/lib/recast/DetourCrowd/Source/DetourPathCorridor.cpp @@ -431,7 +431,7 @@ Behavior: - The new position will be located in the adjusted corridor's first polygon. The expected use case is that the desired position will be 'near' the current corridor. What is considered 'near' -depends on local polygon density, query search extents, etc. +depends on local polygon density, query search half extents, etc. The resulting position will differ from the desired position if the desired position is not on the navigation mesh, or it can't be reached using a local search. @@ -470,7 +470,7 @@ Behavior: - The corridor is automatically adjusted (shorted or lengthened) in order to remain valid. - The new target will be located in the adjusted corridor's last polygon. -The expected use case is that the desired target will be 'near' the current corridor. What is considered 'near' depends on local polygon density, query search extents, etc. +The expected use case is that the desired target will be 'near' the current corridor. What is considered 'near' depends on local polygon density, query search half extents, etc. The resulting target will differ from the desired target if the desired target is not on the navigation mesh, or it can't be reached using a local search. */ diff --git a/Engine/lib/recast/DetourCrowd/Source/DetourProximityGrid.cpp b/Engine/lib/recast/DetourCrowd/Source/DetourProximityGrid.cpp index 7af8efa01..fc1e2e136 100644 --- a/Engine/lib/recast/DetourCrowd/Source/DetourProximityGrid.cpp +++ b/Engine/lib/recast/DetourCrowd/Source/DetourProximityGrid.cpp @@ -48,6 +48,7 @@ inline int hashPos2(int x, int y, int n) dtProximityGrid::dtProximityGrid() : m_cellSize(0), + m_invCellSize(0), m_pool(0), m_poolHead(0), m_poolSize(0), diff --git a/Engine/lib/recast/DetourTileCache/Include/DetourTileCache.h b/Engine/lib/recast/DetourTileCache/Include/DetourTileCache.h index 9c7e01c35..75713366d 100644 --- a/Engine/lib/recast/DetourTileCache/Include/DetourTileCache.h +++ b/Engine/lib/recast/DetourTileCache/Include/DetourTileCache.h @@ -35,13 +35,47 @@ enum ObstacleState DT_OBSTACLE_REMOVING, }; +enum ObstacleType +{ + DT_OBSTACLE_CYLINDER, + DT_OBSTACLE_BOX, // AABB + DT_OBSTACLE_ORIENTED_BOX, // OBB +}; + +struct dtObstacleCylinder +{ + float pos[ 3 ]; + float radius; + float height; +}; + +struct dtObstacleBox +{ + float bmin[ 3 ]; + float bmax[ 3 ]; +}; + +struct dtObstacleOrientedBox +{ + float center[ 3 ]; + float halfExtents[ 3 ]; + float rotAux[ 2 ]; //{ cos(0.5f*angle)*sin(-0.5f*angle); cos(0.5f*angle)*cos(0.5f*angle) - 0.5 } +}; + static const int DT_MAX_TOUCHED_TILES = 8; struct dtTileCacheObstacle { - float pos[3], radius, height; + union + { + dtObstacleCylinder cylinder; + dtObstacleBox box; + dtObstacleOrientedBox orientedBox; + }; + dtCompressedTileRef touched[DT_MAX_TOUCHED_TILES]; dtCompressedTileRef pending[DT_MAX_TOUCHED_TILES]; unsigned short salt; + unsigned char type; unsigned char state; unsigned char ntouched; unsigned char npending; @@ -105,13 +139,27 @@ public: dtStatus removeTile(dtCompressedTileRef ref, unsigned char** data, int* dataSize); + // Cylinder obstacle. dtStatus addObstacle(const float* pos, const float radius, const float height, dtObstacleRef* result); + + // Aabb obstacle. + dtStatus addBoxObstacle(const float* bmin, const float* bmax, dtObstacleRef* result); + + // Box obstacle: can be rotated in Y. + dtStatus addBoxObstacle(const float* center, const float* halfExtents, const float yRadians, dtObstacleRef* result); + dtStatus removeObstacle(const dtObstacleRef ref); dtStatus queryTiles(const float* bmin, const float* bmax, dtCompressedTileRef* results, int* resultCount, const int maxResults) const; - dtStatus update(const float /*dt*/, class dtNavMesh* navmesh); + /// Updates the tile cache by rebuilding tiles touched by unfinished obstacle requests. + /// @param[in] dt The time step size. Currently not used. + /// @param[in] navmesh The mesh to affect when rebuilding tiles. + /// @param[out] upToDate Whether the tile cache is fully up to date with obstacle requests and tile rebuilds. + /// If the tile cache is up to date another (immediate) call to update will have no effect; + /// otherwise another call will continue processing obstacle requests and tile rebuilds. + dtStatus update(const float dt, class dtNavMesh* navmesh, bool* upToDate = 0); dtStatus buildNavMeshTilesAt(const int tx, const int ty, class dtNavMesh* navmesh); diff --git a/Engine/lib/recast/DetourTileCache/Include/DetourTileCacheBuilder.h b/Engine/lib/recast/DetourTileCache/Include/DetourTileCacheBuilder.h index 854183c91..ff6109193 100644 --- a/Engine/lib/recast/DetourTileCache/Include/DetourTileCacheBuilder.h +++ b/Engine/lib/recast/DetourTileCache/Include/DetourTileCacheBuilder.h @@ -127,6 +127,12 @@ void dtFreeTileCachePolyMesh(dtTileCacheAlloc* alloc, dtTileCachePolyMesh* lmesh dtStatus dtMarkCylinderArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch, const float* pos, const float radius, const float height, const unsigned char areaId); +dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch, + const float* bmin, const float* bmax, const unsigned char areaId); + +dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch, + const float* center, const float* halfExtents, const float* rotAux, const unsigned char areaId); + dtStatus dtBuildTileCacheRegions(dtTileCacheAlloc* alloc, dtTileCacheLayer& layer, const int walkableClimb); diff --git a/Engine/lib/recast/DetourTileCache/Source/DetourTileCache.cpp b/Engine/lib/recast/DetourTileCache/Source/DetourTileCache.cpp index 9d8fac2ce..a82cd1350 100644 --- a/Engine/lib/recast/DetourTileCache/Source/DetourTileCache.cpp +++ b/Engine/lib/recast/DetourTileCache/Source/DetourTileCache.cpp @@ -77,6 +77,7 @@ dtTileCache::dtTileCache() : m_nupdate(0) { memset(&m_params, 0, sizeof(m_params)); + memset(m_reqs, 0, sizeof(ObstacleRequest) * MAX_REQUESTS); } dtTileCache::~dtTileCache() @@ -369,9 +370,10 @@ dtStatus dtTileCache::addObstacle(const float* pos, const float radius, const fl memset(ob, 0, sizeof(dtTileCacheObstacle)); ob->salt = salt; ob->state = DT_OBSTACLE_PROCESSING; - dtVcopy(ob->pos, pos); - ob->radius = radius; - ob->height = height; + ob->type = DT_OBSTACLE_CYLINDER; + dtVcopy(ob->cylinder.pos, pos); + ob->cylinder.radius = radius; + ob->cylinder.height = height; ObstacleRequest* req = &m_reqs[m_nreqs++]; memset(req, 0, sizeof(ObstacleRequest)); @@ -384,6 +386,79 @@ dtStatus dtTileCache::addObstacle(const float* pos, const float radius, const fl return DT_SUCCESS; } +dtStatus dtTileCache::addBoxObstacle(const float* bmin, const float* bmax, dtObstacleRef* result) +{ + if (m_nreqs >= MAX_REQUESTS) + return DT_FAILURE | DT_BUFFER_TOO_SMALL; + + dtTileCacheObstacle* ob = 0; + if (m_nextFreeObstacle) + { + ob = m_nextFreeObstacle; + m_nextFreeObstacle = ob->next; + ob->next = 0; + } + if (!ob) + return DT_FAILURE | DT_OUT_OF_MEMORY; + + unsigned short salt = ob->salt; + memset(ob, 0, sizeof(dtTileCacheObstacle)); + ob->salt = salt; + ob->state = DT_OBSTACLE_PROCESSING; + ob->type = DT_OBSTACLE_BOX; + dtVcopy(ob->box.bmin, bmin); + dtVcopy(ob->box.bmax, bmax); + + ObstacleRequest* req = &m_reqs[m_nreqs++]; + memset(req, 0, sizeof(ObstacleRequest)); + req->action = REQUEST_ADD; + req->ref = getObstacleRef(ob); + + if (result) + *result = req->ref; + + return DT_SUCCESS; +} + +dtStatus dtTileCache::addBoxObstacle(const float* center, const float* halfExtents, const float yRadians, dtObstacleRef* result) +{ + if (m_nreqs >= MAX_REQUESTS) + return DT_FAILURE | DT_BUFFER_TOO_SMALL; + + dtTileCacheObstacle* ob = 0; + if (m_nextFreeObstacle) + { + ob = m_nextFreeObstacle; + m_nextFreeObstacle = ob->next; + ob->next = 0; + } + if (!ob) + return DT_FAILURE | DT_OUT_OF_MEMORY; + + unsigned short salt = ob->salt; + memset(ob, 0, sizeof(dtTileCacheObstacle)); + ob->salt = salt; + ob->state = DT_OBSTACLE_PROCESSING; + ob->type = DT_OBSTACLE_ORIENTED_BOX; + dtVcopy(ob->orientedBox.center, center); + dtVcopy(ob->orientedBox.halfExtents, halfExtents); + + float coshalf= cosf(0.5f*yRadians); + float sinhalf = sinf(-0.5f*yRadians); + ob->orientedBox.rotAux[0] = coshalf*sinhalf; + ob->orientedBox.rotAux[1] = coshalf*coshalf - 0.5f; + + ObstacleRequest* req = &m_reqs[m_nreqs++]; + memset(req, 0, sizeof(ObstacleRequest)); + req->action = REQUEST_ADD; + req->ref = getObstacleRef(ob); + + if (result) + *result = req->ref; + + return DT_SUCCESS; +} + dtStatus dtTileCache::removeObstacle(const dtObstacleRef ref) { if (!ref) @@ -440,7 +515,8 @@ dtStatus dtTileCache::queryTiles(const float* bmin, const float* bmax, return DT_SUCCESS; } -dtStatus dtTileCache::update(const float /*dt*/, dtNavMesh* navmesh) +dtStatus dtTileCache::update(const float /*dt*/, dtNavMesh* navmesh, + bool* upToDate) { if (m_nupdate == 0) { @@ -499,12 +575,13 @@ dtStatus dtTileCache::update(const float /*dt*/, dtNavMesh* navmesh) m_nreqs = 0; } + dtStatus status = DT_SUCCESS; // Process updates if (m_nupdate) { // Build mesh const dtCompressedTileRef ref = m_update[0]; - dtStatus status = buildNavMeshTile(ref, navmesh); + status = buildNavMeshTile(ref, navmesh); m_nupdate--; if (m_nupdate > 0) memmove(m_update, m_update+1, m_nupdate*sizeof(dtCompressedTileRef)); @@ -547,12 +624,12 @@ dtStatus dtTileCache::update(const float /*dt*/, dtNavMesh* navmesh) } } } - - if (dtStatusFailed(status)) - return status; } - return DT_SUCCESS; + if (upToDate) + *upToDate = m_nupdate == 0 && m_nreqs == 0; + + return status; } @@ -604,8 +681,21 @@ dtStatus dtTileCache::buildNavMeshTile(const dtCompressedTileRef ref, dtNavMesh* continue; if (contains(ob->touched, ob->ntouched, ref)) { - dtMarkCylinderArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch, - ob->pos, ob->radius, ob->height, 0); + if (ob->type == DT_OBSTACLE_CYLINDER) + { + dtMarkCylinderArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch, + ob->cylinder.pos, ob->cylinder.radius, ob->cylinder.height, 0); + } + else if (ob->type == DT_OBSTACLE_BOX) + { + dtMarkBoxArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch, + ob->box.bmin, ob->box.bmax, 0); + } + else if (ob->type == DT_OBSTACLE_ORIENTED_BOX) + { + dtMarkBoxArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch, + ob->orientedBox.center, ob->orientedBox.halfExtents, ob->orientedBox.rotAux, 0); + } } } @@ -616,7 +706,7 @@ dtStatus dtTileCache::buildNavMeshTile(const dtCompressedTileRef ref, dtNavMesh* bc.lcset = dtAllocTileCacheContourSet(m_talloc); if (!bc.lcset) - return status; + return DT_FAILURE | DT_OUT_OF_MEMORY; status = dtBuildTileCacheContours(m_talloc, *bc.layer, walkableClimbVx, m_params.maxSimplificationError, *bc.lcset); if (dtStatusFailed(status)) @@ -624,7 +714,7 @@ dtStatus dtTileCache::buildNavMeshTile(const dtCompressedTileRef ref, dtNavMesh* bc.lmesh = dtAllocTileCachePolyMesh(m_talloc); if (!bc.lmesh) - return status; + return DT_FAILURE | DT_OUT_OF_MEMORY; status = dtBuildTileCachePolyMesh(m_talloc, *bc.lcset, *bc.lmesh); if (dtStatusFailed(status)) return status; @@ -699,10 +789,32 @@ void dtTileCache::calcTightTileBounds(const dtTileCacheLayerHeader* header, floa void dtTileCache::getObstacleBounds(const struct dtTileCacheObstacle* ob, float* bmin, float* bmax) const { - bmin[0] = ob->pos[0] - ob->radius; - bmin[1] = ob->pos[1]; - bmin[2] = ob->pos[2] - ob->radius; - bmax[0] = ob->pos[0] + ob->radius; - bmax[1] = ob->pos[1] + ob->height; - bmax[2] = ob->pos[2] + ob->radius; + if (ob->type == DT_OBSTACLE_CYLINDER) + { + const dtObstacleCylinder &cl = ob->cylinder; + + bmin[0] = cl.pos[0] - cl.radius; + bmin[1] = cl.pos[1]; + bmin[2] = cl.pos[2] - cl.radius; + bmax[0] = cl.pos[0] + cl.radius; + bmax[1] = cl.pos[1] + cl.height; + bmax[2] = cl.pos[2] + cl.radius; + } + else if (ob->type == DT_OBSTACLE_BOX) + { + dtVcopy(bmin, ob->box.bmin); + dtVcopy(bmax, ob->box.bmax); + } + else if (ob->type == DT_OBSTACLE_ORIENTED_BOX) + { + const dtObstacleOrientedBox &orientedBox = ob->orientedBox; + + float maxr = 1.41f*dtMax(orientedBox.halfExtents[0], orientedBox.halfExtents[2]); + bmin[0] = orientedBox.center[0] - maxr; + bmax[0] = orientedBox.center[0] + maxr; + bmin[1] = orientedBox.center[1] - orientedBox.halfExtents[1]; + bmax[1] = orientedBox.center[1] + orientedBox.halfExtents[1]; + bmin[2] = orientedBox.center[2] - maxr; + bmax[2] = orientedBox.center[2] + maxr; + } } diff --git a/Engine/lib/recast/DetourTileCache/Source/DetourTileCacheBuilder.cpp b/Engine/lib/recast/DetourTileCache/Source/DetourTileCacheBuilder.cpp index 2c82cf0a9..1bbbcc483 100644 --- a/Engine/lib/recast/DetourTileCache/Source/DetourTileCacheBuilder.cpp +++ b/Engine/lib/recast/DetourTileCache/Source/DetourTileCacheBuilder.cpp @@ -29,9 +29,7 @@ template class dtFixedArray dtTileCacheAlloc* m_alloc; T* m_ptr; const int m_size; - inline T* operator=(T* p); inline void operator=(dtFixedArray& p); - inline dtFixedArray(); public: inline dtFixedArray(dtTileCacheAlloc* a, const int s) : m_alloc(a), m_ptr((T*)a->alloc(sizeof(T)*s)), m_size(s) {} inline ~dtFixedArray() { if (m_alloc) m_alloc->free(m_ptr); } @@ -2004,6 +2002,98 @@ dtStatus dtMarkCylinderArea(dtTileCacheLayer& layer, const float* orig, const fl return DT_SUCCESS; } +dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch, + const float* bmin, const float* bmax, const unsigned char areaId) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float ics = 1.0f/cs; + const float ich = 1.0f/ch; + + int minx = (int)floorf((bmin[0]-orig[0])*ics); + int miny = (int)floorf((bmin[1]-orig[1])*ich); + int minz = (int)floorf((bmin[2]-orig[2])*ics); + int maxx = (int)floorf((bmax[0]-orig[0])*ics); + int maxy = (int)floorf((bmax[1]-orig[1])*ich); + int maxz = (int)floorf((bmax[2]-orig[2])*ics); + + if (maxx < 0) return DT_SUCCESS; + if (minx >= w) return DT_SUCCESS; + if (maxz < 0) return DT_SUCCESS; + if (minz >= h) return DT_SUCCESS; + + if (minx < 0) minx = 0; + if (maxx >= w) maxx = w-1; + if (minz < 0) minz = 0; + if (maxz >= h) maxz = h-1; + + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const int y = layer.heights[x+z*w]; + if (y < miny || y > maxy) + continue; + layer.areas[x+z*w] = areaId; + } + } + + return DT_SUCCESS; +} + +dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch, + const float* center, const float* halfExtents, const float* rotAux, const unsigned char areaId) +{ + const int w = (int)layer.header->width; + const int h = (int)layer.header->height; + const float ics = 1.0f/cs; + const float ich = 1.0f/ch; + + float cx = (center[0] - orig[0])*ics; + float cz = (center[2] - orig[2])*ics; + + float maxr = 1.41f*dtMax(halfExtents[0], halfExtents[2]); + int minx = (int)floorf(cx - maxr*ics); + int maxx = (int)floorf(cx + maxr*ics); + int minz = (int)floorf(cz - maxr*ics); + int maxz = (int)floorf(cz + maxr*ics); + int miny = (int)floorf((center[1]-halfExtents[1]-orig[1])*ich); + int maxy = (int)floorf((center[1]+halfExtents[1]-orig[1])*ich); + + if (maxx < 0) return DT_SUCCESS; + if (minx >= w) return DT_SUCCESS; + if (maxz < 0) return DT_SUCCESS; + if (minz >= h) return DT_SUCCESS; + + if (minx < 0) minx = 0; + if (maxx >= w) maxx = w-1; + if (minz < 0) minz = 0; + if (maxz >= h) maxz = h-1; + + float xhalf = halfExtents[0]*ics + 0.5f; + float zhalf = halfExtents[2]*ics + 0.5f; + + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + float x2 = 2.0f*(float(x) - cx); + float z2 = 2.0f*(float(z) - cz); + float xrot = rotAux[1]*x2 + rotAux[0]*z2; + if (xrot > xhalf || xrot < -xhalf) + continue; + float zrot = rotAux[1]*z2 - rotAux[0]*x2; + if (zrot > zhalf || zrot < -zhalf) + continue; + const int y = layer.heights[x+z*w]; + if (y < miny || y > maxy) + continue; + layer.areas[x+z*w] = areaId; + } + } + + return DT_SUCCESS; +} dtStatus dtBuildTileCacheLayer(dtTileCacheCompressor* comp, dtTileCacheLayerHeader* header, @@ -2027,7 +2117,11 @@ dtStatus dtBuildTileCacheLayer(dtTileCacheCompressor* comp, const int bufferSize = gridSize*3; unsigned char* buffer = (unsigned char*)dtAlloc(bufferSize, DT_ALLOC_TEMP); if (!buffer) + { + dtFree(data); return DT_FAILURE | DT_OUT_OF_MEMORY; + } + memcpy(buffer, heights, gridSize); memcpy(buffer+gridSize, areas, gridSize); memcpy(buffer+gridSize*2, cons, gridSize); @@ -2038,7 +2132,11 @@ dtStatus dtBuildTileCacheLayer(dtTileCacheCompressor* comp, int compressedSize = 0; dtStatus status = comp->compress(buffer, bufferSize, compressed, maxCompressedSize, &compressedSize); if (dtStatusFailed(status)) + { + dtFree(buffer); + dtFree(data); return status; + } *outData = data; *outDataSize = headerSize + compressedSize; diff --git a/Engine/lib/recast/README.md b/Engine/lib/recast/README.md new file mode 100644 index 000000000..7db799636 --- /dev/null +++ b/Engine/lib/recast/README.md @@ -0,0 +1,89 @@ + +Recast & Detour +=============== + +[![Travis (Linux) Build Status](https://travis-ci.org/recastnavigation/recastnavigation.svg?branch=master)](https://travis-ci.org/recastnavigation/recastnavigation) +[![Appveyor (Windows) Build Status](https://ci.appveyor.com/api/projects/status/20w84u25b3f8h179/branch/master?svg=true)](https://ci.appveyor.com/project/recastnavigation/recastnavigation/branch/master) + +[![Issue Stats](http://www.issuestats.com/github/recastnavigation/recastnavigation/badge/pr?style=flat)](http://www.issuestats.com/github/recastnavigation/recastnavigation) +[![Issue Stats](http://www.issuestats.com/github/recastnavigation/recastnavigation/badge/issue?style=flat)](http://www.issuestats.com/github/recastnavigation/recastnavigation) + +![screenshot of a navmesh baked with the sample program](/RecastDemo/screenshot.png?raw=true) + +## Recast + +Recast is state of the art navigation mesh construction toolset for games. + +* It is automatic, which means that you can throw any level geometry at it and you will get robust mesh out +* It is fast which means swift turnaround times for level designers +* It is open source so it comes with full source and you can customize it to your heart's content. + +The Recast process starts with constructing a voxel mold from a level geometry +and then casting a navigation mesh over it. The process consists of three steps, +building the voxel mold, partitioning the mold into simple regions, peeling off +the regions as simple polygons. + +1. The voxel mold is build from the input triangle mesh by rasterizing the triangles into a multi-layer heightfield. Some simple filters are then applied to the mold to prune out locations where the character would not be able to move. +2. The walkable areas described by the mold are divided into simple overlayed 2D regions. The resulting regions have only one non-overlapping contour, which simplifies the final step of the process tremendously. +3. The navigation polygons are peeled off from the regions by first tracing the boundaries and then simplifying them. The resulting polygons are finally converted to convex polygons which makes them perfect for pathfinding and spatial reasoning about the level. + + +## Detour + +Recast is accompanied with Detour, path-finding and spatial reasoning toolkit. You can use any navigation mesh with Detour, but of course the data generated with Recast fits perfectly. + +Detour offers simple static navigation mesh which is suitable for many simple cases, as well as tiled navigation mesh which allows you to plug in and out pieces of the mesh. The tiled mesh allows you to create systems where you stream new navigation data in and out as the player progresses the level, or you may regenerate tiles as the world changes. + + +## Recast Demo + +You can find a comprehensive demo project in RecastDemo folder. It is a kitchen sink demo containing all the functionality of the library. If you are new to Recast & Detour, check out [Sample_SoloMesh.cpp](/RecastDemo/Source/Sample_SoloMesh.cpp) to get started with building navmeshes and [NavMeshTesterTool.cpp](/RecastDemo/Source/NavMeshTesterTool.cpp) to see how Detour can be used to find paths. + +### Building RecastDemo + +RecastDemo uses [premake5](http://premake.github.io/) to build platform specific projects. Download it and make sure it's available on your path, or specify the path to it. + +#### Linux + +- Install SDl2 and its dependencies according to your distro's guidelines. +- run `premake5 gmake` from the `RecastDemo` folder. +- `cd Build/gmake` then `make` +- Run `RecastDemo\Bin\RecastDemo` + +#### OSX + +- Grab the latest SDL2 development library dmg from [here](https://www.libsdl.org/download-2.0.php) and place `SDL2.framework` in `/Library/Frameworks/` +- Navigate to the `RecastDemo` folder and run `premake5 xcode4` +- Open `Build/xcode4/recastnavigation.xcworkspace` +- Select the "RecastDemo" project in the left pane, go to the "BuildPhases" tab and expand "Link Binary With Libraries" +- Remove the existing entry for SDL2 (it should have a white box icon) and re-add it by hitting the plus, selecting "Add Other", and selecting `/Library/Frameworks/SDL2.framework`. It should now have a suitcase icon. +- Set the RecastDemo project as the target and build. + +#### Windows + +- Grab the latest SDL2 development library release from [here](https://www.libsdl.org/download-2.0.php) and unzip it `RecastDemo\Contrib`. Rename the SDL folder such that the path `RecastDemo\Contrib\SDL\lib\x86` is valid. +- Run `"premake5" vs2015` from the `RecastDemo` folder +- Open the solution, build, and run. + +### Running Unit tests + +- Follow the instructions to build RecastDemo above. Premake should generate another build target called "Tests". +- Build the "Tests" project. This will generate an executable named "Tests" in `RecastDemo/Bin/` +- Run the "Tests" executable. It will execute all the unit tests, indicate those that failed, and display a count of those that succeeded. + +## Integrating with your own project + +It is recommended to add the source directories `DebugUtils`, `Detour`, `DetourCrowd`, `DetourTileCache`, and `Recast` into your own project depending on which parts of the project you need. For example your level building tool could include `DebugUtils`, `Recast`, and `Detour`, and your game runtime could just include `Detour`. + +## Contributing + +See the [Contributing document](CONTRIBUTING.md) for guidelines for making contributions. + +## Discuss + +- Discuss Recast & Detour: http://groups.google.com/group/recastnavigation +- Development blog: http://digestingduck.blogspot.com/ + +## License + +Recast & Detour is licensed under ZLib license, see License.txt for more information. diff --git a/Engine/lib/recast/Recast/Include/Recast.h b/Engine/lib/recast/Recast/Include/Recast.h index 2adcdcb34..e85c0d2e2 100644 --- a/Engine/lib/recast/Recast/Include/Recast.h +++ b/Engine/lib/recast/Recast/Include/Recast.h @@ -293,6 +293,9 @@ struct rcSpanPool /// @ingroup recast struct rcHeightfield { + rcHeightfield(); + ~rcHeightfield(); + 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.) float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] @@ -302,6 +305,11 @@ struct rcHeightfield rcSpan** spans; ///< Heightfield of spans (width*height). rcSpanPool* pools; ///< Linked list of span pools. rcSpan* freelist; ///< The next free span. + +private: + // Explicitly-disabled copy constructor and copy assignment operator. + rcHeightfield(const rcHeightfield&); + rcHeightfield& operator=(const rcHeightfield&); }; /// Provides information on the content of a cell column in a compact heightfield. @@ -886,7 +894,7 @@ bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int nv, bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt, rcHeightfield& solid, const int flagMergeThr = 1); -/// Marks non-walkable spans as walkable if their maximum is within @p walkableClimp of a walkable neihbor. +/// Marks non-walkable spans as walkable if their maximum is within @p walkableClimp of a walkable neighbor. /// @ingroup recast /// @param[in,out] ctx The build context to use during the operation. /// @param[in] walkableClimb Maximum ledge height that is considered to still be traversable. diff --git a/Engine/lib/recast/Recast/Include/RecastAlloc.h b/Engine/lib/recast/Recast/Include/RecastAlloc.h index f1608fb55..3cdd450d4 100644 --- a/Engine/lib/recast/Recast/Include/RecastAlloc.h +++ b/Engine/lib/recast/Recast/Include/RecastAlloc.h @@ -123,7 +123,6 @@ public: template class rcScopedDelete { T* ptr; - inline T* operator=(T* p); public: /// Constructs an instance with a null pointer. diff --git a/Engine/lib/recast/Recast/Include/RecastAssert.h b/Engine/lib/recast/Recast/Include/RecastAssert.h index 2aca0d9a1..e7cc10e49 100644 --- a/Engine/lib/recast/Recast/Include/RecastAssert.h +++ b/Engine/lib/recast/Recast/Include/RecastAssert.h @@ -23,11 +23,34 @@ // Feel free to change the file and include your own implementation instead. #ifdef NDEBUG + // From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ -# define rcAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) +# define rcAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) + #else + +/// An assertion failure function. +// @param[in] expression asserted expression. +// @param[in] file Filename of the failed assertion. +// @param[in] line Line number of the failed assertion. +/// @see rcAssertFailSetCustom +typedef void (rcAssertFailFunc)(const char* expression, const char* file, int line); + +/// Sets the base custom assertion failure function to be used by Recast. +/// @param[in] assertFailFunc The function to be used in case of failure of #dtAssert +void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc); + +/// Gets the base custom assertion failure function to be used by Recast. +rcAssertFailFunc* rcAssertFailGetCustom(); + # include -# define rcAssert assert +# define rcAssert(expression) \ + { \ + rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \ + if(failFunc == NULL) { assert(expression); } \ + else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \ + } + #endif #endif // RECASTASSERT_H diff --git a/Engine/lib/recast/Recast/Source/Recast.cpp b/Engine/lib/recast/Recast/Source/Recast.cpp index 46bc8b781..8308d1973 100644 --- a/Engine/lib/recast/Recast/Source/Recast.cpp +++ b/Engine/lib/recast/Recast/Source/Recast.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "Recast.h" #include "RecastAlloc.h" #include "RecastAssert.h" @@ -72,23 +73,39 @@ void rcContext::log(const rcLogCategory category, const char* format, ...) rcHeightfield* rcAllocHeightfield() { - rcHeightfield* hf = (rcHeightfield*)rcAlloc(sizeof(rcHeightfield), RC_ALLOC_PERM); - memset(hf, 0, sizeof(rcHeightfield)); - return hf; + return new (rcAlloc(sizeof(rcHeightfield), RC_ALLOC_PERM)) rcHeightfield; +} + +rcHeightfield::rcHeightfield() + : width() + , height() + , bmin() + , bmax() + , cs() + , ch() + , spans() + , pools() + , freelist() +{ +} + +rcHeightfield::~rcHeightfield() +{ + // Delete span array. + rcFree(spans); + // Delete span pools. + while (pools) + { + rcSpanPool* next = pools->next; + rcFree(pools); + pools = next; + } } void rcFreeHeightField(rcHeightfield* hf) { if (!hf) return; - // Delete span array. - rcFree(hf->spans); - // Delete span pools. - while (hf->pools) - { - rcSpanPool* next = hf->pools->next; - rcFree(hf->pools); - hf->pools = next; - } + hf->~rcHeightfield(); rcFree(hf); } @@ -109,7 +126,6 @@ void rcFreeCompactHeightfield(rcCompactHeightfield* chf) rcFree(chf); } - rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet() { rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM); @@ -245,11 +261,12 @@ static void calcTriNormal(const float* v0, const float* v1, const float* v2, flo /// /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, - const float* verts, int /*nv*/, + const float* verts, int nv, const int* tris, int nt, unsigned char* areas) { rcIgnoreUnused(ctx); + rcIgnoreUnused(nv); const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI); diff --git a/Engine/lib/recast/Recast/Source/RecastAlloc.cpp b/Engine/lib/recast/Recast/Source/RecastAlloc.cpp index ee1039f2f..453b5fa6a 100644 --- a/Engine/lib/recast/Recast/Source/RecastAlloc.cpp +++ b/Engine/lib/recast/Recast/Source/RecastAlloc.cpp @@ -19,6 +19,7 @@ #include #include #include "RecastAlloc.h" +#include "RecastAssert.h" static void *rcAllocDefault(size_t size, rcAllocHint) { @@ -77,6 +78,7 @@ void rcIntArray::doResize(int n) if (!m_cap) m_cap = n; while (m_cap < n) m_cap *= 2; int* newData = (int*)rcAlloc(m_cap*sizeof(int), RC_ALLOC_TEMP); + rcAssert(newData); if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int)); rcFree(m_data); m_data = newData; diff --git a/Engine/lib/recast/Recast/Source/RecastAssert.cpp b/Engine/lib/recast/Recast/Source/RecastAssert.cpp new file mode 100644 index 000000000..6297d4202 --- /dev/null +++ b/Engine/lib/recast/Recast/Source/RecastAssert.cpp @@ -0,0 +1,35 @@ +// +// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "RecastAssert.h" + +#ifndef NDEBUG + +static rcAssertFailFunc* sRecastAssertFailFunc = 0; + +void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc) +{ + sRecastAssertFailFunc = assertFailFunc; +} + +rcAssertFailFunc* rcAssertFailGetCustom() +{ + return sRecastAssertFailFunc; +} + +#endif diff --git a/Engine/lib/recast/Recast/Source/RecastLayers.cpp b/Engine/lib/recast/Recast/Source/RecastLayers.cpp index 22a357eff..acc97e44f 100644 --- a/Engine/lib/recast/Recast/Source/RecastLayers.cpp +++ b/Engine/lib/recast/Recast/Source/RecastLayers.cpp @@ -27,7 +27,9 @@ #include "RecastAssert.h" -static const int RC_MAX_LAYERS = RC_NOT_CONNECTED; +// Must be 255 or smaller (not 256) because layer IDs are stored as +// a byte where 255 is a special value. +static const int RC_MAX_LAYERS = 63; static const int RC_MAX_NEIS = 16; struct rcLayerRegion @@ -42,25 +44,31 @@ struct rcLayerRegion }; -static void addUnique(unsigned char* a, unsigned char& an, unsigned char v) -{ - const int n = (int)an; - for (int i = 0; i < n; ++i) - if (a[i] == v) - return; - a[an] = v; - an++; -} - static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v) { const int n = (int)an; for (int i = 0; i < n; ++i) + { if (a[i] == v) return true; + } return false; } +static bool addUnique(unsigned char* a, unsigned char& an, int anMax, unsigned char v) +{ + if (contains(a, an, v)) + return true; + + if ((int)an >= anMax) + return false; + + a[an] = v; + an++; + return true; +} + + inline bool overlapRange(const unsigned short amin, const unsigned short amax, const unsigned short bmin, const unsigned short bmax) { @@ -258,8 +266,13 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, const int ay = y + rcGetDirOffsetY(dir); const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); const unsigned char rai = srcReg[ai]; - if (rai != 0xff && rai != ri && regs[ri].nneis < RC_MAX_NEIS) - addUnique(regs[ri].neis, regs[ri].nneis, rai); + if (rai != 0xff && rai != ri) + { + // Don't check return value -- if we cannot add the neighbor + // it will just cause a few more regions to be created, which + // is fine. + addUnique(regs[ri].neis, regs[ri].nneis, RC_MAX_NEIS, rai); + } } } @@ -274,8 +287,13 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, { rcLayerRegion& ri = regs[lregs[i]]; rcLayerRegion& rj = regs[lregs[j]]; - addUnique(ri.layers, ri.nlayers, lregs[j]); - addUnique(rj.layers, rj.nlayers, lregs[i]); + + if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, lregs[j]) || + !addUnique(rj.layers, rj.nlayers, RC_MAX_LAYERS, lregs[i])) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS."); + return false; + } } } } @@ -338,7 +356,13 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, regn.layerId = layerId; // Merge current layers to root. for (int k = 0; k < regn.nlayers; ++k) - addUnique(root.layers, root.nlayers, regn.layers[k]); + { + if (!addUnique(root.layers, root.nlayers, RC_MAX_LAYERS, regn.layers[k])) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS."); + return false; + } + } root.ymin = rcMin(root.ymin, regn.ymin); root.ymax = rcMax(root.ymax, regn.ymax); } @@ -416,7 +440,14 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, rj.layerId = newId; // Add overlaid layers from 'rj' to 'ri'. for (int k = 0; k < rj.nlayers; ++k) - addUnique(ri.layers, ri.nlayers, rj.layers[k]); + { + if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, rj.layers[k])) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS."); + return false; + } + } + // Update height bounds. ri.ymin = rcMin(ri.ymin, rj.ymin); ri.ymax = rcMax(ri.ymax, rj.ymax); diff --git a/Engine/lib/recast/Recast/Source/RecastMesh.cpp b/Engine/lib/recast/Recast/Source/RecastMesh.cpp index 9b6f04e30..e99eaebb7 100644 --- a/Engine/lib/recast/Recast/Source/RecastMesh.cpp +++ b/Engine/lib/recast/Recast/Source/RecastMesh.cpp @@ -379,7 +379,7 @@ static int triangulate(int n, const int* verts, int* indices, int* tris) // We might get here because the contour has overlapping segments, like this: // // A o-o=====o---o B - // / |C D| \ + // / |C D| \. // o o o o // : : : : // We'll try to recover by loosing up the inCone test a bit so that a diagonal diff --git a/Engine/lib/recast/Recast/Source/RecastMeshDetail.cpp b/Engine/lib/recast/Recast/Source/RecastMeshDetail.cpp index f1270cf20..f953132f7 100644 --- a/Engine/lib/recast/Recast/Source/RecastMeshDetail.cpp +++ b/Engine/lib/recast/Recast/Source/RecastMeshDetail.cpp @@ -647,11 +647,10 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, int hull[MAX_VERTS]; int nhull = 0; - nverts = 0; + nverts = nin; for (int i = 0; i < nin; ++i) rcVcopy(&verts[i*3], &in[i*3]); - nverts = nin; edges.resize(0); tris.resize(0); @@ -777,7 +776,7 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, // Tessellate the base mesh. // We're using the triangulateHull instead of delaunayHull as it tends to - // create a bit better triangulation for long thing triangles when there + // create a bit better triangulation for long thin triangles when there // are no internal points. triangulateHull(nverts, verts, nhull, hull, tris); diff --git a/Engine/lib/recast/Recast/Source/RecastRegion.cpp b/Engine/lib/recast/Recast/Source/RecastRegion.cpp index 54acf4b73..38a2bd6bf 100644 --- a/Engine/lib/recast/Recast/Source/RecastRegion.cpp +++ b/Engine/lib/recast/Recast/Source/RecastRegion.cpp @@ -1575,12 +1575,6 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, // Make sure border will not overflow. const int bw = rcMin(w, borderSize); const int bh = rcMin(h, borderSize); - - if (regionId > 0xFFFB) - { - ctx->log(RC_LOG_ERROR, "rcBuildRegions: Region ID overflow"); - return false; - } // Paint regions paintRectRegion(0, bw, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; @@ -1690,7 +1684,7 @@ bool rcBuildLayerRegions(rcContext* ctx, rcCompactHeightfield& chf, rcScopedDelete srcReg((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP)); if (!srcReg) { - ctx->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'src' (%d).", chf.spanCount); + ctx->log(RC_LOG_ERROR, "rcBuildLayerRegions: Out of memory 'src' (%d).", chf.spanCount); return false; } memset(srcReg,0,sizeof(unsigned short)*chf.spanCount); @@ -1699,7 +1693,7 @@ bool rcBuildLayerRegions(rcContext* ctx, rcCompactHeightfield& chf, rcScopedDelete sweeps((rcSweepSpan*)rcAlloc(sizeof(rcSweepSpan)*nsweeps, RC_ALLOC_TEMP)); if (!sweeps) { - ctx->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'sweeps' (%d).", nsweeps); + ctx->log(RC_LOG_ERROR, "rcBuildLayerRegions: Out of memory 'sweeps' (%d).", nsweeps); return false; }