game_machine 0.0.11 → 1.0.2

data/pathfinding/navmesh.cpp

@@ -0,0 +1,408 @@
+
+#include "navmesh.h"
+using namespace std;
+
+myQueryFilter::myQueryFilter() :
+  m_includeFlags(0xffff),
+  m_excludeFlags(0)
+  
+  {
+    for (int i = 0; i < DT_MAX_AREAS; ++i)
+      m_areaCost[i] = 1.0f;
+  }
+
+  myQueryFilter::~myQueryFilter() { }
+
+  void myQueryFilter::setNavmesh(Navmesh* n)
+  {
+  	navmesh = n;
+  }
+
+  bool myQueryFilter::passFilter(const dtPolyRef /*ref*/,
+                 const dtMeshTile* /*tile*/,
+                 const dtPoly* poly) const
+  {
+    return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
+  }
+
+  bool myQueryFilter::isPassable(const float* pa, const float* pb) const
+  {
+      int x = int(pb[0]);
+      int y = int(pb[2]);
+
+      int index = y*navmesh->gwidth+x;
+      int c = navmesh->gmap[ index ];
+
+      if (c > 0)
+      {
+        fprintf (stderr, "isPassable false %f %f %f -> %f %f %f\n", pa[0], pa[1], pa[2],pb[0], pb[1], pb[2]);
+        return false;
+      }
+
+      return true;
+  }
+
+  float myQueryFilter::getCost(const float* pa, const float* pb,
+             const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
+             const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
+             const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
+  {
+   
+    return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
+  }
+
+
+float const Navmesh::STEP_SIZE = 0.5f;
+float const Navmesh::SLOP = 0.01f;
+
+void Navmesh::gmapSetPassable(int a, int b, int radius)
+  {
+    for(int x = a - radius; x <= a + radius; x++)
+    {
+        for(int y = b - radius; y <= b + radius; y++)
+        {
+                gmap[ y*gwidth + x] = 0;
+        }
+    }
+  }
+
+void Navmesh::gmapSetBlocked(int a, int b, int radius)
+  {
+    for(int x = a - radius; x <= a + radius; x++)
+    {
+        for(int y = b - radius; y <= b + radius; y++)
+        {
+                gmap[ y*gwidth + x] = radius;
+        }
+    }
+  }
+
+Navmesh::Navmesh(int id, const char *file) :
+	meshId(id),
+	gmap(2000*2000+1,0)
+	{
+		gwidth = 2000;
+		gheight = 2000;
+		mesh = load_navmesh(file);
+		query = dtAllocNavMeshQuery();
+		query->init(mesh, 4096);
+	}
+
+bool Navmesh::meshLoaded()
+{
+	if (mesh == 0)
+	{
+		return false;
+	}
+	return true;
+}
+
+int Navmesh::findPath(float startx, float starty,
+    float startz, float endx, float endy, float endz, int find_straight_path,
+    float* resultPath) {
+
+  if (query == NULL) {
+    return P_QUERY_NOT_FOUND;
+  }
+
+  float m_spos[3] = {startx,starty,startz};
+  float m_epos[3] = {endx,endy,endz};
+
+
+  dtPolyRef m_polys[MAX_POLYS];
+
+  //dtQueryFilter m_filter;
+  myQueryFilter m_filter;
+  m_filter.setNavmesh(this);
+  float straight[MAX_POLYS*3];
+  int straightPathCount = 0;
+  float polyPickExt[3] = {2,4,2};
+  int m_npolys = 0;
+
+  float m_smoothPath[MAX_SMOOTH*3];
+  int m_nsmoothPath = 0;
+
+  m_filter.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED);
+  m_filter.setExcludeFlags(0);
+
+  dtPolyRef m_startRef, m_endRef;
+
+  dtStatus res;
+
+  res = query->findNearestPoly(m_spos, polyPickExt, &m_filter, &m_startRef, 0);
+  if (res == DT_SUCCESS) {
+    if (m_startRef == 0) {
+      return P_NO_START_POLY;
+    }
+  } else {
+    return P_NO_START_POLY;
+  }
+
+  res = query->findNearestPoly(m_epos, polyPickExt, &m_filter, &m_endRef, 0);
+  if (res == DT_SUCCESS) {
+    if (m_endRef == 0) {
+      return P_NO_END_POLY;
+    }
+  } else {
+    return P_NO_END_POLY;
+  }
+
+  res = query->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
+  if (res != DT_SUCCESS) {
+    return P_PATH_NOT_FOUND;
+  }
+
+
+  if (find_straight_path == 0) {
+    dtPolyRef polys[MAX_POLYS];
+    memcpy(polys, m_polys, sizeof(dtPolyRef)*m_npolys); 
+    int npolys = m_npolys;
+
+    float iterPos[3], targetPos[3];
+    query->closestPointOnPoly(m_startRef, m_spos, iterPos,0);
+    query->closestPointOnPoly(polys[npolys-1], m_epos, targetPos,0);
+
+    m_nsmoothPath = 0;
+    
+    dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
+    m_nsmoothPath++;
+
+    while (npolys && m_nsmoothPath < MAX_SMOOTH) {
+      // Find location to steer towards.
+      float steerPos[3];
+      unsigned char steerPosFlag;
+      dtPolyRef steerPosRef;
+
+      if (!getSteerTarget(query, iterPos, targetPos, SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef)) {
+        break;
+      }
+
+      bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
+      bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
+
+      // Find movement delta.
+      float delta[3], len;
+      dtVsub(delta, steerPos, iterPos);
+      len = dtSqrt(dtVdot(delta,delta));
+      // If the steer target is end of path or off-mesh link, do not move past the location.
+      if ((endOfPath || offMeshConnection) && len < STEP_SIZE) {
+        len = 1;
+      } else {
+        len = STEP_SIZE / len;
+      }
+
+      float moveTgt[3];
+      dtVmad(moveTgt, iterPos, delta, len);
+
+      // Move
+      float result[3];
+      dtPolyRef visited[16];
+      int nvisited = 0;
+      query->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, &nvisited, 16);
+
+      npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
+      npolys = fixupShortcuts(polys, npolys, query);
+      
+      float h = 0;
+      query->getPolyHeight(polys[0], result, &h);
+      result[1] = h;
+      dtVcopy(iterPos, result);
+
+      if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f)) {
+        // Reached end of path.
+        fprintf (stderr, "End of path reached\n");
+        dtVcopy(iterPos, targetPos);
+        if (m_nsmoothPath < MAX_SMOOTH) {
+          dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
+          m_nsmoothPath++;
+        }
+        break;
+      }
+
+      if (m_nsmoothPath < MAX_SMOOTH) {
+        dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
+        m_nsmoothPath++;
+      }
+    }
+
+    memcpy(resultPath, m_smoothPath, sizeof(float)*3*m_nsmoothPath);
+    return m_nsmoothPath;
+  }
+
+  query->findStraightPath(m_spos, m_epos, m_polys, m_npolys, straight, 0, 0, &straightPathCount, MAX_POLYS);
+  memcpy(resultPath, straight, sizeof(float)*3*straightPathCount);
+  return straightPathCount;
+}
+
+float* Navmesh::getPathPtr(int max_paths) {
+  float *path;
+  path = new float[max_paths*3];
+  return path;
+}
+
+void Navmesh::freePath(float* path) {
+  delete [] path;
+}
+
+dtNavMesh* Navmesh::getMesh() {
+	return mesh;
+}
+
+dtNavMeshQuery* Navmesh::getQuery() {
+  return query;
+}
+
+void Navmesh::freeQuery(dtNavMeshQuery* query) {
+  dtFreeNavMeshQuery(query);
+}
+
+
+
+inline bool Navmesh::inRange(const float* v1, const float* v2, const float r, const float h)
+{
+  const float dx = v2[0] - v1[0];
+  const float dy = v2[1] - v1[1];
+  const float dz = v2[2] - v1[2];
+  return (dx*dx + dz*dz) < r*r && fabsf(dy) < h;
+}
+
+int Navmesh::fixupShortcuts(dtPolyRef* path, int npath, dtNavMeshQuery* navQuery)
+{
+  if (npath < 3)
+    return npath;
+
+  // Get connected polygons
+  static const int maxNeis = 16;
+  dtPolyRef neis[maxNeis];
+  int nneis = 0;
+
+  const dtMeshTile* tile = 0;
+  const dtPoly* poly = 0;
+  if (dtStatusFailed(navQuery->getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
+    return npath;
+  
+  for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
+  {
+    const dtLink* link = &tile->links[k];
+    if (link->ref != 0)
+    {
+      if (nneis < maxNeis)
+        neis[nneis++] = link->ref;
+    }
+  }
+
+  // If any of the neighbour polygons is within the next few polygons
+  // in the path, short cut to that polygon directly.
+  static const int maxLookAhead = 6;
+  int cut = 0;
+  for (int i = dtMin(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) {
+    for (int j = 0; j < nneis; j++)
+    {
+      if (path[i] == neis[j]) {
+        cut = i;
+        break;
+      }
+    }
+  }
+  if (cut > 1)
+  {
+    int offset = cut-1;
+    npath -= offset;
+    for (int i = 1; i < npath; i++)
+      path[i] = path[i+offset];
+  }
+
+  return npath;
+}
+
+bool Navmesh::getSteerTarget(dtNavMeshQuery* navQuery, const float* startPos, const float* endPos,
+               const float minTargetDist,
+               const dtPolyRef* path, const int pathSize,
+               float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef,
+               float* outPoints, int* outPointCount)               
+{
+  // Find steer target.
+  static const int MAX_STEER_POINTS = 3;
+  float steerPath[MAX_STEER_POINTS*3];
+  unsigned char steerPathFlags[MAX_STEER_POINTS];
+  dtPolyRef steerPathPolys[MAX_STEER_POINTS];
+  int nsteerPath = 0;
+  navQuery->findStraightPath(startPos, endPos, path, pathSize,
+                 steerPath, steerPathFlags, steerPathPolys, &nsteerPath, MAX_STEER_POINTS);
+  if (!nsteerPath)
+    return false;
+
+  if (outPoints && outPointCount)
+  {
+    *outPointCount = nsteerPath;
+    for (int i = 0; i < nsteerPath; ++i)
+      dtVcopy(&outPoints[i*3], &steerPath[i*3]);
+  }
+
+
+  // Find vertex far enough to steer to.
+  int ns = 0;
+  while (ns < nsteerPath)
+  {
+    // Stop at Off-Mesh link or when point is further than slop away.
+    if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
+      !inRange(&steerPath[ns*3], startPos, minTargetDist, 1000.0f))
+      break;
+    ns++;
+  }
+  // Failed to find good point to steer to.
+  if (ns >= nsteerPath)
+    return false;
+
+  dtVcopy(steerPos, &steerPath[ns*3]);
+  steerPos[1] = startPos[1];
+  steerPosFlag = steerPathFlags[ns];
+  steerPosRef = steerPathPolys[ns];
+
+  return true;
+}
+
+int Navmesh::fixupCorridor(dtPolyRef* path, const int npath, const int maxPath,
+             const dtPolyRef* visited, const int nvisited)
+{
+  int furthestPath = -1;
+  int furthestVisited = -1;
+
+  // Find furthest common polygon.
+  for (int i = npath-1; i >= 0; --i)
+  {
+    bool found = false;
+    for (int j = nvisited-1; j >= 0; --j)
+    {
+      if (path[i] == visited[j])
+      {
+        furthestPath = i;
+        furthestVisited = j;
+        found = true;
+      }
+    }
+    if (found)
+      break;
+  }
+
+  // If no intersection found just return current path. 
+  if (furthestPath == -1 || furthestVisited == -1)
+    return npath;
+
+  // Concatenate paths. 
+
+  // Adjust beginning of the buffer to include the visited.
+  const int req = nvisited - furthestVisited;
+  const int orig = rcMin(furthestPath+1, npath);
+  int size = rcMax(0, npath-orig);
+  if (req+size > maxPath)
+    size = maxPath-req;
+  if (size)
+    memmove(path+req, path+orig, size*sizeof(dtPolyRef));
+
+  // Store visited
+  for (int i = 0; i < req; ++i)
+    path[i] = visited[(nvisited-1)-i];        
+
+  return req+size;
+}

data/pathfinding/overrides/DetourCrowd.cpp

@@ -0,0 +1,1446 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <string.h>
+#include <float.h>
+#include <stdlib.h>
+#include <new>
+#include "DetourCrowd.h"
+#include "DetourNavMesh.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourObstacleAvoidance.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourAssert.h"
+#include "DetourAlloc.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 = 100;
+
+static const int MAX_PATHQUEUE_NODES = 4096;
+static const int MAX_COMMON_NODES = 512;
+
+inline float tween(const float t, const float t0, const float t1)
+{
+	return dtClamp((t-t0) / (t1-t0), 0.0f, 1.0f);
+}
+
+static void integrate(dtCrowdAgent* ag, const float dt)
+{
+	// Fake dynamic constraint.
+	const float maxDelta = ag->params.maxAcceleration * dt;
+	float dv[3];
+	dtVsub(dv, ag->nvel, ag->vel);
+	float ds = dtVlen(dv);
+	if (ds > maxDelta)
+		dtVscale(dv, dv, maxDelta/ds);
+	dtVadd(ag->vel, ag->vel, dv);
+	
+	// Integrate
+	if (dtVlen(ag->vel) > 0.0001f)
+		dtVmad(ag->npos, ag->npos, ag->vel, dt);
+	else
+		dtVset(ag->vel,0,0,0);
+}
+
+static bool overOffmeshConnection(const dtCrowdAgent* ag, const float radius)
+{
+	if (!ag->ncorners)
+		return false;
+	
+	const bool offMeshConnection = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
+	if (offMeshConnection)
+	{
+		const float distSq = dtVdist2DSqr(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]);
+		if (distSq < radius*radius)
+			return true;
+	}
+	
+	return false;
+}
+
+static float getDistanceToGoal(const dtCrowdAgent* ag, const float range)
+{
+	if (!ag->ncorners)
+		return range;
+	
+	const bool endOfPath = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_END) ? true : false;
+	if (endOfPath)
+		return dtMin(dtVdist2D(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]), range);
+	
+	return range;
+}
+
+static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
+{
+	if (!ag->ncorners)
+	{
+		dtVset(dir, 0,0,0);
+		return;
+	}
+	
+	const int ip0 = 0;
+	const int ip1 = dtMin(1, ag->ncorners-1);
+	const float* p0 = &ag->cornerVerts[ip0*3];
+	const float* p1 = &ag->cornerVerts[ip1*3];
+	
+	float dir0[3], dir1[3];
+	dtVsub(dir0, p0, ag->npos);
+	dtVsub(dir1, p1, ag->npos);
+	dir0[1] = 0;
+	dir1[1] = 0;
+	
+	float len0 = dtVlen(dir0);
+	float len1 = dtVlen(dir1);
+	if (len1 > 0.001f)
+		dtVscale(dir1,dir1,1.0f/len1);
+	
+	dir[0] = dir0[0] - dir1[0]*len0*0.5f;
+	dir[1] = 0;
+	dir[2] = dir0[2] - dir1[2]*len0*0.5f;
+	
+	dtVnormalize(dir);
+}
+
+static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir)
+{
+	if (!ag->ncorners)
+	{
+		dtVset(dir, 0,0,0);
+		return;
+	}
+	dtVsub(dir, &ag->cornerVerts[0], ag->npos);
+	dir[1] = 0;
+	dtVnormalize(dir);
+}
+
+static int addNeighbour(const int idx, const float dist,
+						dtCrowdNeighbour* neis, const int nneis, const int maxNeis)
+{
+	// Insert neighbour based on the distance.
+	dtCrowdNeighbour* nei = 0;
+	if (!nneis)
+	{
+		nei = &neis[nneis];
+	}
+	else if (dist >= neis[nneis-1].dist)
+	{
+		if (nneis >= maxNeis)
+			return nneis;
+		nei = &neis[nneis];
+	}
+	else
+	{
+		int i;
+		for (i = 0; i < nneis; ++i)
+			if (dist <= neis[i].dist)
+				break;
+		
+		const int tgt = i+1;
+		const int n = dtMin(nneis-i, maxNeis-tgt);
+		
+		dtAssert(tgt+n <= maxNeis);
+		
+		if (n > 0)
+			memmove(&neis[tgt], &neis[i], sizeof(dtCrowdNeighbour)*n);
+		nei = &neis[i];
+	}
+	
+	memset(nei, 0, sizeof(dtCrowdNeighbour));
+	
+	nei->idx = idx;
+	nei->dist = dist;
+	
+	return dtMin(nneis+1, maxNeis);
+}
+
+static int getNeighbours(const float* pos, const float height, const float range,
+						 const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult,
+						 dtCrowdAgent** agents, const int /*nagents*/, dtProximityGrid* grid)
+{
+	int n = 0;
+	
+	static const int MAX_NEIS = 32;
+	unsigned short ids[MAX_NEIS];
+	int nids = grid->queryItems(pos[0]-range, pos[2]-range,
+								pos[0]+range, pos[2]+range,
+								ids, MAX_NEIS);
+	
+	for (int i = 0; i < nids; ++i)
+	{
+		const dtCrowdAgent* ag = agents[ids[i]];
+		
+		if (ag == skip) continue;
+		
+		// Check for overlap.
+		float diff[3];
+		dtVsub(diff, pos, ag->npos);
+		if (dtMathFabs(diff[1]) >= (height+ag->params.height)/2.0f)
+			continue;
+		diff[1] = 0;
+		const float distSqr = dtVlenSqr(diff);
+		if (distSqr > dtSqr(range))
+			continue;
+		
+		n = addNeighbour(ids[i], distSqr, result, n, maxResult);
+	}
+	return n;
+}
+
+static int addToOptQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents)
+{
+	// Insert neighbour based on greatest time.
+	int slot = 0;
+	if (!nagents)
+	{
+		slot = nagents;
+	}
+	else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime)
+	{
+		if (nagents >= maxAgents)
+			return nagents;
+		slot = nagents;
+	}
+	else
+	{
+		int i;
+		for (i = 0; i < nagents; ++i)
+			if (newag->topologyOptTime >= agents[i]->topologyOptTime)
+				break;
+		
+		const int tgt = i+1;
+		const int n = dtMin(nagents-i, maxAgents-tgt);
+		
+		dtAssert(tgt+n <= maxAgents);
+		
+		if (n > 0)
+			memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n);
+		slot = i;
+	}
+	
+	agents[slot] = newag;
+	
+	return dtMin(nagents+1, maxAgents);
+}
+
+static int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents)
+{
+	// Insert neighbour based on greatest time.
+	int slot = 0;
+	if (!nagents)
+	{
+		slot = nagents;
+	}
+	else if (newag->targetReplanTime <= agents[nagents-1]->targetReplanTime)
+	{
+		if (nagents >= maxAgents)
+			return nagents;
+		slot = nagents;
+	}
+	else
+	{
+		int i;
+		for (i = 0; i < nagents; ++i)
+			if (newag->targetReplanTime >= agents[i]->targetReplanTime)
+				break;
+		
+		const int tgt = i+1;
+		const int n = dtMin(nagents-i, maxAgents-tgt);
+		
+		dtAssert(tgt+n <= maxAgents);
+		
+		if (n > 0)
+			memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n);
+		slot = i;
+	}
+	
+	agents[slot] = newag;
+	
+	return dtMin(nagents+1, maxAgents);
+}
+
+
+/**
+@class dtCrowd
+@par
+
+This is the core class of the @ref crowd module.  See the @ref crowd documentation for a summary
+of the crowd features.
+
+A common method for setting up the crowd is as follows:
+
+-# Allocate the crowd using #dtAllocCrowd.
+-# Initialize the crowd using #init().
+-# Set the avoidance configurations using #setObstacleAvoidanceParams().
+-# Add agents using #addAgent() and make an initial movement request using #requestMoveTarget().
+
+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() when movement goal changes.
+-# Repeat every frame.
+
+Some agent configuration settings can be updated using #updateAgentParameters().  But the crowd owns the
+agent position.  So it is not possible to update an active agent's position.  If agent position
+must be fed back into the crowd, the agent must be removed and re-added.
+
+Notes: 
+
+- Path related information is available for newly added agents only after an #update() has been
+  performed.
+- Agent objects are kept in a pool and re-used.  So it is important when using agent objects to check the value of
+  #dtCrowdAgent::active to determine if the agent is actually in use or not.
+- This class is meant to provide 'local' movement. There is a limit of 256 polygons in the path corridor.  
+  So it is not meant to provide automatic pathfinding services over long distances.
+
+@see dtAllocCrowd(), dtFreeCrowd(), init(), dtCrowdAgent
+
+*/
+
+dtCrowd::dtCrowd() :
+	m_maxAgents(0),
+	m_agents(0),
+	m_activeAgents(0),
+	m_agentAnims(0),
+	m_obstacleQuery(0),
+	m_grid(0),
+	m_pathResult(0),
+	m_maxPathResult(0),
+	m_maxAgentRadius(0),
+	m_velocitySampleCount(0),
+	m_navquery(0)
+{
+}
+
+dtCrowd::~dtCrowd()
+{
+	purge();
+}
+
+void dtCrowd::purge()
+{
+	for (int i = 0; i < m_maxAgents; ++i)
+		m_agents[i].~dtCrowdAgent();
+	dtFree(m_agents);
+	m_agents = 0;
+	m_maxAgents = 0;
+	
+	dtFree(m_activeAgents);
+	m_activeAgents = 0;
+
+	dtFree(m_agentAnims);
+	m_agentAnims = 0;
+	
+	dtFree(m_pathResult);
+	m_pathResult = 0;
+	
+	dtFreeProximityGrid(m_grid);
+	m_grid = 0;
+
+	dtFreeObstacleAvoidanceQuery(m_obstacleQuery);
+	m_obstacleQuery = 0;
+	
+	dtFreeNavMeshQuery(m_navquery);
+	m_navquery = 0;
+}
+
+/// @par
+///
+/// May be called more than once to purge and re-initialize the crowd.
+bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav)
+{
+	purge();
+	
+	m_maxAgents = maxAgents;
+	m_maxAgentRadius = maxAgentRadius;
+
+	dtVset(m_ext, m_maxAgentRadius*2.0f,m_maxAgentRadius*1.5f,m_maxAgentRadius*2.0f);
+	
+	m_grid = dtAllocProximityGrid();
+	if (!m_grid)
+		return false;
+	if (!m_grid->init(m_maxAgents*4, maxAgentRadius*3))
+		return false;
+	
+	m_obstacleQuery = dtAllocObstacleAvoidanceQuery();
+	if (!m_obstacleQuery)
+		return false;
+	if (!m_obstacleQuery->init(6, 8))
+		return false;
+
+	// Init obstacle query params.
+	memset(m_obstacleQueryParams, 0, sizeof(m_obstacleQueryParams));
+	for (int i = 0; i < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS; ++i)
+	{
+		dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[i];
+		params->velBias = 0.4f;
+		params->weightDesVel = 2.0f;
+		params->weightCurVel = 0.75f;
+		params->weightSide = 0.75f;
+		params->weightToi = 2.5f;
+		params->horizTime = 2.5f;
+		params->gridSize = 33;
+		params->adaptiveDivs = 7;
+		params->adaptiveRings = 2;
+		params->adaptiveDepth = 5;
+	}
+	
+	// Allocate temp buffer for merging paths.
+	m_maxPathResult = 256;
+	m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM);
+	if (!m_pathResult)
+		return false;
+	
+	if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav))
+		return false;
+	
+	m_agents = (dtCrowdAgent*)dtAlloc(sizeof(dtCrowdAgent)*m_maxAgents, DT_ALLOC_PERM);
+	if (!m_agents)
+		return false;
+	
+	m_activeAgents = (dtCrowdAgent**)dtAlloc(sizeof(dtCrowdAgent*)*m_maxAgents, DT_ALLOC_PERM);
+	if (!m_activeAgents)
+		return false;
+
+	m_agentAnims = (dtCrowdAgentAnimation*)dtAlloc(sizeof(dtCrowdAgentAnimation)*m_maxAgents, DT_ALLOC_PERM);
+	if (!m_agentAnims)
+		return false;
+	
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		new(&m_agents[i]) dtCrowdAgent();
+		m_agents[i].active = false;
+		if (!m_agents[i].corridor.init(m_maxPathResult))
+			return false;
+	}
+
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		m_agentAnims[i].active = false;
+	}
+
+	// The navquery is mostly used for local searches, no need for large node pool.
+	m_navquery = dtAllocNavMeshQuery();
+	if (!m_navquery)
+		return false;
+	if (dtStatusFailed(m_navquery->init(nav, MAX_COMMON_NODES)))
+		return false;
+	
+	return true;
+}
+
+void dtCrowd::setObstacleAvoidanceParams(const int idx, const dtObstacleAvoidanceParams* params)
+{
+	if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS)
+		memcpy(&m_obstacleQueryParams[idx], params, sizeof(dtObstacleAvoidanceParams));
+}
+
+const dtObstacleAvoidanceParams* dtCrowd::getObstacleAvoidanceParams(const int idx) const
+{
+	if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS)
+		return &m_obstacleQueryParams[idx];
+	return 0;
+}
+
+int dtCrowd::getAgentCount() const
+{
+	return m_maxAgents;
+}
+
+/// @par
+/// 
+/// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
+const dtCrowdAgent* dtCrowd::getAgent(const int idx)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return 0;
+	return &m_agents[idx];
+}
+
+/// 
+/// Agents in the pool may not be in use.  Check #dtCrowdAgent.active before using the returned object.
+dtCrowdAgent* dtCrowd::getEditableAgent(const int idx)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return 0;
+	return &m_agents[idx];
+}
+
+void dtCrowd::updateAgentParameters(const int idx, const dtCrowdAgentParams* params)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return;
+	memcpy(&m_agents[idx].params, params, sizeof(dtCrowdAgentParams));
+}
+
+/// @par
+///
+/// The agent's position will be constrained to the surface of the navigation mesh.
+int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
+{
+	// Find empty slot.
+	int idx = -1;
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		if (!m_agents[i].active)
+		{
+			idx = i;
+			break;
+		}
+	}
+	if (idx == -1)
+		return -1;
+	
+	dtCrowdAgent* ag = &m_agents[idx];		
+
+	updateAgentParameters(idx, params);
+	
+	// Find nearest position on navmesh and place the agent there.
+	float nearest[3];
+	dtPolyRef ref = 0;
+	dtVcopy(nearest, pos);
+	dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
+	if (dtStatusFailed(status))
+	{
+		dtVcopy(nearest, pos);
+		ref = 0;
+	}
+	
+	ag->corridor.reset(ref, nearest);
+	ag->boundary.reset();
+	ag->partial = false;
+
+	ag->topologyOptTime = 0;
+	ag->targetReplanTime = 0;
+	ag->nneis = 0;
+	
+	dtVset(ag->dvel, 0,0,0);
+	dtVset(ag->nvel, 0,0,0);
+	dtVset(ag->vel, 0,0,0);
+	dtVcopy(ag->npos, nearest);
+	
+	ag->desiredSpeed = 0;
+
+	if (ref)
+		ag->state = DT_CROWDAGENT_STATE_WALKING;
+	else
+		ag->state = DT_CROWDAGENT_STATE_INVALID;
+	
+	ag->targetState = DT_CROWDAGENT_TARGET_NONE;
+	
+	ag->active = true;
+
+	return idx;
+}
+
+/// @par
+///
+/// The agent is deactivated and will no longer be processed.  Its #dtCrowdAgent object
+/// is not removed from the pool.  It is marked as inactive so that it is available for reuse.
+void dtCrowd::removeAgent(const int idx)
+{
+	if (idx >= 0 && idx < m_maxAgents)
+	{
+		m_agents[idx].active = false;
+	}
+}
+
+bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return false;
+	
+	dtCrowdAgent* ag = &m_agents[idx];
+	
+	// Initialize request.
+	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;
+	
+	return true;
+}
+
+/// @par
+/// 
+/// This method is used when a new target is set.
+/// 
+/// The position will be constrained to the surface of the navigation mesh.
+///
+/// The request will be processed during the next #update().
+bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return false;
+	if (!ref)
+		return false;
+
+	dtCrowdAgent* ag = &m_agents[idx];
+	
+	// Initialize request.
+	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;
+}
+
+bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
+{
+	if (idx < 0 || idx >= m_maxAgents)
+		return false;
+	
+	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;
+	
+	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;
+}
+
+int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents)
+{
+	int n = 0;
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		if (!m_agents[i].active) continue;
+		if (n < maxAgents)
+			agents[n++] = &m_agents[i];
+	}
+	return n;
+}
+
+
+void dtCrowd::updateMoveRequest(const float /*dt*/)
+{
+	const int PATH_MAX_AGENTS = 8;
+	dtCrowdAgent* queue[PATH_MAX_AGENTS];
+	int nqueue = 0;
+	
+	// Fire off new requests.
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		dtCrowdAgent* ag = &m_agents[i];
+		if (!ag->active)
+			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)
+		{
+			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 search towards the goal.
+			static const int MAX_ITER = 20;
+			m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
+			m_navquery->updateSlicedFindPath(MAX_ITER, 0);
+			dtStatus status = 0;
+			if (ag->targetReplan) // && npath > 10)
+			{
+				// Try to use existing steady path during replan if possible.
+				status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
+			}
+			else
+			{
+				// 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, 0);
+					if (dtStatusFailed(status))
+						reqPathCount = 0;
+				}
+				else
+				{
+					dtVcopy(reqPos, ag->targetPos);
+				}
+			}
+			else
+			{
+				reqPathCount = 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();
+			ag->partial = false;
+
+			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 (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
+		{
+			nqueue = addToPathQueue(ag, queue, nqueue, PATH_MAX_AGENTS);
+		}
+	}
+
+	for (int i = 0; i < nqueue; ++i)
+	{
+		dtCrowdAgent* ag = queue[i];
+		ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
+											 ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->params.queryFilterType]);
+		if (ag->targetPathqRef != DT_PATHQ_INVALID)
+			ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
+	}
+
+	
+	// Update requests.
+	m_pathq.update(MAX_ITERS_PER_UPDATE);
+
+	dtStatus status;
+
+	// Process path results.
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		dtCrowdAgent* ag = &m_agents[i];
+		if (!ag->active)
+			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
+		
+		if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
+		{
+			// Poll path queue.
+			status = m_pathq.getRequestStatus(ag->targetPathqRef);
+			if (dtStatusFailed(status))
+			{
+				// 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))
+			{
+				const dtPolyRef* path = ag->corridor.getPath();
+				const int npath = ag->corridor.getPathCount();
+				dtAssert(npath);
+				
+				// Apply results.
+				float targetPos[3];
+				dtVcopy(targetPos, ag->targetPos);
+				
+				dtPolyRef* res = m_pathResult;
+				bool valid = true;
+				int nres = 0;
+				status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
+				if (dtStatusFailed(status) || !nres)
+					valid = false;
+
+				if (dtStatusDetail(status, DT_PARTIAL_RESULT))
+					ag->partial = true;
+				else
+					ag->partial = false;
+
+				// Merge result and existing path.
+				// The agent might have moved whilst the request is
+				// being processed, so the path may have changed.
+				// We assume that the end of the path is at the same location
+				// where the request was issued.
+				
+				// The last ref in the old path should be the same as
+				// the location where the request was issued..
+				if (valid && path[npath-1] != res[0])
+					valid = false;
+				
+				if (valid)
+				{
+					// Put the old path infront of the old path.
+					if (npath > 1)
+					{
+						// 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.
+					if (res[nres-1] != ag->targetRef)
+					{
+						// Partial path, constrain target position inside the last polygon.
+						float nearest[3];
+						status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
+						if (dtStatusSucceed(status))
+							dtVcopy(targetPos, nearest);
+						else
+							valid = false;
+					}
+				}
+				
+				if (valid)
+				{
+					// Set current corridor.
+					ag->corridor.setCorridor(targetPos, res, nres);
+					// Force to update boundary.
+					ag->boundary.reset();
+					ag->targetState = DT_CROWDAGENT_TARGET_VALID;
+				}
+				else
+				{
+					// Something went wrong.
+					ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
+				}
+
+				ag->targetReplanTime = 0.0;
+			}
+		}
+	}
+	
+}
+
+
+void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt)
+{
+	if (!nagents)
+		return;
+	
+	const float OPT_TIME_THR = 0.5f; // seconds
+	const int OPT_MAX_AGENTS = 1;
+	dtCrowdAgent* queue[OPT_MAX_AGENTS];
+	int nqueue = 0;
+	
+	for (int i = 0; i < nagents; ++i)
+	{
+		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;
+		if (ag->topologyOptTime >= OPT_TIME_THR)
+			nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS);
+	}
+
+	for (int i = 0; i < nqueue; ++i)
+	{
+		dtCrowdAgent* ag = queue[i];
+		ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
+		ag->topologyOptTime = 0;
+	}
+
+}
+
+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)
+	{
+		dtCrowdAgent* ag = agents[i];
+		
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+			
+		ag->targetReplanTime += dt;
+
+		bool replan = false;
+
+		// 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_filters[ag->params.queryFilterType]))
+		{
+			// Current location is not valid, try to reposition.
+			// TODO: this can snap agents, how to handle that?
+			float nearest[3];
+			dtVcopy(nearest, agentPos);
+			agentRef = 0;
+			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->params.queryFilterType], &agentRef, nearest);
+			dtVcopy(agentPos, nearest);
+
+			if (!agentRef)
+			{
+				// Could not find location in navmesh, set state to invalid.
+				ag->corridor.reset(0, agentPos);
+				ag->partial = false;
+				ag->boundary.reset();
+				ag->state = DT_CROWDAGENT_STATE_INVALID;
+				continue;
+			}
+
+			// Make sure the first polygon is valid, but leave other valid
+			// polygons in the path so that replanner can adjust the path better.
+			ag->corridor.fixPathStart(agentRef, agentPos);
+//			ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
+			ag->boundary.reset();
+			dtVcopy(ag->npos, agentPos);
+
+			replan = true;
+		}
+
+		// If the agent does not have move target or is controlled by velocity, no need to recover the target nor replan.
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+			continue;
+
+		// Try to recover move request position.
+		if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
+		{
+			if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filters[ag->params.queryFilterType]))
+			{
+				// Current target is not valid, try to reposition.
+				float nearest[3];
+				dtVcopy(nearest, ag->targetPos);
+				ag->targetRef = 0;
+				m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest);
+				dtVcopy(ag->targetPos, nearest);
+				replan = true;
+			}
+			if (!ag->targetRef)
+			{
+				// Failed to reposition target, fail moverequest.
+				ag->corridor.reset(agentRef, agentPos);
+				ag->partial = false;
+				ag->targetState = DT_CROWDAGENT_TARGET_NONE;
+			}
+		}
+
+		// If nearby corridor is not valid, replan.
+		if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->params.queryFilterType]))
+		{
+			// Fix current path.
+//			ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
+//			ag->boundary.reset();
+			replan = true;
+		}
+		
+		// If the end of the path is near and it is not the requested location, replan.
+		if (ag->targetState == DT_CROWDAGENT_TARGET_VALID)
+		{
+			if (ag->targetReplanTime > TARGET_REPLAN_DELAY &&
+				ag->corridor.getPathCount() < CHECK_LOOKAHEAD &&
+				ag->corridor.getLastPoly() != ag->targetRef)
+				replan = true;
+		}
+
+		// Try to replan path to goal.
+		if (replan)
+		{
+			if (ag->targetState != DT_CROWDAGENT_TARGET_NONE)
+			{
+				requestMoveTargetReplan(idx, ag->targetRef, ag->targetPos);
+			}
+		}
+	}
+}
+	
+void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
+{
+	m_velocitySampleCount = 0;
+	
+	const int debugIdx = debug ? debug->idx : -1;
+	
+	dtCrowdAgent** agents = m_activeAgents;
+	int nagents = getActiveAgents(agents, m_maxAgents);
+
+	// Check that all agents still have valid paths.
+	checkPathValidity(agents, nagents, dt);
+	
+	// Update async move request and path finder.
+	updateMoveRequest(dt);
+
+	// Optimize path topology.
+	updateTopologyOptimization(agents, nagents, dt);
+	
+	// Register agents to proximity grid.
+	m_grid->clear();
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		const float* p = ag->npos;
+		const float r = ag->params.radius;
+		m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
+	}
+	
+	// Get nearby navmesh segments and agents to collide with.
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+
+		// 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) ||
+			!ag->boundary.isValid(m_navquery, &m_filters[ag->params.queryFilterType]))
+		{
+			ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
+								m_navquery, &m_filters[ag->params.queryFilterType]);
+		}
+		// Query neighbour agents
+		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.
+	for (int i = 0; i < nagents; ++i)
+	{
+		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;
+		
+		// Find corners for steering
+		ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
+												DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.queryFilterType]);
+		
+		// Check to see if the corner after the next corner is directly visible,
+		// and short cut to there.
+		if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
+		{
+			const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
+			ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->params.queryFilterType]);
+			
+			// Copy data for debug purposes.
+			if (debugIdx == i)
+			{
+				dtVcopy(debug->optStart, ag->corridor.getPos());
+				dtVcopy(debug->optEnd, target);
+			}
+		}
+		else
+		{
+			// Copy data for debug purposes.
+			if (debugIdx == i)
+			{
+				dtVset(debug->optStart, 0,0,0);
+				dtVset(debug->optEnd, 0,0,0);
+			}
+		}
+	}
+	
+	// Trigger off-mesh connections (depends on corners).
+	for (int i = 0; i < nagents; ++i)
+	{
+		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;
+		
+		// Check 
+		const float triggerRadius = ag->params.radius*2.25f;
+		if (overOffmeshConnection(ag, triggerRadius))
+		{
+			// Prepare to off-mesh connection.
+			const int idx = (int)(ag - m_agents);
+			dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
+			
+			// Adjust the path over the off-mesh connection.
+			dtPolyRef refs[2];
+			if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
+													   anim->startPos, anim->endPos, m_navquery))
+			{
+				dtVcopy(anim->initPos, ag->npos);
+				anim->polyRef = refs[1];
+				anim->active = true;
+				anim->t = 0.0f;
+				anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
+				
+				ag->state = DT_CROWDAGENT_STATE_OFFMESH;
+				ag->ncorners = 0;
+				ag->nneis = 0;
+				continue;
+			}
+			else
+			{
+				// Path validity check will ensure that bad/blocked connections will be replanned.
+			}
+		}
+	}
+		
+	// Calculate steering.
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
+			continue;
+		
+		float dvel[3] = {0,0,0};
+
+		if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
+		{
+			dtVcopy(dvel, ag->targetPos);
+			ag->desiredSpeed = dtVlen(ag->targetPos);
+		}
+		else
+		{
+			// Calculate steering direction.
+			if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
+				calcSmoothSteerDirection(ag, dvel);
+			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;
+				
+			ag->desiredSpeed = ag->params.maxSpeed;
+			dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
+		}
+
+		// Separation
+		if (ag->params.updateFlags & DT_CROWD_SEPARATION)
+		{
+			const float separationDist = ag->params.collisionQueryRange; 
+			const float invSeparationDist = 1.0f / separationDist; 
+			const float separationWeight = ag->params.separationWeight;
+			
+			float w = 0;
+			float disp[3] = {0,0,0};
+			
+			for (int j = 0; j < ag->nneis; ++j)
+			{
+				const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
+				
+				float diff[3];
+				dtVsub(diff, ag->npos, nei->npos);
+				diff[1] = 0;
+				
+				const float distSqr = dtVlenSqr(diff);
+				if (distSqr < 0.00001f)
+					continue;
+				if (distSqr > dtSqr(separationDist))
+					continue;
+				const float dist = dtMathSqrtf(distSqr);
+				const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
+				
+				dtVmad(disp, disp, diff, weight/dist);
+				w += 1.0f;
+			}
+			
+			if (w > 0.0001f)
+			{
+				// Adjust desired velocity.
+				dtVmad(dvel, dvel, disp, 1.0f/w);
+				// Clamp desired velocity to desired speed.
+				const float speedSqr = dtVlenSqr(dvel);
+				const float desiredSqr = dtSqr(ag->desiredSpeed);
+				if (speedSqr > desiredSqr)
+					dtVscale(dvel, dvel, desiredSqr/speedSqr);
+			}
+		}
+		
+		// Set the desired velocity.
+		dtVcopy(ag->dvel, dvel);
+	}
+	
+	// Velocity planning.	
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+		
+		if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
+		{
+			m_obstacleQuery->reset();
+			
+			// Add neighbours as obstacles.
+			for (int j = 0; j < ag->nneis; ++j)
+			{
+				const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
+				m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
+			}
+
+			// Append neighbour segments as obstacles.
+			for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
+			{
+				const float* s = ag->boundary.getSegment(j);
+				if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
+					continue;
+				m_obstacleQuery->addSegment(s, s+3);
+			}
+
+			dtObstacleAvoidanceDebugData* vod = 0;
+			if (debugIdx == i) 
+				vod = debug->vod;
+			
+			// Sample new safe velocity.
+			bool adaptive = true;
+			int ns = 0;
+
+			const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
+				
+			if (adaptive)
+			{
+				ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
+															 ag->vel, ag->dvel, ag->nvel, params, vod);
+			}
+			else
+			{
+				ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
+														 ag->vel, ag->dvel, ag->nvel, params, vod);
+			}
+			m_velocitySampleCount += ns;
+		}
+		else
+		{
+			// If not using velocity planning, new velocity is directly the desired velocity.
+			dtVcopy(ag->nvel, ag->dvel);
+		}
+	}
+
+	// Integrate.
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+		integrate(ag, dt);
+	}
+	
+	// Handle collisions.
+	static const float COLLISION_RESOLVE_FACTOR = 0.7f;
+	
+	for (int iter = 0; iter < 4; ++iter)
+	{
+		for (int i = 0; i < nagents; ++i)
+		{
+			dtCrowdAgent* ag = agents[i];
+			const int idx0 = getAgentIndex(ag);
+			
+			if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+				continue;
+
+			dtVset(ag->disp, 0,0,0);
+			
+			float w = 0;
+
+			for (int j = 0; j < ag->nneis; ++j)
+			{
+				const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
+				const int idx1 = getAgentIndex(nei);
+
+				float diff[3];
+				dtVsub(diff, ag->npos, nei->npos);
+				diff[1] = 0;
+				
+				float dist = dtVlenSqr(diff);
+				if (dist > dtSqr(ag->params.radius + nei->params.radius))
+					continue;
+				dist = dtMathSqrtf(dist);
+				float pen = (ag->params.radius + nei->params.radius) - dist;
+				if (dist < 0.0001f)
+				{
+					// Agents on top of each other, try to choose diverging separation directions.
+					if (idx0 > idx1)
+						dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
+					else
+						dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
+					pen = 0.01f;
+				}
+				else
+				{
+					pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
+				}
+				
+				dtVmad(ag->disp, ag->disp, diff, pen);			
+				
+				w += 1.0f;
+			}
+			
+			if (w > 0.0001f)
+			{
+				const float iw = 1.0f / w;
+				dtVscale(ag->disp, ag->disp, iw);
+			}
+		}
+		
+		for (int i = 0; i < nagents; ++i)
+		{
+			dtCrowdAgent* ag = agents[i];
+			if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+				continue;
+			
+			dtVadd(ag->npos, ag->npos, ag->disp);
+		}
+	}
+	
+	for (int i = 0; i < nagents; ++i)
+	{
+		dtCrowdAgent* ag = agents[i];
+		if (ag->state != DT_CROWDAGENT_STATE_WALKING)
+			continue;
+		
+		// Move along navmesh.
+		ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
+		// 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);
+			ag->partial = false;
+		}
+
+	}
+	
+	// Update agents using off-mesh connection.
+	for (int i = 0; i < m_maxAgents; ++i)
+	{
+		dtCrowdAgentAnimation* anim = &m_agentAnims[i];
+		if (!anim->active)
+			continue;
+		dtCrowdAgent* ag = agents[i];
+
+		anim->t += dt;
+		if (anim->t > anim->tmax)
+		{
+			// Reset animation
+			anim->active = false;
+			// Prepare agent for walking.
+			ag->state = DT_CROWDAGENT_STATE_WALKING;
+			continue;
+		}
+		
+		// Update position
+		const float ta = anim->tmax*0.15f;
+		const float tb = anim->tmax;
+		if (anim->t < ta)
+		{
+			const float u = tween(anim->t, 0.0, ta);
+			dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
+		}
+		else
+		{
+			const float u = tween(anim->t, ta, tb);
+			dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
+		}
+			
+		// Update velocity.
+		dtVset(ag->vel, 0,0,0);
+		dtVset(ag->dvel, 0,0,0);
+	}
+	
+}