From 54f1e217ded39a988aeae6ae12fc9a2e45f9ab19 Mon Sep 17 00:00:00 2001 From: Kawe Mazidjatari <48657826+Mauler125@users.noreply.github.com> Date: Sat, 31 Aug 2024 15:13:24 +0200 Subject: [PATCH] Recast: start of navmesh query modifications Start of implementing changes of Titanfall 2 and Apex into dtNavMeshQuery. --- .../recast/Detour/Include/DetourNavMesh.h | 3 + .../Detour/Include/DetourNavMeshQuery.h | 54 +++++---- .../recast/Detour/Include/DetourNode.h | 5 +- .../Detour/Source/DetourNavMeshQuery.cpp | 107 +++++++++++------- .../recast/Shared/Include/SharedCommon.h | 2 + 5 files changed, 102 insertions(+), 69 deletions(-) diff --git a/src/thirdparty/recast/Detour/Include/DetourNavMesh.h b/src/thirdparty/recast/Detour/Include/DetourNavMesh.h index 1566d190..de10dd81 100644 --- a/src/thirdparty/recast/Detour/Include/DetourNavMesh.h +++ b/src/thirdparty/recast/Detour/Include/DetourNavMesh.h @@ -300,6 +300,9 @@ inline int dtGetDetailTriEdgeFlags(unsigned char triFlags, int edgeIndex) /// @see dtMeshTile struct dtLink { + inline bool hasTraverseType() const { return traverseType != DT_NULL_TRAVERSE_TYPE; } + inline unsigned char getTraverseType() const {return traverseType & (DT_MAX_TRAVERSE_TYPES-1); } + dtPolyRef ref; ///< Neighbour reference. (The neighbor that is linked to.) unsigned int next; ///< Index of the next link. unsigned char edge; ///< Index of the polygon edge that owns this link. diff --git a/src/thirdparty/recast/Detour/Include/DetourNavMeshQuery.h b/src/thirdparty/recast/Detour/Include/DetourNavMeshQuery.h index 17e4580a..3f0cd70a 100644 --- a/src/thirdparty/recast/Detour/Include/DetourNavMeshQuery.h +++ b/src/thirdparty/recast/Detour/Include/DetourNavMeshQuery.h @@ -34,10 +34,10 @@ /// @ingroup detour class dtQueryFilter { - float m_areaCost[DT_MAX_AREAS]; ///< Cost per area type. (Used by default implementation.) - unsigned short m_includeFlags; ///< Flags for polygons that can be visited. (Used by default implementation.) - unsigned short m_excludeFlags; ///< Flags for polygons that should not be visited. (Used by default implementation.) - unsigned int m_traverseFlags; ///< Flags for links dictating which traverse types are allowed to be used. (See [r5apex_ds + CA6FE9.]) + float m_traverseCost[DT_MAX_TRAVERSE_TYPES]; ///< Cost per traverse type. (Used by default implementation.) + unsigned short m_includeFlags; ///< Flags for polygons that can be visited. (Used by default implementation.) + unsigned short m_excludeFlags; ///< Flags for polygons that should not be visited. (Used by default implementation.) + unsigned int m_traverseFlags; ///< Flags for links dictating which traverse types are allowed to be used. (See [r5apex_ds + CA6FE9.]) public: dtQueryFilter(); @@ -51,14 +51,19 @@ public: /// @param[in] tile The tile containing the polygon. /// @param[in] poly The polygon to test. #ifdef DT_VIRTUAL_QUERYFILTER - virtual bool passFilter(const dtPolyRef ref, + virtual +#endif + bool passFilter(const dtPolyRef ref, const dtMeshTile* tile, const dtPoly* poly) const; -#else - bool passFilter(const dtPolyRef ref, - const dtMeshTile* tile, - const dtPoly* poly) const; + +#ifdef DT_VIRTUAL_QUERYFILTER + virtual #endif + bool traverseFilter(const dtPolyRef ref, + const dtMeshTile* tile, + const dtPoly* poly) const; + /// Returns cost to move from the beginning to the end of a line segment /// that is fully contained within a polygon. @@ -74,16 +79,12 @@ public: /// @param[in] nextTile The tile containing the next polygon. [opt] /// @param[in] nextPoly The next polygon. [opt] #ifdef DT_VIRTUAL_QUERYFILTER - virtual float getCost(const float* pa, const float* pb, + virtual +#endif + float getCost(const float* pa, const float* pb, const dtLink* link, const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; -#else - float getCost(const float* pa, const float* pb, - const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly, - const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly, - const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const; -#endif /// @name Getters and setters for the default implementation data. ///@{ @@ -91,12 +92,12 @@ public: /// Returns the traversal cost of the area. /// @param[in] i The id of the area. /// @returns The traversal cost of the area. - inline float getAreaCost(const int i) const { return m_areaCost[i]; } + inline float getAreaCost(const int i) const { return m_traverseCost[i]; } /// Sets the traversal cost of the area. /// @param[in] i The id of the area. /// @param[in] cost The new cost of traversing the area. - inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } + inline void setAreaCost(const int i, const float cost) { m_traverseCost[i] = cost; } /// Returns the include flags for the filter. /// Any polygons that include one or more of these flags will be @@ -125,17 +126,14 @@ public: /// @ingroup detour struct dtRaycastHit { + /// Pointer to an array of reference ids of the visited polygons. [opt] + dtPolyRef* path; + /// 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]; - - /// The index of the edge on the final polygon where the wall was hit. - int hitEdgeIndex; - - /// Pointer to an array of reference ids of the visited polygons. [opt] - dtPolyRef* path; /// The number of visited polygons. [opt] int pathCount; @@ -145,6 +143,9 @@ struct dtRaycastHit /// The cost of the path until hit. float pathCost; + + /// The reference of the start polygon. + dtPolyRef startRef; }; /// Provides information about straight path generation @@ -552,6 +553,11 @@ public: /// @returns The node pool. class dtNodePool* getNodePool() const { return m_nodePool; } + /// Sets the navigation mesh the query object is using. + /// Only use this directly after init, before the query has been used! + /// @param[in] nav The navigation mesh to attach. + const void attachNavMeshUnsafe(const dtNavMesh* nav) { m_nav = nav; } + /// Gets the navigation mesh the query object is using. /// @return The navigation mesh the query object is using. const dtNavMesh* getAttachedNavMesh() const { return m_nav; } diff --git a/src/thirdparty/recast/Detour/Include/DetourNode.h b/src/thirdparty/recast/Detour/Include/DetourNode.h index ee8b155e..8407c15e 100644 --- a/src/thirdparty/recast/Detour/Include/DetourNode.h +++ b/src/thirdparty/recast/Detour/Include/DetourNode.h @@ -33,6 +33,8 @@ static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0; static const int DT_NODE_PARENT_BITS = 19; static const int DT_NODE_STATE_BITS = 2; +static const int DT_NODE_FLAGS_BITS = 3; +static const int DT_NODE_JUMPT_BITS = 8; struct dtNode { float pos[3]; ///< Position of the node. @@ -40,7 +42,8 @@ struct dtNode float total; ///< Cost up to the node. unsigned int pidx : DT_NODE_PARENT_BITS; ///< Index to parent node. unsigned int state : DT_NODE_STATE_BITS; ///< 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. + unsigned int flags : DT_NODE_FLAGS_BITS; ///< Node flags. A combination of dtNodeFlags. + unsigned int jumpt : DT_NODE_JUMPT_BITS; ///< Node jump type. dtPolyRef id; ///< Polygon ref the node corresponds to. }; diff --git a/src/thirdparty/recast/Detour/Source/DetourNavMeshQuery.cpp b/src/thirdparty/recast/Detour/Source/DetourNavMeshQuery.cpp index ff5de078..c46af84a 100644 --- a/src/thirdparty/recast/Detour/Source/DetourNavMeshQuery.cpp +++ b/src/thirdparty/recast/Detour/Source/DetourNavMeshQuery.cpp @@ -59,43 +59,54 @@ dtQueryFilter::dtQueryFilter() : m_includeFlags(0xffff), - m_excludeFlags(0) + m_excludeFlags(0), + m_traverseFlags(0) { - for (int i = 0; i < DT_MAX_AREAS; ++i) - m_areaCost[i] = 1.0f; + for (int i = 0; i < DT_MAX_TRAVERSE_TYPES; ++i) + m_traverseCost[i] = 1.0f; } -#ifdef DT_VIRTUAL_QUERYFILTER +#ifndef DT_VIRTUAL_QUERYFILTER +rdForceInline +#endif bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, - const dtMeshTile* /*tile*/, - const dtPoly* poly) const -{ - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; -} - -float dtQueryFilter::getCost(const float* pa, const float* pb, - const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, - const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, - const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const -{ - return rdVdist(pa, pb) * m_areaCost[curPoly->getArea()]; -} -#else -inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, const dtMeshTile* /*tile*/, const dtPoly* poly) const { return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; } -inline float dtQueryFilter::getCost(const float* pa, const float* pb, +#ifndef DT_VIRTUAL_QUERYFILTER +rdForceInline +#endif +bool dtQueryFilter::traverseFilter(const dtPolyRef /*ref*/, + const dtMeshTile* tile, + const dtPoly* poly) const +{ + const dtLink* link = &tile->links[poly->firstLink]; + + if (link->hasTraverseType()) + { + if (!(rdBitCellBit(link->getTraverseType()) & m_traverseFlags)) + return false; + } + + return true; +} + +#ifndef DT_VIRTUAL_QUERYFILTER +rdForceInline +#endif +float dtQueryFilter::getCost(const float* pa, const float* pb, const dtLink* link, const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, - const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, + const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* /*curPoly*/, const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const { - return rdVdist(pa, pb) * m_areaCost[curPoly->getArea()]; + if (link && link->hasTraverseType()) + return m_traverseCost[link->getTraverseType()]; + + return rdVdist(pa, pb); } -#endif static const float H_SCALE = 0.999f; // Search heuristic scale. @@ -400,6 +411,9 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f // Do not advance if the polygon is excluded by the filter. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; + + if (!filter->traverseFilter(bestRef, bestTile, bestPoly)) + continue; // Find edge and calc distance to the edge. float va[3], vb[3]; @@ -1015,14 +1029,15 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { - dtPolyRef neighbourRef = bestTile->links[i].ref; + const dtLink& bestLink = bestTile->links[i]; + dtPolyRef neighbourRef = bestLink.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. + // The API input has been checked already, skip checking internal data. const dtMeshTile* neighbourTile = 0; const dtPoly* neighbourPoly = 0; m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); @@ -1032,8 +1047,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // deal explicitly with crossing tile boundaries unsigned char crossSide = 0; - if (bestTile->links[i].side != 0xff) - crossSide = bestTile->links[i].side >> 1; + if (bestLink.side != 0xff) + crossSide = bestLink.side >> 1; // get the node dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); @@ -1059,11 +1074,11 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, if (neighbourRef == endRef) { // Cost - const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, + const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, &bestLink, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); - const float endCost = filter->getCost(neighbourNode->pos, endPos, + const float endCost = filter->getCost(neighbourNode->pos, endPos, 0, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0, 0, 0); @@ -1074,7 +1089,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, else { // Cost - const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, + const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, &bestLink, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); @@ -1332,7 +1347,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters, for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { - dtPolyRef neighbourRef = bestTile->links[i].ref; + const dtLink& bestLink = bestTile->links[i]; + dtPolyRef neighbourRef = bestLink.ref; // Skip invalid ids and do not expand back to where we came from. if (!neighbourRef || neighbourRef == parentRef) @@ -1349,8 +1365,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters, unsigned char crossSide = 0; // See https://github.com/recastnavigation/recastnavigation/issues/438 - if (bestTile->links[i].side != 0xff) - crossSide = bestTile->links[i].side >> 1; + if (bestLink.side != 0xff) + crossSide = bestLink.side >> 1; // get the neighbor node dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); @@ -1394,7 +1410,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters, else { // No shortcut found. - const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, + const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, &bestLink, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); @@ -1404,7 +1420,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters, // Special case for last node. if (neighbourRef == m_query.endRef) { - const float endCost = filter->getCost(neighbourNode->pos, m_query.endPos, + const float endCost = filter->getCost(neighbourNode->pos, m_query.endPos, 0, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0, 0, 0); @@ -2529,7 +2545,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons return status; } - hit->hitEdgeIndex = segMax; + hit->startRef = startRef; // Keep track of furthest t so far. if (tmax > hit->t) @@ -2549,7 +2565,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons // add the cost if (options & DT_RAYCAST_USE_COSTS) - hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); + hit->pathCost += filter->getCost(curPos, endPos, 0, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); return status; } @@ -2561,7 +2577,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons const dtLink* link = &tile->links[i]; // Find link which contains this edge. - if ((int)link->edge != segMax) + if ((int)link->edge != segMax || link->hasTraverseType()) continue; // Get pointer to the next polygon. @@ -2649,7 +2665,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons float s = rdSqr(eDir[0]) > rdSqr(eDir[1]) ? diff[0] / eDir[0] : diff[1] / eDir[1]; curPos[2] = e1[2] + eDir[2] * s; - hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly); + hit->pathCost += filter->getCost(lastPos, curPos, 0, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly); } if (!nextRef) @@ -2663,9 +2679,9 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons const float* vb = &verts[b*3]; const float dx = vb[0] - va[0]; const float dy = vb[1] - va[1]; - hit->hitNormal[0] = -dy; - hit->hitNormal[1] = dx; - hit->hitNormal[2] = 0; + hit->hitNormal[0] = dy; + hit->hitNormal[1] = 0.0f; + hit->hitNormal[2] = -dx; rdVnormalize(hit->hitNormal); hit->pathCount = n; @@ -2835,7 +2851,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* rdVlerp(neighbourNode->pos, va, vb, 0.5f); float cost = filter->getCost( - bestNode->pos, neighbourNode->pos, + bestNode->pos, neighbourNode->pos, link, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); @@ -3018,7 +3034,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v rdVlerp(neighbourNode->pos, va, vb, 0.5f); float cost = filter->getCost( - bestNode->pos, neighbourNode->pos, + bestNode->pos, neighbourNode->pos, link, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); @@ -3183,6 +3199,9 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* // Do not advance if the polygon is excluded by the filter. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; + + if (!filter->traverseFilter(curRef, curTile, curPoly)) + continue; // Find edge and calc distance to the edge. float va[3], vb[3]; diff --git a/src/thirdparty/recast/Shared/Include/SharedCommon.h b/src/thirdparty/recast/Shared/Include/SharedCommon.h index 014c6926..6db84b8a 100644 --- a/src/thirdparty/recast/Shared/Include/SharedCommon.h +++ b/src/thirdparty/recast/Shared/Include/SharedCommon.h @@ -21,6 +21,8 @@ #include "Shared/Include/SharedMath.h" +#define rdForceInline __forceinline + /// The total number of bits in an bit cell integer. static const int RD_BITS_PER_BIT_CELL = 32;