From 0f2b03a8c0cc30e143bde1697789c753df2b3e38 Mon Sep 17 00:00:00 2001
From: JimmyJames707 <RATIODE@gmail.com>
Date: Fri, 11 Apr 2014 16:50:57 +1000
Subject: [PATCH] I would like to submit a query filter for each agent type:

https://groups.google.com/forum/#!searchin/recastnavigation/dtQueryFilter|sort:relevance|spell:false/recastnavigation/t-rFg2Ku9IY/PJNNkCjARccJ

Search on
queryFilterType and m_filters to see the changes.

Also in this code there are bugfixes, that have already been submitted
as issues. They can be ignored.

Possible bug with requestMoveVelocity and navmesh regeneration
https://groups.google.com/forum/#!topic/recastnavigation/E6khmmKJkzk

Bug fix for No guard on dtCrowd::getAgent?
https://groups.google.com/forum/#!topic/recastnavigation/yJ1EEVbOaKg
---
 DetourCrowd/Include/DetourCrowd.h  | 29 ++++++++++++---
 DetourCrowd/Source/DetourCrowd.cpp | 57 ++++++++++++++++++++----------
 2 files changed, 63 insertions(+), 23 deletions(-)

diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h
index 243ac5a..c8cc392 100644
--- a/DetourCrowd/Include/DetourCrowd.h
+++ b/DetourCrowd/Include/DetourCrowd.h
@@ -87,6 +87,9 @@ struct dtCrowdAgentParams
 	/// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
 	unsigned char obstacleAvoidanceType;	
 
+	// [F]
+	unsigned char queryFilterType;
+
 	/// User defined data attached to the agent.
 	void* userData;
 };
@@ -102,6 +105,9 @@ enum MoveRequestState
 	DT_CROWDAGENT_TARGET_VELOCITY,
 };
 
+// [F]
+static const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16;
+
 /// Represents an agent managed by a #dtCrowd object.
 /// @ingroup crowd
 struct dtCrowdAgent
@@ -157,6 +163,9 @@ struct dtCrowdAgent
 	dtPathQueueRef targetPathqRef;		///< Path finder ref.
 	bool targetReplan;					///< Flag indicating that the current path is being replanned.
 	float targetReplanTime;				/// <Time since the agent's target was replanned.
+
+	// [F]
+	unsigned char queryFilterType;
 };
 
 struct dtCrowdAgentAnimation
@@ -190,6 +199,8 @@ struct dtCrowdAgentDebugInfo
 /// @ingroup crowd
 class dtCrowd
 {
+	public:
+
 	int m_maxAgents;
 	dtCrowdAgent* m_agents;
 	dtCrowdAgent** m_activeAgents;
@@ -206,8 +217,10 @@ class dtCrowd
 	int m_maxPathResult;
 	
 	float m_ext[3];
-	dtQueryFilter m_filter;
-	
+	//dtQueryFilter m_filter;
+	// [F]
+	dtQueryFilter m_filters[DT_CROWD_MAX_QUERY_FILTER_TYPE];
+
 	float m_maxAgentRadius;
 
 	int m_velocitySampleCount;
@@ -249,7 +262,7 @@ public:
 	/// Gets the specified agent from the pool.
 	///	 @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
 	/// @return The requested agent.
-	const dtCrowdAgent* getAgent(const int idx);
+	dtCrowdAgent* getAgent(const int idx);
 
 	/// The maximum number of agents that can be managed by the object.
 	/// @return The maximum number of agents.
@@ -301,11 +314,17 @@ public:
 	
 	/// Gets the filter used by the crowd.
 	/// @return The filter used by the crowd.
-	const dtQueryFilter* getFilter() const { return &m_filter; }
+	//const dtQueryFilter* getFilter() const { return &m_filter; }
+	// [F]
+	//const dtQueryFilter* getFilter() const {return &m_filters[0];}
+	const dtQueryFilter* getIndexedFilter(const int i = 0) const { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
 
 	/// Gets the filter used by the crowd.
 	/// @return The filter used by the crowd.
-	dtQueryFilter* getEditableFilter() { return &m_filter; }
+	//dtQueryFilter* getEditableFilter() { return &m_filter; }
+	// [F]
+	//dtQueryFilter* getEditableFilter() {return &m_filters[0];}
+	dtQueryFilter* getIndexedEditableFilter(const int i = 0) { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
 
 	/// Gets the search extents [(x, y, z)] used by the crowd for query operations. 
 	/// @return The search extents used by the crowd. [(x, y, z)]
diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp
index cf7a027..4746f82 100644
--- a/DetourCrowd/Source/DetourCrowd.cpp
+++ b/DetourCrowd/Source/DetourCrowd.cpp
@@ -482,8 +482,10 @@ int dtCrowd::getAgentCount() const
 /// @par
 /// 
 /// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
-const dtCrowdAgent* dtCrowd::getAgent(const int idx)
+dtCrowdAgent* dtCrowd::getAgent(const int idx)
 {
+	if (idx < 0 || idx >= m_maxAgents)
+		return 0;
 	return &m_agents[idx];
 }
 
@@ -514,11 +516,18 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	
 	dtCrowdAgent* ag = &m_agents[idx];
 
+	// [F]
+	if (params->queryFilterType < DT_CROWD_MAX_QUERY_FILTER_TYPE)
+		ag->queryFilterType = params->queryFilterType;
+	else
+		ag->queryFilterType = 0;
+		
+
 	// Find nearest position on navmesh and place the agent there.
 	float nearest[3];
 	dtPolyRef ref = 0;
 	dtVcopy(nearest, pos);
-	dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest);
+	dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->queryFilterType], &ref, nearest);
 	if (dtStatusFailed(status))
 	{
 		dtVcopy(nearest, pos);
@@ -691,7 +700,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 
 			// Quick seach towards the goal.
 			static const int MAX_ITER = 20;
-			m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter);
+			m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->queryFilterType]);
 			m_navquery->updateSlicedFindPath(MAX_ITER, 0);
 			dtStatus status = 0;
 			if (ag->targetReplan) // && npath > 10)
@@ -758,7 +767,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 	{
 		dtCrowdAgent* ag = queue[i];
 		ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
-											 ag->corridor.getTarget(), ag->targetPos, &m_filter);
+											 ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->queryFilterType]);
 		if (ag->targetPathqRef != DT_PATHQ_INVALID)
 			ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
 	}
@@ -912,7 +921,7 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
 	for (int i = 0; i < nqueue; ++i)
 	{
 		dtCrowdAgent* ag = queue[i];
-		ag->corridor.optimizePathTopology(m_navquery, &m_filter);
+		ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->queryFilterType]);
 		ag->topologyOptTime = 0;
 	}
 
@@ -930,8 +939,9 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
 
-		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
-			continue;
+		// BUG FIX: https://groups.google.com/forum/#!topic/recastnavigation/E6khmmKJkzk
+		//if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+		//	continue;
 			
 		ag->targetReplanTime += dt;
 
@@ -942,14 +952,14 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 		float agentPos[3];
 		dtPolyRef agentRef = ag->corridor.getFirstPoly();
 		dtVcopy(agentPos, ag->npos);
-		if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
+		if (!m_navquery->isValidPolyRef(agentRef, &m_filters[ag->queryFilterType]))
 		{
 			// Current location is not valid, try to reposition.
 			// TODO: this can snap agents, how to handle that?
 			float nearest[3];
 			dtVcopy(nearest, agentPos);
 			agentRef = 0;
-			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
+			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->queryFilterType], &agentRef, nearest);
 			dtVcopy(agentPos, nearest);
 
 			if (!agentRef)
@@ -971,16 +981,20 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 			replan = true;
 		}
 
+		// Davo BUG FIX: https://groups.google.com/forum/#!topic/recastnavigation/E6khmmKJkzk
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
+
 		// Try to recover move request position.
 		if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
 		{
-			if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
+			if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filters[ag->queryFilterType]))
 			{
 				// Current target is not valid, try to reposition.
 				float nearest[3];
 				dtVcopy(nearest, ag->targetPos);
 				ag->targetRef = 0;
-				m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
+				m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->queryFilterType], &ag->targetRef, nearest);
 				dtVcopy(ag->targetPos, nearest);
 				replan = true;
 			}
@@ -993,7 +1007,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 		}
 
 		// If nearby corridor is not valid, replan.
-		if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
+		if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->queryFilterType]))
 		{
 			// Fix current path.
 //			ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
@@ -1030,6 +1044,13 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	dtCrowdAgent** agents = m_activeAgents;
 	int nagents = getActiveAgents(agents, m_maxAgents);
 	
+	// [F]
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		ag->queryFilterType = ag->params.queryFilterType;
+	}
+
 	// Check that all agents still have valid paths.
 	checkPathValidity(agents, nagents, dt);
 	
@@ -1060,10 +1081,10 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		// if it has become invalid.
 		const float updateThr = ag->params.collisionQueryRange*0.25f;
 		if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
-			!ag->boundary.isValid(m_navquery, &m_filter))
+			!ag->boundary.isValid(m_navquery, &m_filters[ag->queryFilterType]))
 		{
 			ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
-								m_navquery, &m_filter);
+								m_navquery, &m_filters[ag->queryFilterType]);
 		}
 		// Query neighbour agents
 		ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
@@ -1085,14 +1106,14 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		
 		// Find corners for steering
 		ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
-												DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
+												DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->queryFilterType]);
 		
 		// Check to see if the corner after the next corner is directly visible,
 		// and short cut to there.
 		if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
 		{
 			const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
-			ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
+			ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->queryFilterType]);
 			
 			// Copy data for debug purposes.
 			if (debugIdx == i)
@@ -1264,7 +1285,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			if (debugIdx == i) 
 				vod = debug->vod;
 			
-			// Sample new safe velocity.
+			// MySample new safe velocity.
 			bool adaptive = true;
 			int ns = 0;
 
@@ -1372,7 +1393,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			continue;
 		
 		// Move along navmesh.
-		ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
+		ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->queryFilterType]);
 		// Get valid constrained position back.
 		dtVcopy(ag->npos, ag->corridor.getPos());