Merge pull request #175 from Janiels/fix-107
Add API to query polygons in batches
This commit is contained in:
commit
0a9e36d91b
@ -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,
|
||||
|
@ -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;
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user