diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index d3737a4..6af61bf 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/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. @@ -312,6 +323,14 @@ public: 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] extents 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* extents, + 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 +498,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, diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 48f9c36..c1f57df 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -697,15 +697,64 @@ 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) + { + 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, const dtQueryFilter* filter, dtPolyRef* nearestRef, float* nearestPt) const @@ -715,70 +764,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, extents, 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) { @@ -787,7 +795,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. @@ -804,25 +812,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 @@ -831,17 +848,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; @@ -859,16 +873,60 @@ 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) + { + 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 @@ -881,9 +939,35 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents, 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, extents, 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 extents +/// 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* extents, + const dtQueryFilter* filter, dtPolyQuery* query) const { dtAssert(m_nav); - + + if (!center || !extents || !filter || !query) + return DT_FAILURE | DT_INVALID_PARAM; + float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); @@ -896,7 +980,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) @@ -904,16 +987,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; }