server_common/recastnavigation_v1.6.0
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add API to query polygons in batches

Browse Source 
This adds a version of queryPolygon that takes an interface which is
passed batches of polygons in the search area. This new API is then used
from the old queryPolygons to collect polygons, and from findNearestRef
to find the nearest poly in an arbitrarily sized area. Previously
findNearestPoly had an arbitrary poly limit of 128 which could cause it
to return potentially wrong results.

Fix #107
This commit is contained in:
Jakob Botsch Nielsen 2016-01-25 23:17:46 +01:00
parent 6ee79b25a8
commit fc5df2c6fd
2 changed files with 181 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
Detour/Include/DetourNavMeshQuery.h
View File

@ -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,

233
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -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;
dtFindNearestPolyQuery query(this, center);
*nearestRef = 0;
dtStatus status = queryPolygons(center, extents, filter, &query);
if (dtStatusFailed(status))
return status;
if (polyCount == 0)
return DT_SUCCESS;
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestPoint[3];
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)
{
@ -807,7 +815,6 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
// 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);
@ -818,8 +825,18 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
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++;
}
}
}
@ -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;
}