From 907b67bd184d056844ab2354d2004d15a956276c Mon Sep 17 00:00:00 2001
From: JimmyJames707 <RATIODE@gmail.com>
Date: Fri, 11 Apr 2014 22:17:21 +1000
Subject: [PATCH] query filter for each agent type with changes

---
 DetourCrowd/Include/DetourCrowd.h  | 35 +++++++---------
 DetourCrowd/Source/DetourCrowd.cpp | 65 +++++++++++++-----------------
 2 files changed, 41 insertions(+), 59 deletions(-)

diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h
index c8cc392..24c815b 100644
--- a/DetourCrowd/Include/DetourCrowd.h
+++ b/DetourCrowd/Include/DetourCrowd.h
@@ -45,6 +45,9 @@ static const int DT_CROWDAGENT_MAX_CORNERS = 4;
 ///		 dtCrowdAgentParams::obstacleAvoidanceType
 static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8;
 
+/// The maximum number of query filter types
+static const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16;
+
 /// Provides neighbor data for agents managed by the crowd.
 /// @ingroup crowd
 /// @see dtCrowdAgent::neis, dtCrowd
@@ -87,7 +90,7 @@ struct dtCrowdAgentParams
 	/// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
 	unsigned char obstacleAvoidanceType;	
 
-	// [F]
+	/// The index of the query filter used by this agent.
 	unsigned char queryFilterType;
 
 	/// User defined data attached to the agent.
@@ -105,9 +108,6 @@ 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
@@ -163,9 +163,6 @@ 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
@@ -199,8 +196,6 @@ struct dtCrowdAgentDebugInfo
 /// @ingroup crowd
 class dtCrowd
 {
-	public:
-
 	int m_maxAgents;
 	dtCrowdAgent* m_agents;
 	dtCrowdAgent** m_activeAgents;
@@ -217,8 +212,7 @@ class dtCrowd
 	int m_maxPathResult;
 	
 	float m_ext[3];
-	//dtQueryFilter m_filter;
-	// [F]
+
 	dtQueryFilter m_filters[DT_CROWD_MAX_QUERY_FILTER_TYPE];
 
 	float m_maxAgentRadius;
@@ -262,7 +256,12 @@ public:
 	/// Gets the specified agent from the pool.
 	///	 @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
 	/// @return The requested agent.
-	dtCrowdAgent* getAgent(const int idx);
+	const dtCrowdAgent* getAgent(const int idx);
+
+	/// Gets the specified agent from the pool.
+	///	 @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	/// @return The requested agent.
+	dtCrowdAgent* getEditableAgent(const int idx);
 
 	/// The maximum number of agents that can be managed by the object.
 	/// @return The maximum number of agents.
@@ -314,17 +313,11 @@ public:
 	
 	/// Gets the filter used by the crowd.
 	/// @return The filter used by the crowd.
-	//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; }
-
+	inline const dtQueryFilter* getFilter(const int i) 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; }
-	// [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; }
+	inline dtQueryFilter* getEditableFilter(const int i) { 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 4746f82..4297b65 100644
--- a/DetourCrowd/Source/DetourCrowd.cpp
+++ b/DetourCrowd/Source/DetourCrowd.cpp
@@ -482,7 +482,16 @@ int dtCrowd::getAgentCount() const
 /// @par
 /// 
 /// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
-dtCrowdAgent* dtCrowd::getAgent(const int idx)
+const dtCrowdAgent* dtCrowd::getAgent(const int idx)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return 0;
+	return &m_agents[idx];
+}
+
+/// 
+/// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
+dtCrowdAgent* dtCrowd::getEditableAgent(const int idx)
 {
 	if (idx < 0 || idx >= m_maxAgents)
 		return 0;
@@ -514,20 +523,13 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	if (idx == -1)
 		return -1;
 	
-	dtCrowdAgent* ag = &m_agents[idx];
-
-	// [F]
-	if (params->queryFilterType < DT_CROWD_MAX_QUERY_FILTER_TYPE)
-		ag->queryFilterType = params->queryFilterType;
-	else
-		ag->queryFilterType = 0;
-		
+	dtCrowdAgent* ag = &m_agents[idx];		
 
 	// 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_filters[ag->queryFilterType], &ref, nearest);
+	dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
 	if (dtStatusFailed(status))
 	{
 		dtVcopy(nearest, pos);
@@ -700,7 +702,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_filters[ag->queryFilterType]);
+			m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
 			m_navquery->updateSlicedFindPath(MAX_ITER, 0);
 			dtStatus status = 0;
 			if (ag->targetReplan) // && npath > 10)
@@ -767,7 +769,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_filters[ag->queryFilterType]);
+											 ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->params.queryFilterType]);
 		if (ag->targetPathqRef != DT_PATHQ_INVALID)
 			ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
 	}
@@ -921,7 +923,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_filters[ag->queryFilterType]);
+		ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
 		ag->topologyOptTime = 0;
 	}
 
@@ -938,10 +940,6 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 		
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			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;
 
@@ -952,14 +950,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_filters[ag->queryFilterType]))
+		if (!m_navquery->isValidPolyRef(agentRef, &m_filters[ag->params.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_filters[ag->queryFilterType], &agentRef, nearest);
+			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->params.queryFilterType], &agentRef, nearest);
 			dtVcopy(agentPos, nearest);
 
 			if (!agentRef)
@@ -981,20 +979,20 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 			replan = true;
 		}
 
-		// Davo BUG FIX: https://groups.google.com/forum/#!topic/recastnavigation/E6khmmKJkzk
+		// If the agent does not have move target or is controlled by velocity, no need to recover the target nor replan.
 		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_filters[ag->queryFilterType]))
+			if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filters[ag->params.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_filters[ag->queryFilterType], &ag->targetRef, nearest);
+				m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest);
 				dtVcopy(ag->targetPos, nearest);
 				replan = true;
 			}
@@ -1007,7 +1005,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_filters[ag->queryFilterType]))
+		if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->params.queryFilterType]))
 		{
 			// Fix current path.
 //			ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
@@ -1043,13 +1041,6 @@ 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);
@@ -1081,10 +1072,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_filters[ag->queryFilterType]))
+			!ag->boundary.isValid(m_navquery, &m_filters[ag->params.queryFilterType]))
 		{
 			ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
-								m_navquery, &m_filters[ag->queryFilterType]);
+								m_navquery, &m_filters[ag->params.queryFilterType]);
 		}
 		// Query neighbour agents
 		ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
@@ -1106,14 +1097,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_filters[ag->queryFilterType]);
+												DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.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_filters[ag->queryFilterType]);
+			ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->params.queryFilterType]);
 			
 			// Copy data for debug purposes.
 			if (debugIdx == i)
@@ -1285,7 +1276,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			if (debugIdx == i) 
 				vod = debug->vod;
 			
-			// MySample new safe velocity.
+			// Sample new safe velocity.
 			bool adaptive = true;
 			int ns = 0;
 
@@ -1393,7 +1384,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			continue;
 		
 		// Move along navmesh.
-		ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->queryFilterType]);
+		ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
 		// Get valid constrained position back.
 		dtVcopy(ag->npos, ag->corridor.getPos());
 
@@ -1443,5 +1434,3 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	}
 	
 }
-
-