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

Merge pull request #175 from Janiels/fix-107

Browse Source 
Add API to query polygons in batches
This commit is contained in:
Ben Hymers 2016-01-27 15:18:04 +00:00
commit 0a9e36d91b
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,

243
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;
*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;
}