diff --git a/DebugUtils/Include/DetourDebugDraw.h b/DebugUtils/Include/DetourDebugDraw.h
index eb89ad4..d076db0 100755
--- a/DebugUtils/Include/DetourDebugDraw.h
+++ b/DebugUtils/Include/DetourDebugDraw.h
@@ -35,6 +35,7 @@ void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& m
 void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query);
 void duDebugDrawNavMeshBVTree(struct duDebugDraw* dd, const dtNavMesh& mesh);
 void duDebugDrawNavMeshPortals(struct duDebugDraw* dd, const dtNavMesh& mesh);
+void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh, const unsigned short polyFlags, const unsigned int col);
 void duDebugDrawNavMeshPoly(struct duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col);
 
 void duDebugDrawTileCacheLayer(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch);
diff --git a/DebugUtils/Source/DetourDebugDraw.cpp b/DebugUtils/Source/DetourDebugDraw.cpp
index be5370f..f9af0e8 100755
--- a/DebugUtils/Source/DetourDebugDraw.cpp
+++ b/DebugUtils/Source/DetourDebugDraw.cpp
@@ -424,6 +424,26 @@ void duDebugDrawNavMeshPortals(duDebugDraw* dd, const dtNavMesh& mesh)
 	}
 }
 
+void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh,
+									  const unsigned short polyFlags, const unsigned int col)
+{
+	if (!dd) return;
+	
+	for (int i = 0; i < mesh.getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = mesh.getTile(i);
+		if (!tile->header) continue;
+		dtPolyRef base = mesh.getPolyRefBase(tile);
+
+		for (int j = 0; j < tile->header->polyCount; ++j)
+		{
+			const dtPoly* p = &tile->polys[j];
+			if ((p->flags & polyFlags) == 0) continue;
+			duDebugDrawNavMeshPoly(dd, mesh, base|(dtPolyRef)j, col);
+		}
+	}
+}
+
 void duDebugDrawNavMeshPoly(duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col)
 {
 	if (!dd) return;
diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h
index f876b10..d178bec 100644
--- a/Detour/Include/DetourNavMeshQuery.h
+++ b/Detour/Include/DetourNavMeshQuery.h
@@ -328,7 +328,10 @@ public:
 	///  @returns true if over polygon.
 	dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const;
 		
-	/// Returns true if poly reference ins in closed list.
+	// Returns true if polygon reference points to valid data and passes the filter.
+	bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const;
+	
+	// Returns true if poly reference ins in closed list.
 	bool isInClosedList(dtPolyRef ref) const;
 	
 	class dtNodePool* getNodePool() const { return m_nodePool; }
diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp
index 7f2b1f7..48b6bb4 100644
--- a/Detour/Source/DetourNavMeshQuery.cpp
+++ b/Detour/Source/DetourNavMeshQuery.cpp
@@ -1200,8 +1200,14 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				// Next portal.
 				if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
 				{
+					// Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
+					// Clamp the end point to path[i], and return the path so far.
+					
 					if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
+					{
+						// This should only happen when the first polygon is invalid.
 						return DT_FAILURE | DT_INVALID_PARAM;
+					}
 					
 					dtVcopy(&straightPath[n*3], closestEndPos);
 					if (straightPathFlags)
@@ -1210,7 +1216,9 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 						straightPathRefs[n] = path[i];
 					n++;
 					
-					return DT_SUCCESS;
+					*straightPathCount = n;
+					
+					return DT_SUCCESS | DT_PARTIAL_RESULT | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
 				}
 				
 				// If starting really close the portal, advance.
@@ -2706,6 +2714,20 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 	return status;
 }
 
+bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const
+{
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly);
+	// If cannot get polygon, assume it does not exists and boundary is invalid.
+	if (dtStatusFailed(status))
+		return false;
+	// If cannot pass filter, assume flags has changed and boundary is invalid.
+	if (!filter->passFilter(ref, tile, poly))
+		return false;
+	return true;
+}
+
 bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
 {
 	if (!m_nodePool) return false;
diff --git a/DetourCrowd/Include/DetourCrowd.h b/DetourCrowd/Include/DetourCrowd.h
index ec4c02c..96c9c44 100644
--- a/DetourCrowd/Include/DetourCrowd.h
+++ b/DetourCrowd/Include/DetourCrowd.h
@@ -38,6 +38,7 @@ struct dtCrowdNeighbour
 
 enum CrowdAgentState
 {
+	DT_CROWDAGENT_STATE_INVALID,
 	DT_CROWDAGENT_STATE_WALKING,
 	DT_CROWDAGENT_STATE_OFFMESH,
 };
@@ -138,6 +139,7 @@ class dtCrowd
 
 	enum MoveRequestState
 	{
+		MR_TARGET_NONE,
 		MR_TARGET_FAILED,
 		MR_TARGET_VALID,
 		MR_TARGET_REQUESTING,
@@ -158,6 +160,7 @@ class dtCrowd
 		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;
@@ -166,9 +169,13 @@ class dtCrowd
 
 	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);
 
 	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);
+
 	void purge();
 	
 public:
@@ -194,6 +201,7 @@ public:
 	void update(const float dt, dtCrowdAgentDebugInfo* debug);
 	
 	const dtQueryFilter* getFilter() const { return &m_filter; }
+	dtQueryFilter* getEditableFilter() { return &m_filter; }
 	const float* getQueryExtents() const { return m_ext; }
 	
 	inline int getVelocitySampleCount() const { return m_velocitySampleCount; }
@@ -203,5 +211,8 @@ public:
 	const dtNavMeshQuery* getNavMeshQuery() const { return m_navquery; }
 };
 
+dtCrowd* dtAllocCrowd();
+void dtFreeCrowd(dtCrowd* ptr);
 
-#endif // CROWDMANAGER_H
+
+#endif // DETOURCROWD_H
diff --git a/DetourCrowd/Include/DetourLocalBoundary.h b/DetourCrowd/Include/DetourLocalBoundary.h
index d307803..d77a136 100644
--- a/DetourCrowd/Include/DetourLocalBoundary.h
+++ b/DetourCrowd/Include/DetourLocalBoundary.h
@@ -24,7 +24,8 @@
 
 class dtLocalBoundary
 {
-	static const int MAX_SEGS = 8;
+	static const int MAX_LOCAL_SEGS = 8;
+	static const int MAX_LOCAL_POLYS = 16;
 	
 	struct Segment
 	{
@@ -33,9 +34,12 @@ class dtLocalBoundary
 	};
 	
 	float m_center[3];
-	Segment m_segs[MAX_SEGS];
+	Segment m_segs[MAX_LOCAL_SEGS];
 	int m_nsegs;
 	
+	dtPolyRef m_polys[MAX_LOCAL_POLYS];
+	int m_npolys;
+
 	void addSegment(const float dist, const float* seg);
 	
 public:
@@ -47,6 +51,8 @@ public:
 	void update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
 				dtNavMeshQuery* navquery, const dtQueryFilter* filter);
 	
+	bool isValid(dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
 	inline const float* getCenter() const { return m_center; }
 	inline int getSegmentCount() const { return m_nsegs; }
 	inline const float* getSegment(int i) const { return m_segs[i].s; }
diff --git a/DetourCrowd/Include/DetourPathCorridor.h b/DetourCrowd/Include/DetourPathCorridor.h
index 3a1f904..63373cd 100644
--- a/DetourCrowd/Include/DetourPathCorridor.h
+++ b/DetourCrowd/Include/DetourPathCorridor.h
@@ -51,6 +51,11 @@ public:
 	bool moveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs,
 								   float* startPos, float* endPos,
 								   dtNavMeshQuery* navquery);
+
+	bool trimInvalidPath(dtPolyRef safeRef, const float* safePos,
+						 dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	bool isValid(const int maxLookAhead, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
 	
 	void movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
 	void moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
@@ -61,6 +66,7 @@ public:
 	inline const float* getTarget() const { return m_target; }
 	
 	inline dtPolyRef getFirstPoly() const { return m_npath ? m_path[0] : 0; }
+	inline dtPolyRef getLastPoly() const { return m_npath ? m_path[m_npath-1] : 0; }
 	
 	inline const dtPolyRef* getPath() const { return m_path; }
 	inline int getPathCount() const { return m_npath; } 	
diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp
index b0dc211..66d8b0c 100644
--- a/DetourCrowd/Source/DetourCrowd.cpp
+++ b/DetourCrowd/Source/DetourCrowd.cpp
@@ -31,12 +31,31 @@
 #include "DetourAlloc.h"
 
 
+#include <stdio.h>
+
+
+
+dtCrowd* dtAllocCrowd()
+{
+	void* mem = dtAlloc(sizeof(dtCrowd), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtCrowd;
+}
+
+void dtFreeCrowd(dtCrowd* ptr)
+{
+	if (!ptr) return;
+	ptr->~dtCrowd();
+	dtFree(ptr);
+}
+
+
 static const int MAX_ITERS_PER_UPDATE = 10;
 
 static const int MAX_PATHQUEUE_NODES = 4096;
 static const int MAX_COMMON_NODES = 512;
 
-inline float between(const float t, const float t0, const float t1)
+inline float tween(const float t, const float t0, const float t1)
 {
 	return dtClamp((t-t0) / (t1-t0), 0.0f, 1.0f);
 }
@@ -396,11 +415,6 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	float nearest[3];
 	dtPolyRef ref;
 	m_navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest);
-	if (!ref)
-	{
-		// Could not find a location on navmesh.
-		return -1;
-	}
 	
 	ag->corridor.reset(ref, nearest);
 	ag->boundary.reset();
@@ -419,7 +433,11 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
 	ag->t = 0;
 	ag->var = (rand() % 10) / 9.0f;
 
-	ag->state = DT_CROWDAGENT_STATE_WALKING;
+	if (ref)
+		ag->state = DT_CROWDAGENT_STATE_WALKING;
+	else
+		ag->state = DT_CROWDAGENT_STATE_INVALID;
+	
 	ag->active = 1;
 
 	return idx;
@@ -433,6 +451,65 @@ 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));
+	}
+	
+	// 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;
+	
+	req->temp[0] = ref;
+	req->ntemp = 1;
+
+	return true;
+}
+
+
 bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
 {
 	if (idx < 0 || idx > m_maxAgents)
@@ -464,6 +541,7 @@ bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
 	dtVcopy(req->pos, pos);
 	req->pathqRef = DT_PATHQ_INVALID;
 	req->state = MR_TARGET_REQUESTING;
+	req->replan = false;
 	
 	req->temp[0] = ref;
 	req->ntemp = 1;
@@ -471,6 +549,7 @@ bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
 	return true;
 }
 
+
 bool dtCrowd::adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos)
 {
 	if (idx < 0 || idx > m_maxAgents)
@@ -562,26 +641,68 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 		
 		if (req->state == MR_TARGET_REQUESTING)
 		{
-			// Calculate request position.
-			// 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)
+			// If agent location is invalid, try to recover.
+			if (ag->state == DT_CROWDAGENT_STATE_INVALID)
 			{
-				ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
-				req->state = MR_TARGET_WAITING_FOR_PATH;
+				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;
+				}
 			}
 		}
 	}
@@ -647,10 +768,26 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 						// Make space for the old path.
 						if ((npath-1)+nres > m_maxPathResult)
 							nres = m_maxPathResult - (npath-1);
+						
 						memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
 						// Copy old path in the beginning.
 						memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
 						nres += npath-1;
+						
+						// Remove trackbacks
+						for (int j = 0; j < nres; ++j)
+						{
+							if (j-1 >= 0 && j+1 < nres)
+							{
+								if (res[j-1] == res[j+1])
+								{
+									memmove(res+(j-1), res+(j+1), sizeof(dtPolyRef)*(nres-(j+1)));
+									nres -= 2;
+									j -= 2;
+								}
+							}
+						}
+						
 					}
 					
 					// Check for partial path.
@@ -667,7 +804,10 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 				
 				if (valid)
 				{
+					// Set current corridor.
 					ag->corridor.setCorridor(targetPos, res, nres);
+					// Force to update boundary.
+					ag->boundary.reset();
 					req->state = MR_TARGET_VALID;
 				}
 				else
@@ -741,7 +881,7 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
 	for (int i = 0; i < nagents; ++i)
 	{
 		dtCrowdAgent* ag = agents[i];
-		if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
 		if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
 			continue;
@@ -759,6 +899,101 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
 
 }
 
+void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const float dt)
+{
+	static const int CHECK_LOOKAHEAD = 10;
+	
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+		
+		// Skip if the corridor is valid
+		if (ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
+			continue;
+
+		// The current path is bad, try to recover.
+
+		// 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.
+		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)
+		{
+			// Count not find location in navmesh, set state to invalid.
+			ag->corridor.reset(0, agentPos);
+			ag->boundary.reset();
+			ag->state = DT_CROWDAGENT_STATE_INVALID;
+			continue;
+		}
+		
+		// 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))
+		{
+			// 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 (!targetRef)
+		{
+			// Target not reachable anymore, cannot replan.
+			// TODO: indicate failure!
+			continue;
+		}
+		
+		// Try to replan path to goal.
+		requestMoveTargetReplan(idx, targetRef, targetPos);
+	}
+}
+	
 void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 {
 	m_velocitySampleCount = 0;
@@ -768,6 +1003,9 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	dtCrowdAgent** agents = m_activeAgents;
 	int nagents = getActiveAgents(agents, m_maxAgents);
 	
+	// Check that all agents still have valid paths.
+	checkPathValidty(agents, nagents, dt);
+	
 	// Update async move request and path finder.
 	updateMoveRequest(dt);
 
@@ -788,12 +1026,14 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	for (int i = 0; i < nagents; ++i)
 	{
 		dtCrowdAgent* ag = agents[i];
-		if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
 
-		// Only update the collision boundary after certain distance has been passed.
+		// Update the collision boundary after certain distance has been passed or
+		// if it has become invalid.
 		const float updateThr = ag->params.collisionQueryRange*0.25f;
-		if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr))
+		if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
+			!ag->boundary.isValid(m_navquery, &m_filter))
 		{
 			ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
 								m_navquery, &m_filter);
@@ -809,7 +1049,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 	{
 		dtCrowdAgent* ag = agents[i];
 		
-		if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
 			continue;
 		
 		// Find corners for steering
@@ -829,7 +1069,6 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 				dtVcopy(debug->optStart, ag->corridor.getPos());
 				dtVcopy(debug->optEnd, target);
 			}
-			
 		}
 		else
 		{
@@ -1119,12 +1358,12 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
 		const float tb = anim->tmax;
 		if (anim->t < ta)
 		{
-			const float u = between(anim->t, 0.0, ta);
+			const float u = tween(anim->t, 0.0, ta);
 			dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
 		}
 		else
 		{
-			const float u = between(anim->t, ta, tb);
+			const float u = tween(anim->t, ta, tb);
 			dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
 		}
 			
diff --git a/DetourCrowd/Source/DetourLocalBoundary.cpp b/DetourCrowd/Source/DetourLocalBoundary.cpp
index 7ad28b4..79d4322 100644
--- a/DetourCrowd/Source/DetourLocalBoundary.cpp
+++ b/DetourCrowd/Source/DetourLocalBoundary.cpp
@@ -25,7 +25,8 @@
 
 
 dtLocalBoundary::dtLocalBoundary() :
-	m_nsegs(0)
+	m_nsegs(0),
+	m_npolys(0)
 {
 	dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
 }
@@ -37,6 +38,7 @@ dtLocalBoundary::~dtLocalBoundary()
 void dtLocalBoundary::reset()
 {
 	dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
+	m_npolys = 0;
 	m_nsegs = 0;
 }
 
@@ -52,7 +54,7 @@ void dtLocalBoundary::addSegment(const float dist, const float* s)
 	else if (dist >= m_segs[m_nsegs-1].d)
 	{
 		// Further than the last segment, skip.
-		if (m_nsegs >= MAX_SEGS)
+		if (m_nsegs >= MAX_LOCAL_SEGS)
 			return;
 		// Last, trivial accept.
 		seg = &m_segs[m_nsegs];
@@ -65,8 +67,8 @@ void dtLocalBoundary::addSegment(const float dist, const float* s)
 			if (dist <= m_segs[i].d)
 				break;
 		const int tgt = i+1;
-		const int n = dtMin(m_nsegs-i, MAX_SEGS-tgt);
-		dtAssert(tgt+n <= MAX_SEGS);
+		const int n = dtMin(m_nsegs-i, MAX_LOCAL_SEGS-tgt);
+		dtAssert(tgt+n <= MAX_LOCAL_SEGS);
 		if (n > 0)
 			memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n);
 		seg = &m_segs[i];
@@ -75,38 +77,36 @@ void dtLocalBoundary::addSegment(const float dist, const float* s)
 	seg->d = dist;
 	memcpy(seg->s, s, sizeof(float)*6);
 	
-	if (m_nsegs < MAX_SEGS)
+	if (m_nsegs < MAX_LOCAL_SEGS)
 		m_nsegs++;
 }
 
 void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
 							 dtNavMeshQuery* navquery, const dtQueryFilter* filter)
 {
-	static const int MAX_LOCAL_POLYS = 16;
 	static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*3;
 	
 	if (!ref)
 	{
 		dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
 		m_nsegs = 0;
+		m_npolys = 0;
 		return;
 	}
 	
 	dtVcopy(m_center, pos);
 	
 	// First query non-overlapping polygons.
-	dtPolyRef locals[MAX_LOCAL_POLYS];
-	int nlocals = 0;
 	navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange,
-									 filter, locals, 0, &nlocals, MAX_LOCAL_POLYS);
+									 filter, m_polys, 0, &m_npolys, MAX_LOCAL_POLYS);
 	
 	// Secondly, store all polygon edges.
 	m_nsegs = 0;
 	float segs[MAX_SEGS_PER_POLY*6];
 	int nsegs = 0;
-	for (int j = 0; j < nlocals; ++j)
+	for (int j = 0; j < m_npolys; ++j)
 	{
-		navquery->getPolyWallSegments(locals[j], filter, segs, 0, &nsegs, MAX_SEGS_PER_POLY);
+		navquery->getPolyWallSegments(m_polys[j], filter, segs, 0, &nsegs, MAX_SEGS_PER_POLY);
 		for (int k = 0; k < nsegs; ++k)
 		{
 			const float* s = &segs[k*6];
@@ -119,3 +119,19 @@ void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collis
 		}
 	}
 }
+
+bool dtLocalBoundary::isValid(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	if (!m_npolys)
+		return false;
+	
+	// Check that all polygons still pass query filter.
+	for (int i = 0; i < m_npolys; ++i)
+	{
+		if (!navquery->isValidPolyRef(m_polys[i], filter))
+			return false;
+	}
+	
+	return true;
+}
+
diff --git a/DetourCrowd/Source/DetourPathCorridor.cpp b/DetourCrowd/Source/DetourPathCorridor.cpp
index d53d7b2..2702b51 100644
--- a/DetourCrowd/Source/DetourPathCorridor.cpp
+++ b/DetourCrowd/Source/DetourPathCorridor.cpp
@@ -155,6 +155,7 @@ int dtMergeCorridorStartShortcut(dtPolyRef* path, const int npath, const int max
 	return req+size;
 }
 
+
 dtPathCorridor::dtPathCorridor() :
 	m_path(0),
 	m_npath(0),
@@ -263,6 +264,8 @@ void dtPathCorridor::optimizePathVisibility(const float* next, const float pathO
 
 bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
 {
+	dtAssert(navquery);
+	dtAssert(filter);
 	dtAssert(m_path);
 	
 	if (m_npath < 3)
@@ -384,3 +387,55 @@ void dtPathCorridor::setCorridor(const float* target, const dtPolyRef* path, con
 	memcpy(m_path, path, sizeof(dtPolyRef)*npath);
 	m_npath = npath;
 }
+
+bool dtPathCorridor::trimInvalidPath(dtPolyRef safeRef, const float* safePos,
+									 dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	dtAssert(navquery);
+	dtAssert(filter);
+	dtAssert(m_path);
+	
+	// Keep valid path as far as possible.
+	int n = 0;
+	while (n < m_npath && navquery->isValidPolyRef(m_path[n], filter)) {
+		n++;
+	}
+	
+	if (n == m_npath)
+	{
+		// All valid, no need to fix.
+		return true;
+	}
+	else if (n == 0)
+	{
+		// The first polyref is bad, use current safe values.
+		dtVcopy(m_pos, safePos);
+		m_path[0] = safeRef;
+		m_npath = 1;
+	}
+	else
+	{
+		// The path is partially usable.
+		m_npath = n;
+	}
+	
+	// Clamp target pos to last poly
+	float tgt[3];
+	dtVcopy(tgt, m_target);
+	navquery->closestPointOnPolyBoundary(m_path[m_npath-1], tgt, m_target);
+	
+	return true;
+}
+
+bool dtPathCorridor::isValid(const int maxLookAhead, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	// Check that all polygons still pass query filter.
+	const int n = dtMin(m_npath, maxLookAhead);
+	for (int i = 0; i < n; ++i)
+	{
+		if (!navquery->isValidPolyRef(m_path[i], filter))
+			return false;
+	}
+
+	return true;
+}
diff --git a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast
index 9918fb4..d43910f 100755
Binary files a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast and b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast differ
diff --git a/RecastDemo/Include/CrowdTool.h b/RecastDemo/Include/CrowdTool.h
index 7ba904b..e0d9088 100644
--- a/RecastDemo/Include/CrowdTool.h
+++ b/RecastDemo/Include/CrowdTool.h
@@ -35,7 +35,6 @@ class CrowdTool : public SampleTool
 	float m_targetPos[3];
 	dtPolyRef m_targetRef;
 
-
 	bool m_expandSelectedDebugDraw;
 	bool m_showCorners;
 	bool m_showCollisionSegments;
@@ -73,16 +72,16 @@ class CrowdTool : public SampleTool
 	};
 	AgentTrail m_trails[MAX_AGENTS];
 	
-	dtCrowd m_crowd;
-		
 	ValueHistory m_crowdTotalTime;
 	ValueHistory m_crowdSampleCount;
 	
+	
 	enum ToolMode
 	{
 		TOOLMODE_CREATE,
 		TOOLMODE_MOVE_TARGET,
 		TOOLMODE_SELECT,
+		TOOLMODE_TOGGLE_POLYS,
 	};
 	ToolMode m_mode;
 	
diff --git a/RecastDemo/Include/Sample.h b/RecastDemo/Include/Sample.h
index 87e8425..9b617dd 100644
--- a/RecastDemo/Include/Sample.h
+++ b/RecastDemo/Include/Sample.h
@@ -49,11 +49,12 @@ enum SamplePolyAreas
 };
 enum SamplePolyFlags
 {
-	SAMPLE_POLYFLAGS_WALK = 0x01,		///< Ability to walk (ground, grass, road)
-	SAMPLE_POLYFLAGS_SWIM = 0x02,		///< Ability to swim (water).
-	SAMPLE_POLYFLAGS_DOOR = 0x04,		///< Ability to move through doors.
-	SAMPLE_POLYFLAGS_JUMP = 0x08,		///< Ability to jump.
-	SAMPLE_POLYFLAGS_ALL = 0xffff		///< All abilities.
+	SAMPLE_POLYFLAGS_WALK		= 0x01,		// Ability to walk (ground, grass, road)
+	SAMPLE_POLYFLAGS_SWIM		= 0x02,		// Ability to swim (water).
+	SAMPLE_POLYFLAGS_DOOR		= 0x04,		// Ability to move through doors.
+	SAMPLE_POLYFLAGS_JUMP		= 0x08,		// Ability to jump.
+	SAMPLE_POLYFLAGS_DISABLED	= 0x10,		// Disabled polygon
+	SAMPLE_POLYFLAGS_ALL		= 0xffff	// All abilities.
 };
 
 struct SampleTool
@@ -78,6 +79,8 @@ protected:
 	class InputGeom* m_geom;
 	class dtNavMesh* m_navMesh;
 	class dtNavMeshQuery* m_navQuery;
+	class dtCrowd* m_crowd;
+
 	unsigned char m_navMeshDrawFlags;
 
 	float m_cellSize;
@@ -122,6 +125,7 @@ public:
 	virtual class InputGeom* getInputGeom() { return m_geom; }
 	virtual class dtNavMesh* getNavMesh() { return m_navMesh; }
 	virtual class dtNavMeshQuery* getNavMeshQuery() { return m_navQuery; }
+	virtual class dtCrowd* getCrowd() { return m_crowd; }
 	virtual float getAgentRadius() { return m_agentRadius; }
 	virtual float getAgentHeight() { return m_agentHeight; }
 	virtual float getAgentClimb() { return m_agentMaxClimb; }
diff --git a/RecastDemo/Source/CrowdTool.cpp b/RecastDemo/Source/CrowdTool.cpp
index 7d4006f..3d1c582 100644
--- a/RecastDemo/Source/CrowdTool.cpp
+++ b/RecastDemo/Source/CrowdTool.cpp
@@ -147,42 +147,47 @@ void CrowdTool::init(Sample* sample)
 	}
 		
 	dtNavMesh* nav = m_sample->getNavMesh();
-	if (nav)
+	dtCrowd* crowd = m_sample->getCrowd();
+	if (nav && crowd && crowd->getAgentCount() == 0)
 	{
-		m_crowd.init(MAX_AGENTS, m_sample->getAgentRadius(), nav);
+		crowd->init(MAX_AGENTS, m_sample->getAgentRadius(), nav);
+
+		// Make polygons with 'disabled' flag invalid.
+		crowd->getEditableFilter()->setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);
 		
 		// Setup local avoidance params to different qualities.
 		dtObstacleAvoidanceParams params;
 		// Use mostly default settings, copy from dtCrowd.
-		memcpy(&params, m_crowd.getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
+		memcpy(&params, crowd->getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
 		
 		// Low (11)
 		params.velBias = 0.5f;
 		params.adaptiveDivs = 5;
 		params.adaptiveRings = 2;
 		params.adaptiveDepth = 1;
-		m_crowd.setObstacleAvoidanceParams(0, &params);
+		crowd->setObstacleAvoidanceParams(0, &params);
 
 		// Medium (22)
 		params.velBias = 0.5f;
 		params.adaptiveDivs = 5;
 		params.adaptiveRings = 2;
 		params.adaptiveDepth = 2;
-		m_crowd.setObstacleAvoidanceParams(1, &params);
+		crowd->setObstacleAvoidanceParams(1, &params);
 		
 		// Good (45)
 		params.velBias = 0.5f;
 		params.adaptiveDivs = 7;
 		params.adaptiveRings = 2;
 		params.adaptiveDepth = 3;
-		m_crowd.setObstacleAvoidanceParams(2, &params);
+		crowd->setObstacleAvoidanceParams(2, &params);
 		
 		// High (66)
 		params.velBias = 0.5f;
 		params.adaptiveDivs = 7;
 		params.adaptiveRings = 3;
 		params.adaptiveDepth = 3;
-		m_crowd.setObstacleAvoidanceParams(3, &params);		
+		
+		crowd->setObstacleAvoidanceParams(3, &params);
 	}
 
 }
@@ -201,6 +206,8 @@ void CrowdTool::handleMenu()
 		m_mode = TOOLMODE_MOVE_TARGET;
 	if (imguiCheck("Select Agent", m_mode == TOOLMODE_SELECT))
 		m_mode = TOOLMODE_SELECT;
+	if (imguiCheck("Toggle Polys", m_mode == TOOLMODE_TOGGLE_POLYS))
+		m_mode = TOOLMODE_TOGGLE_POLYS;
 	
 	imguiSeparatorLine();
 		
@@ -291,6 +298,8 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 	if (!m_sample) return;
 	InputGeom* geom = m_sample->getInputGeom();
 	if (!geom) return;
+	dtCrowd* crowd = m_sample->getCrowd();
+	if (!crowd) return;
 
 	if (m_mode == TOOLMODE_CREATE)
 	{
@@ -300,9 +309,9 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 			int isel = -1;
 			float tsel = FLT_MAX;
 			
-			for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+			for (int i = 0; i < crowd->getAgentCount(); ++i)
 			{
-				const dtCrowdAgent* ag = m_crowd.getAgent(i);
+				const dtCrowdAgent* ag = crowd->getAgent(i);
 				if (!ag->active) continue;
 				float bmin[3], bmax[3];
 				getAgentBounds(ag, bmin, bmax);
@@ -318,7 +327,7 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 			}
 			if (isel != -1)
 			{
-				m_crowd.removeAgent(isel);
+				crowd->removeAgent(isel);
 			}
 		}
 		else
@@ -330,7 +339,7 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 			ap.height = m_sample->getAgentHeight();
 			ap.maxAcceleration = 8.0f;
 			ap.maxSpeed = 3.5f;
-			ap.collisionQueryRange = ap.radius * 8.0f;
+			ap.collisionQueryRange = ap.radius * 12.0f;
 			ap.pathOptimizationRange = ap.radius * 30.0f;
 			ap.updateFlags = 0; 
 			if (m_anticipateTurns)
@@ -346,11 +355,11 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 			ap.obstacleAvoidanceType = (unsigned char)m_obstacleAvoidanceType;
 			ap.separationWeight = m_separationWeight;
 			
-			int idx = m_crowd.addAgent(p, &ap);
+			int idx = crowd->addAgent(p, &ap);
 			if (idx != -1)
 			{
 				if (m_targetRef)
-					m_crowd.requestMoveTarget(idx, m_targetRef, m_targetPos);
+					crowd->requestMoveTarget(idx, m_targetRef, m_targetPos);
 
 				// Init trail
 				AgentTrail* trail = &m_trails[idx];
@@ -364,29 +373,47 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 	{
 		// Find nearest point on navmesh and set move request to that location.
 		dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
-		const dtQueryFilter* filter = m_crowd.getFilter();
-		const float* ext = m_crowd.getQueryExtents();
+		const dtQueryFilter* filter = crowd->getFilter();
+		const float* ext = crowd->getQueryExtents();
 		
 		navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
 		
 		if (shift)
 		{
 			// Adjust target using tiny local search.
-			for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+			if (m_agentDebug.idx != -1)
 			{
-				const dtCrowdAgent* ag = m_crowd.getAgent(i);
-				if (!ag->active) continue;
-				m_crowd.adjustMoveTarget(i, m_targetRef, m_targetPos);
+				const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
+				if (ag && ag->active)
+					crowd->adjustMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos);
+			}
+			else
+			{
+				for (int i = 0; i < crowd->getAgentCount(); ++i)
+				{
+					const dtCrowdAgent* ag = crowd->getAgent(i);
+					if (!ag->active) continue;
+					crowd->adjustMoveTarget(i, m_targetRef, m_targetPos);
+				}
 			}
 		}
 		else
 		{
 			// Move target using paht finder
-			for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+			if (m_agentDebug.idx != -1)
 			{
-				const dtCrowdAgent* ag = m_crowd.getAgent(i);
-				if (!ag->active) continue;
-				m_crowd.requestMoveTarget(i, m_targetRef, m_targetPos);
+				const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
+				if (ag && ag->active)
+					crowd->requestMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos);
+			}
+			else
+			{
+				for (int i = 0; i < crowd->getAgentCount(); ++i)
+				{
+					const dtCrowdAgent* ag = crowd->getAgent(i);
+					if (!ag->active) continue;
+					crowd->requestMoveTarget(i, m_targetRef, m_targetPos);
+				}
 			}
 		}
 	}
@@ -396,9 +423,9 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 		m_agentDebug.idx = -1;
 		
 		float tsel = FLT_MAX;
-		for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+		for (int i = 0; i < crowd->getAgentCount(); ++i)
 		{
-			const dtCrowdAgent* ag = m_crowd.getAgent(i);
+			const dtCrowdAgent* ag = crowd->getAgent(i);
 			if (!ag->active) continue;
 			float bmin[3], bmax[3];
 			getAgentBounds(ag, bmin, bmax);
@@ -413,10 +440,36 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
 			}
 		}
 	}
+	else if (m_mode == TOOLMODE_TOGGLE_POLYS)
+	{
+		dtNavMesh* nav = m_sample->getNavMesh();
+		dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
+		if (nav && navquery)
+		{
+			dtQueryFilter filter;
+			const float* ext = crowd->getQueryExtents();
+			float tgt[3];
+			dtPolyRef ref;
+			navquery->findNearestPoly(p, ext, &filter, &ref, tgt);
+			if (ref)
+			{
+				unsigned short flags = 0;
+				if (dtStatusSucceed(nav->getPolyFlags(ref, &flags)))
+				{
+					flags ^= SAMPLE_POLYFLAGS_DISABLED;
+					nav->setPolyFlags(ref, flags);
+				}
+			}
+		}
+	}
+
 }
 
 void CrowdTool::updateAgentParams()
 {
+	dtCrowd* crowd = m_sample->getCrowd();
+	if (!crowd) return;
+
 	unsigned char updateFlags = 0;
 	unsigned char obstacleAvoidanceType = 0;
 	
@@ -437,34 +490,34 @@ void CrowdTool::updateAgentParams()
 
 	dtCrowdAgentParams params;
 	
-	for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+	for (int i = 0; i < crowd->getAgentCount(); ++i)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(i);
+		const dtCrowdAgent* ag = crowd->getAgent(i);
 		if (!ag->active) continue;
 		memcpy(&params, &ag->params, sizeof(dtCrowdAgentParams));
 		params.updateFlags = updateFlags;
 		params.obstacleAvoidanceType = obstacleAvoidanceType;
 		params.separationWeight = m_separationWeight;
-		m_crowd.updateAgentParameters(i, &params);
+		crowd->updateAgentParameters(i, &params);
 	}	
 }
 
 void CrowdTool::updateTick(const float dt)
 {
 	dtNavMesh* nav = m_sample->getNavMesh();
-	if (!nav)
-		return;
+	dtCrowd* crowd = m_sample->getCrowd();
+	if (!nav || !crowd) return;
 
 	TimeVal startTime = getPerfTime();
 	
-	m_crowd.update(dt, &m_agentDebug);
+	crowd->update(dt, &m_agentDebug);
 	
 	TimeVal endTime = getPerfTime();
 	
 	// Update agent trails
-	for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+	for (int i = 0; i < crowd->getAgentCount(); ++i)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(i);
+		const dtCrowdAgent* ag = crowd->getAgent(i);
 		AgentTrail* trail = &m_trails[i];
 		if (!ag->active)
 			continue;
@@ -475,7 +528,7 @@ void CrowdTool::updateTick(const float dt)
 	
 	m_agentDebug.vod->normalizeSamples();
 	
-	m_crowdSampleCount.addSample((float)m_crowd.getVelocitySampleCount());
+	m_crowdSampleCount.addSample((float)crowd->getVelocitySampleCount());
 	m_crowdTotalTime.addSample(getPerfDeltaTimeUsec(startTime, endTime) / 1000.0f);
 }
 
@@ -503,13 +556,16 @@ void CrowdTool::handleRender()
 	DebugDrawGL dd;
 	const float s = m_sample->getAgentRadius();
 	
-	dtNavMesh* nmesh = m_sample->getNavMesh();
-	if (!nmesh)
+	dtNavMesh* nav = m_sample->getNavMesh();
+	dtCrowd* crowd = m_sample->getCrowd();
+	if (!nav || !crowd)
 		return;
 
-	if (m_showNodes && m_crowd.getPathQueue())
+	duDebugDrawNavMeshPolysWithFlags(&dd, *nav, SAMPLE_POLYFLAGS_DISABLED, duRGBA(220,0,0,128));
+
+	if (m_showNodes && crowd->getPathQueue())
 	{
-		const dtNavMeshQuery* navquery = m_crowd.getPathQueue()->getNavQuery();
+		const dtNavMeshQuery* navquery = crowd->getPathQueue()->getNavQuery();
 		if (navquery)
 			duDebugDrawNavMeshNodes(&dd, *navquery);
 	}
@@ -521,14 +577,14 @@ void CrowdTool::handleRender()
 	{
 		if (m_agentDebug.idx != -1)
 		{
-			const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
+			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, *nmesh, path[i], duRGBA(0,0,0,16));
+					duDebugDrawNavMeshPoly(&dd, *nav, path[i], duRGBA(0,0,0,16));
 			}
 		}
 	}
@@ -540,9 +596,9 @@ void CrowdTool::handleRender()
 	if (m_showGrid)
 	{
 		float gridy = -FLT_MAX;
-		for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+		for (int i = 0; i < crowd->getAgentCount(); ++i)
 		{
-			const dtCrowdAgent* ag = m_crowd.getAgent(i);
+			const dtCrowdAgent* ag = crowd->getAgent(i);
 			if (!ag->active) continue;
 			const float* pos = ag->corridor.getPos();
 			gridy = dtMax(gridy, pos[1]);
@@ -550,7 +606,7 @@ void CrowdTool::handleRender()
 		gridy += 1.0f;
 		
 		dd.begin(DU_DRAW_QUADS);
-		const dtProximityGrid* grid = m_crowd.getGrid();
+		const dtProximityGrid* grid = crowd->getGrid();
 		const int* bounds = grid->getBounds();
 		const float cs = grid->getCellSize();
 		for (int y = bounds[1]; y <= bounds[3]; ++y)
@@ -570,9 +626,9 @@ void CrowdTool::handleRender()
 	}
 	
 	// Trail
-	for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+	for (int i = 0; i < crowd->getAgentCount(); ++i)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(i);
+		const dtCrowdAgent* ag = crowd->getAgent(i);
 		if (!ag->active) continue;
 		
 		const AgentTrail* trail = &m_trails[i];
@@ -598,7 +654,7 @@ void CrowdTool::handleRender()
 	// Corners & co
 	if (m_agentDebug.idx != -1)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
+		const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
 		if (ag->active)
 		{
 		
@@ -684,13 +740,13 @@ void CrowdTool::handleRender()
 					// TODO: fix this properly.
 					int n = ag->neis[j].idx;
 					const dtCrowdAgent* nei = 0;
-					for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+					for (int i = 0; i < crowd->getAgentCount(); ++i)
 					{
-						const dtCrowdAgent* ag = m_crowd.getAgent(i);
+						const dtCrowdAgent* ag = crowd->getAgent(i);
 						if (!ag->active) continue;
 						if (n == 0)
 						{
-							nei = m_crowd.getAgent(ag->neis[j].idx);
+							nei = crowd->getAgent(ag->neis[j].idx);
 							break;
 						}
 						n--;
@@ -715,9 +771,9 @@ void CrowdTool::handleRender()
 	}
 	
 	// Agent cylinders.
-	for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+	for (int i = 0; i < crowd->getAgentCount(); ++i)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(i);
+		const dtCrowdAgent* ag = crowd->getAgent(i);
 		if (!ag->active) continue;
 		
 		const float radius = ag->params.radius;
@@ -726,9 +782,9 @@ void CrowdTool::handleRender()
 		duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f);
 	}
 
-	for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+	for (int i = 0; i < crowd->getAgentCount(); ++i)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(i);
+		const dtCrowdAgent* ag = crowd->getAgent(i);
 		if (!ag->active) continue;
 		
 		const float height = ag->params.height;
@@ -746,7 +802,7 @@ void CrowdTool::handleRender()
 	
 	if (m_agentDebug.idx != -1)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
+		const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
 		if (ag->active)
 		{
 			if (m_showVO)
@@ -780,9 +836,9 @@ void CrowdTool::handleRender()
 	}
 	
 	// Velocity stuff.
-	for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+	for (int i = 0; i < crowd->getAgentCount(); ++i)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(i);
+		const dtCrowdAgent* ag = crowd->getAgent(i);
 		if (!ag->active) continue;
 		
 		const float radius = ag->params.radius;
@@ -824,40 +880,47 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
 
 	if (m_showLabels)
 	{
-		for (int i = 0; i < m_crowd.getAgentCount(); ++i)
+		dtCrowd* crowd = m_sample->getCrowd();
+		if (crowd)
 		{
-			const dtCrowdAgent* ag = m_crowd.getAgent(i);
-			if (!ag->active) continue;
-			const float* pos = ag->npos;
-			const float h = ag->params.height;
-			if (gluProject((GLdouble)pos[0], (GLdouble)pos[1]+h, (GLdouble)pos[2],
-						   model, proj, view, &x, &y, &z))
+			for (int i = 0; i < crowd->getAgentCount(); ++i)
 			{
-				snprintf(label, 32, "%d", i);
-				imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(0,0,0,220));
-			}
-			
+				const dtCrowdAgent* ag = crowd->getAgent(i);
+				if (!ag->active) continue;
+				const float* pos = ag->npos;
+				const float h = ag->params.height;
+				if (gluProject((GLdouble)pos[0], (GLdouble)pos[1]+h, (GLdouble)pos[2],
+							   model, proj, view, &x, &y, &z))
+				{
+					snprintf(label, 32, "%d", i);
+					imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(0,0,0,220));
+				}
+			}			
 		}
 	}
 	if (m_agentDebug.idx != -1)
 	{
-		const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
-		if (ag->active)
+		dtCrowd* crowd = m_sample->getCrowd();
+		if (crowd) 
 		{
-			const float radius = ag->params.radius;
-			
-			if (m_showNeis)
+			const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
+			if (ag->active)
 			{
-				for (int j = 0; j < ag->nneis; ++j)
+				const float radius = ag->params.radius;
+				
+				if (m_showNeis)
 				{
-					const dtCrowdAgent* nei = m_crowd.getAgent(ag->neis[j].idx);
-					if (!nei->active) continue;
-					
-					if (gluProject((GLdouble)nei->npos[0], (GLdouble)nei->npos[1]+radius, (GLdouble)nei->npos[2],
-								   model, proj, view, &x, &y, &z))
+					for (int j = 0; j < ag->nneis; ++j)
 					{
-						snprintf(label, 32, "%.3f", ag->neis[j].dist);
-						imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(255,255,255,220));
+						const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
+						if (!nei->active) continue;
+						
+						if (gluProject((GLdouble)nei->npos[0], (GLdouble)nei->npos[1]+radius, (GLdouble)nei->npos[2],
+									   model, proj, view, &x, &y, &z))
+						{
+							snprintf(label, 32, "%.3f", ag->neis[j].dist);
+							imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(255,255,255,220));
+						}
 					}
 				}
 			}
@@ -899,5 +962,9 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
 	}
 	ty -= 20;
 	imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "SPACE: Run/Pause simulation.  1: Step simulation.", imguiRGBA(255,255,255,192));	
-	
+	ty -= 20;
+	if (m_run)
+		imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "- RUNNING -", imguiRGBA(255,32,16,255));	
+	else 
+		imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "- PAUSED -", imguiRGBA(255,255,255,128));	
 }
diff --git a/RecastDemo/Source/Sample.cpp b/RecastDemo/Source/Sample.cpp
index 2794701..4dffe9f 100644
--- a/RecastDemo/Source/Sample.cpp
+++ b/RecastDemo/Source/Sample.cpp
@@ -26,6 +26,7 @@
 #include "DetourDebugDraw.h"
 #include "DetourNavMesh.h"
 #include "DetourNavMeshQuery.h"
+#include "DetourCrowd.h"
 #include "imgui.h"
 #include "SDL.h"
 #include "SDL_opengl.h"
@@ -38,18 +39,21 @@ Sample::Sample() :
 	m_geom(0),
 	m_navMesh(0),
 	m_navQuery(0),
+	m_crowd(0),
 	m_navMeshDrawFlags(DU_DRAWNAVMESH_OFFMESHCONS|DU_DRAWNAVMESH_CLOSEDLIST),
 	m_tool(0),
 	m_ctx(0)
 {
 	resetCommonSettings();
 	m_navQuery = dtAllocNavMeshQuery();
+	m_crowd = dtAllocCrowd();
 }
 
 Sample::~Sample()
 {
 	dtFreeNavMeshQuery(m_navQuery);
 	dtFreeNavMesh(m_navMesh);
+	dtFreeCrowd(m_crowd);
 	delete m_tool;
 }
 
diff --git a/RecastDemo/Source/Sample_TempObstacles.cpp b/RecastDemo/Source/Sample_TempObstacles.cpp
index df0ad24..b0f22b2 100644
--- a/RecastDemo/Source/Sample_TempObstacles.cpp
+++ b/RecastDemo/Source/Sample_TempObstacles.cpp
@@ -1166,6 +1166,18 @@ bool Sample_TempObstacles::handleBuild()
 	m_cacheBuildTimeMs = m_ctx->getAccumulatedTime(RC_TIMER_TOTAL)/1000.0f;
 	m_cacheBuildMemUsage = m_talloc->high;
 	
+
+	const dtNavMesh* nav = m_navMesh;
+	int navmeshMemUsage = 0;
+	for (int i = 0; i < nav->getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = nav->getTile(i);
+		if (tile->header)
+			navmeshMemUsage += tile->dataSize;
+	}
+	printf("navmeshMemUsage = %.1f kB", navmeshMemUsage/1024.0f);
+		
+	
 	if (m_tool)
 		m_tool->init(this);
 
diff --git a/RecastDemo/Source/main.cpp b/RecastDemo/Source/main.cpp
index 1b18555..ecf299b 100644
--- a/RecastDemo/Source/main.cpp
+++ b/RecastDemo/Source/main.cpp
@@ -423,10 +423,10 @@ int main(int /*argc*/, char** /*argv*/)
 		}
 
 		// Clamp the framerate so that we do not hog all the CPU.
-		const float FRAME_RATE = 40;
-		if (dt < FRAME_RATE)
+		const float MIN_FRAME_TIME = 1.0f/40.0f;
+		if (dt < MIN_FRAME_TIME)
 		{
-			int ms = (int)((FRAME_RATE - dt)*1000.0f);
+			int ms = (int)((MIN_FRAME_TIME - dt)*1000.0f);
 			if (ms > 10) ms = 10;
 			if (ms >= 0)
 				SDL_Delay(ms);