dtNavMeshQuery Class Reference

#include <DetourNavMeshQuery.h>

Classes
struct	dtQueryData

Public Member Functions
	dtNavMeshQuery ()
	~dtNavMeshQuery ()
dtStatus	init (const dtNavMesh *nav, const int maxNodes)
Standard Pathfinding Functions
dtStatus	findPath (dtPolyRef startRef, dtPolyRef endRef, const float *startPos, const float *endPos, const dtQueryFilter *filter, dtPolyRef *path, int *pathCount, const int maxPath) const
dtStatus	findStraightPath (const float *startPos, const float *endPos, const dtPolyRef *path, const int pathSize, float *straightPath, unsigned char *straightPathFlags, dtPolyRef *straightPathRefs, int *straightPathCount, const int maxStraightPath, const int options=0) const
Sliced Pathfinding Functions
Common use case: Call initSlicedFindPath() to initialize the sliced path query. Call updateSlicedFindPath() until it returns complete. Call finalizeSlicedFindPath() to get the path.
dtStatus	initSlicedFindPath (dtPolyRef startRef, dtPolyRef endRef, const float *startPos, const float *endPos, const dtQueryFilter *filter, const unsigned int options=0)
dtStatus	updateSlicedFindPath (const int maxIter, int *doneIters)
dtStatus	finalizeSlicedFindPath (dtPolyRef *path, int *pathCount, const int maxPath)
dtStatus	finalizeSlicedFindPathPartial (const dtPolyRef *existing, const int existingSize, dtPolyRef *path, int *pathCount, const int maxPath)
Dijkstra Search Functions
dtStatus	findPolysAroundCircle (dtPolyRef startRef, const float *centerPos, const float radius, const dtQueryFilter *filter, dtPolyRef *resultRef, dtPolyRef *resultParent, float *resultCost, int *resultCount, const int maxResult) const
dtStatus	findPolysAroundShape (dtPolyRef startRef, const float *verts, const int nverts, const dtQueryFilter *filter, dtPolyRef *resultRef, dtPolyRef *resultParent, float *resultCost, int *resultCount, const int maxResult) const
Local Query Functions
dtStatus	findNearestPoly (const float *center, const float *extents, const dtQueryFilter *filter, dtPolyRef *nearestRef, float *nearestPt) const
dtStatus	queryPolygons (const float *center, const float *extents, const dtQueryFilter *filter, dtPolyRef *polys, int *polyCount, const int maxPolys) const
dtStatus	findLocalNeighbourhood (dtPolyRef startRef, const float *centerPos, const float radius, const dtQueryFilter *filter, dtPolyRef *resultRef, dtPolyRef *resultParent, int *resultCount, const int maxResult) const
dtStatus	moveAlongSurface (dtPolyRef startRef, const float *startPos, const float *endPos, const dtQueryFilter *filter, float *resultPos, dtPolyRef *visited, int *visitedCount, const int maxVisitedSize) const
dtStatus	raycast (dtPolyRef startRef, const float *startPos, const float *endPos, const dtQueryFilter *filter, float *t, float *hitNormal, dtPolyRef *path, int *pathCount, const int maxPath) const
dtStatus	raycast (dtPolyRef startRef, const float *startPos, const float *endPos, const dtQueryFilter *filter, const unsigned int options, dtRaycastHit *hit, dtPolyRef prevRef=0) const
dtStatus	findDistanceToWall (dtPolyRef startRef, const float *centerPos, const float maxRadius, const dtQueryFilter *filter, float *hitDist, float *hitPos, float *hitNormal) const
dtStatus	getPolyWallSegments (dtPolyRef ref, const dtQueryFilter *filter, float *segmentVerts, dtPolyRef *segmentRefs, int *segmentCount, const int maxSegments) const
dtStatus	findRandomPoint (const dtQueryFilter *filter, float(*frand)(), dtPolyRef *randomRef, float *randomPt) const
dtStatus	findRandomPointAroundCircle (dtPolyRef startRef, const float *centerPos, const float maxRadius, const dtQueryFilter *filter, float(*frand)(), dtPolyRef *randomRef, float *randomPt) const
dtStatus	closestPointOnPoly (dtPolyRef ref, const float *pos, float *closest, bool *posOverPoly) const
dtStatus	closestPointOnPolyBoundary (dtPolyRef ref, const float *pos, float *closest) const
dtStatus	getPolyHeight (dtPolyRef ref, const float *pos, float *height) const
Miscellaneous Functions
bool	isValidPolyRef (dtPolyRef ref, const dtQueryFilter *filter) const
bool	isInClosedList (dtPolyRef ref) const
class dtNodePool *	getNodePool () const
const dtNavMesh *	getAttachedNavMesh () const

Private Member Functions
dtMeshTile *	getNeighbourTileAt (int x, int y, int side) const
	Returns neighbour tile based on side. More...
int	queryPolygonsInTile (const dtMeshTile *tile, const float *qmin, const float *qmax, const dtQueryFilter *filter, dtPolyRef *polys, const int maxPolys) const
	Queries polygons within a tile. More...
dtStatus	getPortalPoints (dtPolyRef from, dtPolyRef to, float *left, float *right, unsigned char &fromType, unsigned char &toType) const
	Returns portal points between two polygons. More...
dtStatus	getPortalPoints (dtPolyRef from, const dtPoly *fromPoly, const dtMeshTile *fromTile, dtPolyRef to, const dtPoly *toPoly, const dtMeshTile *toTile, float *left, float *right) const
dtStatus	getEdgeMidPoint (dtPolyRef from, dtPolyRef to, float *mid) const
	Returns edge mid point between two polygons. More...
dtStatus	getEdgeMidPoint (dtPolyRef from, const dtPoly *fromPoly, const dtMeshTile *fromTile, dtPolyRef to, const dtPoly *toPoly, const dtMeshTile *toTile, float *mid) const
dtStatus	appendVertex (const float *pos, const unsigned char flags, const dtPolyRef ref, float *straightPath, unsigned char *straightPathFlags, dtPolyRef *straightPathRefs, int *straightPathCount, const int maxStraightPath) const
dtStatus	appendPortals (const int startIdx, const int endIdx, const float *endPos, const dtPolyRef *path, float *straightPath, unsigned char *straightPathFlags, dtPolyRef *straightPathRefs, int *straightPathCount, const int maxStraightPath, const int options) const

Private Attributes
const dtNavMesh *	m_nav
	Pointer to navmesh data. More...
dtQueryData	m_query
	Sliced query state. More...
class dtNodePool *	m_tinyNodePool
	Pointer to small node pool. More...
class dtNodePool *	m_nodePool
	Pointer to node pool. More...
class dtNodeQueue *	m_openList
	Pointer to open list queue. More...

Detailed Description

Provides the ability to perform pathfinding related queries against a navigation mesh.

For methods that support undersized buffers, if the buffer is too small to hold the entire result set the return status of the method will include the DT_BUFFER_TOO_SMALL flag.

Constant member functions can be used by multiple clients without side effects. (E.g. No change to the closed list. No impact on an in-progress sliced path query. Etc.)

Walls and portals: A wall is a polygon segment that is considered impassable. A portal is a passable segment between polygons. A portal may be treated as a wall based on the dtQueryFilter used for a query.

See also

dtNavMesh, dtQueryFilter, dtAllocNavMeshQuery(), dtAllocNavMeshQuery()

Constructor & Destructor Documentation

dtNavMeshQuery::dtNavMeshQuery

(

)

 :
 m_nav(0),
 m_tinyNodePool(0),
 m_nodePool(0),
 m_openList(0)
{
 memset(&m_query, 0, sizeof(dtQueryData));
}

dtNavMeshQuery::~dtNavMeshQuery

(

)

{
 if (m_tinyNodePool)
 m_tinyNodePool->~dtNodePool();
 if (m_nodePool)
 m_nodePool->~dtNodePool();
 if (m_openList)
 m_openList->~dtNodeQueue();
 dtFree(m_tinyNodePool);
 dtFree(m_nodePool);
 dtFree(m_openList);
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Function Documentation

dtStatus dtNavMeshQuery::appendPortals ( const int  startIdx, const int  endIdx, const float *  endPos, const dtPolyRef *  path, float *  straightPath, unsigned char *  straightPathFlags, dtPolyRef *  straightPathRefs, int *  straightPathCount, const int  maxStraightPath, const int  options  ) const

private

{
 const float* startPos = &straightPath[(*straightPathCount-1)*3];
 // Append or update last vertex
 dtStatus stat = 0;
 for (int i = startIdx; i < endIdx; i++)
 {
 // Calculate portal
 const dtPolyRef from = path[i];
 const dtMeshTile* fromTile = 0;
 const dtPoly* fromPoly = 0;
 if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
 return DT_FAILURE | DT_INVALID_PARAM;
 
 const dtPolyRef to = path[i+1];
 const dtMeshTile* toTile = 0;
 const dtPoly* toPoly = 0;
 if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
 return DT_FAILURE | DT_INVALID_PARAM;
 
 float left[3], right[3];
 if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
 break;
 
 if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
 {
 // Skip intersection if only area crossings are requested.
 if (fromPoly->getArea() == toPoly->getArea())
 continue;
 }
 
 // Append intersection
 float s,t;
 if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
 {
 float pt[3];
 dtVlerp(pt, left,right, t);

 stat = appendVertex(pt, 0, path[i+1],
 straightPath, straightPathFlags, straightPathRefs,
 straightPathCount, maxStraightPath);
 if (stat != DT_IN_PROGRESS)
 return stat;
 }
 }
 return DT_IN_PROGRESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::appendVertex ( const float *  pos, const unsigned char  flags, const dtPolyRef  ref, float *  straightPath, unsigned char *  straightPathFlags, dtPolyRef *  straightPathRefs, int *  straightPathCount, const int  maxStraightPath  ) const

private

{
 if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
 {
 // The vertices are equal, update flags and poly.
 if (straightPathFlags)
 straightPathFlags[(*straightPathCount)-1] = flags;
 if (straightPathRefs)
 straightPathRefs[(*straightPathCount)-1] = ref;
 }
 else
 {
 // Append new vertex.
 dtVcopy(&straightPath[(*straightPathCount)*3], pos);
 if (straightPathFlags)
 straightPathFlags[(*straightPathCount)] = flags;
 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)
 {
 return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
 }
 }
 return DT_IN_PROGRESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::closestPointOnPoly	(	dtPolyRef	ref,
		const float *	pos,
		float *	closest,
		bool *	posOverPoly
	)		const

Finds the closest point on the specified polygon.

Parameters

[in]	ref	The reference id of the polygon.
[in]	pos	The position to check. [(x, y, z)]
[out]	closest	The closest point on the polygon. [(x, y, z)]
[out]	posOverPoly	True of the position is over the polygon.

Returns

The status flags for the query.

Uses the detail polygons to find the surface height. (Most accurate.)

pos does not have to be within the bounds of the polygon or navigation mesh.

See closestPointOnPolyBoundary() for a limited but faster option.

{
 dtAssert(m_nav);
 const dtMeshTile* tile = 0;
 const dtPoly* poly = 0;
 if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
 return DT_FAILURE | DT_INVALID_PARAM;
 if (!tile)
 return DT_FAILURE | DT_INVALID_PARAM;
 
 // Off-mesh connections don't have detail polygons.
 if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 {
 const float* v0 = &tile->verts[poly->verts[0]*3];
 const float* v1 = &tile->verts[poly->verts[1]*3];
 const float d0 = dtVdist(pos, v0);
 const float d1 = dtVdist(pos, v1);
 const float u = d0 / (d0+d1);
 dtVlerp(closest, v0, v1, u);
 if (posOverPoly)
 *posOverPoly = false;
 return DT_SUCCESS;
 }

 const unsigned int ip = (unsigned int)(poly - tile->polys);
 const dtPolyDetail* pd = &tile->detailMeshes[ip];

 // Clamp point to be inside the polygon.
 float verts[DT_VERTS_PER_POLYGON*3]; 
 float edged[DT_VERTS_PER_POLYGON];
 float edget[DT_VERTS_PER_POLYGON];
 const int nv = poly->vertCount;
 for (int i = 0; i < nv; ++i)
 dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
 
 dtVcopy(closest, pos);
 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)
 {
 if (edged[i] < dmin)
 {
 dmin = edged[i];
 imin = i;
 }
 }
 const float* va = &verts[imin*3];
 const float* vb = &verts[((imin+1)%nv)*3];
 dtVlerp(closest, va, vb, edget[imin]);

 if (posOverPoly)
 *posOverPoly = false;
 }
 else
 {
 if (posOverPoly)
 *posOverPoly = true;
 }

 // Find height at the location.
 for (int j = 0; j < pd->triCount; ++j)
 {
 const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
 const float* v[3];
 for (int k = 0; k < 3; ++k)
 {
 if (t[k] < poly->vertCount)
 v[k] = &tile->verts[poly->verts[t[k]]*3];
 else
 v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
 }
 float h;
 if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
 {
 closest[1] = h;
 break;
 }
 }
 
 return DT_SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::closestPointOnPolyBoundary	(	dtPolyRef	ref,
		const float *	pos,
		float *	closest
	)		const

Returns a point on the boundary closest to the source point if the source point is outside the polygon's xz-bounds.

Parameters

[in]	ref	The reference id to the polygon.
[in]	pos	The position to check. [(x, y, z)]
[out]	closest	The closest point. [(x, y, z)]

Returns

The status flags for the query.

Much faster than closestPointOnPoly().

If the provided position lies within the polygon's xz-bounds (above or below), then pos and closest will be equal.

The height of closest will be the polygon boundary. The height detail is not used.

pos does not have to be within the bounds of the polybon or the navigation mesh.

{
 dtAssert(m_nav);
 
 const dtMeshTile* tile = 0;
 const dtPoly* poly = 0;
 if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
 return DT_FAILURE | DT_INVALID_PARAM;
 
 // Collect vertices.
 float verts[DT_VERTS_PER_POLYGON*3]; 
 float edged[DT_VERTS_PER_POLYGON];
 float edget[DT_VERTS_PER_POLYGON];
 int nv = 0;
 for (int i = 0; i < (int)poly->vertCount; ++i)
 {
 dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
 nv++;
 } 
 
 bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
 if (inside)
 {
 // Point is inside the polygon, return the point.
 dtVcopy(closest, pos);
 }
 else
 {
 // Point is outside the polygon, dtClamp to nearest edge.
 float dmin = FLT_MAX;
 int imin = -1;
 for (int i = 0; i < nv; ++i)
 {
 if (edged[i] < dmin)
 {
 dmin = edged[i];
 imin = i;
 }
 }
 const float* va = &verts[imin*3];
 const float* vb = &verts[((imin+1)%nv)*3];
 dtVlerp(closest, va, vb, edget[imin]);
 }
 
 return DT_SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::finalizeSlicedFindPath	(	dtPolyRef *	path,
		int *	pathCount,
		const int	maxPath
	)

Finalizes and returns the results of a sliced path query.

Parameters

[out]	path	An ordered list of polygon references representing the path. (Start to end.) [(polyRef) * pathCount]
[out]	pathCount	The number of polygons returned in the path array.
[in]	maxPath	The max number of polygons the path array can hold. [Limit: >= 1]

Returns

The status flags for the query.

{
 *pathCount = 0;
 
 if (dtStatusFailed(m_query.status))
 {
 // Reset query.
 memset(&m_query, 0, sizeof(dtQueryData));
 return DT_FAILURE;
 }

 int n = 0;

 if (m_query.startRef == m_query.endRef)
 {
 // Special case: the search starts and ends at same poly.
 path[n++] = m_query.startRef;
 }
 else
 {
 // Reverse the path.
 dtAssert(m_query.lastBestNode);
 
 if (m_query.lastBestNode->id != m_query.endRef)
 m_query.status |= DT_PARTIAL_RESULT;
 
 dtNode* prev = 0;
 dtNode* node = m_query.lastBestNode;
 int prevRay = 0;
 do
 {
 dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
 node->pidx = m_nodePool->getNodeIdx(prev);
 prev = node;
 int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
 node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
 prevRay = nextRay;
 node = next;
 }
 while (node);
 
 // Store path
 node = prev;
 do
 {
 dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
 dtStatus status = 0;
 if (node->flags & DT_NODE_PARENT_DETACHED)
 {
 float t, normal[3];
 int m;
 status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n);
 n += m;
 // raycast ends on poly boundary and the path might include the next poly boundary.
 if (path[n-1] == next->id)
 n--; // remove to avoid duplicates
 }
 else
 {
 path[n++] = node->id;
 if (n >= maxPath)
 status = DT_BUFFER_TOO_SMALL;
 }

 if (status & DT_STATUS_DETAIL_MASK)
 {
 m_query.status |= status & DT_STATUS_DETAIL_MASK;
 break;
 }
 node = next;
 }
 while (node);
 }
 
 const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;

 // Reset query.
 memset(&m_query, 0, sizeof(dtQueryData));
 
 *pathCount = n;
 
 return DT_SUCCESS | details;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial	(	const dtPolyRef *	existing,
		const int	existingSize,
		dtPolyRef *	path,
		int *	pathCount,
		const int	maxPath
	)

Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest polygon on the existing path that was visited during the search.

Parameters

[in]	existing	An array of polygon references for the existing path.
[in]	existingSize	The number of polygon in the existing array.
[out]	path	An ordered list of polygon references representing the path. (Start to end.) [(polyRef) * pathCount]
[out]	pathCount	The number of polygons returned in the path array.
[in]	maxPath	The max number of polygons the path array can hold. [Limit: >= 1]

Returns

The status flags for the query.

{
 *pathCount = 0;
 
 if (existingSize == 0)
 {
 return DT_FAILURE;
 }
 
 if (dtStatusFailed(m_query.status))
 {
 // Reset query.
 memset(&m_query, 0, sizeof(dtQueryData));
 return DT_FAILURE;
 }
 
 int n = 0;
 
 if (m_query.startRef == m_query.endRef)
 {
 // Special case: the search starts and ends at same poly.
 path[n++] = m_query.startRef;
 }
 else
 {
 // Find furthest existing node that was visited.
 dtNode* prev = 0;
 dtNode* node = 0;
 for (int i = existingSize-1; i >= 0; --i)
 {
 m_nodePool->findNodes(existing[i], &node, 1);
 if (node)
 break;
 }
 
 if (!node)
 {
 m_query.status |= DT_PARTIAL_RESULT;
 dtAssert(m_query.lastBestNode);
 node = m_query.lastBestNode;
 }
 
 // Reverse the path.
 int prevRay = 0;
 do
 {
 dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
 node->pidx = m_nodePool->getNodeIdx(prev);
 prev = node;
 int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
 node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
 prevRay = nextRay;
 node = next;
 }
 while (node);
 
 // Store path
 node = prev;
 do
 {
 dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
 dtStatus status = 0;
 if (node->flags & DT_NODE_PARENT_DETACHED)
 {
 float t, normal[3];
 int m;
 status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n);
 n += m;
 // raycast ends on poly boundary and the path might include the next poly boundary.
 if (path[n-1] == next->id)
 n--; // remove to avoid duplicates
 }
 else
 {
 path[n++] = node->id;
 if (n >= maxPath)
 status = DT_BUFFER_TOO_SMALL;
 }

 if (status & DT_STATUS_DETAIL_MASK)
 {
 m_query.status |= status & DT_STATUS_DETAIL_MASK;
 break;
 }
 node = next;
 }
 while (node);
 }
 
 const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;

 // Reset query.
 memset(&m_query, 0, sizeof(dtQueryData));
 
 *pathCount = n;
 
 return DT_SUCCESS | details;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findDistanceToWall	(	dtPolyRef	startRef,
		const float *	centerPos,
		const float	maxRadius,
		const dtQueryFilter *	filter,
		float *	hitDist,
		float *	hitPos,
		float *	hitNormal
	)		const

Finds the distance from the specified position to the nearest polygon wall.

Parameters

[in]	startRef	The reference id of the polygon containing centerPos.
[in]	centerPos	The center of the search circle. [(x, y, z)]
[in]	maxRadius	The radius of the search circle.
[in]	filter	The polygon filter to apply to the query.
[out]	hitDist	The distance to the nearest wall from centerPos.
[out]	hitPos	The nearest position on the wall that was hit. [(x, y, z)]
[out]	hitNormal	The normalized ray formed from the wall point to the source point. [(x, y, z)]

Returns

The status flags for the query.

hitPos is not adjusted using the height detail data.

hitDist will equal the search radius if there is no wall within the radius. In this case the values of hitPos and hitNormal are undefined.

The normal will become unpredicable if hitDist is a very small number.

{
 dtAssert(m_nav);
 dtAssert(m_nodePool);
 dtAssert(m_openList);
 
 // Validate input
 if (!startRef || !m_nav->isValidPolyRef(startRef))
 return DT_FAILURE | DT_INVALID_PARAM;
 
 m_nodePool->clear();
 m_openList->clear();
 
 dtNode* startNode = m_nodePool->getNode(startRef);
 dtVcopy(startNode->pos, centerPos);
 startNode->pidx = 0;
 startNode->cost = 0;
 startNode->total = 0;
 startNode->id = startRef;
 startNode->flags = DT_NODE_OPEN;
 m_openList->push(startNode);
 
 float radiusSqr = dtSqr(maxRadius);
 
 dtStatus status = DT_SUCCESS;
 
 while (!m_openList->empty())
 {
 dtNode* bestNode = m_openList->pop();
 bestNode->flags &= ~DT_NODE_OPEN;
 bestNode->flags |= DT_NODE_CLOSED;
 
 // Get poly and tile.
 // The API input has been cheked already, skip checking internal data.
 const dtPolyRef bestRef = bestNode->id;
 const dtMeshTile* bestTile = 0;
 const dtPoly* bestPoly = 0;
 m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
 
 // Get parent poly and tile.
 dtPolyRef parentRef = 0;
 const dtMeshTile* parentTile = 0;
 const dtPoly* parentPoly = 0;
 if (bestNode->pidx)
 parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
 if (parentRef)
 m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
 
 // Hit test walls.
 for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++)
 {
 // Skip non-solid edges.
 if (bestPoly->neis[j] & DT_EXT_LINK)
 {
 // Tile border.
 bool solid = true;
 for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next)
 {
 const dtLink* link = &bestTile->links[k];
 if (link->edge == j)
 {
 if (link->ref != 0)
 {
 const dtMeshTile* neiTile = 0;
 const dtPoly* neiPoly = 0;
 m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
 if (filter->passFilter(link->ref, neiTile, neiPoly))
 solid = false;
 }
 break;
 }
 }
 if (!solid) continue;
 }
 else if (bestPoly->neis[j])
 {
 // Internal edge
 const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1);
 const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx;
 if (filter->passFilter(ref, bestTile, &bestTile->polys[idx]))
 continue;
 }
 
 // Calc distance to the edge.
 const float* vj = &bestTile->verts[bestPoly->verts[j]*3];
 const float* vi = &bestTile->verts[bestPoly->verts[i]*3];
 float tseg;
 float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);
 
 // Edge is too far, skip.
 if (distSqr > radiusSqr)
 continue;
 
 // Hit wall, update radius.
 radiusSqr = distSqr;
 // Calculate hit pos.
 hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
 hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
 hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
 }
 
 for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
 {
 const dtLink* link = &bestTile->links[i];
 dtPolyRef neighbourRef = link->ref;
 // Skip invalid neighbours and do not follow back to parent.
 if (!neighbourRef || neighbourRef == parentRef)
 continue;
 
 // Expand to neighbour.
 const dtMeshTile* neighbourTile = 0;
 const dtPoly* neighbourPoly = 0;
 m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
 
 // Skip off-mesh connections.
 if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 continue;
 
 // Calc distance to the edge.
 const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3];
 const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3];
 float tseg;
 float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
 
 // If the circle is not touching the next polygon, skip it.
 if (distSqr > radiusSqr)
 continue;
 
 if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
 continue;

 dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 if (!neighbourNode)
 {
 status |= DT_OUT_OF_NODES;
 continue;
 }
 
 if (neighbourNode->flags & DT_NODE_CLOSED)
 continue;
 
 // Cost
 if (neighbourNode->flags == 0)
 {
 getEdgeMidPoint(bestRef, bestPoly, bestTile,
 neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
 }
 
 const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
 
 // 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;
 
 if (neighbourNode->flags & DT_NODE_OPEN)
 {
 m_openList->modify(neighbourNode);
 }
 else
 {
 neighbourNode->flags |= DT_NODE_OPEN;
 m_openList->push(neighbourNode);
 }
 }
 }
 
 // Calc hit normal.
 dtVsub(hitNormal, centerPos, hitPos);
 dtVnormalize(hitNormal);
 
 *hitDist = sqrtf(radiusSqr);
 
 return status;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findLocalNeighbourhood	(	dtPolyRef	startRef,
		const float *	centerPos,
		const float	radius,
		const dtQueryFilter *	filter,
		dtPolyRef *	resultRef,
		dtPolyRef *	resultParent,
		int *	resultCount,
		const int	maxResult
	)		const

Finds the non-overlapping navigation polygons in the local neighbourhood around the center position.

Parameters

[in]	startRef	The reference id of the polygon where the search starts.
[in]	centerPos	The center of the query circle. [(x, y, z)]
[in]	radius	The radius of the query circle.
[in]	filter	The polygon filter to apply to the query.
[out]	resultRef	The reference ids of the polygons touched by the circle.
[out]	resultParent	The reference ids of the parent polygons for each result. Zero if a result polygon has no parent. [opt]
[out]	resultCount	The number of polygons found.
[in]	maxResult	The maximum number of polygons the result arrays can hold.

Returns

The status flags for the query.

This method is optimized for a small search radius and small number of result polygons.

Candidate polygons are found by searching the navigation graph beginning at the start polygon.

The same intersection test restrictions that apply to the findPolysAroundCircle mehtod applies to this method.

The value of the center point is used as the start point for cost calculations. It is not projected onto the surface of the mesh, so its y-value will effect the costs.

Intersection tests occur in 2D. All polygons and the search circle are projected onto the xz-plane. So the y-value of the center point does not effect intersection tests.

If the result arrays are is too small to hold the entire result set, they will be filled to capacity.

{
 dtAssert(m_nav);
 dtAssert(m_tinyNodePool);
 
 *resultCount = 0;

 // Validate input
 if (!startRef || !m_nav->isValidPolyRef(startRef))
 return DT_FAILURE | DT_INVALID_PARAM;
 
 static const int MAX_STACK = 48;
 dtNode* stack[MAX_STACK];
 int nstack = 0;
 
 m_tinyNodePool->clear();
 
 dtNode* startNode = m_tinyNodePool->getNode(startRef);
 startNode->pidx = 0;
 startNode->id = startRef;
 startNode->flags = DT_NODE_CLOSED;
 stack[nstack++] = startNode;
 
 const float radiusSqr = dtSqr(radius);
 
 float pa[DT_VERTS_PER_POLYGON*3];
 float pb[DT_VERTS_PER_POLYGON*3];
 
 dtStatus status = DT_SUCCESS;
 
 int n = 0;
 if (n < maxResult)
 {
 resultRef[n] = startNode->id;
 if (resultParent)
 resultParent[n] = 0;
 ++n;
 }
 else
 {
 status |= DT_BUFFER_TOO_SMALL;
 }
 
 while (nstack)
 {
 // Pop front.
 dtNode* curNode = stack[0];
 for (int i = 0; i < nstack-1; ++i)
 stack[i] = stack[i+1];
 nstack--;
 
 // Get poly and tile.
 // The API input has been cheked already, skip checking internal data.
 const dtPolyRef curRef = curNode->id;
 const dtMeshTile* curTile = 0;
 const dtPoly* curPoly = 0;
 m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
 
 for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
 {
 const dtLink* link = &curTile->links[i];
 dtPolyRef neighbourRef = link->ref;
 // Skip invalid neighbours.
 if (!neighbourRef)
 continue;
 
 // Skip if cannot alloca more nodes.
 dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
 if (!neighbourNode)
 continue;
 // Skip visited.
 if (neighbourNode->flags & DT_NODE_CLOSED)
 continue;
 
 // Expand to neighbour
 const dtMeshTile* neighbourTile = 0;
 const dtPoly* neighbourPoly = 0;
 m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
 
 // Skip off-mesh connections.
 if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 continue;
 
 // Do not advance if the polygon is excluded by the filter.
 if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
 continue;
 
 // Find edge and calc distance to the edge.
 float va[3], vb[3];
 if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
 continue;
 
 // If the circle is not touching the next polygon, skip it.
 float tseg;
 float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
 if (distSqr > radiusSqr)
 continue;
 
 // Mark node visited, this is done before the overlap test so that
 // we will not visit the poly again if the test fails.
 neighbourNode->flags |= DT_NODE_CLOSED;
 neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
 
 // Check that the polygon does not collide with existing polygons.
 
 // Collect vertices of the neighbour poly.
 const int npa = neighbourPoly->vertCount;
 for (int k = 0; k < npa; ++k)
 dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]);
 
 bool overlap = false;
 for (int j = 0; j < n; ++j)
 {
 dtPolyRef pastRef = resultRef[j];
 
 // Connected polys do not overlap.
 bool connected = false;
 for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
 {
 if (curTile->links[k].ref == pastRef)
 {
 connected = true;
 break;
                    }
                }
                if (connected)
                    continue;
                
                // Potentially overlapping.
                const dtMeshTile* pastTile = 0;
                const dtPoly* pastPoly = 0;
                m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);
                
                // Get vertices and test overlap
                const int npb = pastPoly->vertCount;
                for (int k = 0; k < npb; ++k)
                    dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]);
                
                if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
                {
                    overlap = true;
                    break;
                }
            }
            if (overlap)
                continue;
            
            // This poly is fine, store and advance to the poly.
            if (n < maxResult)
            {
                resultRef[n] = neighbourRef;
                if (resultParent)
                    resultParent[n] = curRef;
                ++n;
            }
            else
            {
                status |= DT_BUFFER_TOO_SMALL;
            }
            
            if (nstack < MAX_STACK)
            {
                stack[nstack++] = neighbourNode;
            }
        }
    }
    
    *resultCount = n;
    
    return status;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findNearestPoly	(	const float *	center,
		const float *	extents,
		const dtQueryFilter *	filter,
		dtPolyRef *	nearestRef,
		float *	nearestPt
	)		const

Finds the polygon nearest to the specified center point.

Parameters

[in]	center	The center of the search box. [(x, y, z)]
[in]	extents	The search distance along each axis. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[out]	nearestRef	The reference id of the nearest polygon.
[out]	nearestPt	The nearest point on the polygon. [opt] [(x, y, z)]

Returns

The status flags for the query.

Note

If the search box does not intersect any polygons the search will return DT_SUCCESS, but nearestRef will be zero. So if in doubt, check nearestRef before using 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.

{
    dtAssert(m_nav);

    *nearestRef = 0;
    
    // 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;
    
    // Find nearest polygon amongst the nearby polygons.
    dtPolyRef nearest = 0;
    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)
        {
            if (nearestPt)
                dtVcopy(nearestPt, closestPtPoly);
            nearestDistanceSqr = d;
            nearest = ref;
        }
    }
    
    if (nearestRef)
        *nearestRef = nearest;
    
    return DT_SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::findPath	(	dtPolyRef	startRef,
		dtPolyRef	endRef,
		const float *	startPos,
		const float *	endPos,
		const dtQueryFilter *	filter,
		dtPolyRef *	path,
		int *	pathCount,
		const int	maxPath
	)		const

Finds a path from the start polygon to the end polygon.

Parameters

[in]	startRef	The refrence id of the start polygon.
[in]	endRef	The reference id of the end polygon.
[in]	startPos	A position within the start polygon. [(x, y, z)]
[in]	endPos	A position within the end polygon. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[out]	path	An ordered list of polygon references representing the path. (Start to end.) [(polyRef) * pathCount]
[out]	pathCount	The number of polygons returned in the path array.
[in]	maxPath	The maximum number of polygons the path array can hold. [Limit: >= 1]

If the end polygon cannot be reached through the navigation graph, the last polygon in the path will be the nearest the end polygon.

If the path array is to small to hold the full result, it will be filled as far as possible from the start polygon toward the end polygon.

The start and end positions are used to calculate traversal costs. (The y-values impact the result.)

{
    dtAssert(m_nav);
    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;
    
    // Validate input
    if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    if (startRef == endRef)
    {
        path[0] = startRef;
        *pathCount = 1;
        return DT_SUCCESS;
    }
    
    m_nodePool->clear();
    m_openList->clear();
    
    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, startPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = dtVdist(startPos, endPos) * H_SCALE;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);
    
    dtNode* lastBestNode = startNode;
    float lastBestNodeCost = startNode->total;
    
    dtStatus status = DT_SUCCESS;
    
    while (!m_openList->empty())
    {
        // Remove node from open list and put it in closed list.
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;
        
        // Reached the goal, stop searching.
        if (bestNode->id == endRef)
        {
            lastBestNode = bestNode;
            break;
        }
        
        // Get current poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
        
        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
        
        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            dtPolyRef neighbourRef = bestTile->links[i].ref;
            
            // Skip invalid ids and do not expand back to where we came from.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;
            
            // Get neighbour poly and tile.
            // The API input has been cheked already, skip checking internal data.
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);         
            
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;

            // deal explicitly with crossing tile boundaries
            unsigned char crossSide = 0;
            if (bestTile->links[i].side != 0xff)
                crossSide = bestTile->links[i].side >> 1;

            // get the node
            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }
            
            // If the node is visited the first time, calculate node position.
            if (neighbourNode->flags == 0)
            {
                getEdgeMidPoint(bestRef, bestPoly, bestTile,
                                neighbourRef, neighbourPoly, neighbourTile,
                                neighbourNode->pos);
            }

            // Calculate cost and heuristic.
            float cost = 0;
            float heuristic = 0;
            
            // Special case for last node.
            if (neighbourRef == endRef)
            {
                // Cost
                const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
                                                      parentRef, parentTile, parentPoly,
                                                      bestRef, bestTile, bestPoly,
                                                      neighbourRef, neighbourTile, neighbourPoly);
                const float endCost = filter->getCost(neighbourNode->pos, endPos,
                                                      bestRef, bestTile, bestPoly,
                                                      neighbourRef, neighbourTile, neighbourPoly,
                                                      0, 0, 0);
                
                cost = bestNode->cost + curCost + endCost;
                heuristic = 0;
            }
            else
            {
                // Cost
                const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
                                                      parentRef, parentTile, parentPoly,
                                                      bestRef, bestTile, bestPoly,
                                                      neighbourRef, neighbourTile, neighbourPoly);
                cost = bestNode->cost + curCost;
                heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
            }

            const float total = cost + heuristic;
            
            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;
            // The node is already visited and process, and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
                continue;
            
            // Add or update the node.
            neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
            neighbourNode->id = neighbourRef;
            neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
            neighbourNode->cost = cost;
            neighbourNode->total = total;
            
            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                // Already in open, update node location.
                m_openList->modify(neighbourNode);
            }
            else
            {
                // Put the node in open list.
                neighbourNode->flags |= DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }
            
            // Update nearest node to target so far.
            if (heuristic < lastBestNodeCost)
            {
                lastBestNodeCost = heuristic;
                lastBestNode = neighbourNode;
            }
        }
    }
    
    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;
    
    return status;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::findPolysAroundCircle	(	dtPolyRef	startRef,
		const float *	centerPos,
		const float	radius,
		const dtQueryFilter *	filter,
		dtPolyRef *	resultRef,
		dtPolyRef *	resultParent,
		float *	resultCost,
		int *	resultCount,
		const int	maxResult
	)		const

Finds the polygons along the navigation graph that touch the specified circle.

Parameters

[in]	startRef	The reference id of the polygon where the search starts.
[in]	centerPos	The center of the search circle. [(x, y, z)]
[in]	radius	The radius of the search circle.
[in]	filter	The polygon filter to apply to the query.
[out]	resultRef	The reference ids of the polygons touched by the circle. [opt]
[out]	resultParent	The reference ids of the parent polygons for each result. Zero if a result polygon has no parent. [opt]
[out]	resultCost	The search cost from centerPos to the polygon. [opt]
[out]	resultCount	The number of polygons found. [opt]
[in]	maxResult	The maximum number of polygons the result arrays can hold.

Returns

The status flags for the query.

At least one result array must be provided.

The order of the result set is from least to highest cost to reach the polygon.

A common use case for this method is to perform Dijkstra searches. Candidate polygons are found by searching the graph beginning at the start polygon.

If a polygon is not found via the graph search, even if it intersects the search circle, it will not be included in the result set. For example:

polyA is the start polygon. polyB shares an edge with polyA. (Is adjacent.) polyC shares an edge with polyB, but not with polyA Even if the search circle overlaps polyC, it will not be included in the result set unless polyB is also in the set.

The value of the center point is used as the start position for cost calculations. It is not projected onto the surface of the mesh, so its y-value will effect the costs.

Intersection tests occur in 2D. All polygons and the search circle are projected onto the xz-plane. So the y-value of the center point does not effect intersection tests.

If the result arrays are to small to hold the entire result set, they will be filled to capacity.

{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    *resultCount = 0;
    
    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    m_nodePool->clear();
    m_openList->clear();
    
    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, centerPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);
    
    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);
    
    while (!m_openList->empty())
    {
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;
        
        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
        
        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
        
        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            const dtLink* link = &bestTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours and do not follow back to parent.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;
            
            // Expand to neighbour
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
        
            // Do not advance if the polygon is excluded by the filter.
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;
            
            // Find edge and calc distance to the edge.
            float va[3], vb[3];
            if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
                continue;
            
            // If the circle is not touching the next polygon, skip it.
            float tseg;
            float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
            if (distSqr > radiusSqr)
                continue;
            
            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }
                
            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;
            
            // Cost
            if (neighbourNode->flags == 0)
                dtVlerp(neighbourNode->pos, va, vb, 0.5f);
            
            const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
            
            // 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;
            
            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                m_openList->modify(neighbourNode);
            }
            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);
            }
        }
    }
    
    *resultCount = n;
    
    return status;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findPolysAroundShape	(	dtPolyRef	startRef,
		const float *	verts,
		const int	nverts,
		const dtQueryFilter *	filter,
		dtPolyRef *	resultRef,
		dtPolyRef *	resultParent,
		float *	resultCost,
		int *	resultCount,
		const int	maxResult
	)		const

Finds the polygons along the naviation graph that touch the specified convex polygon.

Parameters

[in]	startRef	The reference id of the polygon where the search starts.
[in]	verts	The vertices describing the convex polygon. (CCW) [(x, y, z) * nverts]
[in]	nverts	The number of vertices in the polygon.
[in]	filter	The polygon filter to apply to the query.
[out]	resultRef	The reference ids of the polygons touched by the search polygon. [opt]
[out]	resultParent	The reference ids of the parent polygons for each result. Zero if a result polygon has no parent. [opt]
[out]	resultCost	The search cost from the centroid point to the polygon. [opt]
[out]	resultCount	The number of polygons found.
[in]	maxResult	The maximum number of polygons the result arrays can hold.

Returns

The status flags for the query.

The order of the result set is from least to highest cost.

At least one result array must be provided.

A common use case for this method is to perform Dijkstra searches. Candidate polygons are found by searching the graph beginning at the start polygon.

The same intersection test restrictions that apply to findPolysAroundCircle() method apply to this method.

The 3D centroid of the search polygon is used as the start position for cost calculations.

Intersection tests occur in 2D. All polygons are projected onto the xz-plane. So the y-values of the vertices do not effect intersection tests.

If the result arrays are is too small to hold the entire result set, they will be filled to capacity.

{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);
    
    *resultCount = 0;
    
    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    m_nodePool->clear();
    m_openList->clear();
    
    float centerPos[3] = {0,0,0};
    for (int i = 0; i < nverts; ++i)
        dtVadd(centerPos,centerPos,&verts[i*3]);
    dtVscale(centerPos,centerPos,1.0f/nverts);

    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, centerPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);
    
    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())
    {
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;
        
        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
        
        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
        
        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            const dtLink* link = &bestTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours and do not follow back to parent.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;
            
            // Expand to neighbour
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
            
            // Do not advance if the polygon is excluded by the filter.
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;
            
            // Find edge and calc distance to the edge.
            float va[3], vb[3];
            if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
                continue;
            
            // If the poly is not touching the edge to the next polygon, skip the connection it.
            float tmin, tmax;
            int segMin, segMax;
            if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax))
                continue;
            if (tmin > 1.0f || tmax < 0.0f)
                continue;
            
            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }
            
            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;
            
            // Cost
            if (neighbourNode->flags == 0)
                dtVlerp(neighbourNode->pos, va, vb, 0.5f);
            
            const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
            
            // 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;
            
            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                m_openList->modify(neighbourNode);
            }
            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);
            }
        }
    }
    
    *resultCount = n;
    
    return status;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findRandomPoint	(	const dtQueryFilter *	filter,
		float(*)()	frand,
		dtPolyRef *	randomRef,
		float *	randomPt
	)		const

Returns random location on navmesh. Polygons are chosen weighted by area. The search runs in linear related to number of polygon.

Parameters

[in]	filter	The polygon filter to apply to the query.
[in]	frand	Function returning a random number [0..1).
[out]	randomRef	The reference id of the random location.
[out]	randomPt	The random location.

Returns

The status flags for the query.

{
    dtAssert(m_nav);
    
    // Randomly pick one tile. Assume that all tiles cover roughly the same area.
    const dtMeshTile* tile = 0;
    float tsum = 0.0f;
    for (int i = 0; i < m_nav->getMaxTiles(); i++)
    {
        const dtMeshTile* t = m_nav->getTile(i);
        if (!t || !t->header) continue;
        
        // Choose random tile using reservoi sampling.
        const float area = 1.0f; // Could be tile area too.
        tsum += area;
        const float u = frand();
        if (u*tsum <= area)
            tile = t;
    }
    if (!tile)
        return DT_FAILURE;

    // Randomly pick one polygon weighted by polygon area.
    const dtPoly* poly = 0;
    dtPolyRef polyRef = 0;
    const dtPolyRef base = m_nav->getPolyRefBase(tile);

    float areaSum = 0.0f;
    for (int i = 0; i < tile->header->polyCount; ++i)
    {
        const dtPoly* p = &tile->polys[i];
        // Do not return off-mesh connection polygons.
        if (p->getType() != DT_POLYTYPE_GROUND)
            continue;
        // Must pass filter
        const dtPolyRef ref = base | (dtPolyRef)i;
        if (!filter->passFilter(ref, tile, p))
            continue;

        // Calc area of the polygon.
        float polyArea = 0.0f;
        for (int j = 2; j < p->vertCount; ++j)
        {
            const float* va = &tile->verts[p->verts[0]*3];
            const float* vb = &tile->verts[p->verts[j-1]*3];
            const float* vc = &tile->verts[p->verts[j]*3];
            polyArea += dtTriArea2D(va,vb,vc);
        }

        // Choose random polygon weighted by area, using reservoi sampling.
        areaSum += polyArea;
        const float u = frand();
        if (u*areaSum <= polyArea)
        {
            poly = p;
            polyRef = ref;
        }
    }
    
    if (!poly)
        return DT_FAILURE;

    // Randomly pick point on polygon.
    const float* v = &tile->verts[poly->verts[0]*3];
    float verts[3*DT_VERTS_PER_POLYGON];
    float areas[DT_VERTS_PER_POLYGON];
    dtVcopy(&verts[0*3],v);
    for (int j = 1; j < poly->vertCount; ++j)
    {
        v = &tile->verts[poly->verts[j]*3];
        dtVcopy(&verts[j*3],v);
    }
    
    const float s = frand();
    const float t = frand();
    
    float pt[3];
    dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
    
    float h = 0.0f;
    dtStatus status = getPolyHeight(polyRef, pt, &h);
    if (dtStatusFailed(status))
        return status;
    pt[1] = h;
    
    dtVcopy(randomPt, pt);
    *randomRef = polyRef;

    return DT_SUCCESS;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findRandomPointAroundCircle	(	dtPolyRef	startRef,
		const float *	centerPos,
		const float	maxRadius,
		const dtQueryFilter *	filter,
		float(*)()	frand,
		dtPolyRef *	randomRef,
		float *	randomPt
	)		const

Returns random location on navmesh within the reach of specified location. Polygons are chosen weighted by area. The search runs in linear related to number of polygon. The location is not exactly constrained by the circle, but it limits the visited polygons.

Parameters

[in]	startRef	The reference id of the polygon where the search starts.
[in]	centerPos	The center of the search circle. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[in]	frand	Function returning a random number [0..1).
[out]	randomRef	The reference id of the random location.
[out]	randomPt	The random location. [(x, y, z)]

Returns

The status flags for the query.

{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);
    
    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    const dtMeshTile* startTile = 0;
    const dtPoly* startPoly = 0;
    m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
    if (!filter->passFilter(startRef, startTile, startPoly))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    m_nodePool->clear();
    m_openList->clear();
    
    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, centerPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);
    
    dtStatus status = DT_SUCCESS;
    
    const float radiusSqr = dtSqr(maxRadius);
    float areaSum = 0.0f;

    const dtMeshTile* randomTile = 0;
    const dtPoly* randomPoly = 0;
    dtPolyRef randomPolyRef = 0;

    while (!m_openList->empty())
    {
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;
        
        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

        // Place random locations on on ground.
        if (bestPoly->getType() == DT_POLYTYPE_GROUND)
        {
            // Calc area of the polygon.
            float polyArea = 0.0f;
            for (int j = 2; j < bestPoly->vertCount; ++j)
            {
                const float* va = &bestTile->verts[bestPoly->verts[0]*3];
                const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3];
                const float* vc = &bestTile->verts[bestPoly->verts[j]*3];
                polyArea += dtTriArea2D(va,vb,vc);
            }
            // Choose random polygon weighted by area, using reservoi sampling.
            areaSum += polyArea;
            const float u = frand();
            if (u*areaSum <= polyArea)
            {
                randomTile = bestTile;
                randomPoly = bestPoly;
                randomPolyRef = bestRef;
            }
        }
        
        
        // Get parent poly and tile.
        dtPolyRef parentRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        if (bestNode->pidx)
            parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
        if (parentRef)
            m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
        
        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            const dtLink* link = &bestTile->links[i];
            dtPolyRef neighbourRef = link->ref;
            // Skip invalid neighbours and do not follow back to parent.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;
            
            // Expand to neighbour
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
            
            // Do not advance if the polygon is excluded by the filter.
            if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;
            
            // Find edge and calc distance to the edge.
            float va[3], vb[3];
            if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
                continue;
            
            // If the circle is not touching the next polygon, skip it.
            float tseg;
            float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
            if (distSqr > radiusSqr)
                continue;
            
            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
            if (!neighbourNode)
            {
                status |= DT_OUT_OF_NODES;
                continue;
            }
            
            if (neighbourNode->flags & DT_NODE_CLOSED)
                continue;
            
            // Cost
            if (neighbourNode->flags == 0)
                dtVlerp(neighbourNode->pos, va, vb, 0.5f);
            
            const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
            
            // 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;
            
            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                m_openList->modify(neighbourNode);
            }
            else
            {
                neighbourNode->flags = DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }
        }
    }
    
    if (!randomPoly)
        return DT_FAILURE;
    
    // Randomly pick point on polygon.
    const float* v = &randomTile->verts[randomPoly->verts[0]*3];
    float verts[3*DT_VERTS_PER_POLYGON];
    float areas[DT_VERTS_PER_POLYGON];
    dtVcopy(&verts[0*3],v);
    for (int j = 1; j < randomPoly->vertCount; ++j)
    {
        v = &randomTile->verts[randomPoly->verts[j]*3];
        dtVcopy(&verts[j*3],v);
    }
    
    const float s = frand();
    const float t = frand();
    
    float pt[3];
    dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
    
    float h = 0.0f;
    dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
    if (dtStatusFailed(status))
        return stat;
    pt[1] = h;
    
    dtVcopy(randomPt, pt);
    *randomRef = randomPolyRef;
    
    return DT_SUCCESS;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::findStraightPath	(	const float *	startPos,
		const float *	endPos,
		const dtPolyRef *	path,
		const int	pathSize,
		float *	straightPath,
		unsigned char *	straightPathFlags,
		dtPolyRef *	straightPathRefs,
		int *	straightPathCount,
		const int	maxStraightPath,
		const int	options = 0
	)		const

Finds the straight path from the start to the end position within the polygon corridor.

Parameters

[in]	startPos	Path start position. [(x, y, z)]
[in]	endPos	Path end position. [(x, y, z)]
[in]	path	An array of polygon references that represent the path corridor.
[in]	pathSize	The number of polygons in the path array.
[out]	straightPath	Points describing the straight path. [(x, y, z) * straightPathCount].
[out]	straightPathFlags	Flags describing each point. (See: dtStraightPathFlags) [opt]
[out]	straightPathRefs	The reference id of the polygon that is being entered at each point. [opt]
[out]	straightPathCount	The number of points in the straight path.
[in]	maxStraightPath	The maximum number of points the straight path arrays can hold. [Limit: > 0]
[in]	options	Query options. (see: dtStraightPathOptions)

Returns

The status flags for the query.

This method peforms what is often called 'string pulling'.

The start position is clamped to the first polygon in the path, and the end position is clamped to the last. So the start and end positions should normally be within or very near the first and last polygons respectively.

The returned polygon references represent the reference id of the polygon that is entered at the associated path position. The reference id associated with the end point will always be zero. This allows, for example, matching off-mesh link points to their representative polygons.

If the provided result buffers are too small for the entire result set, they will be filled as far as possible from the start toward the end position.

{
    dtAssert(m_nav);
    
    *straightPathCount = 0;
    
    if (!maxStraightPath)
        return DT_FAILURE | DT_INVALID_PARAM;
    
    if (!path[0])
        return DT_FAILURE | DT_INVALID_PARAM;
    
    dtStatus stat = 0;
    
    // TODO: Should this be callers responsibility?
    float closestStartPos[3];
    if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
        return DT_FAILURE | DT_INVALID_PARAM;

    float closestEndPos[3];
    if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    // Add start point.
    stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
                        straightPath, straightPathFlags, straightPathRefs,
                        straightPathCount, maxStraightPath);
    if (stat != DT_IN_PROGRESS)
        return stat;
    
    if (pathSize > 1)
    {
        float portalApex[3], portalLeft[3], portalRight[3];
        dtVcopy(portalApex, closestStartPos);
        dtVcopy(portalLeft, portalApex);
        dtVcopy(portalRight, portalApex);
        int apexIndex = 0;
        int leftIndex = 0;
        int rightIndex = 0;
        
        unsigned char leftPolyType = 0;
        unsigned char rightPolyType = 0;
        
        dtPolyRef leftPolyRef = path[0];
        dtPolyRef rightPolyRef = path[0];
        
        for (int i = 0; i < pathSize; ++i)
        {
            float left[3], right[3];
            unsigned char fromType, toType;
            
            if (i+1 < pathSize)
            {
                // Next portal.
                if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
                {
                    // Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
                    // Clamp the end point to path[i], and return the path so far.
                    
                    if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
                    {
                        // This should only happen when the first polygon is invalid.
                        return DT_FAILURE | DT_INVALID_PARAM;
                    }

                    // Apeend portals along the current straight path segment.
                    if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
                    {
                        stat = appendPortals(apexIndex, i, closestEndPos, path,
                                             straightPath, straightPathFlags, straightPathRefs,
                                             straightPathCount, maxStraightPath, options);
                    }

                    stat = appendVertex(closestEndPos, 0, path[i],
                                        straightPath, straightPathFlags, straightPathRefs,
                                        straightPathCount, maxStraightPath);
                    
                    return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
                }
                
                // If starting really close the portal, advance.
                if (i == 0)
                {
                    float t;
                    if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f))
                        continue;
                }
            }
            else
            {
                // End of the path.
                dtVcopy(left, closestEndPos);
                dtVcopy(right, closestEndPos);
                
                fromType = toType = DT_POLYTYPE_GROUND;
            }
            
            // Right vertex.
            if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
            {
                if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
                {
                    dtVcopy(portalRight, right);
                    rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
                    rightPolyType = toType;
                    rightIndex = i;
                }
                else
                {
                    // Append portals along the current straight path segment.
                    if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
                    {
                        stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
                                             straightPath, straightPathFlags, straightPathRefs,
                                             straightPathCount, maxStraightPath, options);
                        if (stat != DT_IN_PROGRESS)
                            return stat;                    
                    }
                
                    dtVcopy(portalApex, portalLeft);
                    apexIndex = leftIndex;
                    
                    unsigned char flags = 0;
                    if (!leftPolyRef)
                        flags = DT_STRAIGHTPATH_END;
                    else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
                        flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
                    dtPolyRef ref = leftPolyRef;
                    
                    // Append or update vertex
                    stat = appendVertex(portalApex, flags, ref,
                                        straightPath, straightPathFlags, straightPathRefs,
                                        straightPathCount, maxStraightPath);
                    if (stat != DT_IN_PROGRESS)
                        return stat;
                    
                    dtVcopy(portalLeft, portalApex);
                    dtVcopy(portalRight, portalApex);
                    leftIndex = apexIndex;
                    rightIndex = apexIndex;
                    
                    // Restart
                    i = apexIndex;
                    
                    continue;
                }
            }
            
            // Left vertex.
            if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
            {
                if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
                {
                    dtVcopy(portalLeft, left);
                    leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
                    leftPolyType = toType;
                    leftIndex = i;
                }
                else
                {
                    // Append portals along the current straight path segment.
                    if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
                    {
                        stat = appendPortals(apexIndex, rightIndex, portalRight, path,
                                             straightPath, straightPathFlags, straightPathRefs,
                                             straightPathCount, maxStraightPath, options);
                        if (stat != DT_IN_PROGRESS)
                            return stat;
                    }

                    dtVcopy(portalApex, portalRight);
                    apexIndex = rightIndex;
                    
                    unsigned char flags = 0;
                    if (!rightPolyRef)
                        flags = DT_STRAIGHTPATH_END;
                    else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
                        flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
                    dtPolyRef ref = rightPolyRef;

                    // Append or update vertex
                    stat = appendVertex(portalApex, flags, ref,
                                        straightPath, straightPathFlags, straightPathRefs,
                                        straightPathCount, maxStraightPath);
                    if (stat != DT_IN_PROGRESS)
                        return stat;
                    
                    dtVcopy(portalLeft, portalApex);
                    dtVcopy(portalRight, portalApex);
                    leftIndex = apexIndex;
                    rightIndex = apexIndex;
                    
                    // Restart
                    i = apexIndex;
                    
                    continue;
                }
            }
        }

        // Append portals along the current straight path segment.
        if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
        {
            stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
                                 straightPath, straightPathFlags, straightPathRefs,
                                 straightPathCount, maxStraightPath, options);
            if (stat != DT_IN_PROGRESS)
                return stat;
        }
    }

    stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
                        straightPath, straightPathFlags, straightPathRefs,
                        straightPathCount, maxStraightPath);
    
    return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
}

Here is the call graph for this function:

Here is the caller graph for this function:

const dtNavMesh* dtNavMeshQuery::getAttachedNavMesh ( ) const

inline

Gets the navigation mesh the query object is using.

Returns

The navigation mesh the query object is using.

{ return m_nav; }

dtStatus dtNavMeshQuery::getEdgeMidPoint ( dtPolyRef  from, dtPolyRef  to, float *  mid  ) const

private

Returns edge mid point between two polygons.

{
    float left[3], right[3];
    unsigned char fromType, toType;
    if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
        return DT_FAILURE | DT_INVALID_PARAM;
    mid[0] = (left[0]+right[0])*0.5f;
    mid[1] = (left[1]+right[1])*0.5f;
    mid[2] = (left[2]+right[2])*0.5f;
    return DT_SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::getEdgeMidPoint ( dtPolyRef  from, const dtPoly *  fromPoly, const dtMeshTile *  fromTile, dtPolyRef  to, const dtPoly *  toPoly, const dtMeshTile *  toTile, float *  mid  ) const

private

{
    float left[3], right[3];
    if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
        return DT_FAILURE | DT_INVALID_PARAM;
    mid[0] = (left[0]+right[0])*0.5f;
    mid[1] = (left[1]+right[1])*0.5f;
    mid[2] = (left[2]+right[2])*0.5f;
    return DT_SUCCESS;
}

Here is the call graph for this function:

dtMeshTile* dtNavMeshQuery::getNeighbourTileAt ( int  x, int  y, int  side  ) const

private

Returns neighbour tile based on side.

class dtNodePool* dtNavMeshQuery::getNodePool ( ) const

inline

Gets the node pool.

Returns

The node pool.

{ return m_nodePool; }

dtStatus dtNavMeshQuery::getPolyHeight	(	dtPolyRef	ref,
		const float *	pos,
		float *	height
	)		const

Gets the height of the polygon at the provided position using the height detail. (Most accurate.)

Parameters

[in]	ref	The reference id of the polygon.
[in]	pos	A position within the xz-bounds of the polygon. [(x, y, z)]
[out]	height	The height at the surface of the polygon.

Returns

The status flags for the query.

Will return DT_FAILURE if the provided position is outside the xz-bounds of the polygon.

{
    dtAssert(m_nav);

    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
    {
        const float* v0 = &tile->verts[poly->verts[0]*3];
        const float* v1 = &tile->verts[poly->verts[1]*3];
        const float d0 = dtVdist2D(pos, v0);
        const float d1 = dtVdist2D(pos, v1);
        const float u = d0 / (d0+d1);
        if (height)
            *height = v0[1] + (v1[1] - v0[1]) * u;
        return DT_SUCCESS;
    }
    else
    {
        const unsigned int ip = (unsigned int)(poly - tile->polys);
        const dtPolyDetail* pd = &tile->detailMeshes[ip];
        for (int j = 0; j < pd->triCount; ++j)
        {
            const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
            const float* v[3];
            for (int k = 0; k < 3; ++k)
            {
                if (t[k] < poly->vertCount)
                    v[k] = &tile->verts[poly->verts[t[k]]*3];
                else
                    v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
            }
            float h;
            if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
            {
                if (height)
                    *height = h;
                return DT_SUCCESS;
            }
        }
    }
    
    return DT_FAILURE | DT_INVALID_PARAM;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::getPolyWallSegments	(	dtPolyRef	ref,
		const dtQueryFilter *	filter,
		float *	segmentVerts,
		dtPolyRef *	segmentRefs,
		int *	segmentCount,
		const int	maxSegments
	)		const

Returns the segments for the specified polygon, optionally including portals.

Parameters

[in]	ref	The reference id of the polygon.
[in]	filter	The polygon filter to apply to the query.
[out]	segmentVerts	The segments. [(ax, ay, az, bx, by, bz) * segmentCount]
[out]	segmentRefs	The reference ids of each segment's neighbor polygon. Or zero if the segment is a wall. [opt] [(parentRef) * segmentCount]
[out]	segmentCount	The number of segments returned.
[in]	maxSegments	The maximum number of segments the result arrays can hold.

Returns

The status flags for the query.

If the segmentRefs parameter is provided, then all polygon segments will be returned. Otherwise only the wall segments are returned.

A segment that is normally a portal will be included in the result set as a wall if the filter results in the neighbor polygon becoomming impassable.

The segmentVerts and segmentRefs buffers should normally be sized for the maximum segments per polygon of the source navigation mesh.

{
    dtAssert(m_nav);
    
    *segmentCount = 0;
    
    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    int n = 0;
    static const int MAX_INTERVAL = 16;
    dtSegInterval ints[MAX_INTERVAL];
    int nints;
    
    const bool storePortals = segmentRefs != 0;
    
    dtStatus status = DT_SUCCESS;
    
    for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
    {
        // Skip non-solid edges.
        nints = 0;
        if (poly->neis[j] & DT_EXT_LINK)
        {
            // Tile border.
            for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
            {
                const dtLink* link = &tile->links[k];
                if (link->edge == j)
                {
                    if (link->ref != 0)
                    {
                        const dtMeshTile* neiTile = 0;
                        const dtPoly* neiPoly = 0;
                        m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
                        if (filter->passFilter(link->ref, neiTile, neiPoly))
                        {
                            insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref);
                        }
                    }
                }
            }
        }
        else
        {
            // Internal edge
            dtPolyRef neiRef = 0;
            if (poly->neis[j])
            {
                const unsigned int idx = (unsigned int)(poly->neis[j]-1);
                neiRef = m_nav->getPolyRefBase(tile) | idx;
                if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
                    neiRef = 0;
            }

            // If the edge leads to another polygon and portals are not stored, skip.
            if (neiRef != 0 && !storePortals)
                continue;
            
            if (n < maxSegments)
            {
                const float* vj = &tile->verts[poly->verts[j]*3];
                const float* vi = &tile->verts[poly->verts[i]*3];
                float* seg = &segmentVerts[n*6];
                dtVcopy(seg+0, vj);
                dtVcopy(seg+3, vi);
                if (segmentRefs)
                    segmentRefs[n] = neiRef;
                n++;
            }
            else
            {
                status |= DT_BUFFER_TOO_SMALL;
            }
            
            continue;
        }
        
        // Add sentinels
        insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
        insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
        
        // Store segments.
        const float* vj = &tile->verts[poly->verts[j]*3];
        const float* vi = &tile->verts[poly->verts[i]*3];
        for (int k = 1; k < nints; ++k)
        {
            // Portal segment.
            if (storePortals && ints[k].ref)
            {
                const float tmin = ints[k].tmin/255.0f; 
                const float tmax = ints[k].tmax/255.0f; 
                if (n < maxSegments)
                {
                    float* seg = &segmentVerts[n*6];
                    dtVlerp(seg+0, vj,vi, tmin);
                    dtVlerp(seg+3, vj,vi, tmax);
                    if (segmentRefs)
                        segmentRefs[n] = ints[k].ref;
                    n++;
                }
                else
                {
                    status |= DT_BUFFER_TOO_SMALL;
                }
            }

            // Wall segment.
            const int imin = ints[k-1].tmax;
            const int imax = ints[k].tmin;
            if (imin != imax)
            {
                const float tmin = imin/255.0f; 
                const float tmax = imax/255.0f; 
                if (n < maxSegments)
                {
                    float* seg = &segmentVerts[n*6];
                    dtVlerp(seg+0, vj,vi, tmin);
                    dtVlerp(seg+3, vj,vi, tmax);
                    if (segmentRefs)
                        segmentRefs[n] = 0;
                    n++;
                }
                else
                {
                    status |= DT_BUFFER_TOO_SMALL;
                }
            }
        }
    }
    
    *segmentCount = n;
    
    return status;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::getPortalPoints ( dtPolyRef  from, dtPolyRef  to, float *  left, float *  right, unsigned char &  fromType, unsigned char &  toType  ) const

private

Returns portal points between two polygons.

{
    dtAssert(m_nav);
    
    const dtMeshTile* fromTile = 0;
    const dtPoly* fromPoly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    fromType = fromPoly->getType();

    const dtMeshTile* toTile = 0;
    const dtPoly* toPoly = 0;
    if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
        return DT_FAILURE | DT_INVALID_PARAM;
    toType = toPoly->getType();
        
    return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::getPortalPoints ( dtPolyRef  from, const dtPoly *  fromPoly, const dtMeshTile *  fromTile, dtPolyRef  to, const dtPoly *  toPoly, const dtMeshTile *  toTile, float *  left, float *  right  ) const

private

{
    // Find the link that points to the 'to' polygon.
    const dtLink* link = 0;
    for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
    {
        if (fromTile->links[i].ref == to)
        {
            link = &fromTile->links[i];
            break;
        }
    }
    if (!link)
        return DT_FAILURE | DT_INVALID_PARAM;
    
    // Handle off-mesh connections.
    if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
    {
        // Find link that points to first vertex.
        for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
        {
            if (fromTile->links[i].ref == to)
            {
                const int v = fromTile->links[i].edge;
                dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]);
                dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]);
                return DT_SUCCESS;
            }
        }
        return DT_FAILURE | DT_INVALID_PARAM;
    }
    
    if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
    {
        for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
        {
            if (toTile->links[i].ref == from)
            {
                const int v = toTile->links[i].edge;
                dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]);
                dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]);
                return DT_SUCCESS;
            }
        }
        return DT_FAILURE | DT_INVALID_PARAM;
    }
    
    // Find portal vertices.
    const int v0 = fromPoly->verts[link->edge];
    const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
    dtVcopy(left, &fromTile->verts[v0*3]);
    dtVcopy(right, &fromTile->verts[v1*3]);
    
    // If the link is at tile boundary, dtClamp the vertices to
    // the link width.
    if (link->side != 0xff)
    {
        // Unpack portal limits.
        if (link->bmin != 0 || link->bmax != 255)
        {
            const float s = 1.0f/255.0f;
            const float tmin = link->bmin*s;
            const float tmax = link->bmax*s;
            dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin);
            dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax);
        }
    }
    
    return DT_SUCCESS;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::init	(	const dtNavMesh *	nav,
		const int	maxNodes
	)

Initializes the query object.

Parameters

[in]	nav	Pointer to the dtNavMesh object to use for all queries.
[in]	maxNodes	Maximum number of search nodes. [Limits: 0 < value <= 65536]

Returns

The status flags for the query.

Must be the first function called after construction, before other functions are used.

This function can be used multiple times.

{
    m_nav = nav;
    
    if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
    {
        if (m_nodePool)
        {
            m_nodePool->~dtNodePool();
            dtFree(m_nodePool);
            m_nodePool = 0;
        }
        m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4));
        if (!m_nodePool)
            return DT_FAILURE | DT_OUT_OF_MEMORY;
    }
    else
    {
        m_nodePool->clear();
    }
    
    if (!m_tinyNodePool)
    {
        m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32);
        if (!m_tinyNodePool)
            return DT_FAILURE | DT_OUT_OF_MEMORY;
    }
    else
    {
        m_tinyNodePool->clear();
    }
    
    // TODO: check the open list size too.
    if (!m_openList || m_openList->getCapacity() < maxNodes)
    {
        if (m_openList)
        {
            m_openList->~dtNodeQueue();
            dtFree(m_openList);
            m_openList = 0;
        }
        m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes);
        if (!m_openList)
            return DT_FAILURE | DT_OUT_OF_MEMORY;
    }
    else
    {
        m_openList->clear();
    }
    
    return DT_SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::initSlicedFindPath	(	dtPolyRef	startRef,
		dtPolyRef	endRef,
		const float *	startPos,
		const float *	endPos,
		const dtQueryFilter *	filter,
		const unsigned int	options = 0
	)

Intializes a sliced path query.

Parameters

[in]	startRef	The refrence id of the start polygon.
[in]	endRef	The reference id of the end polygon.
[in]	startPos	A position within the start polygon. [(x, y, z)]
[in]	endPos	A position within the end polygon. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[in]	options	query options (see: dtFindPathOptions)

Returns

The status flags for the query.

Warning

Calling any non-slice methods before calling finalizeSlicedFindPath() or finalizeSlicedFindPathPartial() may result in corrupted data!

The filter pointer is stored and used for the duration of the sliced path query.

{
    dtAssert(m_nav);
    dtAssert(m_nodePool);
    dtAssert(m_openList);

    // Init path state.
    memset(&m_query, 0, sizeof(dtQueryData));
    m_query.status = DT_FAILURE;
    m_query.startRef = startRef;
    m_query.endRef = endRef;
    dtVcopy(m_query.startPos, startPos);
    dtVcopy(m_query.endPos, endPos);
    m_query.filter = filter;
    m_query.options = options;
    m_query.raycastLimitSqr = FLT_MAX;
    
    if (!startRef || !endRef)
        return DT_FAILURE | DT_INVALID_PARAM;
    
    // Validate input
    if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
        return DT_FAILURE | DT_INVALID_PARAM;

    // trade quality with performance?
    if (options & DT_FINDPATH_ANY_ANGLE)
    {
        // limiting to several times the character radius yields nice results. It is not sensitive 
        // so it is enough to compute it from the first tile.
        const dtMeshTile* tile = m_nav->getTileByRef(startRef);
        float agentRadius = tile->header->walkableRadius;
        m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS);
    }

    if (startRef == endRef)
    {
        m_query.status = DT_SUCCESS;
        return DT_SUCCESS;
    }
    
    m_nodePool->clear();
    m_openList->clear();
    
    dtNode* startNode = m_nodePool->getNode(startRef);
    dtVcopy(startNode->pos, startPos);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = dtVdist(startPos, endPos) * H_SCALE;
    startNode->id = startRef;
    startNode->flags = DT_NODE_OPEN;
    m_openList->push(startNode);
    
    m_query.status = DT_IN_PROGRESS;
    m_query.lastBestNode = startNode;
    m_query.lastBestNodeCost = startNode->total;
    
    return m_query.status;
}

Here is the call graph for this function:

bool dtNavMeshQuery::isInClosedList

(

dtPolyRef

ref

)

const

Returns true if the polygon reference is in the closed list.

Parameters

[in]

ref

The reference id of the polygon to check.

Returns

True if the polygon is in closed list.

The closed list is the list of polygons that were fully evaluated during the last navigation graph search. (A* or Dijkstra)

{
    if (!m_nodePool) return false;
    
    dtNode* nodes[DT_MAX_STATES_PER_NODE];
    int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE);

    for (int i=0; i<n; i++)
    {
        if (nodes[i]->flags & DT_NODE_CLOSED)
            return true;
    }       

    return false;
}

Here is the call graph for this function:

bool dtNavMeshQuery::isValidPolyRef	(	dtPolyRef	ref,
		const dtQueryFilter *	filter
	)		const

Returns true if the polygon reference is valid and passes the filter restrictions.

Parameters

[in]	ref	The polygon reference to check.
[in]	filter	The filter to apply.

{
    const dtMeshTile* tile = 0;
    const dtPoly* poly = 0;
    dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly);
    // If cannot get polygon, assume it does not exists and boundary is invalid.
    if (dtStatusFailed(status))
        return false;
    // If cannot pass filter, assume flags has changed and boundary is invalid.
    if (!filter->passFilter(ref, tile, poly))
        return false;
    return true;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::moveAlongSurface	(	dtPolyRef	startRef,
		const float *	startPos,
		const float *	endPos,
		const dtQueryFilter *	filter,
		float *	resultPos,
		dtPolyRef *	visited,
		int *	visitedCount,
		const int	maxVisitedSize
	)		const

Moves from the start to the end position constrained to the navigation mesh.

Parameters

[in]	startRef	The reference id of the start polygon.
[in]	startPos	A position of the mover within the start polygon. [(x, y, x)]
[in]	endPos	The desired end position of the mover. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[out]	resultPos	The result position of the mover. [(x, y, z)]
[out]	visited	The reference ids of the polygons visited during the move.
[out]	visitedCount	The number of polygons visited during the move.
[in]	maxVisitedSize	The maximum number of polygons the visited array can hold.

Returns

The status flags for the query.

This method is optimized for small delta movement and a small number of polygons. If used for too great a distance, the result set will form an incomplete path.

resultPos will equal the endPos if the end is reached. Otherwise the closest reachable position will be returned.

resultPos is not projected onto the surface of the navigation mesh. Use getPolyHeight if this is needed.

This method treats the end position in the same manner as the raycast method. (As a 2D point.) See that method's documentation for details.

If the visited array is too small to hold the entire result set, it will be filled as far as possible from the start position toward the end position.

{
    dtAssert(m_nav);
    dtAssert(m_tinyNodePool);

    *visitedCount = 0;
    
    // Validate input
    if (!startRef)
        return DT_FAILURE | DT_INVALID_PARAM;
    if (!m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    dtStatus status = DT_SUCCESS;
    
    static const int MAX_STACK = 48;
    dtNode* stack[MAX_STACK];
    int nstack = 0;
    
    m_tinyNodePool->clear();
    
    dtNode* startNode = m_tinyNodePool->getNode(startRef);
    startNode->pidx = 0;
    startNode->cost = 0;
    startNode->total = 0;
    startNode->id = startRef;
    startNode->flags = DT_NODE_CLOSED;
    stack[nstack++] = startNode;
    
    float bestPos[3];
    float bestDist = FLT_MAX;
    dtNode* bestNode = 0;
    dtVcopy(bestPos, startPos);
    
    // Search constraints
    float searchPos[3], searchRadSqr;
    dtVlerp(searchPos, startPos, endPos, 0.5f);
    searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);
    
    float verts[DT_VERTS_PER_POLYGON*3];
    
    while (nstack)
    {
        // Pop front.
        dtNode* curNode = stack[0];
        for (int i = 0; i < nstack-1; ++i)
            stack[i] = stack[i+1];
        nstack--;
        
        // Get poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef curRef = curNode->id;
        const dtMeshTile* curTile = 0;
        const dtPoly* curPoly = 0;
        m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);           
        
        // Collect vertices.
        const int nverts = curPoly->vertCount;
        for (int i = 0; i < nverts; ++i)
            dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]);
        
        // If target is inside the poly, stop search.
        if (dtPointInPolygon(endPos, verts, nverts))
        {
            bestNode = curNode;
            dtVcopy(bestPos, endPos);
            break;
        }
        
        // Find wall edges and find nearest point inside the walls.
        for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++)
        {
            // Find links to neighbours.
            static const int MAX_NEIS = 8;
            int nneis = 0;
            dtPolyRef neis[MAX_NEIS];
            
            if (curPoly->neis[j] & DT_EXT_LINK)
            {
                // Tile border.
                for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
                {
                    const dtLink* link = &curTile->links[k];
                    if (link->edge == j)
                    {
                        if (link->ref != 0)
                        {
                            const dtMeshTile* neiTile = 0;
                            const dtPoly* neiPoly = 0;
                            m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
                            if (filter->passFilter(link->ref, neiTile, neiPoly))
                            {
                                if (nneis < MAX_NEIS)
                                    neis[nneis++] = link->ref;
                            }
                        }
                    }
                }
            }
            else if (curPoly->neis[j])
            {
                const unsigned int idx = (unsigned int)(curPoly->neis[j]-1);
                const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx;
                if (filter->passFilter(ref, curTile, &curTile->polys[idx]))
                {
                    // Internal edge, encode id.
                    neis[nneis++] = ref;
                }
            }
            
            if (!nneis)
            {
                // Wall edge, calc distance.
                const float* vj = &verts[j*3];
                const float* vi = &verts[i*3];
                float tseg;
                const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg);
                if (distSqr < bestDist)
                {
                    // Update nearest distance.
                    dtVlerp(bestPos, vj,vi, tseg);
                    bestDist = distSqr;
                    bestNode = curNode;
                }
            }
            else
            {
                for (int k = 0; k < nneis; ++k)
                {
                    // Skip if no node can be allocated.
                    dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]);
                    if (!neighbourNode)
                        continue;
                    // Skip if already visited.
                    if (neighbourNode->flags & DT_NODE_CLOSED)
                        continue;
                    
                    // Skip the link if it is too far from search constraint.
                    // TODO: Maybe should use getPortalPoints(), but this one is way faster.
                    const float* vj = &verts[j*3];
                    const float* vi = &verts[i*3];
                    float tseg;
                    float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg);
                    if (distSqr > searchRadSqr)
                        continue;
                    
                    // Mark as the node as visited and push to queue.
                    if (nstack < MAX_STACK)
                    {
                        neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
                        neighbourNode->flags |= DT_NODE_CLOSED;
                        stack[nstack++] = neighbourNode;
                    }
                }
            }
        }
    }
    
    int n = 0;
    if (bestNode)
    {
        // Reverse the path.
        dtNode* prev = 0;
        dtNode* node = bestNode;
        do
        {
            dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx);
            node->pidx = m_tinyNodePool->getNodeIdx(prev);
            prev = node;
            node = next;
        }
        while (node);
        
        // Store result
        node = prev;
        do
        {
            visited[n++] = node->id;
            if (n >= maxVisitedSize)
            {
                status |= DT_BUFFER_TOO_SMALL;
                break;
            }
            node = m_tinyNodePool->getNodeAtIdx(node->pidx);
        }
        while (node);
    }
    
    dtVcopy(resultPos, bestPos);
    
    *visitedCount = n;
    
    return status;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::queryPolygons	(	const float *	center,
		const float *	extents,
		const dtQueryFilter *	filter,
		dtPolyRef *	polys,
		int *	polyCount,
		const int	maxPolys
	)		const

Finds polygons that overlap the search box.

Parameters

[in]	center	The center of the search box. [(x, y, z)]
[in]	extents	The search distance along each axis. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[out]	polys	The reference ids of the polygons that overlap the query box.
[out]	polyCount	The number of polygons in the search result.
[in]	maxPolys	The maximum number of polygons the search result can hold.

Returns

The status flags for the query.

If no polygons are found, the function will return DT_SUCCESS with a polyCount of zero.

If polys is too small to hold the entire result set, then the array will be filled to capacity. The method of choosing which polygons from the full set are included in the partial result set is undefined.

{
    dtAssert(m_nav);
    
    float bmin[3], bmax[3];
    dtVsub(bmin, center, extents);
    dtVadd(bmax, center, extents);
    
    // Find tiles the query touches.
    int minx, miny, maxx, maxy;
    m_nav->calcTileLoc(bmin, &minx, &miny);
    m_nav->calcTileLoc(bmax, &maxx, &maxy);

    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)
        {
            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;
                }
            }
        }
    }
    *polyCount = n;
    
    return DT_SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

int dtNavMeshQuery::queryPolygonsInTile ( const dtMeshTile *  tile, const float *  qmin, const float *  qmax, const dtQueryFilter *  filter, dtPolyRef *  polys, const int  maxPolys  ) const

private

Queries polygons within a tile.

{
    dtAssert(m_nav);

    if (tile->bvTree)
    {
        const dtBVNode* node = &tile->bvTree[0];
        const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
        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.
        float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
        float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
        float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
        float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
        float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
        float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
        // Quantize
        bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
        bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
        bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
        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;
                }
            }
            
            if (overlap || isLeafNode)
                node++;
            else
            {
                const int escapeIndex = -node->i;
                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];
            // Do not return off-mesh connection polygons.
            if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
                continue;
            // Must pass filter
            const dtPolyRef ref = base | (dtPolyRef)i;
            if (!filter->passFilter(ref, tile, p))
                continue;
            // Calc polygon bounds.
            const float* v = &tile->verts[p->verts[0]*3];
            dtVcopy(bmin, v);
            dtVcopy(bmax, v);
            for (int j = 1; j < p->vertCount; ++j)
            {
                v = &tile->verts[p->verts[j]*3];
                dtVmin(bmin, v);
                dtVmax(bmax, v);
            }
            if (dtOverlapBounds(qmin,qmax, bmin,bmax))
            {
                if (n < maxPolys)
                    polys[n++] = ref;
            }
        }
        return n;
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::raycast	(	dtPolyRef	startRef,
		const float *	startPos,
		const float *	endPos,
		const dtQueryFilter *	filter,
		float *	t,
		float *	hitNormal,
		dtPolyRef *	path,
		int *	pathCount,
		const int	maxPath
	)		const

Casts a 'walkability' ray along the surface of the navigation mesh from the start position toward the end position.

Note

A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility.

Parameters

[in]	startRef	The reference id of the start polygon.
[in]	startPos	A position within the start polygon representing the start of the ray. [(x, y, z)]
[in]	endPos	The position to cast the ray toward. [(x, y, z)]
[out]	t	The hit parameter. (FLT_MAX if no wall hit.)
[out]	hitNormal	The normal of the nearest wall hit. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[out]	path	The reference ids of the visited polygons. [opt]
[out]	pathCount	The number of visited polygons. [opt]
[in]	maxPath	The maximum number of polygons the path array can hold.

Returns

The status flags for the query.

This method is meant to be used for quick, short distance checks.

If the path array is too small to hold the result, it will be filled as far as possible from the start postion toward the end position.

Using the Hit Parameter (t)

If the hit parameter is a very high value (FLT_MAX), then the ray has hit the end position. In this case the path represents a valid corridor to the end position and the value of hitNormal is undefined.

If the hit parameter is zero, then the start position is on the wall that was hit and the value of hitNormal is undefined.

If 0 < t < 1.0 then the following applies:

distanceToHitBorder = distanceToEndPosition * t
hitPoint = startPos + (endPos - startPos) * t

Use Case Restriction

The raycast ignores the y-value of the end position. (2D check.) This places significant limits on how it can be used. For example:

Consider a scene where there is a main floor with a second floor balcony that hangs over the main floor. So the first floor mesh extends below the balcony mesh. The start position is somewhere on the first floor. The end position is on the balcony.

The raycast will search toward the end position along the first floor mesh. If it reaches the end position's xz-coordinates it will indicate FLT_MAX (no wall hit), meaning it reached the end position. This is one example of why this method is meant for short distance checks.

{
    dtRaycastHit hit;
    hit.path = path;
    hit.maxPath = maxPath;

    dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit);
    
    *t = hit.t;
    if (hitNormal)
        dtVcopy(hitNormal, hit.hitNormal);
    if (pathCount)
        *pathCount = hit.pathCount;

    return status;
}

Here is the call graph for this function:

Here is the caller graph for this function:

dtStatus dtNavMeshQuery::raycast	(	dtPolyRef	startRef,
		const float *	startPos,
		const float *	endPos,
		const dtQueryFilter *	filter,
		const unsigned int	options,
		dtRaycastHit *	hit,
		dtPolyRef	prevRef = 0
	)		const

Casts a 'walkability' ray along the surface of the navigation mesh from the start position toward the end position.

Parameters

[in]	startRef	The reference id of the start polygon.
[in]	startPos	A position within the start polygon representing the start of the ray. [(x, y, z)]
[in]	endPos	The position to cast the ray toward. [(x, y, z)]
[in]	filter	The polygon filter to apply to the query.
[in]	flags	govern how the raycast behaves. See dtRaycastOptions
[out]	hit	Pointer to a raycast hit structure which will be filled by the results.
[in]	prevRef	parent of start ref. Used during for cost calculation [opt]

Returns

The status flags for the query.

This method is meant to be used for quick, short distance checks.

If the path array is too small to hold the result, it will be filled as far as possible from the start postion toward the end position.

Using the Hit Parameter t of RaycastHit

If the hit parameter is a very high value (FLT_MAX), then the ray has hit the end position. In this case the path represents a valid corridor to the end position and the value of hitNormal is undefined.

If the hit parameter is zero, then the start position is on the wall that was hit and the value of hitNormal is undefined.

If 0 < t < 1.0 then the following applies:

distanceToHitBorder = distanceToEndPosition * t
hitPoint = startPos + (endPos - startPos) * t

Use Case Restriction

The raycast ignores the y-value of the end position. (2D check.) This places significant limits on how it can be used. For example:

Consider a scene where there is a main floor with a second floor balcony that hangs over the main floor. So the first floor mesh extends below the balcony mesh. The start position is somewhere on the first floor. The end position is on the balcony.

The raycast will search toward the end position along the first floor mesh. If it reaches the end position's xz-coordinates it will indicate FLT_MAX (no wall hit), meaning it reached the end position. This is one example of why this method is meant for short distance checks.

{
    dtAssert(m_nav);
    
    hit->t = 0;
    hit->pathCount = 0;
    hit->pathCost = 0;

    // Validate input
    if (!startRef || !m_nav->isValidPolyRef(startRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    if (prevRef && !m_nav->isValidPolyRef(prevRef))
        return DT_FAILURE | DT_INVALID_PARAM;
    
    float dir[3], curPos[3], lastPos[3];
    float verts[DT_VERTS_PER_POLYGON*3+3];  
    int n = 0;

    dtVcopy(curPos, startPos);
    dtVsub(dir, endPos, startPos);
    dtVset(hit->hitNormal, 0, 0, 0);

    dtStatus status = DT_SUCCESS;

    const dtMeshTile* prevTile, *tile, *nextTile;
    const dtPoly* prevPoly, *poly, *nextPoly;
    dtPolyRef curRef, nextRef;

    // The API input has been checked already, skip checking internal data.
    nextRef = curRef = startRef;
    tile = 0;
    poly = 0;
    m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
    nextTile = prevTile = tile;
    nextPoly = prevPoly = poly;
    if (prevRef)
        m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly);

    while (curRef)
    {
        // Cast ray against current polygon.
        
        // Collect vertices.
        int nv = 0;
        for (int i = 0; i < (int)poly->vertCount; ++i)
        {
            dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
            nv++;
        }
        
        float tmin, tmax;
        int segMin, segMax;
        if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
        {
            // Could not hit the polygon, keep the old t and report hit.
            hit->pathCount = n;
            return status;
        }
        // Keep track of furthest t so far.
        if (tmax > hit->t)
            hit->t = tmax;
        
        // Store visited polygons.
        if (n < hit->maxPath)
            hit->path[n++] = curRef;
        else
            status |= DT_BUFFER_TOO_SMALL;

        // Ray end is completely inside the polygon.
        if (segMax == -1)
        {
            hit->t = FLT_MAX;
            hit->pathCount = n;
            
            // add the cost
            if (options & DT_RAYCAST_USE_COSTS)
                hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
            return status;
        }

        // Follow neighbours.
        nextRef = 0;
        
        for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
        {
            const dtLink* link = &tile->links[i];
            
            // Find link which contains this edge.
            if ((int)link->edge != segMax)
                continue;
            
            // Get pointer to the next polygon.
            nextTile = 0;
            nextPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
            
            // Skip off-mesh connections.
            if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
                continue;
            
            // Skip links based on filter.
            if (!filter->passFilter(link->ref, nextTile, nextPoly))
                continue;
            
            // If the link is internal, just return the ref.
            if (link->side == 0xff)
            {
                nextRef = link->ref;
                break;
            }
            
            // If the link is at tile boundary,
            
            // Check if the link spans the whole edge, and accept.
            if (link->bmin == 0 && link->bmax == 255)
            {
                nextRef = link->ref;
                break;
            }
            
            // Check for partial edge links.
            const int v0 = poly->verts[link->edge];
            const int v1 = poly->verts[(link->edge+1) % poly->vertCount];
            const float* left = &tile->verts[v0*3];
            const float* right = &tile->verts[v1*3];
            
            // Check that the intersection lies inside the link portal.
            if (link->side == 0 || link->side == 4)
            {
                // Calculate link size.
                const float s = 1.0f/255.0f;
                float lmin = left[2] + (right[2] - left[2])*(link->bmin*s);
                float lmax = left[2] + (right[2] - left[2])*(link->bmax*s);
                if (lmin > lmax) dtSwap(lmin, lmax);
                
                // Find Z intersection.
                float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
                if (z >= lmin && z <= lmax)
                {
                    nextRef = link->ref;
                    break;
                }
            }
            else if (link->side == 2 || link->side == 6)
            {
                // Calculate link size.
                const float s = 1.0f/255.0f;
                float lmin = left[0] + (right[0] - left[0])*(link->bmin*s);
                float lmax = left[0] + (right[0] - left[0])*(link->bmax*s);
                if (lmin > lmax) dtSwap(lmin, lmax);
                
                // Find X intersection.
                float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
                if (x >= lmin && x <= lmax)
                {
                    nextRef = link->ref;
                    break;
                }
            }
        }
        
        // add the cost
        if (options & DT_RAYCAST_USE_COSTS)
        {
            // compute the intersection point at the furthest end of the polygon
            // and correct the height (since the raycast moves in 2d)
            dtVcopy(lastPos, curPos);
            dtVmad(curPos, startPos, dir, hit->t);
            float* e1 = &verts[segMax*3];
            float* e2 = &verts[((segMax+1)%nv)*3];
            float eDir[3], diff[3];
            dtVsub(eDir, e2, e1);
            dtVsub(diff, curPos, e1);
            float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
            curPos[1] = e1[1] + eDir[1] * s;

            hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
        }

        if (!nextRef)
        {
            // No neighbour, we hit a wall.
            
            // Calculate hit normal.
            const int a = segMax;
            const int b = segMax+1 < nv ? segMax+1 : 0;
            const float* va = &verts[a*3];
            const float* vb = &verts[b*3];
            const float dx = vb[0] - va[0];
            const float dz = vb[2] - va[2];
            hit->hitNormal[0] = dz;
            hit->hitNormal[1] = 0;
            hit->hitNormal[2] = -dx;
            dtVnormalize(hit->hitNormal);
            
            hit->pathCount = n;
            return status;
        }

        // No hit, advance to neighbour polygon.
        prevRef = curRef;
        curRef = nextRef;
        prevTile = tile;
        tile = nextTile;
        prevPoly = poly;
        poly = nextPoly;
    }
    
    hit->pathCount = n;
    
    return status;
}

Here is the call graph for this function:

dtStatus dtNavMeshQuery::updateSlicedFindPath	(	const int	maxIter,
		int *	doneIters
	)

Updates an in-progress sliced path query.

Parameters

[in]	maxIter	The maximum number of iterations to perform.
[out]	doneIters	The actual number of iterations completed. [opt]

Returns

The status flags for the query.

{
    if (!dtStatusInProgress(m_query.status))
        return m_query.status;

    // Make sure the request is still valid.
    if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef))
    {
        m_query.status = DT_FAILURE;
        return DT_FAILURE;
    }

    dtRaycastHit rayHit;
    rayHit.maxPath = 0;
        
    int iter = 0;
    while (iter < maxIter && !m_openList->empty())
    {
        iter++;
        
        // Remove node from open list and put it in closed list.
        dtNode* bestNode = m_openList->pop();
        bestNode->flags &= ~DT_NODE_OPEN;
        bestNode->flags |= DT_NODE_CLOSED;
        
        // Reached the goal, stop searching.
        if (bestNode->id == m_query.endRef)
        {
            m_query.lastBestNode = bestNode;
            const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
            m_query.status = DT_SUCCESS | details;
            if (doneIters)
                *doneIters = iter;
            return m_query.status;
        }
        
        // Get current poly and tile.
        // The API input has been cheked already, skip checking internal data.
        const dtPolyRef bestRef = bestNode->id;
        const dtMeshTile* bestTile = 0;
        const dtPoly* bestPoly = 0;
        if (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly)))
        {
            // The polygon has disappeared during the sliced query, fail.
            m_query.status = DT_FAILURE;
            if (doneIters)
                *doneIters = iter;
            return m_query.status;
        }
        
        // Get parent and grand parent poly and tile.
        dtPolyRef parentRef = 0, grandpaRef = 0;
        const dtMeshTile* parentTile = 0;
        const dtPoly* parentPoly = 0;
        dtNode* parentNode = 0;
        if (bestNode->pidx)
        {
            parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
            parentRef = parentNode->id;
            if (parentNode->pidx)
                grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
        }
        if (parentRef)
        {
            bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly));
            if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) )
            {
                // The polygon has disappeared during the sliced query, fail.
                m_query.status = DT_FAILURE;
                if (doneIters)
                    *doneIters = iter;
                return m_query.status;
            }
        }

        // decide whether to test raycast to previous nodes
        bool tryLOS = false;
        if (m_query.options & DT_FINDPATH_ANY_ANGLE)
        {
            if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr))
                tryLOS = true;
        }
        
        for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
        {
            dtPolyRef neighbourRef = bestTile->links[i].ref;
            
            // Skip invalid ids and do not expand back to where we came from.
            if (!neighbourRef || neighbourRef == parentRef)
                continue;
            
            // Get neighbour poly and tile.
            // The API input has been cheked already, skip checking internal data.
            const dtMeshTile* neighbourTile = 0;
            const dtPoly* neighbourPoly = 0;
            m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);         
            
            if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
                continue;
            
            // get the neighbor node
            dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0);
            if (!neighbourNode)
            {
                m_query.status |= DT_OUT_OF_NODES;
                continue;
            }
            
            // do not expand to nodes that were already visited from the same parent
            if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx)
                continue;

            // If the node is visited the first time, calculate node position.
            if (neighbourNode->flags == 0)
            {
                getEdgeMidPoint(bestRef, bestPoly, bestTile,
                                neighbourRef, neighbourPoly, neighbourTile,
                                neighbourNode->pos);
            }
            
            // Calculate cost and heuristic.
            float cost = 0;
            float heuristic = 0;
            
            // raycast parent
            bool foundShortCut = false;
            rayHit.pathCost = rayHit.t = 0;
            if (tryLOS)
            {
                raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
                foundShortCut = rayHit.t >= 1.0f;
            }

            // update move cost
            if (foundShortCut)
            {
                // shortcut found using raycast. Using shorter cost instead
                cost = parentNode->cost + rayHit.pathCost;
            }
            else
            {
                // No shortcut found.
                const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
                                                              parentRef, parentTile, parentPoly,
                                                            bestRef, bestTile, bestPoly,
                                                            neighbourRef, neighbourTile, neighbourPoly);
                cost = bestNode->cost + curCost;
            }

            // Special case for last node.
            if (neighbourRef == m_query.endRef)
            {
                const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
                                                              bestRef, bestTile, bestPoly,
                                                              neighbourRef, neighbourTile, neighbourPoly,
                                                              0, 0, 0);
                
                cost = cost + endCost;
                heuristic = 0;
            }
            else
            {
                heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
            }
            
            const float total = cost + heuristic;
            
            // The node is already in open list and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
                continue;
            // The node is already visited and process, and the new result is worse, skip.
            if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
                continue;
            
            // Add or update the node.
            neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode);
            neighbourNode->id = neighbourRef;
            neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED));
            neighbourNode->cost = cost;
            neighbourNode->total = total;
            if (foundShortCut)
                neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED);
            
            if (neighbourNode->flags & DT_NODE_OPEN)
            {
                // Already in open, update node location.
                m_openList->modify(neighbourNode);
            }
            else
            {
                // Put the node in open list.
                neighbourNode->flags |= DT_NODE_OPEN;
                m_openList->push(neighbourNode);
            }
            
            // Update nearest node to target so far.
            if (heuristic < m_query.lastBestNodeCost)
            {
                m_query.lastBestNodeCost = heuristic;
                m_query.lastBestNode = neighbourNode;
            }
        }
    }
    
    // Exhausted all nodes, but could not find path.
    if (m_openList->empty())
    {
        const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
        m_query.status = DT_SUCCESS | details;
    }

    if (doneIters)
        *doneIters = iter;

    return m_query.status;
}

Here is the call graph for this function:

Member Data Documentation

const dtNavMesh* dtNavMeshQuery::m_nav

private

Pointer to navmesh data.

class dtNodePool* dtNavMeshQuery::m_nodePool

private

Pointer to node pool.

class dtNodeQueue* dtNavMeshQuery::m_openList

private

Pointer to open list queue.

dtQueryData dtNavMeshQuery::m_query

private

Sliced query state.

class dtNodePool* dtNavMeshQuery::m_tinyNodePool

private

Pointer to small node pool.

---

The documentation for this class was generated from the following files:

• dep/recastnavigation/Detour/Include/DetourNavMeshQuery.h
• dep/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp