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

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

diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h
index 24c815b..c8cc392 100644
--- a/DetourCrowd/Include/DetourCrowd.h
+++ b/DetourCrowd/Include/DetourCrowd.h
@@ -45,9 +45,6 @@ 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
@@ -90,7 +87,7 @@ struct dtCrowdAgentParams
 	/// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
 	unsigned char obstacleAvoidanceType;	
 
-	/// The index of the query filter used by this agent.
+	// [F]
 	unsigned char queryFilterType;
 
 	/// User defined data attached to the agent.
@@ -108,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
@@ -163,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
@@ -196,6 +199,8 @@ struct dtCrowdAgentDebugInfo
 /// @ingroup crowd
 class dtCrowd
 {
+	public:
+
 	int m_maxAgents;
 	dtCrowdAgent* m_agents;
 	dtCrowdAgent** m_activeAgents;
@@ -212,7 +217,8 @@ 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;
@@ -256,12 +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);
-
-	/// 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);
+	dtCrowdAgent* getAgent(const int idx);
 
 	/// The maximum number of agents that can be managed by the object.
 	/// @return The maximum number of agents.
@@ -313,11 +314,17 @@ public:
 	
 	/// Gets the filter used by the crowd.
 	/// @return The filter used by the crowd.
-	inline const dtQueryFilter* getFilter(const int i) const { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
-	
+	//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.
-	inline dtQueryFilter* getEditableFilter(const int i) { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
+	//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 4297b65..4746f82 100644
--- a/DetourCrowd/Source/DetourCrowd.cpp
+++ b/DetourCrowd/Source/DetourCrowd.cpp
@@ -482,16 +482,7 @@ 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)
-{
-	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)
+dtCrowdAgent* dtCrowd::getAgent(const int idx)
 {
 	if (idx < 0 || idx >= m_maxAgents)
 		return 0;
@@ -523,13 +514,20 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	if (idx == -1)
 		return -1;
 	
-	dtCrowdAgent* ag = &m_agents[idx];		
+	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_filters[ag->params.queryFilterType], &ref, nearest);
+	dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->queryFilterType], &ref, nearest);
 	if (dtStatusFailed(status))
 	{
 		dtVcopy(nearest, pos);
@@ -702,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_filters[ag->params.queryFilterType]);
+			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)
@@ -769,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_filters[ag->params.queryFilterType]);
+											 ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->queryFilterType]);
 		if (ag->targetPathqRef != DT_PATHQ_INVALID)
 			ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
 	}
@@ -923,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_filters[ag->params.queryFilterType]);
+		ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->queryFilterType]);
 		ag->topologyOptTime = 0;
 	}
 
@@ -940,6 +938,10 @@ 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;
 
@@ -950,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_filters[ag->params.queryFilterType]))
+		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_filters[ag->params.queryFilterType], &agentRef, nearest);
+			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->queryFilterType], &agentRef, nearest);
 			dtVcopy(agentPos, nearest);
 
 			if (!agentRef)
@@ -979,20 +981,20 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 			replan = true;
 		}
 
-		// If the agent does not have move target or is controlled by velocity, no need to recover the target nor replan.
+		// 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_filters[ag->params.queryFilterType]))
+			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_filters[ag->params.queryFilterType], &ag->targetRef, nearest);
+				m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->queryFilterType], &ag->targetRef, nearest);
 				dtVcopy(ag->targetPos, nearest);
 				replan = true;
 			}
@@ -1005,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_filters[ag->params.queryFilterType]))
+		if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->queryFilterType]))
 		{
 			// Fix current path.
 //			ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
@@ -1041,6 +1043,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);
@@ -1072,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_filters[ag->params.queryFilterType]))
+			!ag->boundary.isValid(m_navquery, &m_filters[ag->queryFilterType]))
 		{
 			ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
-								m_navquery, &m_filters[ag->params.queryFilterType]);
+								m_navquery, &m_filters[ag->queryFilterType]);
 		}
 		// Query neighbour agents
 		ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
@@ -1097,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_filters[ag->params.queryFilterType]);
+												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_filters[ag->params.queryFilterType]);
+			ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->queryFilterType]);
 			
 			// Copy data for debug purposes.
 			if (debugIdx == i)
@@ -1276,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;
 
@@ -1384,7 +1393,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			continue;
 		
 		// Move along navmesh.
-		ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
+		ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->queryFilterType]);
 		// Get valid constrained position back.
 		dtVcopy(ag->npos, ag->corridor.getPos());
 
@@ -1434,3 +1443,5 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	}
 	
 }
+
+