Recast: start of navmesh query modifications

Start of implementing changes of Titanfall 2 and Apex into dtNavMeshQuery.

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.

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.)
+	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_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 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.])
+	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,
+#ifdef DT_VIRTUAL_QUERYFILTER
-					const dtMeshTile* tile,
+	virtual
-					const dtPoly* poly) const;
 #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,18 +126,15 @@ 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; }

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.
 };
 

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)
+	for (int i = 0; i < DT_MAX_TRAVERSE_TYPES; ++i)
-		m_areaCost[i] = 1.0f;
+		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.
 
@@ -401,6 +412,9 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
 			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];
 			if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
@@ -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)
+			if (bestLink.side != 0xff)
-				crossSide = bestTile->links[i].side >> 1;
+				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)
+			if (bestLink.side != 0xff)
-				crossSide = bestTile->links[i].side >> 1;
+				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[0] = dy;
-			hit->hitNormal[1] = dx;
+			hit->hitNormal[1] = 0.0f;
-			hit->hitNormal[2] = 0;
+			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);
@@ -3184,6 +3200,9 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 			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];
 			if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))

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;