diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp
index 1606b90..87b6169 100644
--- a/Detour/Source/DetourNavMeshQuery.cpp
+++ b/Detour/Source/DetourNavMeshQuery.cpp
@@ -1491,7 +1491,9 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		
 		if (!node)
 		{
-			return DT_FAILURE;
+			m_query.status |= DT_PARTIAL_RESULT;
+			dtAssert(m_query.lastBestNode);
+			node = m_query.lastBestNode;
 		}
 		
 		// Reverse the path.
diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h
index 52c5fd7..e789fd3 100644
--- a/DetourCrowd/Include/DetourCrowd.h
+++ b/DetourCrowd/Include/DetourCrowd.h
@@ -91,6 +91,17 @@ struct dtCrowdAgentParams
 	void* userData;
 };
 
+enum MoveRequestState
+{
+	DT_CROWDAGENT_TARGET_NONE = 0,
+	DT_CROWDAGENT_TARGET_FAILED,
+	DT_CROWDAGENT_TARGET_VALID,
+	DT_CROWDAGENT_TARGET_REQUESTING,
+	DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE,
+	DT_CROWDAGENT_TARGET_WAITING_FOR_PATH,
+	DT_CROWDAGENT_TARGET_VELOCITY,
+};
+
 /// Represents an agent managed by a #dtCrowd object.
 /// @ingroup crowd
 struct dtCrowdAgent
@@ -107,10 +118,7 @@ struct dtCrowdAgent
 	/// The local boundary data for the agent.
 	dtLocalBoundary boundary;
 	
-	float t;
-	float var;
-
-	/// The last time the agent's path corridor was optimized.
+	/// Time since the agent's path corridor was optimized.
 	float topologyOptTime;
 	
 	/// The known neighbors of the agent.
@@ -142,6 +150,13 @@ struct dtCrowdAgent
 
 	/// The number of corners.
 	int ncorners;
+	
+	unsigned char targetState;			///< State of the movement request.
+	dtPolyRef targetRef;				///< Target polyref of the movement request.
+	float targetPos[3];					///< Target position of the movement request (or velocity in case of DT_CROWDAGENT_TARGET_VELOCITY).
+	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.
 };
 
 struct dtCrowdAgentAnimation
@@ -197,42 +212,13 @@ class dtCrowd
 
 	int m_velocitySampleCount;
 
-	enum MoveRequestState
-	{
-		MR_TARGET_NONE,
-		MR_TARGET_FAILED,
-		MR_TARGET_VALID,
-		MR_TARGET_REQUESTING,
-		MR_TARGET_WAITING_FOR_PATH,
-		MR_TARGET_ADJUST,
-	};
-	
-	static const int MAX_TEMP_PATH = 32;
-
-	struct MoveRequest
-	{
-		unsigned char state;			///< State of the request
-		int idx;						///< Agent index
-		dtPolyRef ref;					///< Goal ref
-		float pos[3];					///< Goal position
-		dtPathQueueRef pathqRef;		///< Path find query ref
-		dtPolyRef aref;					///< Goal adjustment ref
-		float apos[3];					///< Goal adjustment pos
-		dtPolyRef temp[MAX_TEMP_PATH];	///< Adjusted path to the goal
-		int ntemp;
-		bool replan;
-	};
-	MoveRequest* m_moveRequests;
-	int m_moveRequestCount;
-	
 	dtNavMeshQuery* m_navquery;
 
 	void updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt);
 	void updateMoveRequest(const float dt);
-	void checkPathValidty(dtCrowdAgent** agents, const int nagents, const float dt);
+	void checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt);
 
 	inline int getAgentIndex(const dtCrowdAgent* agent) const  { return agent - m_agents; }
-	const MoveRequest* getActiveMoveTarget(const int idx) const;
 
 	bool requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos);
 
@@ -291,12 +277,16 @@ public:
 	/// @return True if the request was successfully submitted.
 	bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos);
 
-	/// Sumbits a request to adjust the target position of the specified agent.
+	/// Submits a new move request for the specified agent.
 	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
-	///  @param[in]		ref		The position's polygon reference.
-	///  @param[in]		pos		The position within the polygon. [(x, y, z)]
+	///  @param[in]		vel		The movement velocity. [(x, y, z)]
 	/// @return True if the request was successfully submitted.
-	bool adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos);
+	bool requestMoveVelocity(const int idx, const float* vel);
+
+	/// Resets any request for the specified agent.
+	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	/// @return True if the request was successfully reseted.
+	bool resetMoveTarget(const int idx);
 
 	/// Gets the active agents int the agent pool.
 	///  @param[out]	agents		An array of agent pointers. [(#dtCrowdAgent *) * maxAgents]
diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp
index 7e0cf68..e14c7ed 100644
--- a/DetourCrowd/Source/DetourCrowd.cpp
+++ b/DetourCrowd/Source/DetourCrowd.cpp
@@ -236,7 +236,7 @@ A common process for managing the crowd is as follows:
 
 -# Call #update() to allow the crowd to manage its agents.
 -# Retrieve agent information using #getActiveAgents().
--# Make movement requests using #requestMoveTarget() and #adjustMoveTarget().
+-# Make movement requests using #requestMoveTarget() when movement goal changes.
 -# Repeat every frame.
 
 Some agent configuration settings can be updated using #updateAgentParameters().  But the crowd owns the
@@ -267,8 +267,6 @@ dtCrowd::dtCrowd() :
 	m_maxPathResult(0),
 	m_maxAgentRadius(0),
 	m_velocitySampleCount(0),
-	m_moveRequests(0),
-	m_moveRequestCount(0),
 	m_navquery(0)
 {
 }
@@ -295,10 +293,6 @@ void dtCrowd::purge()
 	dtFree(m_pathResult);
 	m_pathResult = 0;
 	
-	dtFree(m_moveRequests);
-	m_moveRequests = 0;
-	m_moveRequestCount = 0;
-	
 	dtFreeProximityGrid(m_grid);
 	m_grid = 0;
 
@@ -356,11 +350,6 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
 	if (!m_pathResult)
 		return false;
 	
-	m_moveRequests = (MoveRequest*)dtAlloc(sizeof(MoveRequest)*m_maxAgents, DT_ALLOC_PERM);
-	if (!m_moveRequests)
-		return false;
-	m_moveRequestCount = 0;
-	
 	if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav))
 		return false;
 	
@@ -463,6 +452,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	updateAgentParameters(idx, params);
 	
 	ag->topologyOptTime = 0;
+	ag->targetReplanTime = 0;
 	ag->nneis = 0;
 	
 	dtVset(ag->dvel, 0,0,0);
@@ -471,14 +461,14 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	dtVcopy(ag->npos, nearest);
 	
 	ag->desiredSpeed = 0;
-	ag->t = 0;
-	ag->var = (rand() % 10) / 9.0f;
 
 	if (ref)
 		ag->state = DT_CROWDAGENT_STATE_WALKING;
 	else
 		ag->state = DT_CROWDAGENT_STATE_INVALID;
 	
+	ag->targetState = DT_CROWDAGENT_TARGET_NONE;
+	
 	ag->active = 1;
 
 	return idx;
@@ -496,69 +486,29 @@ void dtCrowd::removeAgent(const int idx)
 	}
 }
 
-const dtCrowd::MoveRequest* dtCrowd::getActiveMoveTarget(const int idx) const
-{
-	if (idx < 0 || idx > m_maxAgents)
-		return 0;
-	
-	MoveRequest* req = 0;
-	// Check if there is existing request and update that instead.
-	for (int i = 0; i < m_moveRequestCount; ++i)
-	{
-		if (m_moveRequests[i].idx == idx)
-		{
-			req = &m_moveRequests[i];
-			break;
-		}
-	}
-	
-	return req;
-}
-
 bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos)
 {
 	if (idx < 0 || idx > m_maxAgents)
 		return false;
-	if (!ref)
-		return false;
 	
-	MoveRequest* req = 0;
-	// Check if there is existing request and update that instead.
-	for (int i = 0; i < m_moveRequestCount; ++i)
-	{
-		if (m_moveRequests[i].idx == idx)
-		{
-			req = &m_moveRequests[i];
-			break;
-		}
-	}
-	if (!req)
-	{
-		if (m_moveRequestCount >= m_maxAgents)
-			return false;
-		req = &m_moveRequests[m_moveRequestCount++];
-		memset(req, 0, sizeof(MoveRequest));
-	}
+	dtCrowdAgent* ag = &m_agents[idx];
 	
 	// Initialize request.
-	req->idx = idx;
-	req->ref = ref;
-	dtVcopy(req->pos, pos);
-	req->pathqRef = DT_PATHQ_INVALID;
-	req->state = MR_TARGET_REQUESTING;
-	req->replan = true;
+	ag->targetRef = ref;
+	dtVcopy(ag->targetPos, pos);
+	ag->targetPathqRef = DT_PATHQ_INVALID;
+	ag->targetReplan = true;
+	if (ag->targetRef)
+		ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
+	else
+		ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
 	
-	req->temp[0] = ref;
-	req->ntemp = 1;
-
 	return true;
 }
 
 /// @par
 /// 
-/// This method is used when a new target is set.  Use #adjustMoveTarget() when 
-/// only small local adjustments are needed. (Such as happens when following a 
-/// moving target.)
+/// This method is used when a new target is set.
 /// 
 /// The position will be constrained to the surface of the navigation mesh.
 ///
@@ -569,81 +519,53 @@ bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
 		return false;
 	if (!ref)
 		return false;
-	
-	MoveRequest* req = 0;
-	// Check if there is existing request and update that instead.
-	for (int i = 0; i < m_moveRequestCount; ++i)
-	{
-		if (m_moveRequests[i].idx == idx)
-		{
-			req = &m_moveRequests[i];
-			break;
-		}
-	}
-	if (!req)
-	{
-		if (m_moveRequestCount >= m_maxAgents)
-			return false;
-		req = &m_moveRequests[m_moveRequestCount++];
-		memset(req, 0, sizeof(MoveRequest));
-	}
+
+	dtCrowdAgent* ag = &m_agents[idx];
 	
 	// Initialize request.
-	req->idx = idx;
-	req->ref = ref;
-	dtVcopy(req->pos, pos);
-	req->pathqRef = DT_PATHQ_INVALID;
-	req->state = MR_TARGET_REQUESTING;
-	req->replan = false;
-	
-	req->temp[0] = ref;
-	req->ntemp = 1;
+	ag->targetRef = ref;
+	dtVcopy(ag->targetPos, pos);
+	ag->targetPathqRef = DT_PATHQ_INVALID;
+	ag->targetReplan = false;
+	if (ag->targetRef)
+		ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
+	else
+		ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
 
 	return true;
 }
 
-/// @par
-/// 
-/// This method is used when to make small local adjustments to the current 
-/// target. (Such as happens when following a moving target.) Use 
-/// #requestMoveTarget() when a new target is needed. 
-/// 
-/// The position will be constrained to the surface of the navigation mesh.
-///
-/// The request will be processed during the next #update().
-bool dtCrowd::adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos)
+bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
 {
 	if (idx < 0 || idx > m_maxAgents)
 		return false;
-	if (!ref)
+	
+	dtCrowdAgent* ag = &m_agents[idx];
+	
+	// Initialize request.
+	ag->targetRef = 0;
+	dtVcopy(ag->targetPos, vel);
+	ag->targetPathqRef = DT_PATHQ_INVALID;
+	ag->targetReplan = false;
+	ag->targetState = DT_CROWDAGENT_TARGET_VELOCITY;
+	
+	return true;
+}
+
+bool dtCrowd::resetMoveTarget(const int idx)
+{
+	if (idx < 0 || idx > m_maxAgents)
 		return false;
 	
-	MoveRequest* req = 0;
-	// Check if there is existing request and update that instead.
-	for (int i = 0; i < m_moveRequestCount; ++i)
-	{
-		if (m_moveRequests[i].idx == idx)
-		{
-			req = &m_moveRequests[i];
-			break;
-		}
-	}
-	if (!req)
-	{
-		if (m_moveRequestCount >= m_maxAgents)
-			return false;
-		req = &m_moveRequests[m_moveRequestCount++];
-		memset(req, 0, sizeof(MoveRequest));
-
-		// New adjust request
-		req->state = MR_TARGET_ADJUST;
-		req->idx = idx;
-	}
-
-	// Set adjustment request.
-	req->aref = ref;
-	dtVcopy(req->apos, pos);
-
+	dtCrowdAgent* ag = &m_agents[idx];
+	
+	// Initialize request.
+	ag->targetRef = 0;
+	dtVset(ag->targetPos, 0,0,0);
+	ag->targetPathqRef = DT_PATHQ_INVALID;
+	ag->targetReplan = false;
+	ag->targetState = DT_CROWDAGENT_TARGET_NONE;
+	
 	return true;
 }
 
@@ -663,108 +585,91 @@ int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents)
 void dtCrowd::updateMoveRequest(const float /*dt*/)
 {
 	// Fire off new requests.
-	for (int i = 0; i < m_moveRequestCount; ++i)
+	for (int i = 0; i < m_maxAgents; ++i)
 	{
-		MoveRequest* req = &m_moveRequests[i];
-		dtCrowdAgent* ag = &m_agents[req->idx];
-		
-		// Agent not active anymore, kill request.
+		dtCrowdAgent* ag = &m_agents[i];
 		if (!ag->active)
-			req->state = MR_TARGET_FAILED;
-		
-		// Adjust target
-		if (req->aref)
+			continue;
+		if (ag->state == DT_CROWDAGENT_STATE_INVALID)
+			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
+
+		if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING)
 		{
-			if (req->state == MR_TARGET_ADJUST)
+			const dtPolyRef* path = ag->corridor.getPath();
+			const int npath = ag->corridor.getPathCount();
+			dtAssert(npath);
+
+			static const int MAX_RES = 32;
+			float reqPos[3];
+			dtPolyRef reqPath[MAX_RES];	// The path to the request location
+			int reqPathCount = 0;
+
+			// 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->updateSlicedFindPath(MAX_ITER, 0);
+			dtStatus status = 0;
+			if (ag->targetReplan && npath > 10)
 			{
-				// Adjust existing path.
-				ag->corridor.moveTargetPosition(req->apos, m_navquery, &m_filter);
-				req->state = MR_TARGET_VALID;
+				// Try to use existing steady path during replan if possible.
+				status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
 			}
 			else
 			{
-				// Adjust on the flight request.
-				float result[3];
-				static const int MAX_VISITED = 16;
-				dtPolyRef visited[MAX_VISITED];
-				int nvisited = 0;
-				m_navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, &m_filter,
-											 result, visited, &nvisited, MAX_VISITED);
-				req->ntemp = dtMergeCorridorEndMoved(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited);
-				dtVcopy(req->pos, result);
+				// Try to move towards target when goal changes.
+				status = m_navquery->finalizeSlicedFindPath(reqPath, &reqPathCount, MAX_RES);
+			}
+
+			if (!dtStatusFailed(status) && reqPathCount > 0)
+			{
+				// In progress or succeed.
+				if (reqPath[reqPathCount-1] != ag->targetRef)
+				{
+					// Partial path, constrain target position inside the last polygon.
+					status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos);
+					if (dtStatusFailed(status))
+						reqPathCount = 0;
+				}
+				else
+				{
+					dtVcopy(reqPos, ag->targetPos);
+				}
+			}
+			else
+			{
+				reqPathCount = 0;
+			}
 				
-				// Reset adjustment.
-				dtVset(req->apos, 0,0,0);
-				req->aref = 0;
+			if (!reqPathCount)
+			{
+				// Could not find path, start the request from current location.
+				dtVcopy(reqPos, ag->npos);
+				reqPath[0] = path[0];
+				reqPathCount = 1;
+			}
+
+			ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
+			ag->boundary.reset();
+
+			if (reqPath[reqPathCount-1] == ag->targetRef)
+			{
+				ag->targetState = DT_CROWDAGENT_TARGET_VALID;
+				ag->targetReplanTime = 0.0;
+			}
+			else
+			{
+				// The path is longer or potentially unreachable, full plan.
+				ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE;
 			}
 		}
 		
-		
-		if (req->state == MR_TARGET_REQUESTING)
+		if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
 		{
-			// If agent location is invalid, try to recover.
-			if (ag->state == DT_CROWDAGENT_STATE_INVALID)
-			{
-				float agentPos[3];
-				dtVcopy(agentPos, ag->npos);
-				dtPolyRef agentRef = ag->corridor.getFirstPoly();
-				if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
-				{
-					// Current location is not valid, try to reposition.
-					// TODO: this can snap agents, how to handle that?
-					float nearest[3];
-					agentRef = 0;
-					m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
-					dtVcopy(agentPos, nearest);
-				}
-				if (!agentRef)
-				{
-					// Current location still outside navmesh, fail the request.
-					req->state = MR_TARGET_FAILED;
-					continue;
-				}
-
-				// Could relocation agent on navmesh again.
-				ag->state = DT_CROWDAGENT_STATE_WALKING;
-				ag->corridor.reset(agentRef, agentPos);
-			}
-			
-			// Calculate request position.
-			
-			if (req->replan)
-			{
-				// Replan from the end of the current path.
-				dtPolyRef reqRef = ag->corridor.getLastPoly();
-				float reqPos[3];
-				dtVcopy(reqPos, ag->corridor.getTarget());
-				req->pathqRef = m_pathq.request(reqRef, req->ref, reqPos, req->pos, &m_filter);
-				if (req->pathqRef != DT_PATHQ_INVALID)
-				{
-					req->state = MR_TARGET_WAITING_FOR_PATH;
-				}
-			}
-			else
-			{
-				// If there is a lot of latency between requests, it is possible to
-				// project the current position ahead and use raycast to find the actual
-				// location and path.
-				const dtPolyRef* path = ag->corridor.getPath();
-				const int npath = ag->corridor.getPathCount();
-				dtAssert(npath);
-				// Here we take the simple approach and set the path to be just the current location.
-				float reqPos[3];
-				dtVcopy(reqPos, ag->corridor.getPos());	// The location of the request
-				dtPolyRef reqPath[8];					// The path to the request location
-				reqPath[0] = path[0];
-				int reqPathCount = 1;
-				
-				req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter);
-				if (req->pathqRef != DT_PATHQ_INVALID)
-				{
-					ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
-					req->state = MR_TARGET_WAITING_FOR_PATH;
-				}
-			}
+			ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef, ag->corridor.getTarget(), ag->targetPos, &m_filter);
+			if (ag->targetPathqRef != DT_PATHQ_INVALID)
+				ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
 		}
 	}
 
@@ -775,19 +680,27 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 	dtStatus status;
 
 	// Process path results.
-	for (int i = 0; i < m_moveRequestCount; ++i)
+	for (int i = 0; i < m_maxAgents; ++i)
 	{
-		MoveRequest* req = &m_moveRequests[i];
-		dtCrowdAgent* ag = &m_agents[req->idx];
+		dtCrowdAgent* ag = &m_agents[i];
+		if (!ag->active)
+			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
 		
-		if (req->state == MR_TARGET_WAITING_FOR_PATH)
+		if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
 		{
 			// Poll path queue.
-			status = m_pathq.getRequestStatus(req->pathqRef);
+			status = m_pathq.getRequestStatus(ag->targetPathqRef);
 			if (dtStatusFailed(status))
 			{
-				req->pathqRef = DT_PATHQ_INVALID;
-				req->state = MR_TARGET_FAILED;
+				// Path find failed, retry if the target location is still valid.
+				ag->targetPathqRef = DT_PATHQ_INVALID;
+				if (ag->targetRef)
+					ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
+				else
+					ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
+				ag->targetReplanTime = 0.0;
 			}
 			else if (dtStatusSucceed(status))
 			{
@@ -797,21 +710,15 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 				
 				// Apply results.
 				float targetPos[3];
-				dtVcopy(targetPos, req->pos);
+				dtVcopy(targetPos, ag->targetPos);
 				
 				dtPolyRef* res = m_pathResult;
 				bool valid = true;
 				int nres = 0;
-				status = m_pathq.getPathResult(req->pathqRef, res, &nres, m_maxPathResult);
+				status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
 				if (dtStatusFailed(status) || !nres)
 					valid = false;
 				
-				// Merge with any target adjustment that happened during the search.
-				if (req->ntemp > 1)
-				{
-					nres = dtMergeCorridorEndMoved(res, nres, m_maxPathResult, req->temp, req->ntemp);
-				}
-				
 				// Merge result and existing path.
 				// The agent might have moved whilst the request is
 				// being processed, so the path may have changed.
@@ -854,11 +761,12 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 					}
 					
 					// Check for partial path.
-					if (res[nres-1] != req->ref)
+					if (res[nres-1] != ag->targetRef)
 					{
 						// Partial path, constrain target position inside the last polygon.
 						float nearest[3];
-						if (m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS)
+						status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest);
+						if (dtStatusSucceed(status))
 							dtVcopy(targetPos, nearest);
 						else
 							valid = false;
@@ -871,24 +779,17 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 					ag->corridor.setCorridor(targetPos, res, nres);
 					// Force to update boundary.
 					ag->boundary.reset();
-					req->state = MR_TARGET_VALID;
+					ag->targetState = DT_CROWDAGENT_TARGET_VALID;
 				}
 				else
 				{
 					// Something went wrong.
-					req->state = MR_TARGET_FAILED;
+					ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
 				}
+
+				ag->targetReplanTime = 0.0;
 			}
 		}
-		
-		// Remove request when done with it.
-		if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED)
-		{
-			m_moveRequestCount--;
-			if (i != m_moveRequestCount)
-				memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest));
-			--i;
-		}
 	}
 	
 }
@@ -946,6 +847,8 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
 		dtCrowdAgent* ag = agents[i];
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
 		if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
 			continue;
 		ag->topologyOptTime += dt;
@@ -962,9 +865,10 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
 
 }
 
-void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const float dt)
+void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt)
 {
 	static const int CHECK_LOOKAHEAD = 10;
+	static const float TARGET_REPLAN_DELAY = 1.0; // seconds
 	
 	for (int i = 0; i < nagents; ++i)
 	{
@@ -972,43 +876,19 @@ void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const f
 		
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
-		
-		// Skip if the corridor is valid
-		if (ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
+
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
 			continue;
+			
+		ag->targetReplanTime += dt;
 
-		// The current path is bad, try to recover.
+		bool replan = false;
 
-		// Store current state.
-		float agentPos[3];
-		dtPolyRef agentRef = 0;
-		dtVcopy(agentPos, ag->npos);
-		agentRef = ag->corridor.getFirstPoly();
-		
-		float targetPos[3];
-		dtPolyRef targetRef = 0;
-		dtVcopy(targetPos, ag->corridor.getTarget());
-		targetRef = ag->corridor.getLastPoly();
-		// If has active move target, get target location from that instead.
-		const int idx = getAgentIndex(ag);
-		const MoveRequest* req = getActiveMoveTarget(idx);
-		if (req)
-		{
-			if (req->state == MR_TARGET_REQUESTING ||
-				req->state == MR_TARGET_WAITING_FOR_PATH ||
-				req->state == MR_TARGET_VALID)
-			{
-				targetRef = req->ref;
-				dtVcopy(targetPos, req->pos);
-			}
-			else if (req->state == MR_TARGET_ADJUST)
-			{
-				targetRef = req->aref;
-				dtVcopy(targetPos, req->apos);
-			}
-		}
-		
 		// First check that the current location is valid.
+		const int idx = getAgentIndex(ag);
+		float agentPos[3];
+		dtPolyRef agentRef = ag->corridor.getFirstPoly();
+		dtVcopy(agentPos, ag->npos);
 		if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
 		{
 			// Current location is not valid, try to reposition.
@@ -1017,43 +897,67 @@ void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const f
 			agentRef = 0;
 			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
 			dtVcopy(agentPos, nearest);
+
+			if (!agentRef)
+			{
+				// Could not find location in navmesh, set state to invalid.
+				ag->corridor.reset(0, agentPos);
+				ag->boundary.reset();
+				ag->state = DT_CROWDAGENT_STATE_INVALID;
+				continue;
+			}
+
+			ag->boundary.reset();
+			dtVcopy(ag->npos, agentPos);
+
+			replan = true;
 		}
 
-		if (!agentRef)
+		// Try to recover move request position.
+		if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
 		{
-			// Count not find location in navmesh, set state to invalid.
-			ag->corridor.reset(0, agentPos);
+			if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
+			{
+				// Current target is not valid, try to reposition.
+				float nearest[3];
+				m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
+				dtVcopy(ag->targetPos, nearest);
+				replan = true;
+			}
+			if (!ag->targetRef)
+			{
+				// Failed to reposition target, fail moverequest.
+				ag->corridor.reset(agentRef, agentPos);
+				ag->targetState = DT_CROWDAGENT_TARGET_NONE;
+			}
+		}
+
+		// If nearby corridor is not valid, replan.
+		if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
+		{
+			// Fix current path.
+			ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
 			ag->boundary.reset();
-			ag->state = DT_CROWDAGENT_STATE_INVALID;
-			continue;
+			replan = true;
 		}
 		
-		// TODO: make temp path by raycasting towards current velocity.
-		
-		ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
-		ag->boundary.reset();
-		dtVcopy(ag->npos, agentPos);
-		
-		
-		// Check that target is still reachable.
-		if (!m_navquery->isValidPolyRef(targetRef, &m_filter))
+		// If the end of the path is near and it is not the requested location, replan.
+		if (ag->targetState == DT_CROWDAGENT_TARGET_VALID)
 		{
-			// Current target is not valid, try to reposition.
-			float nearest[3];
-			targetRef = 0;
-			m_navquery->findNearestPoly(targetPos, m_ext, &m_filter, &targetRef, nearest);
-			dtVcopy(targetPos, nearest);
+			if (ag->targetReplanTime > TARGET_REPLAN_DELAY &&
+				ag->corridor.getPathCount() < CHECK_LOOKAHEAD &&
+				ag->corridor.getLastPoly() != ag->targetRef)
+				replan = true;
 		}
-		
-		if (!targetRef)
-		{
-			// Target not reachable anymore, cannot replan.
-			// TODO: indicate failure!
-			continue;
-		}
-		
+
 		// Try to replan path to goal.
-		requestMoveTargetReplan(idx, targetRef, targetPos);
+		if (replan)
+		{
+			if (ag->targetState != DT_CROWDAGENT_TARGET_NONE)
+			{
+				requestMoveTargetReplan(idx, ag->targetRef, ag->targetPos);
+			}
+		}
 	}
 }
 	
@@ -1067,7 +971,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	int nagents = getActiveAgents(agents, m_maxAgents);
 	
 	// Check that all agents still have valid paths.
-	checkPathValidty(agents, nagents, dt);
+	checkPathValidity(agents, nagents, dt);
 	
 	// Update async move request and path finder.
 	updateMoveRequest(dt);
@@ -1105,6 +1009,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
 								  ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
 								  agents, nagents, m_grid);
+		for (int j = 0; j < ag->nneis; j++)
+			ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
 	}
 	
 	// Find next corner to steer to.
@@ -1114,6 +1020,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
 		
 		// Find corners for steering
 		ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
@@ -1151,6 +1059,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
 		
 		// Check 
 		const float triggerRadius = ag->params.radius*2.25f;
@@ -1178,9 +1088,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			}
 			else
 			{
-				// TODO: Off-mesh connection failed, replan.
+				// Path validity check will ensure that bad/blocked connections will be replanned.
 			}
-			
 		}
 	}
 		
@@ -1191,21 +1100,31 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 
 		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
+			continue;
 		
 		float dvel[3] = {0,0,0};
-		
-		// Calculate steering direction.
-		if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
-			calcSmoothSteerDirection(ag, dvel);
+
+		if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+		{
+			dtVcopy(dvel, ag->targetPos);
+			ag->desiredSpeed = dtVlen(ag->targetPos);
+		}
 		else
-			calcStraightSteerDirection(ag, dvel);
-		
-		// Calculate speed scale, which tells the agent to slowdown at the end of the path.
-		const float slowDownRadius = ag->params.radius*2;	// TODO: make less hacky.
-		const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
+		{
+			// Calculate steering direction.
+			if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
+				calcSmoothSteerDirection(ag, dvel);
+			else
+				calcStraightSteerDirection(ag, dvel);
 			
-		ag->desiredSpeed = ag->params.maxSpeed;
-		dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
+			// Calculate speed scale, which tells the agent to slowdown at the end of the path.
+			const float slowDownRadius = ag->params.radius*2;	// TODO: make less hacky.
+			const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
+				
+			ag->desiredSpeed = ag->params.maxSpeed;
+			dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
+		}
 
 		// Separation
 		if (ag->params.updateFlags & DT_CROWD_SEPARATION)
@@ -1268,7 +1187,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 			// Add neighbours as obstacles.
 			for (int j = 0; j < ag->nneis; ++j)
 			{
-				const dtCrowdAgent* nei = agents[ag->neis[j].idx];
+				const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
 				m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
 			}
 
@@ -1338,7 +1257,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 
 			for (int j = 0; j < ag->nneis; ++j)
 			{
-				const dtCrowdAgent* nei = agents[ag->neis[j].idx];
+				const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
 				const int idx1 = getAgentIndex(nei);
 
 				float diff[3];
@@ -1396,6 +1315,13 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
 		// Get valid constrained position back.
 		dtVcopy(ag->npos, ag->corridor.getPos());
+
+		// If not using path, truncate the corridor to just one poly.
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+		{
+			ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
+		}
+
 	}
 	
 	// Update agents using off-mesh connection.
diff --git a/DetourCrowd/Source/DetourPathCorridor.cpp b/DetourCrowd/Source/DetourPathCorridor.cpp
index 30ea97b..23b16f2 100644
--- a/DetourCrowd/Source/DetourPathCorridor.cpp
+++ b/DetourCrowd/Source/DetourPathCorridor.cpp
@@ -368,7 +368,7 @@ bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQuer
 	navquery->updateSlicedFindPath(MAX_ITER, 0);
 	dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES);
 	
-	if (status == DT_SUCCESS && nres > 0)
+	if (dtStatusSucceed(status) && nres > 0)
 	{
 		m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
 		return true;
diff --git a/RecastDemo/Include/CrowdTool.h b/RecastDemo/Include/CrowdTool.h
index 566b82e..956b240 100644
--- a/RecastDemo/Include/CrowdTool.h
+++ b/RecastDemo/Include/CrowdTool.h
@@ -42,6 +42,7 @@ struct CrowdToolParams
 	bool m_showGrid;
 	bool m_showNodes;
 	bool m_showPerfGraph;
+	bool m_showDetailAll;
 	
 	bool m_expandOptions;
 	bool m_anticipateTurns;
diff --git a/RecastDemo/Source/CrowdTool.cpp b/RecastDemo/Source/CrowdTool.cpp
index c42b7ba..5caa6bd 100644
--- a/RecastDemo/Source/CrowdTool.cpp
+++ b/RecastDemo/Source/CrowdTool.cpp
@@ -31,6 +31,7 @@
 #include "DetourDebugDraw.h"
 #include "DetourObstacleAvoidance.h"
 #include "DetourCommon.h"
+#include "DetourNode.h"
 #include "SampleInterfaces.h"
 
 #ifdef WIN32
@@ -109,6 +110,7 @@ CrowdToolState::CrowdToolState() :
 	m_toolParams.m_showGrid = false;
 	m_toolParams.m_showNodes = false;
 	m_toolParams.m_showPerfGraph = false;
+	m_toolParams.m_showDetailAll = false;
 	m_toolParams.m_expandOptions = true;
 	m_toolParams.m_anticipateTurns = true;
 	m_toolParams.m_optimizeVis = true;
@@ -211,23 +213,23 @@ void CrowdToolState::handleRender()
 		if (navquery)
 			duDebugDrawNavMeshNodes(&dd, *navquery);
 	}
-	
+
 	dd.depthMask(false);
 	
 	// Draw paths
 	if (m_toolParams.m_showPath)
 	{
-		if (m_agentDebug.idx != -1)
+		for (int i = 0; i < crowd->getAgentCount(); i++)
 		{
-			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
-			if (ag->active)
-			{
-				
-				const dtPolyRef* path = ag->corridor.getPath();
-				const int npath = ag->corridor.getPathCount();			
-				for (int i = 0; i < npath; ++i)
-					duDebugDrawNavMeshPoly(&dd, *nav, path[i], duRGBA(0,0,0,16));
-			}
+			if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
+				continue;
+			const dtCrowdAgent* ag =crowd->getAgent(i);
+			if (!ag->active)
+				continue;
+			const dtPolyRef* path = ag->corridor.getPath();
+			const int npath = ag->corridor.getPathCount();			
+			for (int j = 0; j < npath; ++j)
+				duDebugDrawNavMeshPoly(&dd, *nav, path[j], duRGBA(255,255,255,24));
 		}
 	}
 	
@@ -294,122 +296,111 @@ void CrowdToolState::handleRender()
 	}
 	
 	// Corners & co
-	if (m_agentDebug.idx != -1)
+	for (int i = 0; i < crowd->getAgentCount(); i++)
 	{
-		const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
-		if (ag->active)
+		if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
+			continue;
+		const dtCrowdAgent* ag =crowd->getAgent(i);
+		if (!ag->active)
+			continue;
+			
+		const float radius = ag->params.radius;
+		const float* pos = ag->npos;
+		
+		if (m_toolParams.m_showCorners)
 		{
-			
-			const float radius = ag->params.radius;
-			const float* pos = ag->npos;
-			
-			if (m_toolParams.m_showCorners)
-			{
-				if (ag->ncorners)
-				{
-					dd.begin(DU_DRAW_LINES, 2.0f);
-					for (int j = 0; j < ag->ncorners; ++j)
-					{
-						const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
-						const float* vb = &ag->cornerVerts[j*3];
-						dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192));
-						dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192));
-					}
-					if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
-					{
-						const float* v = &ag->cornerVerts[(ag->ncorners-1)*3];
-						dd.vertex(v[0],v[1],v[2], duRGBA(192,0,0,192));
-						dd.vertex(v[0],v[1]+radius*2,v[2], duRGBA(192,0,0,192));
-					}
-					
-					dd.end();
-					
-					
-					if (m_toolParams.m_anticipateTurns)
-					{
-						/*					float dvel[3], pos[3];
-						 calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel);
-						 pos[0] = ag->pos[0] + dvel[0];
-						 pos[1] = ag->pos[1] + dvel[1];
-						 pos[2] = ag->pos[2] + dvel[2];
-						 
-						 const float off = ag->radius+0.1f;
-						 const float* tgt = &ag->cornerVerts[0];
-						 const float y = ag->pos[1]+off;
-						 
-						 dd.begin(DU_DRAW_LINES, 2.0f);
-						 
-						 dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192));
-						 dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
-						 
-						 dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
-						 dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
-						 
-						 dd.end();*/
-					}
-				}
-			}
-			
-			if (m_toolParams.m_showCollisionSegments)
-			{
-				const float* center = ag->boundary.getCenter();
-				duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f);
-				duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange,
-								  duRGBA(192,0,128,128), 2.0f);
-				
-				dd.begin(DU_DRAW_LINES, 3.0f);
-				for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
-				{
-					const float* s = ag->boundary.getSegment(j);
-					unsigned int col = duRGBA(192,0,128,192);
-					if (dtTriArea2D(pos, s, s+3) < 0.0f)
-						col = duDarkenCol(col);
-					
-					duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
-				}
-				dd.end();
-			}
-			
-			if (m_toolParams.m_showNeis)
-			{
-				duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
-								  duRGBA(0,192,128,128), 2.0f);
-				
-				dd.begin(DU_DRAW_LINES, 2.0f);
-				for (int j = 0; j < ag->nneis; ++j)
-				{
-					// Get 'n'th active agent.
-					// TODO: fix this properly.
-					int n = ag->neis[j].idx;
-					const dtCrowdAgent* nei = 0;
-					for (int i = 0; i < crowd->getAgentCount(); ++i)
-					{
-						const dtCrowdAgent* nag = crowd->getAgent(i);
-						if (!nag->active) continue;
-						if (n == 0)
-						{
-							nei = nag;
-							break;
-						}
-						n--;
-					}
-					if (nei)
-					{
-						dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
-						dd.vertex(nei->npos[0],nei->npos[1]+radius,nei->npos[2], duRGBA(0,192,128,128));
-					}
-				}
-				dd.end();
-			}
-			
-			if (m_toolParams.m_showOpt)
+			if (ag->ncorners)
 			{
 				dd.begin(DU_DRAW_LINES, 2.0f);
-				dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192));
-				dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192));
+				for (int j = 0; j < ag->ncorners; ++j)
+				{
+					const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
+					const float* vb = &ag->cornerVerts[j*3];
+					dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192));
+					dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192));
+				}
+				if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
+				{
+					const float* v = &ag->cornerVerts[(ag->ncorners-1)*3];
+					dd.vertex(v[0],v[1],v[2], duRGBA(192,0,0,192));
+					dd.vertex(v[0],v[1]+radius*2,v[2], duRGBA(192,0,0,192));
+				}
+				
 				dd.end();
+				
+				
+				if (m_toolParams.m_anticipateTurns)
+				{
+					/*					float dvel[3], pos[3];
+					 calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel);
+					 pos[0] = ag->pos[0] + dvel[0];
+					 pos[1] = ag->pos[1] + dvel[1];
+					 pos[2] = ag->pos[2] + dvel[2];
+					 
+					 const float off = ag->radius+0.1f;
+					 const float* tgt = &ag->cornerVerts[0];
+					 const float y = ag->pos[1]+off;
+					 
+					 dd.begin(DU_DRAW_LINES, 2.0f);
+					 
+					 dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192));
+					 dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
+					 
+					 dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
+					 dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
+					 
+					 dd.end();*/
+				}
 			}
 		}
+		
+		if (m_toolParams.m_showCollisionSegments)
+		{
+			const float* center = ag->boundary.getCenter();
+			duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f);
+			duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange,
+							  duRGBA(192,0,128,128), 2.0f);
+			
+			dd.begin(DU_DRAW_LINES, 3.0f);
+			for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
+			{
+				const float* s = ag->boundary.getSegment(j);
+				unsigned int col = duRGBA(192,0,128,192);
+				if (dtTriArea2D(pos, s, s+3) < 0.0f)
+					col = duDarkenCol(col);
+				
+				duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
+			}
+			dd.end();
+		}
+		
+		if (m_toolParams.m_showNeis)
+		{
+			duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
+							  duRGBA(0,192,128,128), 2.0f);
+			
+			dd.begin(DU_DRAW_LINES, 2.0f);
+			for (int j = 0; j < ag->nneis; ++j)
+			{
+				// Get 'n'th active agent.
+				// TODO: fix this properly.
+				const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
+				if (nei)
+				{
+					dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
+					dd.vertex(nei->npos[0],nei->npos[1]+radius,nei->npos[2], duRGBA(0,192,128,128));
+				}
+			}
+			dd.end();
+		}
+		
+		if (m_toolParams.m_showOpt)
+		{
+			dd.begin(DU_DRAW_LINES, 2.0f);
+			dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192));
+			dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192));
+			dd.end();
+		}
 	}
 	
 	// Agent cylinders.
@@ -421,7 +412,11 @@ void CrowdToolState::handleRender()
 		const float radius = ag->params.radius;
 		const float* pos = ag->npos;
 		
-		duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f);
+		unsigned int col = duRGBA(0,0,0,32);
+		if (m_agentDebug.idx == i)
+			col = duRGBA(255,0,0,128);
+			
+		duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, col, 2.0f);
 	}
 	
 	for (int i = 0; i < crowd->getAgentCount(); ++i)
@@ -434,46 +429,54 @@ void CrowdToolState::handleRender()
 		const float* pos = ag->npos;
 		
 		unsigned int col = duRGBA(220,220,220,128);
-		if (m_agentDebug.idx == i)
-			col = duRGBA(255,192,0,128);
+		if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
+			col = duLerpCol(col, duRGBA(128,0,255,128), 32);
+		else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
+			col = duLerpCol(col, duRGBA(128,0,255,128), 128);
+		else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
+			col = duRGBA(255,32,16,128);
+		else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			col = duLerpCol(col, duRGBA(64,255,0,128), 128);
 		
 		duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
 							pos[0]+radius, pos[1]+height, pos[2]+radius, col);
 	}
 	
 	
-	if (m_agentDebug.idx != -1)
+	if (m_toolParams.m_showVO)
 	{
-		const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
-		if (ag->active)
+		for (int i = 0; i < crowd->getAgentCount(); i++)
 		{
-			if (m_toolParams.m_showVO)
+			if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
+				continue;
+			const dtCrowdAgent* ag =crowd->getAgent(i);
+			if (!ag->active)
+				continue;
+		
+			// Draw detail about agent sela
+			const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
+			
+			const float dx = ag->npos[0];
+			const float dy = ag->npos[1]+ag->params.height;
+			const float dz = ag->npos[2];
+			
+			duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
+			
+			dd.begin(DU_DRAW_QUADS);
+			for (int j = 0; j < vod->getSampleCount(); ++j)
 			{
-				// Draw detail about agent sela
-				const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
-				
-				const float dx = ag->npos[0];
-				const float dy = ag->npos[1]+ag->params.height;
-				const float dz = ag->npos[2];
-				
-				duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
-				
-				dd.begin(DU_DRAW_QUADS);
-				for (int i = 0; i < vod->getSampleCount(); ++i)
-				{
-					const float* p = vod->getSampleVelocity(i);
-					const float sr = vod->getSampleSize(i);
-					const float pen = vod->getSamplePenalty(i);
-					const float pen2 = vod->getSamplePreferredSidePenalty(i);
-					unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255));
-					col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128));
-					dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
-					dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
-					dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
-					dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
-				}
-				dd.end();
-			}		
+				const float* p = vod->getSampleVelocity(j);
+				const float sr = vod->getSampleSize(j);
+				const float pen = vod->getSamplePenalty(j);
+				const float pen2 = vod->getSamplePreferredSidePenalty(j);
+				unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255));
+				col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128));
+				dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
+				dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
+				dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
+				dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
+			}
+			dd.end();
 		}
 	}
 	
@@ -490,8 +493,14 @@ void CrowdToolState::handleRender()
 		const float* dvel = ag->dvel;
 		
 		unsigned int col = duRGBA(220,220,220,192);
-		if (m_agentDebug.idx == i)
-			col = duRGBA(255,192,0,192);
+		if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
+			col = duLerpCol(col, duRGBA(128,0,255,192), 32);
+		else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
+			col = duLerpCol(col, duRGBA(128,0,255,192), 128);
+		else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
+			col = duRGBA(255,32,16,192);
+		else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			col = duLerpCol(col, duRGBA(64,255,0,192), 128);
 		
 		duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
 		
@@ -521,6 +530,36 @@ void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
 	
 	char label[32];
 	
+	if (m_toolParams.m_showNodes)
+	{
+		dtCrowd* crowd = m_sample->getCrowd();
+		if (crowd && crowd->getPathQueue())
+		{
+			const dtNavMeshQuery* navquery = crowd->getPathQueue()->getNavQuery();
+			const dtNodePool* pool = navquery->getNodePool();
+			if (pool)
+			{
+				const float off = 0.5f;
+				for (int i = 0; i < pool->getHashSize(); ++i)
+				{
+					for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j))
+					{
+						const dtNode* node = pool->getNodeAtIdx(j+1);
+						if (!node) continue;
+
+						if (gluProject((GLdouble)node->pos[0],(GLdouble)node->pos[1]+off,(GLdouble)node->pos[2],
+									   model, proj, view, &x, &y, &z))
+						{
+							const float heuristic = node->total;// - node->cost;
+							snprintf(label, 32, "%.2f", heuristic);
+							imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(0,0,0,220));
+						}
+					}
+				}
+			}
+		}
+	}
+	
 	if (m_toolParams.m_showLabels)
 	{
 		dtCrowd* crowd = m_sample->getCrowd();
@@ -546,11 +585,14 @@ void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
 		dtCrowd* crowd = m_sample->getCrowd();
 		if (crowd) 
 		{
-			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
-			if (ag->active)
+			for (int i = 0; i < crowd->getAgentCount(); i++)
 			{
+				if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
+					continue;
+				const dtCrowdAgent* ag =crowd->getAgent(i);
+				if (!ag->active)
+					continue;
 				const float radius = ag->params.radius;
-				
 				if (m_toolParams.m_showNeis)
 				{
 					for (int j = 0; j < ag->nneis; ++j)
@@ -650,6 +692,14 @@ void CrowdToolState::hilightAgent(const int idx)
 	m_agentDebug.idx = idx;
 }
 
+static void calcVel(float* vel, const float* pos, const float* tgt, const float speed)
+{
+	dtVsub(vel, tgt, pos);
+	vel[1] = 0.0;
+	dtVnormalize(vel);
+	dtVscale(vel, vel, speed);
+}
+
 void CrowdToolState::setMoveTarget(const float* p, bool adjust)
 {
 	if (!m_sample) return;
@@ -659,17 +709,19 @@ void CrowdToolState::setMoveTarget(const float* p, bool adjust)
 	dtCrowd* crowd = m_sample->getCrowd();
 	const dtQueryFilter* filter = crowd->getFilter();
 	const float* ext = crowd->getQueryExtents();
-	
-	navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
-	
+
 	if (adjust)
 	{
-		// Adjust target using tiny local search.
+		float vel[3];
+		// Request velocity
 		if (m_agentDebug.idx != -1)
 		{
 			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
 			if (ag && ag->active)
-				crowd->adjustMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos);
+			{
+				calcVel(vel, ag->npos, p, ag->params.maxSpeed);
+				crowd->requestMoveVelocity(m_agentDebug.idx, vel);
+			}
 		}
 		else
 		{
@@ -677,13 +729,15 @@ void CrowdToolState::setMoveTarget(const float* p, bool adjust)
 			{
 				const dtCrowdAgent* ag = crowd->getAgent(i);
 				if (!ag->active) continue;
-				crowd->adjustMoveTarget(i, m_targetRef, m_targetPos);
+				calcVel(vel, ag->npos, p, ag->params.maxSpeed);
+				crowd->requestMoveVelocity(i, vel);
 			}
 		}
 	}
 	else
 	{
-		// Move target using path finder
+		navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
+		
 		if (m_agentDebug.idx != -1)
 		{
 			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
@@ -931,6 +985,8 @@ void CrowdTool::handleMenu()
 			params->m_showNodes = !params->m_showNodes;
 		if (imguiCheck("Show Perf Graph", params->m_showPerfGraph))
 			params->m_showPerfGraph = !params->m_showPerfGraph;
+		if (imguiCheck("Show Detail All", params->m_showDetailAll))
+			params->m_showDetailAll = !params->m_showDetailAll;
 		imguiUnindent();
 	}
 }
@@ -1030,9 +1086,9 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
 	}
 	else if (m_mode == TOOLMODE_MOVE_TARGET)
 	{
-		imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: set move target.  Shift+LMB: adjust target.", imguiRGBA(255,255,255,192));	
+		imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: set move target.  Shift+LMB: adjust set velocity.", imguiRGBA(255,255,255,192));	
 		ty -= 20;
-		imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "Adjusting allows to change the target location in short range without pathfinder.", imguiRGBA(255,255,255,192));	
+		imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "Setting velocity will move the agents without pathfinder.", imguiRGBA(255,255,255,192));	
 	}
 	else if (m_mode == TOOLMODE_SELECT)
 	{