diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index ae8cec9..1060845 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -134,7 +134,7 @@ enum dtRaycastOptions /// Limit raycasting during any angle pahfinding /// The limit is given as a multiple of the character radius -static const float RAY_CAST_LIMIT_PROPORTIONS = 50.0f; +static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f; /// Flags representing the type of a navigation mesh polygon. enum dtPolyTypes diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 848ea27..c7b360d 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -124,7 +124,7 @@ public: /// Provides information about raycast hit /// filled by dtNavMeshQuery::raycast /// @ingroup detour -struct RaycastHit +struct dtRaycastHit { /// The hit parameter. (FLT_MAX if no wall hit.) float t; @@ -132,7 +132,7 @@ struct RaycastHit /// hitNormal The normal of the nearest wall hit. [(x, y, z)] float hitNormal[3]; - /// The reference ids of the visited polygons. [opt] + /// Pointer to an array of reference ids of the visited polygons. [opt] dtPolyRef* path; /// The number of visited polygons. [opt] @@ -215,7 +215,7 @@ public: /// @returns The status flags for the query. dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options=0/*DT_FINDPATH_ANY_ANGLE*/); + 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. @@ -365,12 +365,12 @@ public: /// @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 The raycast hit structure. + /// @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, - RaycastHit* hit, dtPolyRef prevRef=0) const; + dtRaycastHit* hit, dtPolyRef prevRef = 0) const; /// Finds the distance from the specified position to the nearest polygon wall. diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index 9094ff2..6fefdc8 100644 --- a/Detour/Include/DetourNode.h +++ b/Detour/Include/DetourNode.h @@ -25,7 +25,7 @@ enum dtNodeFlags { DT_NODE_OPEN = 0x01, DT_NODE_CLOSED = 0x02, - DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not connected/adjacent. Found using raycast. + DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast. }; typedef unsigned short dtNodeIndex; @@ -37,12 +37,15 @@ struct dtNode float cost; ///< Cost from previous node to current node. float total; ///< Cost up to the node. 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. - unsigned int flags : 3; ///< Node flags 0/open/closed. + 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 { @@ -89,7 +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;} + inline int getNodeCount() const { return m_nodeCount; } private: diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index cbec28a..64e8383 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1159,7 +1159,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef dtVcopy(m_query.endPos, endPos); m_query.filter = filter; m_query.options = options; - m_query.raycastLimitSqr = 1E37; + m_query.raycastLimitSqr = FLT_MAX; if (!startRef || !endRef) return DT_FAILURE | DT_INVALID_PARAM; @@ -1175,7 +1175,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef // 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 * RAY_CAST_LIMIT_PROPORTIONS); + m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS); } if (startRef == endRef) @@ -1215,7 +1215,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) return DT_FAILURE; } - RaycastHit rayHit; + dtRaycastHit rayHit; rayHit.maxPath = 0; int iter = 0; @@ -1268,7 +1268,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) if (parentRef) { bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); - if (invalidParent || (grandpaRef && dtStatusFailed(m_nav->isValidPolyRef(grandpaRef))) ) + if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) ) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; @@ -1332,15 +1332,23 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) float heuristic = 0; // raycast parent - rayHit.t = 0; + 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); - cost = parentNode->cost + rayHit.pathCost; + foundShortCut = rayHit.t >= 1.0f; } - if (rayHit.t < 1.0f) // hit + // 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, @@ -1374,12 +1382,12 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) continue; // Add or update the node. - neighbourNode->pidx = rayHit.t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; + 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 (rayHit.t >= 1.0f) + if (foundShortCut) neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); if (neighbourNode->flags & DT_NODE_OPEN) @@ -2301,7 +2309,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons const dtQueryFilter* filter, float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const { - RaycastHit hit; + dtRaycastHit hit; hit.path = path; hit.maxPath = maxPath; @@ -2357,7 +2365,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons /// dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, const unsigned int options, - RaycastHit* hit, dtPolyRef prevRef) const + dtRaycastHit* hit, dtPolyRef prevRef) const { dtAssert(m_nav); @@ -3524,8 +3532,8 @@ bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const { if (!m_nodePool) return false; - dtNode* nodes[4]; - int n= m_nodePool->findNodes(ref, nodes, 4); + dtNode* nodes[DT_MAX_STATES_PER_NODE]; + int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); for (int i=0; iinitSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]); m_navquery->updateSlicedFindPath(MAX_ITER, 0);