From 8f633523cd7521b1eb754b1c6a068715621fa3b2 Mon Sep 17 00:00:00 2001 From: Wojtek Figat Date: Mon, 1 Mar 2021 14:31:28 +0100 Subject: [PATCH] Update Recast navigation lib to `e75adf86f91eb3082220085e42dda62679f9a3ea` --- .../recastnavigation/DetourCommon.cpp | 18 +- .../recastnavigation/DetourCommon.h | 22 ++ .../recastnavigation/DetourCrowd.cpp | 8 +- .../ThirdParty/recastnavigation/DetourMath.h | 1 + .../recastnavigation/DetourNavMesh.cpp | 167 ++++++--- .../recastnavigation/DetourNavMesh.h | 25 +- .../recastnavigation/DetourNavMeshQuery.cpp | 321 +++++++++--------- .../recastnavigation/DetourNavMeshQuery.h | 24 +- .../ThirdParty/recastnavigation/RecastAlloc.h | 47 ++- .../recastnavigation/RecastContour.cpp | 6 +- .../recastnavigation/RecastMeshDetail.cpp | 19 +- .../recastnavigation/RecastRegion.cpp | 10 +- 12 files changed, 416 insertions(+), 252 deletions(-) diff --git a/Source/ThirdParty/recastnavigation/DetourCommon.cpp b/Source/ThirdParty/recastnavigation/DetourCommon.cpp index 3886f14b0..b89d7512c 100644 --- a/Source/ThirdParty/recastnavigation/DetourCommon.cpp +++ b/Source/ThirdParty/recastnavigation/DetourCommon.cpp @@ -203,14 +203,18 @@ void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const floa bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) { + const float EPS = 1e-6f; float v0[3], v1[3], v2[3]; - dtVsub(v0, c,a); - dtVsub(v1, b,a); - dtVsub(v2, p,a); + dtVsub(v0, c, a); + dtVsub(v1, b, a); + dtVsub(v2, p, a); // Compute scaled barycentric coordinates float denom = v0[0] * v1[2] - v0[2] * v1[0]; + if (fabsf(denom) < EPS) + return false; + float u = v1[2] * v2[0] - v1[0] * v2[2]; float v = v0[0] * v2[2] - v0[2] * v2[0]; @@ -220,13 +224,9 @@ bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b v = -v; } - // The (sloppy) epsilon is needed to allow to get height of points which - // are interpolated along the edges of the triangles. - float epsilon = - 1e-4f * denom; - // If point lies inside the triangle, return interpolated ycoord. - if (u >= epsilon && v >= epsilon && (u+v) <= denom - epsilon) { - h = a[1] + (v0[1]*u + v1[1]*v) / denom; + if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) { + h = a[1] + (v0[1] * u + v1[1] * v) / denom; return true; } return false; diff --git a/Source/ThirdParty/recastnavigation/DetourCommon.h b/Source/ThirdParty/recastnavigation/DetourCommon.h index 739858cd9..113e8c336 100644 --- a/Source/ThirdParty/recastnavigation/DetourCommon.h +++ b/Source/ThirdParty/recastnavigation/DetourCommon.h @@ -283,6 +283,28 @@ inline bool dtVequal(const float* p0, const float* p1) return d < thr; } +/// Checks that the specified vector's components are all finite. +/// @param[in] v A point. [(x, y, z)] +/// @return True if all of the point's components are finite, i.e. not NaN +/// or any of the infinities. +inline bool dtVisfinite(const float* v) +{ + bool result = + dtMathIsfinite(v[0]) && + dtMathIsfinite(v[1]) && + dtMathIsfinite(v[2]); + + return result; +} + +/// Checks that the specified vector's 2D components are finite. +/// @param[in] v A point. [(x, y, z)] +inline bool dtVisfinite2D(const float* v) +{ + bool result = dtMathIsfinite(v[0]) && dtMathIsfinite(v[2]); + return result; +} + /// Derives the dot product of two vectors on the xz-plane. (@p u . @p v) /// @param[in] u A vector [(x, y, z)] /// @param[in] v A vector [(x, y, z)] diff --git a/Source/ThirdParty/recastnavigation/DetourCrowd.cpp b/Source/ThirdParty/recastnavigation/DetourCrowd.cpp index 1e76e40ce..3f0311f7f 100644 --- a/Source/ThirdParty/recastnavigation/DetourCrowd.cpp +++ b/Source/ThirdParty/recastnavigation/DetourCrowd.cpp @@ -1409,12 +1409,14 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug) } // Update agents using off-mesh connection. - for (int i = 0; i < m_maxAgents; ++i) + for (int i = 0; i < nagents; ++i) { - dtCrowdAgentAnimation* anim = &m_agentAnims[i]; + dtCrowdAgent* ag = agents[i]; + const int idx = (int)(ag - m_agents); + dtCrowdAgentAnimation* anim = &m_agentAnims[idx]; if (!anim->active) continue; - dtCrowdAgent* ag = agents[i]; + anim->t += dt; if (anim->t > anim->tmax) diff --git a/Source/ThirdParty/recastnavigation/DetourMath.h b/Source/ThirdParty/recastnavigation/DetourMath.h index 95e14f884..5528ead74 100644 --- a/Source/ThirdParty/recastnavigation/DetourMath.h +++ b/Source/ThirdParty/recastnavigation/DetourMath.h @@ -16,5 +16,6 @@ inline float dtMathCeilf(float x) { return ceilf(x); } inline float dtMathCosf(float x) { return cosf(x); } inline float dtMathSinf(float x) { return sinf(x); } inline float dtMathAtan2f(float y, float x) { return atan2f(y, x); } +inline bool dtMathIsfinite(float x) { return isfinite(x); } #endif diff --git a/Source/ThirdParty/recastnavigation/DetourNavMesh.cpp b/Source/ThirdParty/recastnavigation/DetourNavMesh.cpp index 17df26d8c..b119cd541 100644 --- a/Source/ThirdParty/recastnavigation/DetourNavMesh.cpp +++ b/Source/ThirdParty/recastnavigation/DetourNavMesh.cpp @@ -616,63 +616,84 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) } } -void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const +namespace { - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - getTileAndPolyByRefUnsafe(ref, &tile, &poly); - - // Off-mesh connections don't have detail polygons. - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + template + void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) { - const float* v0 = &tile->verts[poly->verts[0]*3]; - const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist(pos, v0); - const float d1 = dtVdist(pos, v1); - const float u = d0 / (d0+d1); - dtVlerp(closest, v0, v1, u); - if (posOverPoly) - *posOverPoly = false; - return; + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + + float dmin = FLT_MAX; + float tmin = 0; + const float* pmin = 0; + const float* pmax = 0; + + for (int i = 0; i < pd->triCount; i++) + { + const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4]; + const int ANY_BOUNDARY_EDGE = + (DT_DETAIL_EDGE_BOUNDARY << 0) | + (DT_DETAIL_EDGE_BOUNDARY << 2) | + (DT_DETAIL_EDGE_BOUNDARY << 4); + if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0) + continue; + + const float* v[3]; + for (int j = 0; j < 3; ++j) + { + if (tris[j] < poly->vertCount) + v[j] = &tile->verts[poly->verts[tris[j]] * 3]; + else + v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3]; + } + + for (int k = 0, j = 2; k < 3; j = k++) + { + if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 && + (onlyBoundary || tris[j] < tris[k])) + { + // Only looking at boundary edges and this is internal, or + // this is an inner edge that we will see again or have already seen. + continue; + } + + float t; + float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t); + if (d < dmin) + { + dmin = d; + tmin = t; + pmin = v[j]; + pmax = v[k]; + } + } + } + + dtVlerp(closest, pmin, pmax, tmin); } - +} + +bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const +{ + // Off-mesh connections do not have detail polys and getting height + // over them does not make sense. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + return false; + const unsigned int ip = (unsigned int)(poly - tile->polys); const dtPolyDetail* pd = &tile->detailMeshes[ip]; - // Clamp point to be inside the polygon. float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; const int nv = poly->vertCount; for (int i = 0; i < nv; ++i) dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); - dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = edged[0]; - int imin = 0; - for (int i = 1; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - - if (posOverPoly) - *posOverPoly = false; - } - else - { - if (posOverPoly) - *posOverPoly = true; - } + if (!dtPointInPolygon(pos, verts, nv)) + return false; + + if (!height) + return true; // Find height at the location. for (int j = 0; j < pd->triCount; ++j) @@ -687,12 +708,53 @@ 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(closest, v[0], v[1], v[2], h)) + if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) { - closest[1] = h; - break; + *height = h; + return true; } } + + // If all triangle checks failed above (can happen with degenerate triangles + // or larger floating point values) the point is on an edge, so just select + // closest. This should almost never happen so the extra iteration here is + // ok. + float closest[3]; + closestPointOnDetailEdges(tile, poly, pos, closest); + *height = closest[1]; + return true; +} + +void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const +{ + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); + + dtVcopy(closest, pos); + if (getPolyHeight(tile, poly, pos, &closest[1])) + { + if (posOverPoly) + *posOverPoly = true; + return; + } + + if (posOverPoly) + *posOverPoly = false; + + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + float t; + dtDistancePtSegSqr2D(pos, v0, v1, t); + dtVlerp(closest, v0, v1, t); + return; + } + + // Outside poly that is not an offmesh connection. + closestPointOnDetailEdges(tile, poly, pos, closest); } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, @@ -852,6 +914,13 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags, return DT_FAILURE | DT_WRONG_MAGIC; if (header->version != DT_NAVMESH_VERSION) return DT_FAILURE | DT_WRONG_VERSION; + +#ifndef DT_POLYREF64 + // Do not allow adding more polygons than specified in the NavMesh's maxPolys constraint. + // Otherwise, the poly ID cannot be represented with the given number of bits. + if (m_polyBits < dtIlog2(dtNextPow2((unsigned int)header->polyCount))) + return DT_FAILURE | DT_INVALID_PARAM; +#endif // Make sure the location is free. if (getTileAt(header->x, header->y, header->layer)) diff --git a/Source/ThirdParty/recastnavigation/DetourNavMesh.h b/Source/ThirdParty/recastnavigation/DetourNavMesh.h index 02ee5e78c..9ac1dc8d6 100644 --- a/Source/ThirdParty/recastnavigation/DetourNavMesh.h +++ b/Source/ThirdParty/recastnavigation/DetourNavMesh.h @@ -130,6 +130,11 @@ enum dtRaycastOptions DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost }; +enum dtDetailTriEdgeFlags +{ + DT_DETAIL_EDGE_BOUNDARY = 0x01, ///< Detail triangle edge is part of the poly boundary +}; + /// Limit raycasting during any angle pahfinding /// The limit is given as a multiple of the character radius @@ -287,7 +292,8 @@ struct dtMeshTile /// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount] float* detailVerts; - /// The detail mesh's triangles. [(vertA, vertB, vertC) * dtMeshHeader::detailTriCount] + /// The detail mesh's triangles. [(vertA, vertB, vertC, triFlags) * dtMeshHeader::detailTriCount]. + /// See dtDetailTriEdgeFlags and dtGetDetailTriEdgeFlags. unsigned char* detailTris; /// The tile bounding volume nodes. [Size: dtMeshHeader::bvNodeCount] @@ -305,6 +311,15 @@ private: dtMeshTile& operator=(const dtMeshTile&); }; +/// Get flags for edge in detail triangle. +/// @param triFlags[in] The flags for the triangle (last component of detail vertices above). +/// @param edgeIndex[in] The index of the first vertex of the edge. For instance, if 0, +/// returns flags for edge AB. +inline int dtGetDetailTriEdgeFlags(unsigned char triFlags, int edgeIndex) +{ + return (triFlags >> (edgeIndex * 2)) & 0x3; +} + /// Configuration parameters used to define multi-tile navigation meshes. /// The values are used to allocate space during the initialization of a navigation mesh. /// @see dtNavMesh::init() @@ -314,8 +329,8 @@ struct dtNavMeshParams float orig[3]; ///< The world space origin of the navigation mesh's tile space. [(x, y, z)] float tileWidth; ///< The width of each tile. (Along the x-axis.) float tileHeight; ///< The height of each tile. (Along the z-axis.) - int maxTiles; ///< The maximum number of tiles the navigation mesh can contain. - int maxPolys; ///< The maximum number of polygons each tile can contain. + int maxTiles; ///< The maximum number of tiles the navigation mesh can contain. This and maxPolys are used to calculate how many bits are needed to identify tiles and polygons uniquely. + int maxPolys; ///< The maximum number of polygons each tile can contain. This and maxTiles are used to calculate how many bits are needed to identify tiles and polygons uniquely. }; /// A navigation mesh based on tiles of convex polygons. @@ -636,6 +651,8 @@ private: /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* halfExtents, float* nearestPt) const; + /// Returns whether position is over the poly and the height at the position if so. + bool getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const; /// Returns closest point on polygon. void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; @@ -655,6 +672,8 @@ private: unsigned int m_tileBits; ///< Number of tile bits in the tile ID. unsigned int m_polyBits; ///< Number of poly bits in the tile ID. #endif + + friend class dtNavMeshQuery; }; /// Allocates a navigation mesh object using the Detour allocator. diff --git a/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp b/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp index 90999f2f6..8ea09c823 100644 --- a/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp +++ b/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp @@ -222,7 +222,10 @@ dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*fr dtPolyRef* randomRef, float* randomPt) const { dtAssert(m_nav); - + + if (!filter || !frand || !randomRef || !randomPt) + return DT_FAILURE | DT_INVALID_PARAM; + // Randomly pick one tile. Assume that all tiles cover roughly the same area. const dtMeshTile* tile = 0; float tsum = 0.0f; @@ -319,8 +322,13 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f dtAssert(m_openList); // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + maxRadius < 0 || !dtMathIsfinite(maxRadius) || + !filter || !frand || !randomRef || !randomPt) + { return DT_FAILURE | DT_INVALID_PARAM; + } const dtMeshTile* startTile = 0; const dtPoly* startPoly = 0; @@ -506,85 +514,14 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { dtAssert(m_nav); - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) + if (!m_nav->isValidPolyRef(ref) || + !pos || !dtVisfinite(pos) || + !closest) + { return DT_FAILURE | DT_INVALID_PARAM; - if (!tile) - return DT_FAILURE | DT_INVALID_PARAM; - - // Off-mesh connections don't have detail polygons. - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - const float* v0 = &tile->verts[poly->verts[0]*3]; - const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist(pos, v0); - const float d1 = dtVdist(pos, v1); - const float u = d0 / (d0+d1); - dtVlerp(closest, v0, v1, u); - if (posOverPoly) - *posOverPoly = false; - return DT_SUCCESS; } - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - // Clamp point to be inside the polygon. - float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; - const int nv = poly->vertCount; - for (int i = 0; i < nv; ++i) - dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); - - dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = edged[0]; - int imin = 0; - for (int i = 1; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - - if (posOverPoly) - *posOverPoly = false; - } - else - { - if (posOverPoly) - *posOverPoly = true; - } - - // Find height at the location. - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h)) - { - closest[1] = h; - break; - } - } - + m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly); return DT_SUCCESS; } @@ -607,6 +544,9 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* const dtPoly* poly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) return DT_FAILURE | DT_INVALID_PARAM; + + if (!pos || !dtVisfinite(pos) || !closest) + return DT_FAILURE | DT_INVALID_PARAM; // Collect vertices. float verts[DT_VERTS_PER_POLYGON*3]; @@ -648,7 +588,7 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* /// @par /// -/// Will return #DT_FAILURE if the provided position is outside the xz-bounds +/// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds /// of the polygon. /// dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const @@ -659,44 +599,28 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h const dtPoly* poly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) return DT_FAILURE | DT_INVALID_PARAM; - + + if (!pos || !dtVisfinite2D(pos)) + return DT_FAILURE | DT_INVALID_PARAM; + + // We used to return success for offmesh connections, but the + // getPolyHeight in DetourNavMesh does not do this, so special + // case it here. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) { const float* v0 = &tile->verts[poly->verts[0]*3]; const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist2D(pos, v0); - const float d1 = dtVdist2D(pos, v1); - const float u = d0 / (d0+d1); + float t; + dtDistancePtSegSqr2D(pos, v0, v1, t); if (height) - *height = v0[1] + (v1[1] - v0[1]) * u; + *height = v0[1] + (v1[1] - v0[1])*t; + return DT_SUCCESS; } - else - { - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - if (height) - *height = h; - return DT_SUCCESS; - } - } - } - - return DT_FAILURE | DT_INVALID_PARAM; + + return m_nav->getPolyHeight(tile, poly, pos, height) + ? DT_SUCCESS + : DT_FAILURE | DT_INVALID_PARAM; } class dtFindNearestPolyQuery : public dtPolyQuery @@ -706,15 +630,17 @@ class dtFindNearestPolyQuery : public dtPolyQuery float m_nearestDistanceSqr; dtPolyRef m_nearestRef; float m_nearestPoint[3]; + bool m_overPoly; public: dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center) - : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint() + : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint(), m_overPoly(false) { } dtPolyRef nearestRef() const { return m_nearestRef; } const float* nearestPoint() const { return m_nearestPoint; } + bool isOverPoly() const { return m_overPoly; } void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) { @@ -748,6 +674,7 @@ public: m_nearestDistanceSqr = d; m_nearestRef = ref; + m_overPoly = posOverPoly; } } } @@ -762,11 +689,22 @@ public: dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* nearestRef, float* nearestPt) const +{ + return findNearestPoly(center, halfExtents, filter, nearestRef, nearestPt, NULL); +} + +// If center and nearestPt point to an equal position, isOverPoly will be true; +// however there's also a special case of climb height inside the polygon (see dtFindNearestPolyQuery) +dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt, bool* isOverPoly) const { dtAssert(m_nav); if (!nearestRef) return DT_FAILURE | DT_INVALID_PARAM; + + // queryPolygons below will check rest of params dtFindNearestPolyQuery query(this, center); @@ -778,7 +716,11 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfE // Only override nearestPt if we actually found a poly so the nearest point // is valid. if (nearestPt && *nearestRef) + { dtVcopy(nearestPt, query.nearestPoint()); + if (isOverPoly) + *isOverPoly = query.isOverPoly(); + } return DT_SUCCESS; } @@ -972,8 +914,12 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt { dtAssert(m_nav); - if (!center || !halfExtents || !filter || !query) + if (!center || !dtVisfinite(center) || + !halfExtents || !dtVisfinite(halfExtents) || + !filter || !query) + { return DT_FAILURE | DT_INVALID_PARAM; + } float bmin[3], bmax[3]; dtVsub(bmin, center, halfExtents); @@ -1021,14 +967,20 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtAssert(m_nav); dtAssert(m_nodePool); dtAssert(m_openList); - - if (pathCount) - *pathCount = 0; + + if (!pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + + *pathCount = 0; // Validate input if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || - !startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount) + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !filter || !path || maxPath <= 0) + { return DT_FAILURE | DT_INVALID_PARAM; + } if (startRef == endRef) { @@ -1263,18 +1215,21 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef m_query.status = DT_FAILURE; m_query.startRef = startRef; m_query.endRef = endRef; - dtVcopy(m_query.startPos, startPos); - dtVcopy(m_query.endPos, endPos); + if (startPos) + dtVcopy(m_query.startPos, startPos); + if (endPos) + dtVcopy(m_query.endPos, endPos); m_query.filter = filter; m_query.options = options; m_query.raycastLimitSqr = FLT_MAX; - if (!startRef || !endRef) - return DT_FAILURE | DT_INVALID_PARAM; - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) + if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || !filter) + { return DT_FAILURE | DT_INVALID_PARAM; + } // trade quality with performance? if (options & DT_FINDPATH_ANY_ANGLE) @@ -1530,7 +1485,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath) { + if (!pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + *pathCount = 0; + + if (!path || maxPath <= 0) + return DT_FAILURE | DT_INVALID_PARAM; if (dtStatusFailed(m_query.status)) { @@ -1615,12 +1576,13 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, dtPolyRef* path, int* pathCount, const int maxPath) { + if (!pathCount) + return DT_FAILURE | DT_INVALID_PARAM; + *pathCount = 0; - - if (existingSize == 0) - { - return DT_FAILURE; - } + + if (!existing || existingSize <= 0 || !path || !pathCount || maxPath <= 0) + return DT_FAILURE | DT_INVALID_PARAM; if (dtStatusFailed(m_query.status)) { @@ -1823,14 +1785,19 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en int* straightPathCount, const int maxStraightPath, const int options) const { dtAssert(m_nav); - + + if (!straightPathCount) + return DT_FAILURE | DT_INVALID_PARAM; + *straightPathCount = 0; - - if (!maxStraightPath) - return DT_FAILURE | DT_INVALID_PARAM; - - if (!path[0]) + + if (!startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !path || pathSize <= 0 || !path[0] || + maxStraightPath <= 0) + { return DT_FAILURE | DT_INVALID_PARAM; + } dtStatus stat = 0; @@ -2070,13 +2037,19 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start dtAssert(m_nav); dtAssert(m_tinyNodePool); + if (!visitedCount) + return DT_FAILURE | DT_INVALID_PARAM; + *visitedCount = 0; - - // Validate input - if (!startRef) - return DT_FAILURE | DT_INVALID_PARAM; - if (!m_nav->isValidPolyRef(startRef)) + + if (!m_nav->isValidPolyRef(startRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !filter || !resultPos || !visited || + maxVisitedSize <= 0) + { return DT_FAILURE | DT_INVALID_PARAM; + } dtStatus status = DT_SUCCESS; @@ -2484,16 +2457,23 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons dtRaycastHit* hit, dtPolyRef prevRef) const { dtAssert(m_nav); - + + if (!hit) + return DT_FAILURE | DT_INVALID_PARAM; + hit->t = 0; hit->pathCount = 0; hit->pathCost = 0; // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) - return DT_FAILURE | DT_INVALID_PARAM; - if (prevRef && !m_nav->isValidPolyRef(prevRef)) + if (!m_nav->isValidPolyRef(startRef) || + !startPos || !dtVisfinite(startPos) || + !endPos || !dtVisfinite(endPos) || + !filter || + (prevRef && !m_nav->isValidPolyRef(prevRef))) + { return DT_FAILURE | DT_INVALID_PARAM; + } float dir[3], curPos[3], lastPos[3]; float verts[DT_VERTS_PER_POLYGON*3+3]; @@ -2735,11 +2715,18 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* dtAssert(m_nodePool); dtAssert(m_openList); - *resultCount = 0; - - // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) + if (!resultCount) return DT_FAILURE | DT_INVALID_PARAM; + + *resultCount = 0; + + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + radius < 0 || !dtMathIsfinite(radius) || + !filter || maxResult < 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } m_nodePool->clear(); m_openList->clear(); @@ -2901,8 +2888,18 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v dtAssert(m_nav); dtAssert(m_nodePool); dtAssert(m_openList); - + + if (!resultCount) + return DT_FAILURE | DT_INVALID_PARAM; + *resultCount = 0; + + if (!m_nav->isValidPolyRef(startRef) || + !verts || nverts < 3 || + !filter || maxResult < 0) + { + return DT_FAILURE | DT_INVALID_PARAM; + } // Validate input if (!startRef || !m_nav->isValidPolyRef(startRef)) @@ -3088,13 +3085,20 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* { dtAssert(m_nav); dtAssert(m_tinyNodePool); - + + if (!resultCount) + return DT_FAILURE | DT_INVALID_PARAM; + *resultCount = 0; - // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + radius < 0 || !dtMathIsfinite(radius) || + !filter || maxResult < 0) + { return DT_FAILURE | DT_INVALID_PARAM; - + } + static const int MAX_STACK = 48; dtNode* stack[MAX_STACK]; int nstack = 0; @@ -3301,13 +3305,19 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* const int maxSegments) const { dtAssert(m_nav); + + if (!segmentCount) + return DT_FAILURE | DT_INVALID_PARAM; *segmentCount = 0; - + const dtMeshTile* tile = 0; const dtPoly* poly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) return DT_FAILURE | DT_INVALID_PARAM; + + if (!filter || !segmentVerts || maxSegments < 0) + return DT_FAILURE | DT_INVALID_PARAM; int n = 0; static const int MAX_INTERVAL = 16; @@ -3455,8 +3465,13 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen dtAssert(m_openList); // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) + if (!m_nav->isValidPolyRef(startRef) || + !centerPos || !dtVisfinite(centerPos) || + maxRadius < 0 || !dtMathIsfinite(maxRadius) || + !filter || !hitDist || !hitPos || !hitNormal) + { return DT_FAILURE | DT_INVALID_PARAM; + } m_nodePool->clear(); m_openList->clear(); diff --git a/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.h b/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.h index d9f940e2d..43c7268f2 100644 --- a/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.h +++ b/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.h @@ -120,8 +120,6 @@ public: }; - - /// Provides information about raycast hit /// filled by dtNavMeshQuery::raycast /// @ingroup detour @@ -316,15 +314,31 @@ public: ///@{ /// Finds the polygon nearest to the specified center point. + /// [opt] means the specified parameter can be a null pointer, in that case the output parameter will not be set. + /// /// @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] 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)] + /// @param[out] nearestRef The reference id of the nearest polygon. Will be set to 0 if no polygon is found. + /// @param[out] nearestPt The nearest point on the polygon. Unchanged if no polygon is found. [opt] [(x, y, z)] /// @returns The status flags for the query. dtStatus findNearestPoly(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* nearestRef, float* nearestPt) const; + + /// Finds the polygon nearest to the specified center point. + /// [opt] means the specified parameter can be a null pointer, in that case the output parameter will not be set. + /// + /// @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[out] nearestRef The reference id of the nearest polygon. Will be set to 0 if no polygon is found. + /// @param[out] nearestPt The nearest point on the polygon. Unchanged if no polygon is found. [opt] [(x, y, z)] + /// @param[out] isOverPoly Set to true if the point's X/Z coordinate lies inside the polygon, false otherwise. Unchanged if no polygon is found. [opt] + /// @returns The status flags for the query. + dtStatus findNearestPoly(const float* center, const float* halfExtents, + const dtQueryFilter* filter, + dtPolyRef* nearestRef, float* nearestPt, bool* isOverPoly) const; /// Finds polygons that overlap the search box. /// @param[in] center The center of the search box. [(x, y, z)] diff --git a/Source/ThirdParty/recastnavigation/RecastAlloc.h b/Source/ThirdParty/recastnavigation/RecastAlloc.h index d9c22a03b..071278d65 100644 --- a/Source/ThirdParty/recastnavigation/RecastAlloc.h +++ b/Source/ThirdParty/recastnavigation/RecastAlloc.h @@ -106,6 +106,8 @@ class rcVectorBase { // Creates an array of the given size, copies all of this vector's data into it, and returns it. T* allocate_and_copy(rcSizeType size); void resize_impl(rcSizeType size, const T* value); + // Requires: min_capacity > m_cap. + rcSizeType get_new_capacity(rcSizeType min_capacity); public: typedef rcSizeType size_type; typedef T value_type; @@ -196,8 +198,7 @@ void rcVectorBase::push_back(const T& value) { return; } - rcAssert(RC_SIZE_MAX / 2 >= m_size); - rcSizeType new_cap = m_size ? 2*m_size : 1; + const rcSizeType new_cap = get_new_capacity(m_cap + 1); T* data = allocate_and_copy(new_cap); // construct between allocate and destroy+free in case value is // in this vector. @@ -208,25 +209,44 @@ void rcVectorBase::push_back(const T& value) { rcFree(m_data); m_data = data; } + +template +rcSizeType rcVectorBase::get_new_capacity(rcSizeType min_capacity) { + rcAssert(min_capacity <= RC_SIZE_MAX); + if (rcUnlikely(m_cap >= RC_SIZE_MAX / 2)) + return RC_SIZE_MAX; + return 2 * m_cap > min_capacity ? 2 * m_cap : min_capacity; +} + template void rcVectorBase::resize_impl(rcSizeType size, const T* value) { if (size < m_size) { destroy_range(size, m_size); m_size = size; } else if (size > m_size) { - T* new_data = allocate_and_copy(size); - // We defer deconstructing/freeing old data until after constructing - // new elements in case "value" is there. - if (value) { - construct_range(new_data + m_size, new_data + size, *value); + if (size <= m_cap) { + if (value) { + construct_range(m_data + m_size, m_data + size, *value); + } else { + construct_range(m_data + m_size, m_data + size); + } + m_size = size; } else { - construct_range(new_data + m_size, new_data + size); + const rcSizeType new_cap = get_new_capacity(size); + T* new_data = allocate_and_copy(new_cap); + // We defer deconstructing/freeing old data until after constructing + // new elements in case "value" is there. + if (value) { + construct_range(new_data + m_size, new_data + size, *value); + } else { + construct_range(new_data + m_size, new_data + size); + } + destroy_range(0, m_size); + rcFree(m_data); + m_data = new_data; + m_cap = new_cap; + m_size = size; } - destroy_range(0, m_size); - rcFree(m_data); - m_data = new_data; - m_cap = size; - m_size = size; } } template @@ -303,6 +323,7 @@ public: rcIntArray(int n) : m_impl(n, 0) {} void push(int item) { m_impl.push_back(item); } void resize(int size) { m_impl.resize(size); } + void clear() { m_impl.clear(); } int pop() { int v = m_impl.back(); diff --git a/Source/ThirdParty/recastnavigation/RecastContour.cpp b/Source/ThirdParty/recastnavigation/RecastContour.cpp index 277ab0150..1293d4fbd 100644 --- a/Source/ThirdParty/recastnavigation/RecastContour.cpp +++ b/Source/ThirdParty/recastnavigation/RecastContour.cpp @@ -921,8 +921,8 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, continue; const unsigned char area = chf.areas[i]; - verts.resize(0); - simplified.resize(0); + verts.clear(); + simplified.clear(); ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE); walkContour(x, y, i, chf, flags, verts); @@ -1009,7 +1009,7 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, if (cset.nconts > 0) { // Calculate winding of all polygons. - rcScopedDelete winding((char*)rcAlloc(sizeof(char)*cset.nconts, RC_ALLOC_TEMP)); + rcScopedDelete winding((signed char*)rcAlloc(sizeof(signed char)*cset.nconts, RC_ALLOC_TEMP)); if (!winding) { ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'hole' (%d).", cset.nconts); diff --git a/Source/ThirdParty/recastnavigation/RecastMeshDetail.cpp b/Source/ThirdParty/recastnavigation/RecastMeshDetail.cpp index 68ab726aa..1999200c1 100644 --- a/Source/ThirdParty/recastnavigation/RecastMeshDetail.cpp +++ b/Source/ThirdParty/recastnavigation/RecastMeshDetail.cpp @@ -653,8 +653,8 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, for (int i = 0; i < nin; ++i) rcVcopy(&verts[i*3], &in[i*3]); - edges.resize(0); - tris.resize(0); + edges.clear(); + tris.clear(); const float cs = chf.cs; const float ics = 1.0f/cs; @@ -803,7 +803,7 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, int x1 = (int)ceilf(bmax[0]/sampleDist); int z0 = (int)floorf(bmin[2]/sampleDist); int z1 = (int)ceilf(bmax[2]/sampleDist); - samples.resize(0); + samples.clear(); for (int z = z0; z < z1; ++z) { for (int x = x0; x < x1; ++x) @@ -864,8 +864,8 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, // Create new triangulation. // TODO: Incremental add instead of full rebuild. - edges.resize(0); - tris.resize(0); + edges.clear(); + tris.clear(); delaunayHull(ctx, nverts, verts, nhull, hull, tris, edges); } } @@ -935,7 +935,7 @@ static void seedArrayWithPolyCenter(rcContext* ctx, const rcCompactHeightfield& pcy /= npoly; // Use seeds array as a stack for DFS - array.resize(0); + array.clear(); array.push(startCellX); array.push(startCellY); array.push(startSpanIndex); @@ -1001,7 +1001,7 @@ static void seedArrayWithPolyCenter(rcContext* ctx, const rcCompactHeightfield& rcSwap(dirs[directDir], dirs[3]); } - array.resize(0); + array.clear(); // getHeightData seeds are given in coordinates with borders array.push(cx+bs); array.push(cy+bs); @@ -1030,7 +1030,7 @@ static void getHeightData(rcContext* ctx, const rcCompactHeightfield& chf, // Note: Reads to the compact heightfield are offset by border size (bs) // since border size offset is already removed from the polymesh vertices. - queue.resize(0); + queue.clear(); // Set all heights to RC_UNSET_HEIGHT. memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); @@ -1141,7 +1141,8 @@ static void getHeightData(rcContext* ctx, const rcCompactHeightfield& chf, static unsigned char getEdgeFlags(const float* va, const float* vb, const float* vpoly, const int npoly) { - // Return true if edge (va,vb) is part of the polygon. + // The flag returned by this function matches dtDetailTriEdgeFlags in Detour. + // Figure out if edge (va,vb) is part of the polygon boundary. static const float thrSqr = rcSqr(0.001f); for (int i = 0, j = npoly-1; i < npoly; j=i++) { diff --git a/Source/ThirdParty/recastnavigation/RecastRegion.cpp b/Source/ThirdParty/recastnavigation/RecastRegion.cpp index e1fc0ee78..48318688b 100644 --- a/Source/ThirdParty/recastnavigation/RecastRegion.cpp +++ b/Source/ThirdParty/recastnavigation/RecastRegion.cpp @@ -650,7 +650,7 @@ static bool mergeRegions(rcRegion& rega, rcRegion& regb) return false; // Merge neighbours. - rega.connections.resize(0); + rega.connections.clear(); for (int i = 0, ni = acon.size(); i < ni-1; ++i) rega.connections.push(acon[(insa+1+i) % ni]); @@ -876,8 +876,8 @@ static bool mergeAndFilterRegions(rcContext* ctx, int minRegionArea, int mergeRe // Also keep track of the regions connects to a tile border. bool connectsToBorder = false; int spanCount = 0; - stack.resize(0); - trace.resize(0); + stack.clear(); + trace.clear(); reg.visited = true; stack.push(i); @@ -1068,7 +1068,7 @@ static bool mergeAndFilterLayerRegions(rcContext* ctx, int minRegionArea, { const rcCompactCell& c = chf.cells[x+y*w]; - lregs.resize(0); + lregs.clear(); for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) { @@ -1139,7 +1139,7 @@ static bool mergeAndFilterLayerRegions(rcContext* ctx, int minRegionArea, // Start search. root.id = layerId; - stack.resize(0); + stack.clear(); stack.push(i); while (stack.size() > 0)