TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-14 08:04:40 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update recast

Browse source
This commit is contained in:
Johxz 2018-02-28 22:15:31 -06:00
parent e079536122
commit 8aa48ff21b
34 changed files with 1138 additions and 407 deletions
Download patch file Download diff file Expand all files Collapse all files

35
Engine/lib/recast/Detour/Source/DetourAssert.cpp Normal file
View file

@ -0,0 +1,35 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include "DetourAssert.h"
#ifndef NDEBUG
static dtAssertFailFunc* sAssertFailFunc = 0;
void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc)
{
sAssertFailFunc = assertFailFunc;
}
dtAssertFailFunc* dtAssertFailGetCustom()
{
return sAssertFailFunc;
}
#endif

4
Engine/lib/recast/Detour/Source/DetourCommon.cpp
View file

@ -342,8 +342,8 @@ void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas,
// Find sub triangle weighted by area.
const float thr = s*areasum;
float acc = 0.0f;
float u = 0.0f;
int tri = 0;
float u = 1.0f;
int tri = npts - 1;
for (int i = 2; i < npts; i++) {
const float dacc = areas[i];
if (thr >= acc && thr < (acc+dacc))

61
Engine/lib/recast/Detour/Source/DetourNavMesh.cpp
View file

@ -304,7 +304,7 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
if (!tile) return 0;
float amin[2], amax[2];
calcSlabEndPoints(va,vb, amin,amax, side);
calcSlabEndPoints(va, vb, amin, amax, side);
const float apos = getSlabCoord(va, side);
// Remove links pointing to 'side' and compact the links array.
@ -470,12 +470,12 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int
if (targetPoly->firstLink == DT_NULL_LINK)
continue;
const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad };
const float halfExtents[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad };
// Find polygon to connect to.
const float* p = &targetCon->pos[3];
float nearestPt[3];
dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt);
dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt);
if (!ref)
continue;
// findNearestPoly may return too optimistic results, further check to make sure.
@ -570,12 +570,12 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
dtOffMeshConnection* con = &tile->offMeshCons[i];
dtPoly* poly = &tile->polys[con->poly];
const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad };
const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad };
// Find polygon to connect to.
const float* p = &con->pos[0]; // First vertex
float nearestPt[3];
dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt);
dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt);
if (!ref) continue;
// findNearestPoly may return too optimistic results, further check to make sure.
if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad))
@ -651,9 +651,9 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close
if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = FLT_MAX;
int imin = -1;
for (int i = 0; i < nv; ++i)
float dmin = edged[0];
int imin = 0;
for (int i = 1; i < nv; ++i)
{
if (edged[i] < dmin)
{
@ -687,7 +687,7 @@ 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(pos, v[0], v[1], v[2], h))
if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h))
{
closest[1] = h;
break;
@ -696,12 +696,12 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close
}
dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
const float* center, const float* extents,
const float* center, const float* halfExtents,
float* nearestPt) const
{
float bmin[3], bmax[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
dtVsub(bmin, center, halfExtents);
dtVadd(bmax, center, halfExtents);
// Get nearby polygons from proximity grid.
dtPolyRef polys[128];
@ -917,14 +917,14 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount);
unsigned char* d = data + headerSize;
tile->verts = (float*)d; d += vertsSize;
tile->polys = (dtPoly*)d; d += polysSize;
tile->links = (dtLink*)d; d += linksSize;
tile->detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
tile->detailVerts = (float*)d; d += detailVertsSize;
tile->detailTris = (unsigned char*)d; d += detailTrisSize;
tile->bvTree = (dtBVNode*)d; d += bvtreeSize;
tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize;
tile->verts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
tile->polys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
tile->links = dtGetThenAdvanceBufferPointer<dtLink>(d, linksSize);
tile->detailMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
tile->detailVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
tile->detailTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
tile->bvTree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvtreeSize);
tile->offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshLinksSize);
// If there are no items in the bvtree, reset the tree pointer.
if (!bvtreeSize)
@ -943,7 +943,10 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
tile->flags = flags;
connectIntLinks(tile);
// Base off-mesh connections to their starting polygons and connect connections inside the tile.
baseOffMeshLinks(tile);
connectExtOffMeshLinks(tile, tile, -1);
// Create connections with neighbour tiles.
static const int MAX_NEIS = 32;
@ -954,11 +957,11 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
if (neis[j] != tile)
{
connectExtLinks(tile, neis[j], -1);
connectExtLinks(neis[j], tile, -1);
}
if (neis[j] == tile)
continue;
connectExtLinks(tile, neis[j], -1);
connectExtLinks(neis[j], tile, -1);
connectExtOffMeshLinks(tile, neis[j], -1);
connectExtOffMeshLinks(neis[j], tile, -1);
}
@ -1322,8 +1325,8 @@ dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data,
if (maxDataSize < sizeReq)
return DT_FAILURE | DT_BUFFER_TOO_SMALL;
dtTileState* tileState = (dtTileState*)data; data += dtAlign4(sizeof(dtTileState));
dtPolyState* polyStates = (dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount);
dtTileState* tileState = dtGetThenAdvanceBufferPointer<dtTileState>(data, dtAlign4(sizeof(dtTileState)));
dtPolyState* polyStates = dtGetThenAdvanceBufferPointer<dtPolyState>(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount));
// Store tile state.
tileState->magic = DT_NAVMESH_STATE_MAGIC;
@ -1354,8 +1357,8 @@ dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data
if (maxDataSize < sizeReq)
return DT_FAILURE | DT_INVALID_PARAM;
const dtTileState* tileState = (const dtTileState*)data; data += dtAlign4(sizeof(dtTileState));
const dtPolyState* polyStates = (const dtPolyState*)data; data += dtAlign4(sizeof(dtPolyState) * tile->header->polyCount);
const dtTileState* tileState = dtGetThenAdvanceBufferPointer<const dtTileState>(data, dtAlign4(sizeof(dtTileState)));
const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer<const dtPolyState>(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount));
// Check that the restore is possible.
if (tileState->magic != DT_NAVMESH_STATE_MAGIC)

127
Engine/lib/recast/Detour/Source/DetourNavMeshBuilder.cpp
View file

@ -106,7 +106,6 @@ inline int longestAxis(unsigned short x, unsigned short y, unsigned short z)
if (z > maxVal)
{
axis = 2;
maxVal = z;
}
return axis;
}
@ -169,45 +168,72 @@ static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNod
}
}
static int createBVTree(const unsigned short* verts, const int /*nverts*/,
const unsigned short* polys, const int npolys, const int nvp,
const float cs, const float ch,
const int /*nnodes*/, dtBVNode* nodes)
static int createBVTree(dtNavMeshCreateParams* params, dtBVNode* nodes, int /*nnodes*/)
{
// Build tree
BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*npolys, DT_ALLOC_TEMP);
for (int i = 0; i < npolys; i++)
float quantFactor = 1 / params->cs;
BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*params->polyCount, DT_ALLOC_TEMP);
for (int i = 0; i < params->polyCount; i++)
{
BVItem& it = items[i];
it.i = i;
// Calc polygon bounds.
const unsigned short* p = &polys[i*nvp*2];
it.bmin[0] = it.bmax[0] = verts[p[0]*3+0];
it.bmin[1] = it.bmax[1] = verts[p[0]*3+1];
it.bmin[2] = it.bmax[2] = verts[p[0]*3+2];
for (int j = 1; j < nvp; ++j)
// Calc polygon bounds. Use detail meshes if available.
if (params->detailMeshes)
{
if (p[j] == MESH_NULL_IDX) break;
unsigned short x = verts[p[j]*3+0];
unsigned short y = verts[p[j]*3+1];
unsigned short z = verts[p[j]*3+2];
if (x < it.bmin[0]) it.bmin[0] = x;
if (y < it.bmin[1]) it.bmin[1] = y;
if (z < it.bmin[2]) it.bmin[2] = z;
if (x > it.bmax[0]) it.bmax[0] = x;
if (y > it.bmax[1]) it.bmax[1] = y;
if (z > it.bmax[2]) it.bmax[2] = z;
int vb = (int)params->detailMeshes[i*4+0];
int ndv = (int)params->detailMeshes[i*4+1];
float bmin[3];
float bmax[3];
const float* dv = &params->detailVerts[vb*3];
dtVcopy(bmin, dv);
dtVcopy(bmax, dv);
for (int j = 1; j < ndv; j++)
{
dtVmin(bmin, &dv[j * 3]);
dtVmax(bmax, &dv[j * 3]);
}
// BV-tree uses cs for all dimensions
it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0])*quantFactor), 0, 0xffff);
it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1])*quantFactor), 0, 0xffff);
it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2])*quantFactor), 0, 0xffff);
it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0])*quantFactor), 0, 0xffff);
it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1])*quantFactor), 0, 0xffff);
it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2])*quantFactor), 0, 0xffff);
}
else
{
const unsigned short* p = &params->polys[i*params->nvp * 2];
it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0];
it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1];
it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2];
for (int j = 1; j < params->nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
unsigned short x = params->verts[p[j] * 3 + 0];
unsigned short y = params->verts[p[j] * 3 + 1];
unsigned short z = params->verts[p[j] * 3 + 2];
if (x < it.bmin[0]) it.bmin[0] = x;
if (y < it.bmin[1]) it.bmin[1] = y;
if (z < it.bmin[2]) it.bmin[2] = z;
if (x > it.bmax[0]) it.bmax[0] = x;
if (y > it.bmax[1]) it.bmax[1] = y;
if (z > it.bmax[2]) it.bmax[2] = z;
}
// Remap y
it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs);
it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs);
}
// Remap y
it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1]*ch/cs);
it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1]*ch/cs);
}
int curNode = 0;
subdivide(items, npolys, 0, npolys, curNode, nodes);
subdivide(items, params->polyCount, 0, params->polyCount, curNode, nodes);
dtFree(items);
@ -421,15 +447,16 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
memset(data, 0, dataSize);
unsigned char* d = data;
dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize;
float* navVerts = (float*)d; d += vertsSize;
dtPoly* navPolys = (dtPoly*)d; d += polysSize;
d += linksSize;
dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
float* navDVerts = (float*)d; d += detailVertsSize;
unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize;
dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize;
dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize;
dtMeshHeader* header = dtGetThenAdvanceBufferPointer<dtMeshHeader>(d, headerSize);
float* navVerts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
dtPoly* navPolys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
d += linksSize; // Ignore links; just leave enough space for them. They'll be created on load.
dtPolyDetail* navDMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
float* navDVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
unsigned char* navDTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
dtBVNode* navBvtree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvTreeSize);
dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshConsSize);
// Store header
@ -595,11 +622,9 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
}
// Store and create BVtree.
// TODO: take detail mesh into account! use byte per bbox extent?
if (params->buildBvTree)
{
createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
createBVTree(params, navBvtree, 2*params->polyCount);
}
// Store Off-Mesh connections.
@ -705,14 +730,16 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount);
unsigned char* d = data + headerSize;
float* verts = (float*)d; d += vertsSize;
dtPoly* polys = (dtPoly*)d; d += polysSize;
/*dtLink* links = (dtLink*)d;*/ d += linksSize;
dtPolyDetail* detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
float* detailVerts = (float*)d; d += detailVertsSize;
/*unsigned char* detailTris = (unsigned char*)d;*/ d += detailTrisSize;
dtBVNode* bvTree = (dtBVNode*)d; d += bvtreeSize;
dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize;
float* verts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
dtPoly* polys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
d += linksSize; // Ignore links; they technically should be endian-swapped but all their data is overwritten on load anyway.
//dtLink* links = dtGetThenAdvanceBufferPointer<dtLink>(d, linksSize);
dtPolyDetail* detailMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
float* detailVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
d += detailTrisSize; // Ignore detail tris; single bytes can't be endian-swapped.
//unsigned char* detailTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
dtBVNode* bvTree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvtreeSize);
dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshLinksSize);
// Vertices
for (int i = 0; i < header->vertCount*3; ++i)

519
Engine/lib/recast/Detour/Source/DetourNavMeshQuery.cpp
View file

@ -542,9 +542,9 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = FLT_MAX;
int imin = -1;
for (int i = 0; i < nv; ++i)
float dmin = edged[0];
int imin = 0;
for (int i = 1; i < nv; ++i)
{
if (edged[i] < dmin)
{
@ -578,7 +578,7 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
}
float h;
if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h))
{
closest[1] = h;
break;
@ -628,9 +628,9 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float*
else
{
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = FLT_MAX;
int imin = -1;
for (int i = 0; i < nv; ++i)
float dmin = edged[0];
int imin = 0;
for (int i = 1; i < nv; ++i)
{
if (edged[i] < dmin)
{
@ -699,16 +699,67 @@ 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)
{
dtIgnoreUnused(polys);
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,
dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents,
const dtQueryFilter* filter,
dtPolyRef* nearestRef, float* nearestPt) const
{
@ -717,70 +768,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, halfExtents, 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)
{
@ -789,7 +799,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.
@ -806,25 +816,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
@ -833,17 +852,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;
@ -861,16 +877,63 @@ 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)
{
dtIgnoreUnused(tile);
dtIgnoreUnused(polys);
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
@ -880,15 +943,41 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
/// be filled to capacity. The method of choosing which polygons from the
/// full set are included in the partial result set is undefined.
///
dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents,
dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
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, halfExtents, 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 halfExtents
/// 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* halfExtents,
const dtQueryFilter* filter, dtPolyQuery* query) const
{
dtAssert(m_nav);
if (!center || !halfExtents || !filter || !query)
return DT_FAILURE | DT_INVALID_PARAM;
float bmin[3], bmax[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
dtVsub(bmin, center, halfExtents);
dtVadd(bmax, center, halfExtents);
// Find tiles the query touches.
int minx, miny, maxx, maxy;
@ -898,7 +987,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)
@ -906,16 +994,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;
}
@ -940,18 +1022,14 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtAssert(m_nodePool);
dtAssert(m_openList);
*pathCount = 0;
if (!startRef || !endRef)
return DT_FAILURE | DT_INVALID_PARAM;
if (!maxPath)
return DT_FAILURE | DT_INVALID_PARAM;
if (pathCount)
*pathCount = 0;
// Validate input
if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
!startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount)
return DT_FAILURE | DT_INVALID_PARAM;
if (startRef == endRef)
{
path[0] = startRef;
@ -974,7 +1052,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode* lastBestNode = startNode;
float lastBestNodeCost = startNode->total;
dtStatus status = DT_SUCCESS;
bool outOfNodes = false;
while (!m_openList->empty())
{
@ -1032,7 +1110,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
if (!neighbourNode)
{
status |= DT_OUT_OF_NODES;
outOfNodes = true;
continue;
}
@ -1111,42 +1189,59 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
}
}
}
dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath);
if (lastBestNode->id != endRef)
status |= DT_PARTIAL_RESULT;
// Reverse the path.
dtNode* prev = 0;
dtNode* node = lastBestNode;
do
{
dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
node->pidx = m_nodePool->getNodeIdx(prev);
prev = node;
node = next;
}
while (node);
// Store path
node = prev;
int n = 0;
do
{
path[n++] = node->id;
if (n >= maxPath)
{
status |= DT_BUFFER_TOO_SMALL;
break;
}
node = m_nodePool->getNodeAtIdx(node->pidx);
}
while (node);
*pathCount = n;
if (outOfNodes)
status |= DT_OUT_OF_NODES;
return status;
}
dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const
{
// Find the length of the entire path.
dtNode* curNode = endNode;
int length = 0;
do
{
length++;
curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
} while (curNode);
// If the path cannot be fully stored then advance to the last node we will be able to store.
curNode = endNode;
int writeCount;
for (writeCount = length; writeCount > maxPath; writeCount--)
{
dtAssert(curNode);
curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
}
// Write path
for (int i = writeCount - 1; i >= 0; i--)
{
dtAssert(curNode);
path[i] = curNode->id;
curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
}
dtAssert(!curNode);
*pathCount = dtMin(length, maxPath);
if (length > maxPath)
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
return DT_SUCCESS;
}
/// @par
///
/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath()
@ -1639,10 +1734,17 @@ dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flag
if (straightPathRefs)
straightPathRefs[(*straightPathCount)] = ref;
(*straightPathCount)++;
// If reached end of path or there is no space to append more vertices, return.
if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath)
// If there is no space to append more vertices, return.
if ((*straightPathCount) >= maxStraightPath)
{
return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
}
// If reached end of path, return.
if (flags == DT_STRAIGHTPATH_END)
{
return DT_SUCCESS;
}
}
return DT_IN_PROGRESS;
@ -1767,10 +1869,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
for (int i = 0; i < pathSize; ++i)
{
float left[3], right[3];
unsigned char fromType, toType;
unsigned char toType;
if (i+1 < pathSize)
{
unsigned char fromType; // fromType is ignored.
// Next portal.
if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
{
@ -1786,12 +1890,14 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
// Apeend portals along the current straight path segment.
if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
{
stat = appendPortals(apexIndex, i, closestEndPos, path,
// Ignore status return value as we're just about to return anyway.
appendPortals(apexIndex, i, closestEndPos, path,
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath, options);
}
stat = appendVertex(closestEndPos, 0, path[i],
// Ignore status return value as we're just about to return anyway.
appendVertex(closestEndPos, 0, path[i],
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath);
@ -1812,7 +1918,7 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
dtVcopy(left, closestEndPos);
dtVcopy(right, closestEndPos);
fromType = toType = DT_POLYTYPE_GROUND;
toType = DT_POLYTYPE_GROUND;
}
// Right vertex.
@ -1929,7 +2035,8 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
}
}
stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
// Ignore status return value as we're just about to return anyway.
appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath);
@ -2400,10 +2507,10 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
const dtMeshTile* prevTile, *tile, *nextTile;
const dtPoly* prevPoly, *poly, *nextPoly;
dtPolyRef curRef, nextRef;
dtPolyRef curRef;
// The API input has been checked already, skip checking internal data.
nextRef = curRef = startRef;
curRef = startRef;
tile = 0;
poly = 0;
m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
@ -2458,7 +2565,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
}
// Follow neighbours.
nextRef = 0;
dtPolyRef nextRef = 0;
for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
{
@ -2649,20 +2756,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
dtStatus status = DT_SUCCESS;
int n = 0;
if (n < maxResult)
{
if (resultRef)
resultRef[n] = startNode->id;
if (resultParent)
resultParent[n] = 0;
if (resultCost)
resultCost[n] = 0;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
const float radiusSqr = dtSqr(radius);
@ -2687,6 +2780,21 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
if (parentRef)
m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
if (n < maxResult)
{
if (resultRef)
resultRef[n] = bestRef;
if (resultParent)
resultParent[n] = parentRef;
if (resultCost)
resultCost[n] = bestNode->total;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
@ -2730,14 +2838,19 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
if (neighbourNode->flags == 0)
dtVlerp(neighbourNode->pos, va, vb, 0.5f);
const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
float cost = filter->getCost(
bestNode->pos, neighbourNode->pos,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
const float total = bestNode->total + cost;
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
neighbourNode->id = neighbourRef;
neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
@ -2747,20 +2860,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
}
else
{
if (n < maxResult)
{
if (resultRef)
resultRef[n] = neighbourNode->id;
if (resultParent)
resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id;
if (resultCost)
resultCost[n] = neighbourNode->total;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
neighbourNode->flags = DT_NODE_OPEN;
m_openList->push(neighbourNode);
}
@ -2829,20 +2928,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
dtStatus status = DT_SUCCESS;
int n = 0;
if (n < maxResult)
{
if (resultRef)
resultRef[n] = startNode->id;
if (resultParent)
resultParent[n] = 0;
if (resultCost)
resultCost[n] = 0;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
while (!m_openList->empty())
{
@ -2865,6 +2950,22 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
if (parentRef)
m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
if (n < maxResult)
{
if (resultRef)
resultRef[n] = bestRef;
if (resultParent)
resultParent[n] = parentRef;
if (resultCost)
resultCost[n] = bestNode->total;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
@ -2910,14 +3011,19 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
if (neighbourNode->flags == 0)
dtVlerp(neighbourNode->pos, va, vb, 0.5f);
const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
float cost = filter->getCost(
bestNode->pos, neighbourNode->pos,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
const float total = bestNode->total + cost;
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
neighbourNode->id = neighbourRef;
neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
@ -2927,20 +3033,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
}
else
{
if (n < maxResult)
{
if (resultRef)
resultRef[n] = neighbourNode->id;
if (resultParent)
resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id;
if (resultCost)
resultCost[n] = neighbourNode->total;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
neighbourNode->flags = DT_NODE_OPEN;
m_openList->push(neighbourNode);
}
@ -2952,6 +3044,21 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
return status;
}
dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const
{
if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0)
return DT_FAILURE | DT_INVALID_PARAM;
*pathCount = 0;
dtNode* endNode;
if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 ||
(endNode->flags & DT_NODE_CLOSED) == 0)
return DT_FAILURE | DT_INVALID_PARAM;
return getPathToNode(endNode, path, pathCount, maxPath);
}
/// @par
///
/// This method is optimized for a small search radius and small number of result