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

Merge pull request #17 from axelrodR/master

Browse Source 
improving path quality on tiled meshes: multiple issues
This commit is contained in:
Mikko Mononen 2014-06-20 13:19:50 +03:00
commit eacaa87d9a
7 changed files with 390 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
Detour/Include/DetourNavMesh.h
View File

@ -117,6 +117,25 @@ enum dtStraightPathOptions
DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, ///< Add a vertex at every polygon edge crossing.
};
/// Options for dtNavMeshQuery::findPath
enum dtFindPathOptions
{
DT_FINDPATH_LOW_QUALITY_FAR = 0x01, ///< [provisional] trade quality for performance far from the origin. The idea is that by then a new query will be issued
DT_FINDPATH_ANY_ANGLE = 0x02, ///< use raycasts during pathfind to "shortcut" (raycast still consider costs)
};
/// Options for dtNavMeshQuery::raycast
enum dtRaycastOptions
{
DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost
};
/// Limit raycasting during any angle pahfinding
/// The limit is given as a multiple of the character radius
static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f;
/// Flags representing the type of a navigation mesh polygon.
enum dtPolyTypes
{

50
Detour/Include/DetourNavMeshQuery.h
View File

@ -119,6 +119,34 @@ public:
};
/// Provides information about raycast hit
/// filled by dtNavMeshQuery::raycast
/// @ingroup detour
struct dtRaycastHit
{
/// The hit parameter. (FLT_MAX if no wall hit.)
float t;
/// hitNormal The normal of the nearest wall hit. [(x, y, z)]
float hitNormal[3];
/// Pointer to an array of reference ids of the visited polygons. [opt]
dtPolyRef* path;
/// The number of visited polygons. [opt]
int pathCount;
/// The maximum number of polygons the @p path array can hold.
int maxPath;
/// The cost of the path until hit.
float pathCost;
};
/// Provides the ability to perform pathfinding related queries against
/// a navigation mesh.
/// @ingroup detour
@ -183,10 +211,11 @@ public:
/// @param[in] startPos A position within the start polygon. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] options query options (see: #dtFindPathOptions)
/// @returns The status flags for the query.
dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
const float* startPos, const float* endPos,
const dtQueryFilter* filter);
const dtQueryFilter* filter, const unsigned int options = 0);
/// Updates an in-progress sliced path query.
/// @param[in] maxIter The maximum number of iterations to perform.
@ -312,6 +341,7 @@ public:
/// 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.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
@ -327,6 +357,22 @@ public:
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.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
/// @param[in] endPos The position to cast the ray toward. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] flags govern how the raycast behaves. See dtRaycastOptions
/// @param[out] hit Pointer to a raycast hit structure which will be filled by the results.
/// @param[in] prevRef parent of start ref. Used during for cost calculation [opt]
/// @returns The status flags for the query.
dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
const dtQueryFilter* filter, const unsigned int options,
dtRaycastHit* hit, dtPolyRef prevRef = 0) const;
/// Finds the distance from the specified position to the nearest polygon wall.
/// @param[in] startRef The reference id of the polygon containing @p centerPos.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
@ -467,6 +513,8 @@ private:
dtPolyRef startRef, endRef;
float startPos[3], endPos[3];
const dtQueryFilter* filter;
unsigned int options;
float raycastLimitSqr;
};
dtQueryData m_query; ///< Sliced query state.

19
Detour/Include/DetourNode.h
View File

@ -25,6 +25,7 @@ enum dtNodeFlags
{
DT_NODE_OPEN = 0x01,
DT_NODE_CLOSED = 0x02,
DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast.
};
typedef unsigned short dtNodeIndex;
@ -35,12 +36,17 @@ struct dtNode
float pos[3]; ///< Position of the node.
float cost; ///< Cost from previous node to current node.
float total; ///< Cost up to the node.
unsigned int pidx : 30; ///< Index to parent node.
unsigned int flags : 2; ///< Node flags 0/open/closed.
unsigned int pidx : 24; ///< Index to parent node.
unsigned int state : 2; ///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE
unsigned int flags : 3; ///< Node flags. A combination of dtNodeFlags.
dtPolyRef id; ///< Polygon ref the node corresponds to.
};
static const int DT_MAX_STATES_PER_NODE = 4; // number of extra states per node. See dtNode::state
class dtNodePool
{
public:
@ -48,8 +54,12 @@ public:
~dtNodePool();
inline void operator=(const dtNodePool&) {}
void clear();
dtNode* getNode(dtPolyRef id);
dtNode* findNode(dtPolyRef id);
// Get a dtNode by ref and extra state information. If there is none then - allocate
// There can be more than one node for the same polyRef but with different extra state information
dtNode* getNode(dtPolyRef id, unsigned char state=0);
dtNode* findNode(dtPolyRef id, unsigned char state);
unsigned int findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes);
inline unsigned int getNodeIdx(const dtNode* node) const
{
@ -82,6 +92,7 @@ public:
inline int getHashSize() const { return m_hashSize; }
inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; }
inline dtNodeIndex getNext(int i) const { return m_next[i]; }
inline int getNodeCount() const { return m_nodeCount; }
private:

334
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -1010,7 +1010,13 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
// 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;
@ -1138,7 +1144,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
///
dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
const float* startPos, const float* endPos,
const dtQueryFilter* filter)
const dtQueryFilter* filter, const unsigned int options)
{
dtAssert(m_nav);
dtAssert(m_nodePool);
@ -1152,6 +1158,8 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef 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;
@ -1160,6 +1168,16 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
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;
@ -1197,6 +1215,9 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
return DT_FAILURE;
}
dtRaycastHit rayHit;
rayHit.maxPath = 0;
int iter = 0;
while (iter < maxIter && !m_openList->empty())
{
@ -1232,15 +1253,22 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
return m_query.status;
}
// Get parent poly and tile.
dtPolyRef parentRef = 0;
// 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)
parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
{
parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
parentRef = parentNode->id;
if (parentNode->pidx)
grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
}
if (parentRef)
{
if (dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)))
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;
@ -1250,6 +1278,14 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
}
}
// 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;
@ -1267,13 +1303,22 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
// deal explicitly with crossing tile boundaries
unsigned char crossSide = 0;
if (bestTile->links[i].side != 0xff)
crossSide = bestTile->links[i].side >> 1;
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
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)
{
@ -1286,30 +1331,44 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
float cost = 0;
float heuristic = 0;
// Special case for last node.
if (neighbourRef == m_query.endRef)
// raycast parent
bool foundShortCut = false;
rayHit.pathCost = rayHit.t = 0;
if (tryLOS)
{
// Cost
const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly,
0, 0, 0);
raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
foundShortCut = rayHit.t >= 1.0f;
}
cost = bestNode->cost + curCost + endCost;
heuristic = 0;
// update move cost
if (foundShortCut)
{
// shortcut found using raycast. Using shorter cost instead
cost = parentNode->cost + rayHit.pathCost;
}
else
{
// Cost
// 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;
}
@ -1323,11 +1382,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
continue;
// Add or update the node.
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode);
neighbourNode->id = neighbourRef;
neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
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)
{
@ -1391,11 +1452,15 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
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);
@ -1403,14 +1468,32 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
// 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 |= DT_BUFFER_TOO_SMALL;
m_query.status |= status & DT_STATUS_DETAIL_MASK;
break;
}
node = m_nodePool->getNodeAtIdx(node->pidx);
node = next;
}
while (node);
}
@ -1456,7 +1539,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
dtNode* node = 0;
for (int i = existingSize-1; i >= 0; --i)
{
node = m_nodePool->findNode(existing[i]);
m_nodePool->findNodes(existing[i], &node, 1);
if (node)
break;
}
@ -1469,11 +1552,15 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
}
// 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);
@ -1481,14 +1568,32 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
// 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 |= DT_BUFFER_TOO_SMALL;
m_query.status |= status & DT_STATUS_DETAIL_MASK;
break;
}
node = m_nodePool->getNodeAtIdx(node->pidx);
node = next;
}
while (node);
}
@ -2160,6 +2265,8 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
return DT_SUCCESS;
}
/// @par
///
/// This method is meant to be used for quick, short distance checks.
@ -2201,36 +2308,105 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
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
{
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;
}
/// @par
///
/// 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.
///
/// <b>Using the Hit Parameter t of RaycastHit</b>
///
/// 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 @p hitNormal is undefined.
///
/// If the hit parameter is zero, then the start position is on the wall that
/// was hit and the value of @p hitNormal is undefined.
///
/// If 0 < t < 1.0 then the following applies:
///
/// @code
/// distanceToHitBorder = distanceToEndPosition * t
/// hitPoint = startPos + (endPos - startPos) * t
/// @endcode
///
/// <b>Use Case Restriction</b>
///
/// 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.
///
dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
const dtQueryFilter* filter, const unsigned int options,
dtRaycastHit* hit, dtPolyRef prevRef) const
{
dtAssert(m_nav);
*t = 0;
if (pathCount)
*pathCount = 0;
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;
dtPolyRef curRef = startRef;
float verts[DT_VERTS_PER_POLYGON*3];
float dir[3], curPos[3], lastPos[3];
float verts[DT_VERTS_PER_POLYGON*3+3];
int n = 0;
hitNormal[0] = 0;
hitNormal[1] = 0;
hitNormal[2] = 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, &tile, &poly);
while (curRef)
{
// Cast ray against current polygon.
// The API input has been cheked already, skip checking internal data.
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
// Collect vertices.
int nv = 0;
for (int i = 0; i < (int)poly->vertCount; ++i)
@ -2244,31 +2420,33 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
{
// Could not hit the polygon, keep the old t and report hit.
if (pathCount)
*pathCount = n;
hit->pathCount = n;
return status;
}
// Keep track of furthest t so far.
if (tmax > *t)
*t = tmax;
if (tmax > hit->t)
hit->t = tmax;
// Store visited polygons.
if (n < maxPath)
path[n++] = curRef;
if (n < hit->maxPath)
hit->path[n++] = curRef;
else
status |= DT_BUFFER_TOO_SMALL;
// Ray end is completely inside the polygon.
if (segMax == -1)
{
*t = FLT_MAX;
if (pathCount)
*pathCount = n;
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.
dtPolyRef nextRef = 0;
nextRef = 0;
for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
{
@ -2279,8 +2457,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
continue;
// Get pointer to the next polygon.
const dtMeshTile* nextTile = 0;
const dtPoly* nextPoly = 0;
nextTile = 0;
nextPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
// Skip off-mesh connections.
@ -2348,6 +2526,24 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
}
}
// 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.
@ -2359,22 +2555,25 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
const float* vb = &verts[b*3];
const float dx = vb[0] - va[0];
const float dz = vb[2] - va[2];
hitNormal[0] = dz;
hitNormal[1] = 0;
hitNormal[2] = -dx;
dtVnormalize(hitNormal);
hit->hitNormal[0] = dz;
hit->hitNormal[1] = 0;
hit->hitNormal[2] = -dx;
dtVnormalize(hit->hitNormal);
if (pathCount)
*pathCount = n;
hit->pathCount = n;
return status;
}
// No hit, advance to neighbour polygon.
prevRef = curRef;
curRef = nextRef;
prevTile = tile;
tile = nextTile;
prevPoly = poly;
poly = nextPoly;
}
if (pathCount)
*pathCount = n;
hit->pathCount = n;
return status;
}
@ -3332,6 +3531,15 @@ bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter)
bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
{
if (!m_nodePool) return false;
const dtNode* node = m_nodePool->findNode(ref);
return node && node->flags & DT_NODE_CLOSED;
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;
}

26
Detour/Source/DetourNode.cpp
View File

@ -84,27 +84,46 @@ void dtNodePool::clear()
m_nodeCount = 0;
}
dtNode* dtNodePool::findNode(dtPolyRef id)
unsigned int dtNodePool::findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes)
{
int n = 0;
unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
dtNodeIndex i = m_first[bucket];
while (i != DT_NULL_IDX)
{
if (m_nodes[i].id == id)
{
if (n >= maxNodes)
return n;
nodes[n++] = &m_nodes[i];
}
i = m_next[i];
}
return n;
}
dtNode* dtNodePool::findNode(dtPolyRef id, unsigned char state)
{
unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
dtNodeIndex i = m_first[bucket];
while (i != DT_NULL_IDX)
{
if (m_nodes[i].id == id && m_nodes[i].state == state)
return &m_nodes[i];
i = m_next[i];
}
return 0;
}
dtNode* dtNodePool::getNode(dtPolyRef id)
dtNode* dtNodePool::getNode(dtPolyRef id, unsigned char state)
{
unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
dtNodeIndex i = m_first[bucket];
dtNode* node = 0;
while (i != DT_NULL_IDX)
{
if (m_nodes[i].id == id)
if (m_nodes[i].id == id && m_nodes[i].state == state)
return &m_nodes[i];
i = m_next[i];
}
@ -121,6 +140,7 @@ dtNode* dtNodePool::getNode(dtPolyRef id)
node->cost = 0;
node->total = 0;
node->id = id;
node->state = state;
node->flags = 0;
m_next[i] = m_first[bucket];

2
DetourCrowd/Source/DetourCrowd.cpp
View File

@ -701,7 +701,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
dtPolyRef reqPath[MAX_RES]; // The path to the request location
int reqPathCount = 0;
// Quick seach towards the goal.
// Quick search towards the goal.
static const int MAX_ITER = 20;
m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
m_navquery->updateSlicedFindPath(MAX_ITER, 0);

4
RecastDemo/Source/NavMeshTesterTool.cpp
View File

@ -654,7 +654,7 @@ void NavMeshTesterTool::handleUpdate(const float /*dt*/)
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,
m_straightPathPolys, &m_nstraightPath, MAX_POLYS);
m_straightPathPolys, &m_nstraightPath, MAX_POLYS, DT_STRAIGHTPATH_ALL_CROSSINGS);
}
m_pathFindStatus = DT_FAILURE;
@ -880,7 +880,7 @@ void NavMeshTesterTool::recalc()
m_npolys = 0;
m_nstraightPath = 0;
m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter);
m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, DT_FINDPATH_ANY_ANGLE);
}
else
{