Core/MMAPs: Update recast

Update recast from https://github.com/memononen/recastnavigation/commit/1be9de72d87dcfdecf6bea30f5461745f7d7afe6 to https://github.com/memononen/recastnavigation/commit/740a7ba51600a3c87ce5667ae276a38284a1ce75 .
This differs from https://github.com/TrinityCore/TrinityCore/commit/aa645683b8b25bfb35cb977678daf5c56c1531e6 because it includes a fix https://github.com/memononen/recastnavigation/commit/77ebf643531c90ea712b496e23c48f0b0671f43d which correctly extract tile connections when using rcMergePolyMeshes(), used by mmaps_generator.
MMAPs version is now set to 5.

Re-extract MMAPs using mmaps_generator , any old extracted MMAPs will not work.

25 files changed, 2561 insertions, 1029 deletions

diff --git a/dep/recastnavigation/Detour/DetourCommon.h b/dep/recastnavigation/Detour/DetourCommon.h
index ed7c5149db9..0888614ea9b 100644
--- a/dep/recastnavigation/Detour/DetourCommon.h
+++ b/dep/recastnavigation/Detour/DetourCommon.h
@@ -32,6 +32,11 @@ feature to find minor members.
 /// @name General helper functions
 /// @{
 
+/// Used to ignore a function parameter.  VS complains about unused parameters
+/// and this silences the warning.
+///  @param [in] _ Unused parameter
+template<class T> void dtIgnoreUnused(const T&) { }
+
 /// Swaps the values of the two parameters.
 ///  @param[in,out]	a	Value A
 ///  @param[in,out]	b	Value B
diff --git a/dep/recastnavigation/Detour/DetourNavMesh.cpp b/dep/recastnavigation/Detour/DetourNavMesh.cpp
index 49100b09816..51740509950 100644
--- a/dep/recastnavigation/Detour/DetourNavMesh.cpp
+++ b/dep/recastnavigation/Detour/DetourNavMesh.cpp
@@ -297,6 +297,7 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
 	
 	float amin[2], amax[2];
 	calcSlabEndPoints(va,vb, amin,amax, side);
+	const float apos = getSlabCoord(va, side);
 
 	// Remove links pointing to 'side' and compact the links array. 
 	float bmin[2], bmax[2];
@@ -316,6 +317,13 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
 			
 			const float* vc = &tile->verts[poly->verts[j]*3];
 			const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3];
+			const float bpos = getSlabCoord(vc, side);
+			
+			// Segments are not close enough.
+			if (dtAbs(apos-bpos) > 0.01f)
+				continue;
+			
+			// Check if the segments touch.
 			calcSlabEndPoints(vc,vd, bmin,bmax, side);
 			
 			if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue;
@@ -334,9 +342,11 @@ int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
 	return n;
 }
 
-void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side)
+void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, dtMeshTile* target)
 {
-	if (!tile) return;
+	if (!tile || !target) return;
+
+	const unsigned int targetNum = decodePolyIdTile(getTileRef(target));
 
 	for (int i = 0; i < tile->header->polyCount; ++i)
 	{
@@ -345,7 +355,8 @@ void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side)
 		unsigned int pj = DT_NULL_LINK;
 		while (j != DT_NULL_LINK)
 		{
-			if (tile->links[j].side == side)
+			if (tile->links[j].side != 0xff &&
+				decodePolyIdTile(tile->links[j].ref) == targetNum)
 			{
 				// Revove link.
 				unsigned int nj = tile->links[j].next;
@@ -376,20 +387,25 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side)
 		dtPoly* poly = &tile->polys[i];
 
 		// Create new links.
-		unsigned short m = DT_EXT_LINK | (unsigned short)side;
+//		unsigned short m = DT_EXT_LINK | (unsigned short)side;
 		
 		const int nv = poly->vertCount;
 		for (int j = 0; j < nv; ++j)
 		{
 			// Skip non-portal edges.
-			if (poly->neis[j] != m) continue;
+			if ((poly->neis[j] & DT_EXT_LINK) == 0)
+				continue;
+			
+			const int dir = (int)(poly->neis[j] & 0xff);
+			if (side != -1 && dir != side)
+				continue;
 			
 			// Create new links
 			const float* va = &tile->verts[poly->verts[j]*3];
 			const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3];
 			dtPolyRef nei[4];
 			float neia[4*2];
-			int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(side), nei,neia,4);
+			int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(dir), nei,neia,4);
 			for (int k = 0; k < nnei; ++k)
 			{
 				unsigned int idx = allocLink(tile);
@@ -398,13 +414,13 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side)
 					dtLink* link = &tile->links[idx];
 					link->ref = nei[k];
 					link->edge = (unsigned char)j;
-					link->side = (unsigned char)side;
+					link->side = (unsigned char)dir;
 					
 					link->next = poly->firstLink;
 					poly->firstLink = idx;
 
 					// Compress portal limits to a byte value.
-					if (side == 0 || side == 4)
+					if (dir == 0 || dir == 4)
 					{
 						float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]);
 						float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]);
@@ -413,7 +429,7 @@ void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side)
 						link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
 						link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f);
 					}
-					else if (side == 2 || side == 6)
+					else if (dir == 2 || dir == 6)
 					{
 						float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]);
 						float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]);
@@ -434,7 +450,7 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int
 	
 	// Connect off-mesh links.
 	// We are interested on links which land from target tile to this tile.
-	const unsigned char oppositeSide = (unsigned char)dtOppositeTile(side);
+	const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side);
 	
 	for (int i = 0; i < target->header->offMeshConCount; ++i)
 	{
@@ -443,6 +459,9 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int
 			continue;
 
 		dtPoly* targetPoly = &target->polys[targetCon->poly];
+		// Skip off-mesh connections which start location could not be connected at all.
+		if (targetPoly->firstLink == DT_NULL_LINK)
+			continue;
 		
 		const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad };
 		
@@ -476,19 +495,19 @@ void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int
 		// Link target poly to off-mesh connection.
 		if (targetCon->flags & DT_OFFMESH_CON_BIDIR)
 		{
-			unsigned int idx = allocLink(tile);
-			if (idx != DT_NULL_LINK)
+			unsigned int tidx = allocLink(tile);
+			if (tidx != DT_NULL_LINK)
 			{
 				const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
 				dtPoly* landPoly = &tile->polys[landPolyIdx];
-				dtLink* link = &tile->links[idx];
+				dtLink* link = &tile->links[tidx];
 				link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly);
 				link->edge = 0xff;
-				link->side = (unsigned char)side;
+				link->side = (unsigned char)(side == -1 ? 0xff : side);
 				link->bmin = link->bmax = 0;
 				// Add to linked list.
 				link->next = landPoly->firstLink;
-				landPoly->firstLink = idx;
+				landPoly->firstLink = tidx;
 			}
 		}
 	}
@@ -532,13 +551,13 @@ void dtNavMesh::connectIntLinks(dtMeshTile* tile)
 	}
 }
 
-void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile)
+void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
 {
 	if (!tile) return;
 	
 	dtPolyRef base = getPolyRefBase(tile);
 	
-	// Find Off-mesh connection end points.
+	// Base off-mesh connection start points.
 	for (int i = 0; i < tile->header->offMeshConCount; ++i)
 	{
 		dtOffMeshConnection* con = &tile->offMeshCons[i];
@@ -546,72 +565,109 @@ void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile)
 	
 		const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad };
 		
-		for (int j = 0; j < 2; ++j)
-		{
-			unsigned char side = j == 0 ? 0xff : con->side;
+		// Find polygon to connect to.
+		const float* p = &con->pos[0]; // First vertex
+		float nearestPt[3];
+		dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt);
+		if (!ref) continue;
+		// findNearestPoly may return too optimistic results, further check to make sure. 
+		if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad))
+			continue;
+		// Make sure the location is on current mesh.
+		float* v = &tile->verts[poly->verts[0]*3];
+		dtVcopy(v, nearestPt);
 
-			if (side == 0xff)
-			{
-				// Find polygon to connect to.
-				const float* p = &con->pos[j*3];
-				float nearestPt[3];
-				dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt);
-				if (!ref) continue;
-				// findNearestPoly may return too optimistic results, further check to make sure. 
-				if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad))
-					continue;
-				// Make sure the location is on current mesh.
-				float* v = &tile->verts[poly->verts[j]*3];
-				dtVcopy(v, nearestPt);
-
-				// Link off-mesh connection to target poly.
-				unsigned int idx = allocLink(tile);
-				if (idx != DT_NULL_LINK)
-				{
-					dtLink* link = &tile->links[idx];
-					link->ref = ref;
-					link->edge = (unsigned char)j;
-					link->side = 0xff;
-					link->bmin = link->bmax = 0;
-					// Add to linked list.
-					link->next = poly->firstLink;
-					poly->firstLink = idx;
-				}
+		// Link off-mesh connection to target poly.
+		unsigned int idx = allocLink(tile);
+		if (idx != DT_NULL_LINK)
+		{
+			dtLink* link = &tile->links[idx];
+			link->ref = ref;
+			link->edge = (unsigned char)0;
+			link->side = 0xff;
+			link->bmin = link->bmax = 0;
+			// Add to linked list.
+			link->next = poly->firstLink;
+			poly->firstLink = idx;
+		}
 
-				// Start end-point is always connect back to off-mesh connection,
-				// Destination end-point only if it is bidirectional link. 
-				if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR)))
-				{
-					// Link target poly to off-mesh connection.
-					unsigned int idx = allocLink(tile);
-					if (idx != DT_NULL_LINK)
-					{
-						const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
-						dtPoly* landPoly = &tile->polys[landPolyIdx];
-						dtLink* link = &tile->links[idx];
-						link->ref = base | (dtPolyRef)(con->poly);
-						link->edge = 0xff;
-						link->side = 0xff;
-						link->bmin = link->bmax = 0;
-						// Add to linked list.
-						link->next = landPoly->firstLink;
-						landPoly->firstLink = idx;
-					}
-				}
-				
-			}
+		// Start end-point is always connect back to off-mesh connection. 
+		unsigned int tidx = allocLink(tile);
+		if (tidx != DT_NULL_LINK)
+		{
+			const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
+			dtPoly* landPoly = &tile->polys[landPolyIdx];
+			dtLink* link = &tile->links[tidx];
+			link->ref = base | (dtPolyRef)(con->poly);
+			link->edge = 0xff;
+			link->side = 0xff;
+			link->bmin = link->bmax = 0;
+			// Add to linked list.
+			link->next = landPoly->firstLink;
+			landPoly->firstLink = tidx;
 		}
 	}
 }
 
-dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
-											 const float* pos, float* closest) const
+void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
 {
-	const dtPoly* poly = &tile->polys[ip];
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	getTileAndPolyByRefUnsafe(ref, &tile, &poly);
 	
-	float closestDistSqr = FLT_MAX;
+	// Off-mesh connections don't have detail polygons.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		const float* v0 = &tile->verts[poly->verts[0]*3];
+		const float* v1 = &tile->verts[poly->verts[1]*3];
+		const float d0 = dtVdist(pos, v0);
+		const float d1 = dtVdist(pos, v1);
+		const float u = d0 / (d0+d1);
+		dtVlerp(closest, v0, v1, u);
+		if (posOverPoly)
+			*posOverPoly = false;
+		return;
+	}
+	
+	const unsigned int ip = (unsigned int)(poly - tile->polys);
 	const dtPolyDetail* pd = &tile->detailMeshes[ip];
 	
+	// Clamp point to be inside the polygon.
+	float verts[DT_VERTS_PER_POLYGON*3];	
+	float edged[DT_VERTS_PER_POLYGON];
+	float edget[DT_VERTS_PER_POLYGON];
+	const int nv = poly->vertCount;
+	for (int i = 0; i < nv; ++i)
+		dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
+	
+	dtVcopy(closest, pos);
+	if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
+	{
+		// Point is outside the polygon, dtClamp to nearest edge.
+		float dmin = FLT_MAX;
+		int imin = -1;
+		for (int i = 0; i < nv; ++i)
+		{
+			if (edged[i] < dmin)
+			{
+				dmin = edged[i];
+				imin = i;
+			}
+		}
+		const float* va = &verts[imin*3];
+		const float* vb = &verts[((imin+1)%nv)*3];
+		dtVlerp(closest, va, vb, edget[imin]);
+		
+		if (posOverPoly)
+			*posOverPoly = false;
+	}
+	else
+	{
+		if (posOverPoly)
+			*posOverPoly = true;
+	}
+	
+	// Find height at the location.
 	for (int j = 0; j < pd->triCount; ++j)
 	{
 		const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
@@ -623,17 +679,13 @@ dtStatus dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned in
 			else
 				v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
 		}
-		float pt[3];
-		dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
-		float d = dtVdistSqr(pos, pt);
-		if (d < closestDistSqr)
+		float h;
+		if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
 		{
-			dtVcopy(closest, pt);
-			closestDistSqr = d;
+			closest[1] = h;
+			break;
 		}
 	}
-	
-	return DT_SUCCESS;
 }
 
 dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
@@ -655,13 +707,27 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
-		if (closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly) != DT_SUCCESS)
-			continue;
-		float d = dtVdistSqr(center, closestPtPoly);
+		float diff[3];
+		bool posOverPoly = false;
+		float d = 0;
+		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
+
+		// If a point is directly over a polygon and closer than
+		// climb height, favor that instead of straight line nearest point.
+		dtVsub(diff, center, closestPtPoly);
+		if (posOverPoly)
+		{
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
+		}
+		
 		if (d < nearestDistanceSqr)
 		{
-			if (nearestPt)
-				dtVcopy(nearestPt, closestPtPoly);
+			dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
 			nearest = ref;
 		}
@@ -730,8 +796,11 @@ int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, co
 		dtPolyRef base = getPolyRefBase(tile);
 		for (int i = 0; i < tile->header->polyCount; ++i)
 		{
-			// Calc polygon bounds.
 			dtPoly* p = &tile->polys[i];
+			// Do not return off-mesh connection polygons.
+			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			// Calc polygon bounds.
 			const float* v = &tile->verts[p->verts[0]*3];
 			dtVcopy(bmin, v);
 			dtVcopy(bmax, v);
@@ -773,7 +842,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
 		return DT_FAILURE | DT_WRONG_VERSION;
 		
 	// Make sure the location is free.
-	if (getTileAt(header->x, header->y))
+	if (getTileAt(header->x, header->y, header->layer))
 		return DT_FAILURE;
 		
 	// Allocate a tile.
@@ -845,6 +914,10 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
 	tile->bvTree = (dtBVNode*)d; d += bvtreeSize;
 	tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize;
 
+	// If there are no items in the bvtree, reset the tree pointer.
+	if (!bvtreeSize)
+		tile->bvTree = 0;
+
 	// Build links freelist
 	tile->linksFreeList = 0;
 	tile->links[header->maxLinkCount-1].next = DT_NULL_LINK;
@@ -858,18 +931,36 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
 	tile->flags = flags;
 
 	connectIntLinks(tile);
-	connectIntOffMeshLinks(tile);
+	baseOffMeshLinks(tile);
 
 	// Create connections with neighbour tiles.
+	static const int MAX_NEIS = 32;
+	dtMeshTile* neis[MAX_NEIS];
+	int nneis;
+	
+	// Connect with layers in current tile.
+	nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS);
+	for (int j = 0; j < nneis; ++j)
+	{
+		if (neis[j] != tile)
+		{
+			connectExtLinks(tile, neis[j], -1);
+			connectExtLinks(neis[j], tile, -1);
+		}
+		connectExtOffMeshLinks(tile, neis[j], -1);
+		connectExtOffMeshLinks(neis[j], tile, -1);
+	}
+	
+	// Connect with neighbour tiles.
 	for (int i = 0; i < 8; ++i)
 	{
-		dtMeshTile* nei = getNeighbourTileAt(header->x, header->y, i);
-		if (nei)
+		nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS);
+		for (int j = 0; j < nneis; ++j)
 		{
-			connectExtLinks(tile, nei, i);
-			connectExtLinks(nei, tile, dtOppositeTile(i));
-			connectExtOffMeshLinks(tile, nei, i);
-			connectExtOffMeshLinks(nei, tile, dtOppositeTile(i));
+			connectExtLinks(tile, neis[j], i);
+			connectExtLinks(neis[j], tile, dtOppositeTile(i));
+			connectExtOffMeshLinks(tile, neis[j], i);
+			connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i));
 		}
 	}
 	
@@ -879,55 +970,106 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
 	return DT_SUCCESS;
 }
 
-const dtMeshTile* dtNavMesh::getTileAt(int x, int y) const
+const dtMeshTile* dtNavMesh::getTileAt(const int x, const int y, const int layer) const
 {
 	// Find tile based on hash.
 	int h = computeTileHash(x,y,m_tileLutMask);
 	dtMeshTile* tile = m_posLookup[h];
 	while (tile)
 	{
-		if (tile->header && tile->header->x == x && tile->header->y == y)
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y &&
+			tile->header->layer == layer)
+		{
 			return tile;
+		}
 		tile = tile->next;
 	}
 	return 0;
 }
 
-dtMeshTile* dtNavMesh::getNeighbourTileAt(int x, int y, int side) const
+int dtNavMesh::getNeighbourTilesAt(const int x, const int y, const int side, dtMeshTile** tiles, const int maxTiles) const
 {
+	int nx = x, ny = y;
 	switch (side)
 	{
-		case 0: x++; break;
-		case 1: x++; y++; break;
-		case 2: y++; break;
-		case 3: x--; y++; break;
-		case 4: x--; break;
-		case 5: x--; y--; break;
-		case 6: y--; break;
-		case 7: x++; y--; break;
+		case 0: nx++; break;
+		case 1: nx++; ny++; break;
+		case 2: ny++; break;
+		case 3: nx--; ny++; break;
+		case 4: nx--; break;
+		case 5: nx--; ny--; break;
+		case 6: ny--; break;
+		case 7: nx++; ny--; break;
 	};
 
+	return getTilesAt(nx, ny, tiles, maxTiles);
+}
+
+int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const int maxTiles) const
+{
+	int n = 0;
+	
 	// Find tile based on hash.
 	int h = computeTileHash(x,y,m_tileLutMask);
 	dtMeshTile* tile = m_posLookup[h];
 	while (tile)
 	{
-		if (tile->header && tile->header->x == x && tile->header->y == y)
-			return tile;
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y)
+		{
+			if (n < maxTiles)
+				tiles[n++] = tile;
+		}
 		tile = tile->next;
 	}
-	return 0;
+	
+	return n;
+}
+
+/// @par
+///
+/// This function will not fail if the tiles array is too small to hold the
+/// entire result set.  It will simply fill the array to capacity.
+int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile const** tiles, const int maxTiles) const
+{
+	int n = 0;
+	
+	// Find tile based on hash.
+	int h = computeTileHash(x,y,m_tileLutMask);
+	dtMeshTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y)
+		{
+			if (n < maxTiles)
+				tiles[n++] = tile;
+		}
+		tile = tile->next;
+	}
+	
+	return n;
 }
 
-dtTileRef dtNavMesh::getTileRefAt(int x, int y) const
+
+dtTileRef dtNavMesh::getTileRefAt(const int x, const int y, const int layer) const
 {
 	// Find tile based on hash.
 	int h = computeTileHash(x,y,m_tileLutMask);
 	dtMeshTile* tile = m_posLookup[h];
 	while (tile)
 	{
-		if (tile->header && tile->header->x == x && tile->header->y == y)
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y &&
+			tile->header->layer == layer)
+		{
 			return getTileRef(tile);
+		}
 		tile = tile->next;
 	}
 	return 0;
@@ -970,6 +1112,7 @@ void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const
 
 dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const
 {
+	if (!ref) return DT_FAILURE;
 	unsigned int salt, it, ip;
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
@@ -995,6 +1138,7 @@ void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile*
 
 bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const
 {
+	if (!ref) return false;
 	unsigned int salt, it, ip;
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return false;
@@ -1040,14 +1184,27 @@ dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSiz
 	}
 	
 	// Remove connections to neighbour tiles.
-	for (int i = 0; i < 8; ++i)
+	// Create connections with neighbour tiles.
+	static const int MAX_NEIS = 32;
+	dtMeshTile* neis[MAX_NEIS];
+	int nneis;
+	
+	// Connect with layers in current tile.
+	nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS);
+	for (int j = 0; j < nneis; ++j)
 	{
-		dtMeshTile* nei = getNeighbourTileAt(tile->header->x,tile->header->y,i);
-		if (!nei) continue;
-		unconnectExtLinks(nei, dtOppositeTile(i));
+		if (neis[j] == tile) continue;
+		unconnectExtLinks(neis[j], tile);
 	}
 	
-	
+	// Connect with neighbour tiles.
+	for (int i = 0; i < 8; ++i)
+	{
+		nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS);
+		for (int j = 0; j < nneis; ++j)
+			unconnectExtLinks(neis[j], tile);
+	}
+		
 	// Reset tile.
 	if (tile->flags & DT_TILE_FREE_DATA)
 	{
@@ -1091,7 +1248,7 @@ dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSiz
 dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const
 {
 	if (!tile) return 0;
-	const unsigned int it = tile - m_tiles;
+	const unsigned int it = (unsigned int)(tile - m_tiles);
 	return (dtTileRef)encodePolyId(tile->salt, it, 0);
 }
 
@@ -1112,7 +1269,7 @@ dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const
 dtPolyRef dtNavMesh::getPolyRefBase(const dtMeshTile* tile) const
 {
 	if (!tile) return 0;
-	const unsigned int it = tile - m_tiles;
+	const unsigned int it = (unsigned int)(tile - m_tiles);
 	return encodePolyId(tile->salt, it, 0);
 }
 
@@ -1216,6 +1373,9 @@ dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyR
 {
 	unsigned int salt, it, ip;
 
+	if (!polyRef)
+		return DT_FAILURE;
+	
 	// Get current polygon
 	decodePolyId(polyRef, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
@@ -1256,6 +1416,9 @@ const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) c
 {
 	unsigned int salt, it, ip;
 	
+	if (!ref)
+		return 0;
+	
 	// Get current polygon
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return 0;
@@ -1276,6 +1439,7 @@ const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) c
 
 dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags)
 {
+	if (!ref) return DT_FAILURE;
 	unsigned int salt, it, ip;
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
@@ -1292,6 +1456,7 @@ dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags)
 
 dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const
 {
+	if (!ref) return DT_FAILURE;
 	unsigned int salt, it, ip;
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
@@ -1307,6 +1472,7 @@ dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) con
 
 dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area)
 {
+	if (!ref) return DT_FAILURE;
 	unsigned int salt, it, ip;
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
@@ -1322,6 +1488,7 @@ dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area)
 
 dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const
 {
+	if (!ref) return DT_FAILURE;
 	unsigned int salt, it, ip;
 	decodePolyId(ref, salt, it, ip);
 	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
diff --git a/dep/recastnavigation/Detour/DetourNavMesh.h b/dep/recastnavigation/Detour/DetourNavMesh.h
index 7175f8e73eb..cdd473f1aff 100644
--- a/dep/recastnavigation/Detour/DetourNavMesh.h
+++ b/dep/recastnavigation/Detour/DetourNavMesh.h
@@ -66,7 +66,7 @@ static const int DT_VERTS_PER_POLYGON = 6;
 static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V';
 
 /// A version number used to detect compatibility of navigation tile data.
-static const int DT_NAVMESH_VERSION = 6;
+static const int DT_NAVMESH_VERSION = 7;
 
 /// A magic number used to detect the compatibility of navigation tile states.
 static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S';
@@ -112,6 +112,12 @@ enum dtStraightPathFlags
 	DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04,	///< The vertex is the start of an off-mesh connection.
 };
 
+/// Options for dtNavMeshQuery::findStraightPath.
+enum dtStraightPathOptions
+{
+	DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01,	///< Add a vertex at every polygon edge crossing where area changes.
+	DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02,	///< Add a vertex at every polygon edge crossing.
+};
 
 /// Flags representing the type of a navigation mesh polygon.
 enum dtPolyTypes
@@ -225,6 +231,7 @@ struct dtMeshHeader
 	int version;			///< Tile data format version number.
 	int x;					///< The x-position of the tile within the dtNavMesh tile grid. (x, y, layer)
 	int y;					///< The y-position of the tile within the dtNavMesh tile grid. (x, y, layer)
+	int layer;				///< The layer of the tile within the dtNavMesh tile grid. (x, y, layer)
 	unsigned int userId;	///< The user defined id of the tile.
 	int polyCount;			///< The number of polygons in the tile.
 	int vertCount;			///< The number of vertices in the tile.
@@ -347,18 +354,31 @@ public:
 	void calcTileLoc(const float* pos, int* tx, int* ty) const;
 
 	/// Gets the tile at the specified grid location.
-	// Params:
-	//  x,y - (in) Location of the tile to get.
-	// Returns: pointer to tile if tile exists or 0 tile does not exists.
-	const dtMeshTile* getTileAt(int x, int y) const;
-
-	// Returns reference to tile at specified location.
-	// Params:
-	//  x,y - (in) Location of the tile to get.
-	// Returns: reference to tile if tile exists or 0 tile does not exists.
-	dtTileRef getTileRefAt(int x, int y) const;
+	///  @param[in]	x		The tile's x-location. (x, y, layer)
+	///  @param[in]	y		The tile's y-location. (x, y, layer)
+	///  @param[in]	layer	The tile's layer. (x, y, layer)
+	/// @return The tile, or null if the tile does not exist.
+	const dtMeshTile* getTileAt(const int x, const int y, const int layer) const;
+
+	/// Gets all tiles at the specified grid location. (All layers.)
+	///  @param[in]		x			The tile's x-location. (x, y)
+	///  @param[in]		y			The tile's y-location. (x, y)
+	///  @param[out]	tiles		A pointer to an array of tiles that will hold the result.
+	///  @param[in]		maxTiles	The maximum tiles the tiles parameter can hold.
+	/// @return The number of tiles returned in the tiles array.
+	int getTilesAt(const int x, const int y,
+				   dtMeshTile const** tiles, const int maxTiles) const;
 	
-	// Returns tile references of a tile based on tile pointer.
+	/// Gets the tile reference for the tile at specified grid location.
+	///  @param[in]	x		The tile's x-location. (x, y, layer)
+	///  @param[in]	y		The tile's y-location. (x, y, layer)
+	///  @param[in]	layer	The tile's layer. (x, y, layer)
+	/// @return The tile reference of the tile, or 0 if there is none.
+	dtTileRef getTileRefAt(int x, int y, int layer) const;
+
+	/// Gets the tile reference for the specified tile.
+	///  @param[in]	tile	The tile.
+	/// @return The tile reference of the tile.
 	dtTileRef getTileRef(const dtMeshTile* tile) const;
 
 	/// Gets the tile for the specified tile reference.
@@ -532,7 +552,13 @@ private:
 	dtMeshTile* getTile(int i);
 
 	/// Returns neighbour tile based on side.
-	dtMeshTile* getNeighbourTileAt(int x, int y, int side) const;
+	int getTilesAt(const int x, const int y,
+				   dtMeshTile** tiles, const int maxTiles) const;
+
+	/// Returns neighbour tile based on side.
+	int getNeighbourTilesAt(const int x, const int y, const int side,
+							dtMeshTile** tiles, const int maxTiles) const;
+	
 	/// Returns all polygons in neighbour tile based on portal defined by the segment.
 	int findConnectingPolys(const float* va, const float* vb,
 							const dtMeshTile* tile, int side,
@@ -541,7 +567,7 @@ private:
 	/// Builds internal polygons links for a tile.
 	void connectIntLinks(dtMeshTile* tile);
 	/// Builds internal polygons links for a tile.
-	void connectIntOffMeshLinks(dtMeshTile* tile);
+	void baseOffMeshLinks(dtMeshTile* tile);
 
 	/// Builds external polygon links for a tile.
 	void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side);
@@ -549,7 +575,7 @@ private:
 	void connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side);
 	
 	/// Removes external links at specified side.
-	void unconnectExtLinks(dtMeshTile* tile, int side);
+	void unconnectExtLinks(dtMeshTile* tile, dtMeshTile* target);
 	
 
 	// TODO: These methods are duplicates from dtNavMeshQuery, but are needed for off-mesh connection finding.
@@ -561,8 +587,7 @@ private:
 	dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center,
 									const float* extents, float* nearestPt) const;
 	/// Returns closest point on polygon.
-	dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
-									  const float* pos, float* closest) const;
+	void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
 	
 	dtNavMeshParams m_params;			///< Current initialization params. TODO: do not store this info twice.
 	float m_orig[3];					///< Origin of the tile (0,0)
diff --git a/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp b/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp
index fcac215fff4..9d8471b96a1 100644
--- a/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp
+++ b/dep/recastnavigation/Detour/DetourNavMeshBuilder.cpp
@@ -261,8 +261,6 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 		return false;
 	if (!params->polyCount || !params->polys)
 		return false;
-	if (!params->detailMeshes || !params->detailVerts || !params->detailTris)
-		return false;
 
 	const int nvp = params->nvp;
 	
@@ -278,10 +276,50 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 		if (!offMeshConClass)
 			return false;
 
+		// Find tight heigh bounds, used for culling out off-mesh start locations.
+		float hmin = FLT_MAX;
+		float hmax = -FLT_MAX;
+		
+		if (params->detailVerts && params->detailVertsCount)
+		{
+			for (int i = 0; i < params->detailVertsCount; ++i)
+			{
+				const float h = params->detailVerts[i*3+1];
+				hmin = dtMin(hmin,h);
+				hmax = dtMax(hmax,h);
+			}
+		}
+		else
+		{
+			for (int i = 0; i < params->vertCount; ++i)
+			{
+				const unsigned short* iv = &params->verts[i*3];
+				const float h = params->bmin[1] + iv[1] * params->ch;
+				hmin = dtMin(hmin,h);
+				hmax = dtMax(hmax,h);
+			}
+		}
+		hmin -= params->walkableClimb;
+		hmax += params->walkableClimb;
+		float bmin[3], bmax[3];
+		dtVcopy(bmin, params->bmin);
+		dtVcopy(bmax, params->bmax);
+		bmin[1] = hmin;
+		bmax[1] = hmax;
+
 		for (int i = 0; i < params->offMeshConCount; ++i)
 		{
-			offMeshConClass[i*2+0] = classifyOffMeshPoint(&params->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax);
-			offMeshConClass[i*2+1] = classifyOffMeshPoint(&params->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax);
+			const float* p0 = &params->offMeshConVerts[(i*2+0)*3];
+			const float* p1 = &params->offMeshConVerts[(i*2+1)*3];
+			offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax);
+			offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax);
+
+			// Zero out off-mesh start positions which are not even potentially touching the mesh.
+			if (offMeshConClass[i*2+0] == 0xff)
+			{
+				if (p0[1] < bmin[1] || p0[1] > bmax[1])
+					offMeshConClass[i*2+0] = 0;
+			}
 
 			// Cound how many links should be allocated for off-mesh connections.
 			if (offMeshConClass[i*2+0] == 0xff)
@@ -307,23 +345,13 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 		for (int j = 0; j < nvp; ++j)
 		{
 			if (p[j] == MESH_NULL_IDX) break;
-			int nj = j+1;
-			if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0;
-			const unsigned short* va = &params->verts[p[j]*3];
-			const unsigned short* vb = &params->verts[p[nj]*3];
-			
 			edgeCount++;
 			
-			if (params->tileSize > 0)
+			if (p[nvp+j] & 0x8000)
 			{
-				if (va[0] == params->tileSize && vb[0] == params->tileSize)
-					portalCount++; // x+
-				else if (va[2] == params->tileSize && vb[2] == params->tileSize)
-					portalCount++; // z+
-				else if (va[0] == 0 && vb[0] == 0)
-					portalCount++; // x-
-				else if (va[2] == 0 && vb[2] == 0)
-					portalCount++; // z-
+				unsigned short dir = p[nvp+j] & 0xf;
+				if (dir != 0xf)
+					portalCount++;
 			}
 		}
 	}
@@ -332,18 +360,41 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 	
 	// Find unique detail vertices.
 	int uniqueDetailVertCount = 0;
-	for (int i = 0; i < params->polyCount; ++i)
+	int detailTriCount = 0;
+	if (params->detailMeshes)
 	{
-		const unsigned short* p = &params->polys[i*nvp*2];
-		int ndv = params->detailMeshes[i*4+1];
-		int nv = 0;
-		for (int j = 0; j < nvp; ++j)
+		// Has detail mesh, count unique detail vertex count and use input detail tri count.
+		detailTriCount = params->detailTriCount;
+		for (int i = 0; i < params->polyCount; ++i)
 		{
-			if (p[j] == MESH_NULL_IDX) break;
-			nv++;
+			const unsigned short* p = &params->polys[i*nvp*2];
+			int ndv = params->detailMeshes[i*4+1];
+			int nv = 0;
+			for (int j = 0; j < nvp; ++j)
+			{
+				if (p[j] == MESH_NULL_IDX) break;
+				nv++;
+			}
+			ndv -= nv;
+			uniqueDetailVertCount += ndv;
+		}
+	}
+	else
+	{
+		// No input detail mesh, build detail mesh from nav polys.
+		uniqueDetailVertCount = 0; // No extra detail verts.
+		detailTriCount = 0;
+		for (int i = 0; i < params->polyCount; ++i)
+		{
+			const unsigned short* p = &params->polys[i*nvp*2];
+			int nv = 0;
+			for (int j = 0; j < nvp; ++j)
+			{
+				if (p[j] == MESH_NULL_IDX) break;
+				nv++;
+			}
+			detailTriCount += nv-2;
 		}
-		ndv -= nv;
-		uniqueDetailVertCount += ndv;
 	}
 	
 	// Calculate data size
@@ -353,8 +404,8 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 	const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount);
 	const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount);
 	const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount);
-	const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*params->detailTriCount);
-	const int bvTreeSize = dtAlign4(sizeof(dtBVNode)*params->polyCount*2);
+	const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount);
+	const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0;
 	const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount);
 	
 	const int dataSize = headerSize + vertsSize + polysSize + linksSize +
@@ -386,6 +437,7 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 	header->version = DT_NAVMESH_VERSION;
 	header->x = params->tileX;
 	header->y = params->tileY;
+	header->layer = params->tileLayer;
 	header->userId = params->userId;
 	header->polyCount = totPolyCount;
 	header->vertCount = totVertCount;
@@ -394,14 +446,14 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 	dtVcopy(header->bmax, params->bmax);
 	header->detailMeshCount = params->polyCount;
 	header->detailVertCount = uniqueDetailVertCount;
-	header->detailTriCount = params->detailTriCount;
+	header->detailTriCount = detailTriCount;
 	header->bvQuantFactor = 1.0f / params->cs;
 	header->offMeshBase = params->polyCount;
 	header->walkableHeight = params->walkableHeight;
 	header->walkableRadius = params->walkableRadius;
 	header->walkableClimb = params->walkableClimb;
 	header->offMeshConCount = storedOffMeshConCount;
-	header->bvNodeCount = params->polyCount*2;
+	header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0;
 	
 	const int offMeshVertsBase = params->vertCount;
 	const int offMeshPolyBase = params->polyCount;
@@ -445,7 +497,27 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 		{
 			if (src[j] == MESH_NULL_IDX) break;
 			p->verts[j] = src[j];
-			p->neis[j] = (src[nvp+j]+1) & 0xffff;
+			if (src[nvp+j] & 0x8000)
+			{
+				// Border or portal edge.
+				unsigned short dir = src[nvp+j] & 0xf;
+				if (dir == 0xf) // Border
+					p->neis[j] = 0;
+				else if (dir == 0) // Portal x-
+					p->neis[j] = DT_EXT_LINK | 4;
+				else if (dir == 1) // Portal z+
+					p->neis[j] = DT_EXT_LINK | 2;
+				else if (dir == 2) // Portal x+
+					p->neis[j] = DT_EXT_LINK | 0;
+				else if (dir == 3) // Portal z-
+					p->neis[j] = DT_EXT_LINK | 6;
+			}
+			else
+			{
+				// Normal connection
+				p->neis[j] = src[nvp+j]+1;
+			}
+			
 			p->vertCount++;
 		}
 		src += nvp*2;
@@ -467,61 +539,68 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 			n++;
 		}
 	}
-	
-	// Store portal edges.
-	if (params->tileSize > 0)
+
+	// Store detail meshes and vertices.
+	// The nav polygon vertices are stored as the first vertices on each mesh.
+	// We compress the mesh data by skipping them and using the navmesh coordinates.
+	if (params->detailMeshes)
 	{
+		unsigned short vbase = 0;
 		for (int i = 0; i < params->polyCount; ++i)
 		{
-			dtPoly* poly = &navPolys[i];
-			for (int j = 0; j < poly->vertCount; ++j)
+			dtPolyDetail& dtl = navDMeshes[i];
+			const int vb = (int)params->detailMeshes[i*4+0];
+			const int ndv = (int)params->detailMeshes[i*4+1];
+			const int nv = navPolys[i].vertCount;
+			dtl.vertBase = (unsigned int)vbase;
+			dtl.vertCount = (unsigned char)(ndv-nv);
+			dtl.triBase = (unsigned int)params->detailMeshes[i*4+2];
+			dtl.triCount = (unsigned char)params->detailMeshes[i*4+3];
+			// Copy vertices except the first 'nv' verts which are equal to nav poly verts.
+			if (ndv-nv)
 			{
-				int nj = j+1;
-				if (nj >= poly->vertCount) nj = 0;
-
-				const unsigned short* va = &params->verts[poly->verts[j]*3];
-				const unsigned short* vb = &params->verts[poly->verts[nj]*3];
-							
-				if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+
-					poly->neis[j] = DT_EXT_LINK | 0;
-				else if (va[2] == params->tileSize && vb[2]  == params->tileSize) // z+
-					poly->neis[j] = DT_EXT_LINK | 2;
-				else if (va[0] == 0 && vb[0] == 0) // x-
-					poly->neis[j] = DT_EXT_LINK | 4;
-				else if (va[2] == 0 && vb[2] == 0) // z-
-					poly->neis[j] = DT_EXT_LINK | 6;
+				memcpy(&navDVerts[vbase*3], &params->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv));
+				vbase += (unsigned short)(ndv-nv);
 			}
 		}
+		// Store triangles.
+		memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount);
 	}
-
-	// Store detail meshes and vertices.
-	// The nav polygon vertices are stored as the first vertices on each mesh.
-	// We compress the mesh data by skipping them and using the navmesh coordinates.
-	unsigned short vbase = 0;
-	for (int i = 0; i < params->polyCount; ++i)
+	else
 	{
-		dtPolyDetail& dtl = navDMeshes[i];
-		const int vb = (int)params->detailMeshes[i*4+0];
-		const int ndv = (int)params->detailMeshes[i*4+1];
-		const int nv = navPolys[i].vertCount;
-		dtl.vertBase = (unsigned int)vbase;
-		dtl.vertCount = (unsigned char)(ndv-nv);
-		dtl.triBase = (unsigned int)params->detailMeshes[i*4+2];
-		dtl.triCount = (unsigned char)params->detailMeshes[i*4+3];
-		// Copy vertices except the first 'nv' verts which are equal to nav poly verts.
-		if (ndv-nv)
+		// Create dummy detail mesh by triangulating polys.
+		int tbase = 0;
+		for (int i = 0; i < params->polyCount; ++i)
 		{
-			memcpy(&navDVerts[vbase*3], &params->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv));
-			vbase += (unsigned short)(ndv-nv);
+			dtPolyDetail& dtl = navDMeshes[i];
+			const int nv = navPolys[i].vertCount;
+			dtl.vertBase = 0;
+			dtl.vertCount = 0;
+			dtl.triBase = (unsigned int)tbase;
+			dtl.triCount = (unsigned char)(nv-2);
+			// Triangulate polygon (local indices).
+			for (int j = 2; j < nv; ++j)
+			{
+				unsigned char* t = &navDTris[tbase*4];
+				t[0] = 0;
+				t[1] = (unsigned char)(j-1);
+				t[2] = (unsigned char)j;
+				// Bit for each edge that belongs to poly boundary.
+				t[3] = (1<<2);
+				if (j == 2) t[3] |= (1<<0);
+				if (j == nv-1) t[3] |= (1<<4);
+				tbase++;
+			}
 		}
 	}
-	// Store triangles.
-	memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount);
 
 	// Store and create BVtree.
 	// TODO: take detail mesh into account! use byte per bbox extent?
-	createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
-				 nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
+	if (params->buildBvTree)
+	{
+		createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
+					 nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
+	}
 	
 	// Store Off-Mesh connections.
 	n = 0;
@@ -553,51 +632,14 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 	return true;
 }
 
-inline void swapByte(unsigned char* a, unsigned char* b)
-{
-	unsigned char tmp = *a;
-	*a = *b;
-	*b = tmp;
-}
-
-inline void swapEndian(unsigned short* v)
-{
-	unsigned char* x = (unsigned char*)v;
-	swapByte(x+0, x+1);
-}
-
-inline void swapEndian(short* v)
-{
-	unsigned char* x = (unsigned char*)v;
-	swapByte(x+0, x+1);
-}
-
-inline void swapEndian(unsigned int* v)
-{
-	unsigned char* x = (unsigned char*)v;
-	swapByte(x+0, x+3); swapByte(x+1, x+2);
-}
-
-inline void swapEndian(int* v)
-{
-	unsigned char* x = (unsigned char*)v;
-	swapByte(x+0, x+3); swapByte(x+1, x+2);
-}
-
-inline void swapEndian(float* v)
-{
-	unsigned char* x = (unsigned char*)v;
-	swapByte(x+0, x+3); swapByte(x+1, x+2);
-}
-
 bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/)
 {
 	dtMeshHeader* header = (dtMeshHeader*)data;
 	
 	int swappedMagic = DT_NAVMESH_MAGIC;
 	int swappedVersion = DT_NAVMESH_VERSION;
-	swapEndian(&swappedMagic);
-	swapEndian(&swappedVersion);
+	dtSwapEndian(&swappedMagic);
+	dtSwapEndian(&swappedVersion);
 	
 	if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) &&
 		(header->magic != swappedMagic || header->version != swappedVersion))
@@ -605,30 +647,31 @@ bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/)
 		return false;
 	}
 		
-	swapEndian(&header->magic);
-	swapEndian(&header->version);
-	swapEndian(&header->x);
-	swapEndian(&header->y);
-	swapEndian(&header->userId);
-	swapEndian(&header->polyCount);
-	swapEndian(&header->vertCount);
-	swapEndian(&header->maxLinkCount);
-	swapEndian(&header->detailMeshCount);
-	swapEndian(&header->detailVertCount);
-	swapEndian(&header->detailTriCount);
-	swapEndian(&header->bvNodeCount);
-	swapEndian(&header->offMeshConCount);
-	swapEndian(&header->offMeshBase);
-	swapEndian(&header->walkableHeight);
-	swapEndian(&header->walkableRadius);
-	swapEndian(&header->walkableClimb);
-	swapEndian(&header->bmin[0]);
-	swapEndian(&header->bmin[1]);
-	swapEndian(&header->bmin[2]);
-	swapEndian(&header->bmax[0]);
-	swapEndian(&header->bmax[1]);
-	swapEndian(&header->bmax[2]);
-	swapEndian(&header->bvQuantFactor);
+	dtSwapEndian(&header->magic);
+	dtSwapEndian(&header->version);
+	dtSwapEndian(&header->x);
+	dtSwapEndian(&header->y);
+	dtSwapEndian(&header->layer);
+	dtSwapEndian(&header->userId);
+	dtSwapEndian(&header->polyCount);
+	dtSwapEndian(&header->vertCount);
+	dtSwapEndian(&header->maxLinkCount);
+	dtSwapEndian(&header->detailMeshCount);
+	dtSwapEndian(&header->detailVertCount);
+	dtSwapEndian(&header->detailTriCount);
+	dtSwapEndian(&header->bvNodeCount);
+	dtSwapEndian(&header->offMeshConCount);
+	dtSwapEndian(&header->offMeshBase);
+	dtSwapEndian(&header->walkableHeight);
+	dtSwapEndian(&header->walkableRadius);
+	dtSwapEndian(&header->walkableClimb);
+	dtSwapEndian(&header->bmin[0]);
+	dtSwapEndian(&header->bmin[1]);
+	dtSwapEndian(&header->bmin[2]);
+	dtSwapEndian(&header->bmax[0]);
+	dtSwapEndian(&header->bmax[1]);
+	dtSwapEndian(&header->bmax[2]);
+	dtSwapEndian(&header->bvQuantFactor);
 
 	// Freelist index and pointers are updated when tile is added, no need to swap.
 
@@ -674,7 +717,7 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
 	// Vertices
 	for (int i = 0; i < header->vertCount*3; ++i)
 	{
-		swapEndian(&verts[i]);
+		dtSwapEndian(&verts[i]);
 	}
 
 	// Polys
@@ -684,10 +727,10 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
 		// poly->firstLink is update when tile is added, no need to swap.
 		for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j)
 		{
-			swapEndian(&p->verts[j]);
-			swapEndian(&p->neis[j]);
+			dtSwapEndian(&p->verts[j]);
+			dtSwapEndian(&p->neis[j]);
 		}
-		swapEndian(&p->flags);
+		dtSwapEndian(&p->flags);
 	}
 
 	// Links are rebuild when tile is added, no need to swap.
@@ -696,14 +739,14 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
 	for (int i = 0; i < header->detailMeshCount; ++i)
 	{
 		dtPolyDetail* pd = &detailMeshes[i];
-		swapEndian(&pd->vertBase);
-		swapEndian(&pd->triBase);
+		dtSwapEndian(&pd->vertBase);
+		dtSwapEndian(&pd->triBase);
 	}
 	
 	// Detail verts
 	for (int i = 0; i < header->detailVertCount*3; ++i)
 	{
-		swapEndian(&detailVerts[i]);
+		dtSwapEndian(&detailVerts[i]);
 	}
 
 	// BV-tree
@@ -712,10 +755,10 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
 		dtBVNode* node = &bvTree[i];
 		for (int j = 0; j < 3; ++j)
 		{
-			swapEndian(&node->bmin[j]);
-			swapEndian(&node->bmax[j]);
+			dtSwapEndian(&node->bmin[j]);
+			dtSwapEndian(&node->bmax[j]);
 		}
-		swapEndian(&node->i);
+		dtSwapEndian(&node->i);
 	}
 
 	// Off-mesh Connections.
@@ -723,9 +766,9 @@ bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
 	{
 		dtOffMeshConnection* con = &offMeshCons[i];
 		for (int j = 0; j < 6; ++j)
-			swapEndian(&con->pos[j]);
-		swapEndian(&con->rad);
-		swapEndian(&con->poly);
+			dtSwapEndian(&con->pos[j]);
+		dtSwapEndian(&con->rad);
+		dtSwapEndian(&con->poly);
 	}
 	
 	return true;
diff --git a/dep/recastnavigation/Detour/DetourNavMeshBuilder.h b/dep/recastnavigation/Detour/DetourNavMeshBuilder.h
index c4a5ac07045..c80d1717630 100644
--- a/dep/recastnavigation/Detour/DetourNavMeshBuilder.h
+++ b/dep/recastnavigation/Detour/DetourNavMeshBuilder.h
@@ -83,6 +83,7 @@ struct dtNavMeshCreateParams
 	unsigned int userId;	///< The user defined id of the tile.
 	int tileX;				///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.)
 	int tileY;				///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.)
+	int tileLayer;			///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.)
 	float bmin[3];			///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu]
 	float bmax[3];			///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu]
 
@@ -95,7 +96,12 @@ struct dtNavMeshCreateParams
 	float walkableClimb;	///< The agent maximum traversable ledge. (Up/Down) [Unit: wu]
 	float cs;				///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu]
 	float ch;				///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu]
-	int tileSize;							// Tile size (width & height) (vx).
+
+	/// True if a bounding volume tree should be built for the tile.
+	/// @note The BVTree is not normally needed for layered navigation meshes.
+	bool buildBvTree;
+
+	/// @}
 };
 
 /// Builds navigation mesh tile data from the provided tile creation data.
diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
index ec0c30460a9..f1709dfd4cf 100644
--- a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
+++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
@@ -81,7 +81,7 @@ float dtQueryFilter::getCost(const float* pa, const float* pb,
 							 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->area];
+	return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
 }
 #else
 inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
@@ -217,6 +217,281 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
 	return DT_SUCCESS;
 }
 
+dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
+										 dtPolyRef* randomRef, float* randomPt) const
+{
+	dtAssert(m_nav);
+	
+	// Randomly pick one tile. Assume that all tiles cover roughly the same area.
+	const dtMeshTile* tile = 0;
+	float tsum = 0.0f;
+	for (int i = 0; i < m_nav->getMaxTiles(); i++)
+	{
+		const dtMeshTile* t = m_nav->getTile(i);
+		if (!t || !t->header) continue;
+		
+		// Choose random tile using reservoi sampling.
+		const float area = 1.0f; // Could be tile area too.
+		tsum += area;
+		const float u = frand();
+		if (u*tsum <= area)
+			tile = t;
+	}
+	if (!tile)
+		return DT_FAILURE;
+
+	// Randomly pick one polygon weighted by polygon area.
+	const dtPoly* poly = 0;
+	dtPolyRef polyRef = 0;
+	const dtPolyRef base = m_nav->getPolyRefBase(tile);
+
+	float areaSum = 0.0f;
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		const dtPoly* p = &tile->polys[i];
+		// Do not return off-mesh connection polygons.
+		if (p->getType() != DT_POLYTYPE_GROUND)
+			continue;
+		// Must pass filter
+		const dtPolyRef ref = base | (dtPolyRef)i;
+		if (!filter->passFilter(ref, tile, p))
+			continue;
+
+		// Calc area of the polygon.
+		float polyArea = 0.0f;
+		for (int j = 2; j < p->vertCount; ++j)
+		{
+			const float* va = &tile->verts[p->verts[0]*3];
+			const float* vb = &tile->verts[p->verts[j-1]*3];
+			const float* vc = &tile->verts[p->verts[j]*3];
+			polyArea += dtTriArea2D(va,vb,vc);
+		}
+
+		// Choose random polygon weighted by area, using reservoi sampling.
+		areaSum += polyArea;
+		const float u = frand();
+		if (u*areaSum <= polyArea)
+		{
+			poly = p;
+			polyRef = ref;
+		}
+	}
+	
+	if (!poly)
+		return DT_FAILURE;
+
+	// Randomly pick point on polygon.
+	const float* v = &tile->verts[poly->verts[0]*3];
+	float verts[3*DT_VERTS_PER_POLYGON];
+	float areas[DT_VERTS_PER_POLYGON];
+	dtVcopy(&verts[0*3],v);
+	for (int j = 1; j < poly->vertCount; ++j)
+	{
+		v = &tile->verts[poly->verts[j]*3];
+		dtVcopy(&verts[j*3],v);
+	}
+	
+	const float s = frand();
+	const float t = frand();
+	
+	float pt[3];
+	dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
+	
+	float h = 0.0f;
+	dtStatus status = getPolyHeight(polyRef, pt, &h);
+	if (dtStatusFailed(status))
+		return status;
+	pt[1] = h;
+	
+	dtVcopy(randomPt, pt);
+	*randomRef = polyRef;
+
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
+													 const dtQueryFilter* filter, float (*frand)(),
+													 dtPolyRef* randomRef, float* randomPt) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+	
+	// Validate input
+	if (!startRef || !m_nav->isValidPolyRef(startRef))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	const dtMeshTile* startTile = 0;
+	const dtPoly* startPoly = 0;
+	m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
+	if (!filter->passFilter(startRef, startTile, startPoly))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, centerPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	dtStatus status = DT_SUCCESS;
+	
+	const float radiusSqr = dtSqr(radius);
+	float areaSum = 0.0f;
+
+	const dtMeshTile* randomTile = 0;
+	const dtPoly* randomPoly = 0;
+	dtPolyRef randomPolyRef = 0;
+
+	while (!m_openList->empty())
+	{
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+
+		// Place random locations on on ground.
+		if (bestPoly->getType() == DT_POLYTYPE_GROUND)
+		{
+			// Calc area of the polygon.
+			float polyArea = 0.0f;
+			for (int j = 2; j < bestPoly->vertCount; ++j)
+			{
+				const float* va = &bestTile->verts[bestPoly->verts[0]*3];
+				const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3];
+				const float* vc = &bestTile->verts[bestPoly->verts[j]*3];
+				polyArea += dtTriArea2D(va,vb,vc);
+			}
+			// Choose random polygon weighted by area, using reservoi sampling.
+			areaSum += polyArea;
+			const float u = frand();
+			if (u*areaSum <= polyArea)
+			{
+				randomTile = bestTile;
+				randomPoly = bestPoly;
+				randomPolyRef = bestRef;
+			}
+		}
+		
+		
+		// Get parent poly and tile.
+		dtPolyRef parentRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		if (bestNode->pidx)
+			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		if (parentRef)
+			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			const dtLink* link = &bestTile->links[i];
+			dtPolyRef neighbourRef = link->ref;
+			// Skip invalid neighbours and do not follow back to parent.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Expand to neighbour
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+			
+			// Do not advance if the polygon is excluded by the filter.
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+			
+			// Find edge and calc distance to the edge.
+			float va[3], vb[3];
+			if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
+				continue;
+			
+			// If the circle is not touching the next polygon, skip it.
+			float tseg;
+			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
+			if (distSqr > radiusSqr)
+				continue;
+			
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
+			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
+				continue;
+			}
+			
+			if (neighbourNode->flags & DT_NODE_CLOSED)
+				continue;
+			
+			// Cost
+			if (neighbourNode->flags == 0)
+				dtVlerp(neighbourNode->pos, va, vb, 0.5f);
+			
+			const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			
+			neighbourNode->id = neighbourRef;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->total = total;
+			
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				neighbourNode->flags = DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+		}
+	}
+	
+	if (!randomPoly)
+		return DT_FAILURE;
+	
+	// Randomly pick point on polygon.
+	const float* v = &randomTile->verts[randomPoly->verts[0]*3];
+	float verts[3*DT_VERTS_PER_POLYGON];
+	float areas[DT_VERTS_PER_POLYGON];
+	dtVcopy(&verts[0*3],v);
+	for (int j = 1; j < randomPoly->vertCount; ++j)
+	{
+		v = &randomTile->verts[randomPoly->verts[j]*3];
+		dtVcopy(&verts[j*3],v);
+	}
+	
+	const float s = frand();
+	const float t = frand();
+	
+	float pt[3];
+	dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
+	
+	float h = 0.0f;
+	dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
+	if (dtStatusFailed(status))
+		return stat;
+	pt[1] = h;
+	
+	dtVcopy(randomPt, pt);
+	*randomRef = randomPolyRef;
+	
+	return DT_SUCCESS;
+}
+
+
 //////////////////////////////////////////////////////////////////////////////////////////
 
 /// @par
@@ -227,7 +502,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
 ///
 /// See closestPointOnPolyBoundary() for a limited but faster option.
 ///
-dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const
+dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
 {
 	dtAssert(m_nav);
 	const dtMeshTile* tile = 0;
@@ -236,97 +511,80 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
 		return DT_FAILURE | DT_INVALID_PARAM;
 	if (!tile)
 		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Off-mesh connections don't have detail polygons.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		const float* v0 = &tile->verts[poly->verts[0]*3];
+		const float* v1 = &tile->verts[poly->verts[1]*3];
+		const float d0 = dtVdist(pos, v0);
+		const float d1 = dtVdist(pos, v1);
+		const float u = d0 / (d0+d1);
+		dtVlerp(closest, v0, v1, u);
+		if (posOverPoly)
+			*posOverPoly = false;
+		return DT_SUCCESS;
+	}
 
-    // Edited by TC
-    if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) 
-        return DT_FAILURE;
+	const unsigned int ip = (unsigned int)(poly - tile->polys);
+	const dtPolyDetail* pd = &tile->detailMeshes[ip];
 
-	if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS)
-		return DT_FAILURE;
-	return DT_SUCCESS;
-}
+	// Clamp point to be inside the polygon.
+	float verts[DT_VERTS_PER_POLYGON*3];	
+	float edged[DT_VERTS_PER_POLYGON];
+	float edget[DT_VERTS_PER_POLYGON];
+	const int nv = poly->vertCount;
+	for (int i = 0; i < nv; ++i)
+		dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
+	
+	dtVcopy(closest, pos);
+	if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
+	{
+		// Point is outside the polygon, dtClamp to nearest edge.
+		float dmin = FLT_MAX;
+		int imin = -1;
+		for (int i = 0; i < nv; ++i)
+		{
+			if (edged[i] < dmin)
+			{
+				dmin = edged[i];
+				imin = i;
+			}
+		}
+		const float* va = &verts[imin*3];
+		const float* vb = &verts[((imin+1)%nv)*3];
+		dtVlerp(closest, va, vb, edget[imin]);
 
-dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
-												  const float* pos, float* closest) const
-{
-    const unsigned int ip = (unsigned int)(poly - tile->polys);
-    const dtPolyDetail* pd = &tile->detailMeshes[ip];
-
-    // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh.
-    // Test and enable.
-    /*
-    // Clamp point to be inside the polygon.
-    float verts[DT_VERTS_PER_POLYGON*3];
-    float edged[DT_VERTS_PER_POLYGON];
-    float edget[DT_VERTS_PER_POLYGON];
-    const int nv = poly->vertCount;
-    for (int i = 0; i < nv; ++i)
-    dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
-
-    dtVcopy(closest, pos);
-    if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
-    {
-    // Point is outside the polygon, dtClamp to nearest edge.
-    float dmin = FLT_MAX;
-    int imin = -1;
-    for (int i = 0; i < nv; ++i)
-    {
-    if (edged[i] < dmin)
-    {
-    dmin = edged[i];
-    imin = i;
-    }
-    }
-    const float* va = &verts[imin*3];
-    const float* vb = &verts[((imin+1)%nv)*3];
-    dtVlerp(closest, va, vb, edget[imin]);
-    }
-
-    // Find height at the location.
-    for (int j = 0; j < pd->triCount; ++j)
-    {
-    const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
-    const float* v[3];
-    for (int k = 0; k < 3; ++k)
-    {
-    if (t[k] < poly->vertCount)
-    v[k] = &tile->verts[poly->verts[t[k]]*3];
-    else
-    v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
-    }
-    float h;
-    if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
-    {
-    closest[1] = h;
-    break;
-    }
-    }
-    */
-    float closestDistSqr = FLT_MAX;
-    for (int j = 0; j < pd->triCount; ++j)
-    {
-        const unsigned char* t = &tile->detailTris[(pd->triBase + j) * 4];
-        const float* v[3];
-        for (int k = 0; k < 3; ++k)
-        {
-            if (t[k] < poly->vertCount)
-                v[k] = &tile->verts[poly->verts[t[k]] * 3];
-            else
-                v[k] = &tile->detailVerts[(pd->vertBase + (t[k] - poly->vertCount)) * 3];
-        }
-
-        float pt[3];
-        dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
-        float d = dtVdistSqr(pos, pt);
-
-        if (d < closestDistSqr)
-        {
-            dtVcopy(closest, pt);
-            closestDistSqr = d;
-        }
-    }
-
-    return DT_SUCCESS;
+		if (posOverPoly)
+			*posOverPoly = false;
+	}
+	else
+	{
+		if (posOverPoly)
+			*posOverPoly = true;
+	}
+
+	// Find height at the location.
+	for (int j = 0; j < pd->triCount; ++j)
+	{
+		const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
+		const float* v[3];
+		for (int k = 0; k < 3; ++k)
+		{
+			if (t[k] < poly->vertCount)
+				v[k] = &tile->verts[poly->verts[t[k]]*3];
+			else
+				v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
+		}
+		float h;
+		if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
+		{
+			closest[1] = h;
+			break;
+		}
+	}
+	
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -405,8 +663,8 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h
 	{
 		const float* v0 = &tile->verts[poly->verts[0]*3];
 		const float* v1 = &tile->verts[poly->verts[1]*3];
-		const float d0 = dtVdist(pos, v0);
-		const float d1 = dtVdist(pos, v1);
+		const float d0 = dtVdist2D(pos, v0);
+		const float d1 = dtVdist2D(pos, v1);
 		const float u = d0 / (d0+d1);
 		if (height)
 			*height = v0[1] + (v1[1] - v0[1]) * u;
@@ -470,9 +728,27 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
-		if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS)
-			continue;
-		float d = dtVdistSqr(center, closestPtPoly);
+		float diff[3];
+		bool posOverPoly = false;
+		float d = 0;
+		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
+
+		// If a point is directly over a polygon and closer than
+		// climb height, favor that instead of straight line nearest point.
+		dtVsub(diff, center, closestPtPoly);
+		if (posOverPoly)
+		{
+			const dtMeshTile* tile = 0;
+			const dtPoly* poly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly);
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
+		}
+		
 		if (d < nearestDistanceSqr)
 		{
 			if (nearestPt)
@@ -488,43 +764,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	return DT_SUCCESS;
 }
 
-dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
-												const dtQueryFilter* filter, float* nearestPt) const
-{
-	dtAssert(m_nav);
-	
-	float bmin[3], bmax[3];
-	dtVsub(bmin, center, extents);
-	dtVadd(bmax, center, extents);
-	
-	// Get nearby polygons from proximity grid.
-	dtPolyRef polys[128];
-	int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);
-	
-	// Find nearest polygon amongst the nearby polygons.
-	dtPolyRef nearest = 0;
-	float nearestDistanceSqr = FLT_MAX;
-	for (int i = 0; i < polyCount; ++i)
-	{
-		dtPolyRef ref = polys[i];
-		const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
-		float closestPtPoly[3];
-		if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS)
-			continue;
-			
-		float d = dtVdistSqr(center, closestPtPoly);
-		if (d < nearestDistanceSqr)
-		{
-			if (nearestPt)
-				dtVcopy(nearestPt, closestPtPoly);
-			nearestDistanceSqr = d;
-			nearest = ref;
-		}
-	}
-	
-	return nearest;
-}
-
 int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
 										const dtQueryFilter* filter,
 										dtPolyRef* polys, const int maxPolys) const
@@ -592,8 +831,15 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
 		const dtPolyRef base = m_nav->getPolyRefBase(tile);
 		for (int i = 0; i < tile->header->polyCount; ++i)
 		{
+			const dtPoly* p = &tile->polys[i];
+			// Do not return off-mesh connection polygons.
+			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			// Must pass filter
+			const dtPolyRef ref = base | (dtPolyRef)i;
+			if (!filter->passFilter(ref, tile, p))
+				continue;
 			// Calc polygon bounds.
-			dtPoly* p = &tile->polys[i];
 			const float* v = &tile->verts[p->verts[0]*3];
 			dtVcopy(bmin, v);
 			dtVcopy(bmax, v);
@@ -605,12 +851,8 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
 			}
 			if (dtOverlapBounds(qmin,qmax, bmin,bmax))
 			{
-				const dtPolyRef ref = base | (dtPolyRef)i;
-				if (filter->passFilter(ref, tile, p))
-				{
-					if (n < maxPolys)
-						polys[n++] = ref;
-				}
+				if (n < maxPolys)
+					polys[n++] = ref;
 			}
 		}
 		return n;
@@ -641,18 +883,23 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents
 	m_nav->calcTileLoc(bmin, &minx, &miny);
 	m_nav->calcTileLoc(bmax, &maxx, &maxy);
 
+	static const int MAX_NEIS = 32;
+	const dtMeshTile* neis[MAX_NEIS];
+	
 	int n = 0;
 	for (int y = miny; y <= maxy; ++y)
 	{
 		for (int x = minx; x <= maxx; ++x)
 		{
-			const dtMeshTile* tile = m_nav->getTileAt(x,y);
-			if (!tile) continue;
-			n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n);
-			if (n >= maxPolys)
+			const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS);
+			for (int j = 0; j < nneis; ++j)
 			{
-				*polyCount = n;
-				return DT_SUCCESS;
+				n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n);
+				if (n >= maxPolys)
+				{
+					*polyCount = n;
+					return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
+				}
 			}
 		}
 	}
@@ -715,6 +962,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	dtNode* lastBestNode = startNode;
 	float lastBestNodeCost = startNode->total;
 	
+	dtStatus status = DT_SUCCESS;
+	
 	while (!m_openList->empty())
 	{
 		// Remove node from open list and put it in closed list.
@@ -764,7 +1013,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
 				continue;
+			}
 			
 			// If the node is visited the first time, calculate node position.
 			if (neighbourNode->flags == 0)
@@ -817,7 +1069,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 			// Add or update the node.
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
 			neighbourNode->cost = cost;
 			neighbourNode->total = total;
 			
@@ -842,6 +1094,9 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 		}
 	}
 	
+	if (lastBestNode->id != endRef)
+		status |= DT_PARTIAL_RESULT;
+	
 	// Reverse the path.
 	dtNode* prev = 0;
 	dtNode* node = lastBestNode;
@@ -860,13 +1115,18 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	do
 	{
 		path[n++] = node->id;
+		if (n >= maxPath)
+		{
+			status |= DT_BUFFER_TOO_SMALL;
+			break;
+		}
 		node = m_nodePool->getNodeAtIdx(node->pidx);
 	}
-	while (node && n < maxPath);
+	while (node);
 	
 	*pathCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 /// @par
@@ -926,7 +1186,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
 	return m_query.status;
 }
 	
-dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
+dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 {
 	if (!dtStatusInProgress(m_query.status))
 		return m_query.status;
@@ -952,7 +1212,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 		if (bestNode->id == m_query.endRef)
 		{
 			m_query.lastBestNode = bestNode;
-			m_query.status = DT_SUCCESS;
+			const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+			m_query.status = DT_SUCCESS | details;
+			if (doneIters)
+				*doneIters = iter;
 			return m_query.status;
 		}
 		
@@ -965,6 +1228,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 		{
 			// The polygon has disappeared during the sliced query, fail.
 			m_query.status = DT_FAILURE;
+			if (doneIters)
+				*doneIters = iter;
 			return m_query.status;
 		}
 		
@@ -980,6 +1245,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 			{
 				// The polygon has disappeared during the sliced query, fail.
 				m_query.status = DT_FAILURE;
+				if (doneIters)
+					*doneIters = iter;
 				return m_query.status;
 			}
 		}
@@ -1003,7 +1270,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 			
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
+			{
+				m_query.status |= DT_OUT_OF_NODES;
 				continue;
+			}
 			
 			// If the node is visited the first time, calculate node position.
 			if (neighbourNode->flags == 0)
@@ -1056,7 +1326,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 			// Add or update the node.
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
 			neighbourNode->cost = cost;
 			neighbourNode->total = total;
 			
@@ -1083,7 +1353,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 	
 	// Exhausted all nodes, but could not find path.
 	if (m_openList->empty())
-		m_query.status = DT_SUCCESS;
+	{
+		const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+		m_query.status = DT_SUCCESS | details;
+	}
+
+	if (doneIters)
+		*doneIters = iter;
 
 	return m_query.status;
 }
@@ -1110,6 +1386,10 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
 	{
 		// Reverse the path.
 		dtAssert(m_query.lastBestNode);
+		
+		if (m_query.lastBestNode->id != m_query.endRef)
+			m_query.status |= DT_PARTIAL_RESULT;
+		
 		dtNode* prev = 0;
 		dtNode* node = m_query.lastBestNode;
 		do
@@ -1126,17 +1406,24 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
 		do
 		{
 			path[n++] = node->id;
+			if (n >= maxPath)
+			{
+				m_query.status |= DT_BUFFER_TOO_SMALL;
+				break;
+			}
 			node = m_nodePool->getNodeAtIdx(node->pidx);
 		}
-		while (node && n < maxPath);
+		while (node);
 	}
 	
+	const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+
 	// Reset query.
 	memset(&m_query, 0, sizeof(dtQueryData));
 	
 	*pathCount = n;
 	
-	return DT_SUCCESS;
+	return DT_SUCCESS | details;
 }
 
 dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
@@ -1149,7 +1436,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		return DT_FAILURE;
 	}
 	
-	if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS)
+	if (dtStatusFailed(m_query.status))
 	{
 		// Reset query.
 		memset(&m_query, 0, sizeof(dtQueryData));
@@ -1177,7 +1464,9 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		
 		if (!node)
 		{
-			return DT_FAILURE;
+			m_query.status |= DT_PARTIAL_RESULT;
+			dtAssert(m_query.lastBestNode);
+			node = m_query.lastBestNode;
 		}
 		
 		// Reverse the path.
@@ -1195,24 +1484,128 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		do
 		{
 			path[n++] = node->id;
+			if (n >= maxPath)
+			{
+				m_query.status |= DT_BUFFER_TOO_SMALL;
+				break;
+			}
 			node = m_nodePool->getNodeAtIdx(node->pidx);
 		}
-		while (node && n < maxPath);
+		while (node);
 	}
 	
+	const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+
 	// Reset query.
 	memset(&m_query, 0, sizeof(dtQueryData));
 	
 	*pathCount = n;
 	
-	return DT_SUCCESS;
+	return DT_SUCCESS | details;
 }
 
 
+dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
+									  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+									  int* straightPathCount, const int maxStraightPath) const
+{
+	if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
+	{
+		// The vertices are equal, update flags and poly.
+		if (straightPathFlags)
+			straightPathFlags[(*straightPathCount)-1] = flags;
+		if (straightPathRefs)
+			straightPathRefs[(*straightPathCount)-1] = ref;
+	}
+	else
+	{
+		// Append new vertex.
+		dtVcopy(&straightPath[(*straightPathCount)*3], pos);
+		if (straightPathFlags)
+			straightPathFlags[(*straightPathCount)] = flags;
+		if (straightPathRefs)
+			straightPathRefs[(*straightPathCount)] = ref;
+		(*straightPathCount)++;
+		// If reached end of path or there is no space to append more vertices, return.
+		if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath)
+		{
+			return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
+		}
+	}
+	return DT_IN_PROGRESS;
+}
+
+dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
+									  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+									  int* straightPathCount, const int maxStraightPath, const int options) const
+{
+	const float* startPos = &straightPath[(*straightPathCount-1)*3];
+	// Append or update last vertex
+	dtStatus stat = 0;
+	for (int i = startIdx; i < endIdx; i++)
+	{
+		// Calculate portal
+		const dtPolyRef from = path[i];
+		const dtMeshTile* fromTile = 0;
+		const dtPoly* fromPoly = 0;
+		if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
+			return DT_FAILURE | DT_INVALID_PARAM;
+		
+		const dtPolyRef to = path[i+1];
+		const dtMeshTile* toTile = 0;
+		const dtPoly* toPoly = 0;
+		if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
+			return DT_FAILURE | DT_INVALID_PARAM;
+		
+		float left[3], right[3];
+		if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
+			break;
+	
+		if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
+		{
+			// Skip intersection if only area crossings are requested.
+			if (fromPoly->getArea() == toPoly->getArea())
+				continue;
+		}
+		
+		// Append intersection
+		float s,t;
+		if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
+		{
+			float pt[3];
+			dtVlerp(pt, left,right, t);
+
+			stat = appendVertex(pt, 0, path[i+1],
+								straightPath, straightPathFlags, straightPathRefs,
+								straightPathCount, maxStraightPath);
+			if (stat != DT_IN_PROGRESS)
+				return stat;
+		}
+	}
+	return DT_IN_PROGRESS;
+}
+
+/// @par
+/// 
+/// This method peforms what is often called 'string pulling'.
+///
+/// The start position is clamped to the first polygon in the path, and the 
+/// end position is clamped to the last. So the start and end positions should 
+/// normally be within or very near the first and last polygons respectively.
+///
+/// The returned polygon references represent the reference id of the polygon 
+/// that is entered at the associated path position. The reference id associated 
+/// with the end point will always be zero.  This allows, for example, matching 
+/// off-mesh link points to their representative polygons.
+///
+/// If the provided result buffers are too small for the entire result set, 
+/// they will be filled as far as possible from the start toward the end 
+/// position.
+///
 dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
 										  const dtPolyRef* path, const int pathSize,
 										  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
-										  int* straightPathCount, const int maxStraightPath) const
+										  int* straightPathCount, const int maxStraightPath, const int options) const
 {
 	dtAssert(m_nav);
 	
@@ -1224,29 +1617,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 	if (!path[0])
 		return DT_FAILURE | DT_INVALID_PARAM;
 	
-	int n = 0;
+	dtStatus stat = 0;
 	
 	// TODO: Should this be callers responsibility?
 	float closestStartPos[3];
 	if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
 		return DT_FAILURE | DT_INVALID_PARAM;
+
+	float closestEndPos[3];
+	if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
+		return DT_FAILURE | DT_INVALID_PARAM;
 	
 	// Add start point.
-	dtVcopy(&straightPath[n*3], closestStartPos);
-	if (straightPathFlags)
-		straightPathFlags[n] = DT_STRAIGHTPATH_START;
-	if (straightPathRefs)
-		straightPathRefs[n] = path[0];
-	n++;
-	if (n >= maxStraightPath)
-	{
-		*straightPathCount = n;
-		return DT_SUCCESS;
-	}
-	
-	float closestEndPos[3];
-	if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS)
-		return DT_FAILURE;
+	stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
+						straightPath, straightPathFlags, straightPathRefs,
+						straightPathCount, maxStraightPath);
+	if (stat != DT_IN_PROGRESS)
+		return stat;
 	
 	if (pathSize > 1)
 	{
@@ -1274,17 +1661,28 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				// Next portal.
 				if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
 				{
-					if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS)
-						return DT_FAILURE;
+					// 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.
 					
-					dtVcopy(&straightPath[n*3], closestEndPos);
-					if (straightPathFlags)
-						straightPathFlags[n] = 0;
-					if (straightPathRefs)
-						straightPathRefs[n] = path[i];
-					n++;
+					if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
+					{
+						// This should only happen when the first polygon is invalid.
+						return DT_FAILURE | DT_INVALID_PARAM;
+					}
+
+					// Apeend portals along the current straight path segment.
+					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+					{
+						stat = appendPortals(apexIndex, i, closestEndPos, path,
+											 straightPath, straightPathFlags, straightPathRefs,
+											 straightPathCount, maxStraightPath, options);
+					}
+
+					stat = appendVertex(closestEndPos, 0, path[i],
+										straightPath, straightPathFlags, straightPathRefs,
+										straightPathCount, maxStraightPath);
 					
-					return DT_SUCCESS;
+					return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
 				}
 				
 				// If starting really close the portal, advance.
@@ -1316,6 +1714,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				}
 				else
 				{
+					// Append portals along the current straight path segment.
+					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+					{
+						stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
+											 straightPath, straightPathFlags, straightPathRefs,
+											 straightPathCount, maxStraightPath, options);
+						if (stat != DT_IN_PROGRESS)
+							return stat;					
+					}
+				
 					dtVcopy(portalApex, portalLeft);
 					apexIndex = leftIndex;
 					
@@ -1326,30 +1734,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 						flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
 					dtPolyRef ref = leftPolyRef;
 					
-					if (!dtVequal(&straightPath[(n-1)*3], portalApex))
-					{
-						// Append new vertex.
-						dtVcopy(&straightPath[n*3], portalApex);
-						if (straightPathFlags)
-							straightPathFlags[n] = flags;
-						if (straightPathRefs)
-							straightPathRefs[n] = ref;
-						n++;
-						// If reached end of path or there is no space to append more vertices, return.
-						if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
-						{
-							*straightPathCount = n;
-							return DT_SUCCESS;
-						}
-					}
-					else
-					{
-						// The vertices are equal, update flags and poly.
-						if (straightPathFlags)
-							straightPathFlags[n-1] = flags;
-						if (straightPathRefs)
-							straightPathRefs[n-1] = ref;
-					}
+					// Append or update vertex
+					stat = appendVertex(portalApex, flags, ref,
+										straightPath, straightPathFlags, straightPathRefs,
+										straightPathCount, maxStraightPath);
+					if (stat != DT_IN_PROGRESS)
+						return stat;
 					
 					dtVcopy(portalLeft, portalApex);
 					dtVcopy(portalRight, portalApex);
@@ -1375,6 +1765,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				}
 				else
 				{
+					// Append portals along the current straight path segment.
+					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+					{
+						stat = appendPortals(apexIndex, rightIndex, portalRight, path,
+											 straightPath, straightPathFlags, straightPathRefs,
+											 straightPathCount, maxStraightPath, options);
+						if (stat != DT_IN_PROGRESS)
+							return stat;
+					}
+
 					dtVcopy(portalApex, portalRight);
 					apexIndex = rightIndex;
 					
@@ -1384,31 +1784,13 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 					else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
 						flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
 					dtPolyRef ref = rightPolyRef;
-					
-					if (!dtVequal(&straightPath[(n-1)*3], portalApex))
-					{
-						// Append new vertex.
-						dtVcopy(&straightPath[n*3], portalApex);
-						if (straightPathFlags)
-							straightPathFlags[n] = flags;
-						if (straightPathRefs)
-							straightPathRefs[n] = ref;
-						n++;
-						// If reached end of path or there is no space to append more vertices, return.
-						if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
-						{
-							*straightPathCount = n;
-							return DT_SUCCESS;
-						}
-					}
-					else
-					{
-						// The vertices are equal, update flags and poly.
-						if (straightPathFlags)
-							straightPathFlags[n-1] = flags;
-						if (straightPathRefs)
-							straightPathRefs[n-1] = ref;
-					}
+
+					// Append or update vertex
+					stat = appendVertex(portalApex, flags, ref,
+										straightPath, straightPathFlags, straightPathRefs,
+										straightPathCount, maxStraightPath);
+					if (stat != DT_IN_PROGRESS)
+						return stat;
 					
 					dtVcopy(portalLeft, portalApex);
 					dtVcopy(portalRight, portalApex);
@@ -1422,25 +1804,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				}
 			}
 		}
+
+		// Append portals along the current straight path segment.
+		if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+		{
+			stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
+								 straightPath, straightPathFlags, straightPathRefs,
+								 straightPathCount, maxStraightPath, options);
+			if (stat != DT_IN_PROGRESS)
+				return stat;
+		}
 	}
+
+	stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
+						straightPath, straightPathFlags, straightPathRefs,
+						straightPathCount, maxStraightPath);
 	
-	// If the point already exists, remove it and add reappend the actual end location.  
-	if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos))
-		n--;
-	
-	// Add end point.
-	if (n < maxStraightPath)
-	{
-		dtVcopy(&straightPath[n*3], closestEndPos);
-		if (straightPathFlags)
-			straightPathFlags[n] = DT_STRAIGHTPATH_END;
-		if (straightPathRefs)
-			straightPathRefs[n] = 0;
-		n++;
-	}
-	
-	*straightPathCount = n;
-	return DT_SUCCESS;
+	return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
 }
 
 /// @par
@@ -1478,6 +1858,8 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
 	if (!m_nav->isValidPolyRef(startRef))
 		return DT_FAILURE | DT_INVALID_PARAM;
 	
+	dtStatus status = DT_SUCCESS;
+	
 	static const int MAX_STACK = 48;
 	dtNode* stack[MAX_STACK];
 	int nstack = 0;
@@ -1641,16 +2023,21 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
 		do
 		{
 			visited[n++] = node->id;
+			if (n >= maxVisitedSize)
+			{
+				status |= DT_BUFFER_TOO_SMALL;
+				break;
+			}
 			node = m_tinyNodePool->getNodeAtIdx(node->pidx);
 		}
-		while (node && n < maxVisitedSize);
+		while (node);
 	}
 	
 	dtVcopy(resultPos, bestPos);
 	
 	*visitedCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 
@@ -1753,7 +2140,7 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mi
 {
 	float left[3], right[3];
 	unsigned char fromType, toType;
-	if (!getPortalPoints(from, to, left,right, fromType, toType))
+	if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
 		return DT_FAILURE | DT_INVALID_PARAM;
 	mid[0] = (left[0]+right[0])*0.5f;
 	mid[1] = (left[1]+right[1])*0.5f;
@@ -1834,6 +2221,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 	hitNormal[1] = 0;
 	hitNormal[2] = 0;
 	
+	dtStatus status = DT_SUCCESS;
+	
 	while (curRef)
 	{
 		// Cast ray against current polygon.
@@ -1858,7 +2247,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			// Could not hit the polygon, keep the old t and report hit.
 			if (pathCount)
 				*pathCount = n;
-			return DT_SUCCESS;
+			return status;
 		}
 		// Keep track of furthest t so far.
 		if (tmax > *t)
@@ -1867,6 +2256,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 		// Store visited polygons.
 		if (n < maxPath)
 			path[n++] = curRef;
+		else
+			status |= DT_BUFFER_TOO_SMALL;
 		
 		// Ray end is completely inside the polygon.
 		if (segMax == -1)
@@ -1874,7 +2265,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			*t = FLT_MAX;
 			if (pathCount)
 				*pathCount = n;
-			return DT_SUCCESS;
+			return status;
 		}
 		
 		// Follow neighbours.
@@ -1976,7 +2367,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			
 			if (pathCount)
 				*pathCount = n;
-			return DT_SUCCESS;
+			return status;
 		}
 		
 		// No hit, advance to neighbour polygon.
@@ -1986,7 +2377,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 	if (pathCount)
 		*pathCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 /// @par
@@ -2045,6 +2436,8 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 	startNode->flags = DT_NODE_OPEN;
 	m_openList->push(startNode);
 	
+	dtStatus status = DT_SUCCESS;
+	
 	int n = 0;
 	if (n < maxResult)
 	{
@@ -2056,6 +2449,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 			resultCost[n] = 0;
 		++n;
 	}
+	else
+	{
+		status |= DT_BUFFER_TOO_SMALL;
+	}
 	
 	const float radiusSqr = dtSqr(radius);
 	
@@ -2111,7 +2508,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 			
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
 				continue;
+			}
 				
 			if (neighbourNode->flags & DT_NODE_CLOSED)
 				continue;
@@ -2127,7 +2527,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 				continue;
 			
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->total = total;
 			
@@ -2147,6 +2547,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 						resultCost[n] = neighbourNode->total;
 					++n;
 				}
+				else
+				{
+					status |= DT_BUFFER_TOO_SMALL;
+				}
 				neighbourNode->flags = DT_NODE_OPEN;
 				m_openList->push(neighbourNode);
 			}
@@ -2155,7 +2559,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 	
 	*resultCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 /// @par
@@ -2212,6 +2616,8 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 	startNode->flags = DT_NODE_OPEN;
 	m_openList->push(startNode);
 	
+	dtStatus status = DT_SUCCESS;
+
 	int n = 0;
 	if (n < maxResult)
 	{
@@ -2223,6 +2629,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 			resultCost[n] = 0;
 		++n;
 	}
+	else
+	{
+		status |= DT_BUFFER_TOO_SMALL;
+	}
 	
 	while (!m_openList->empty())
 	{
@@ -2278,7 +2688,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 			
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
 				continue;
+			}
 			
 			if (neighbourNode->flags & DT_NODE_CLOSED)
 				continue;
@@ -2294,7 +2707,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 				continue;
 			
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->total = total;
 			
@@ -2314,6 +2727,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 						resultCost[n] = neighbourNode->total;
 					++n;
 				}
+				else
+				{
+					status |= DT_BUFFER_TOO_SMALL;
+				}
 				neighbourNode->flags = DT_NODE_OPEN;
 				m_openList->push(neighbourNode);
 			}
@@ -2322,7 +2739,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 	
 	*resultCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 /// @par
@@ -2378,6 +2795,8 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 	float pa[DT_VERTS_PER_POLYGON*3];
 	float pb[DT_VERTS_PER_POLYGON*3];
 	
+	dtStatus status = DT_SUCCESS;
+	
 	int n = 0;
 	if (n < maxResult)
 	{
@@ -2386,6 +2805,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 			resultParent[n] = 0;
 		++n;
 	}
+	else
+	{
+		status |= DT_BUFFER_TOO_SMALL;
+	}
 	
 	while (nstack)
 	{
@@ -2499,6 +2922,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 					resultParent[n] = curRef;
 				++n;
 			}
+			else
+			{
+				status |= DT_BUFFER_TOO_SMALL;
+			}
 			
 			if (nstack < MAX_STACK)
 			{
@@ -2509,17 +2936,18 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 	
 	*resultCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 
 struct dtSegInterval
 {
+	dtPolyRef ref;
 	short tmin, tmax;
 };
 
 static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
-						   const short tmin, const short tmax)
+						   const short tmin, const short tmax, const dtPolyRef ref)
 {
 	if (nints+1 > maxInts) return;
 	// Find insertion point.
@@ -2534,6 +2962,7 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
 	if (nints-idx)
 		memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
 	// Store
+	ints[idx].ref = ref;
 	ints[idx].tmin = tmin;
 	ints[idx].tmax = tmax;
 	nints++;
@@ -2551,7 +2980,8 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
 /// maximum segments per polygon of the source navigation mesh.
 /// 
 dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
-											 float* segments, int* segmentCount, const int maxSegments) const
+											 float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
+											 const int maxSegments) const
 {
 	dtAssert(m_nav);
 	
@@ -2567,6 +2997,10 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
 	dtSegInterval ints[MAX_INTERVAL];
 	int nints;
 	
+	const bool storePortals = segmentRefs != 0;
+	
+	dtStatus status = DT_SUCCESS;
+	
 	for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
 	{
 		// Skip non-solid edges.
@@ -2586,54 +3020,95 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
 						m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
 						if (filter->passFilter(link->ref, neiTile, neiPoly))
 						{
-							insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax);
+							insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref);
 						}
 					}
 				}
 			}
 		}
-		else if (poly->neis[j])
+		else
 		{
 			// Internal edge
-			const unsigned int idx = (unsigned int)(poly->neis[j]-1);
-			const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx;
-			if (filter->passFilter(ref, tile, &tile->polys[idx]))
+			dtPolyRef neiRef = 0;
+			if (poly->neis[j])
+			{
+				const unsigned int idx = (unsigned int)(poly->neis[j]-1);
+				neiRef = m_nav->getPolyRefBase(tile) | idx;
+				if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
+					neiRef = 0;
+			}
+
+			// If the edge leads to another polygon and portals are not stored, skip.
+			if (neiRef != 0 && !storePortals)
 				continue;
+			
+			if (n < maxSegments)
+			{
+				const float* vj = &tile->verts[poly->verts[j]*3];
+				const float* vi = &tile->verts[poly->verts[i]*3];
+				float* seg = &segmentVerts[n*6];
+				dtVcopy(seg+0, vj);
+				dtVcopy(seg+3, vi);
+				if (segmentRefs)
+					segmentRefs[n] = neiRef;
+				n++;
+			}
+			else
+			{
+				status |= DT_BUFFER_TOO_SMALL;
+			}
+			
+			continue;
 		}
 		
 		// Add sentinels
-		insertInterval(ints, nints, MAX_INTERVAL, -1, 0);
-		insertInterval(ints, nints, MAX_INTERVAL, 255, 256);
+		insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
+		insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
 		
-		// Store segment.
+		// Store segments.
 		const float* vj = &tile->verts[poly->verts[j]*3];
 		const float* vi = &tile->verts[poly->verts[i]*3];
 		for (int k = 1; k < nints; ++k)
 		{
-			// Find the space inbetween the opening areas.
-			const int imin = ints[k-1].tmax;
-			const int imax = ints[k].tmin;
-			if (imin == imax) continue;
-			if (imin == 0 && imax == 255)
+			// Portal segment.
+			if (storePortals && ints[k].ref)
 			{
+				const float tmin = ints[k].tmin/255.0f; 
+				const float tmax = ints[k].tmax/255.0f; 
 				if (n < maxSegments)
 				{
-					float* seg = &segments[n*6];
+					float* seg = &segmentVerts[n*6];
+					dtVlerp(seg+0, vj,vi, tmin);
+					dtVlerp(seg+3, vj,vi, tmax);
+					if (segmentRefs)
+						segmentRefs[n] = ints[k].ref;
 					n++;
-					dtVcopy(seg+0, vj);
-					dtVcopy(seg+3, vi);
+				}
+				else
+				{
+					status |= DT_BUFFER_TOO_SMALL;
 				}
 			}
-			else
+
+			// Wall segment.
+			const int imin = ints[k-1].tmax;
+			const int imax = ints[k].tmin;
+			if (imin != imax)
 			{
 				const float tmin = imin/255.0f; 
 				const float tmax = imax/255.0f; 
 				if (n < maxSegments)
 				{
-					float* seg = &segments[n*6];
-					n++;
+					float* seg = &segmentVerts[n*6];
 					dtVlerp(seg+0, vj,vi, tmin);
 					dtVlerp(seg+3, vj,vi, tmax);
+					if (segmentRefs)
+						segmentRefs[n] = 0;
+					n++;
+				}
+				else
+				{
+					status |= DT_BUFFER_TOO_SMALL;
 				}
 			}
 		}
@@ -2641,7 +3116,7 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
 	
 	*segmentCount = n;
 	
-	return DT_SUCCESS;
+	return status;
 }
 
 /// @par
@@ -2680,6 +3155,8 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 	
 	float radiusSqr = dtSqr(maxRadius);
 	
+	dtStatus status = DT_SUCCESS;
+	
 	while (!m_openList->empty())
 	{
 		dtNode* bestNode = m_openList->pop();
@@ -2787,7 +3264,10 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
 				continue;
+			}
 			
 			if (neighbourNode->flags & DT_NODE_CLOSED)
 				continue;
@@ -2806,7 +3286,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 				continue;
 			
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->total = total;
 				
@@ -2828,7 +3308,21 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 	
 	*hitDist = sqrtf(radiusSqr);
 	
-	return DT_SUCCESS;
+	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;
 }
 
 /// @par
diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.h b/dep/recastnavigation/Detour/DetourNavMeshQuery.h
index 31b567e0b50..4a5112c9eb9 100644
--- a/dep/recastnavigation/Detour/DetourNavMeshQuery.h
+++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.h
@@ -130,30 +130,9 @@ public:
 	/// @returns The status flags for the query.
 	dtStatus init(const dtNavMesh* nav, const int maxNodes);
 	
-	// Finds the nearest navigation polygon around the center location.
-	// Params:
-	//	center[3] - (in) The center of the search box.
-	//	extents[3] - (in) The extents of the search box.
-	//  filter - (in) path polygon filter.
-	//  nearestRef - (out) Reference to the nearest polygon.
-	//  nearestPt[3] - (out, opt) The nearest point on found polygon, null if not needed.
-	// Returns: Reference identifier for the polygon, or 0 if no polygons found.
-	dtStatus findNearestPoly(const float* center, const float* extents,
-							 const dtQueryFilter* filter,
-							 dtPolyRef* nearestRef, float* nearestPt) const;
-	
-	// Returns polygons which overlap the query box.
-	// Params:
-	//	center[3] - (in) the center of the search box.
-	//	extents[3] - (in) the extents of the search box.
-	//  filter - (in) path polygon filter.
-	//	polys - (out) array holding the search result.
-	//  polyCount - (out) Number of polygons in search result array.
-	//	maxPolys - (in) The max number of polygons the polys array can hold.
-	dtStatus queryPolygons(const float* center, const float* extents,
-						   const dtQueryFilter* filter,
-						   dtPolyRef* polys, int* polyCount, const int maxPolys) const;
-	
+	/// @name Standard Pathfinding Functions
+	// /@{
+
 	/// Finds a path from the start polygon to the end polygon.
 	///  @param[in]		startRef	The refrence id of the start polygon.
 	///  @param[in]		endRef		The reference id of the end polygon.
@@ -169,6 +148,31 @@ public:
 					  const dtQueryFilter* filter,
 					  dtPolyRef* path, int* pathCount, const int maxPath) const;
 	
+	/// Finds the straight path from the start to the end position within the polygon corridor.
+	///  @param[in]		startPos			Path start position. [(x, y, z)]
+	///  @param[in]		endPos				Path end position. [(x, y, z)]
+	///  @param[in]		path				An array of polygon references that represent the path corridor.
+	///  @param[in]		pathSize			The number of polygons in the @p path array.
+	///  @param[out]	straightPath		Points describing the straight path. [(x, y, z) * @p straightPathCount].
+	///  @param[out]	straightPathFlags	Flags describing each point. (See: #dtStraightPathFlags) [opt]
+	///  @param[out]	straightPathRefs	The reference id of the polygon that is being entered at each point. [opt]
+	///  @param[out]	straightPathCount	The number of points in the straight path.
+	///  @param[in]		maxStraightPath		The maximum number of points the straight path arrays can hold.  [Limit: > 0]
+	///  @param[in]		options				Query options. (see: #dtStraightPathOptions)
+	/// @returns The status flags for the query.
+	dtStatus findStraightPath(const float* startPos, const float* endPos,
+							  const dtPolyRef* path, const int pathSize,
+							  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+							  int* straightPathCount, const int maxStraightPath, const int options = 0) const;
+
+	///@}
+	/// @name Sliced Pathfinding Functions
+	/// Common use case:
+	///	-# Call initSlicedFindPath() to initialize the sliced path query.
+	///	-# Call updateSlicedFindPath() until it returns complete.
+	///	-# Call finalizeSlicedFindPath() to get the path.
+	///@{ 
+
 	/// Intializes a sliced path query.
 	///  @param[in]		startRef	The refrence id of the start polygon.
 	///  @param[in]		endRef		The reference id of the end polygon.
@@ -181,10 +185,10 @@ public:
 								const dtQueryFilter* filter);
 
 	/// Updates an in-progress sliced path query.
-	// Params:
-	//  maxIter - (in) max number of iterations to update.
-	// Returns: Path query state.
-	dtStatus updateSlicedFindPath(const int maxIter);
+	///  @param[in]		maxIter		The maximum number of iterations to perform.
+	///  @param[out]	doneIters	The actual number of iterations completed. [opt]
+	/// @returns The status flags for the query.
+	dtStatus updateSlicedFindPath(const int maxIter, int* doneIters);
 
 	/// Finalizes and returns the results of a sliced path query.
 	///  @param[out]	path		An ordered list of polygon references representing the path. (Start to end.) 
@@ -196,8 +200,8 @@ public:
 	
 	/// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest
 	/// polygon on the existing path that was visited during the search.
-	///  @param[out]	existing		An array of polygon references for the existing path.
-	///  @param[out]	existingSize	The number of polygon in the @p existing array.
+	///  @param[in]		existing		An array of polygon references for the existing path.
+	///  @param[in]		existingSize	The number of polygon in the @p existing array.
 	///  @param[out]	path			An ordered list of polygon references representing the path. (Start to end.) 
 	///  								[(polyRef) * @p pathCount]
 	///  @param[out]	pathCount		The number of polygons returned in the @p path array.
@@ -205,74 +209,11 @@ public:
 	/// @returns The status flags for the query.
 	dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
 										   dtPolyRef* path, int* pathCount, const int maxPath);
-	
-	// Finds a straight path from start to end locations within the corridor
-	// described by the path polygons.
-	// Start and end locations will be clamped on the corridor.
-	// The returned polygon references are point to polygon which was entered when
-	// a path point was added. For the end point, zero will be returned. This allows
-	// to match for example off-mesh link points to their representative polygons.
-	// Params:
-	//	startPos[3] - (in) Path start location.
-	//	endPo[3] - (in) Path end location.
-	//	path - (in) Array of connected polygons describing the corridor.
-	//	pathSize - (in) Number of polygons in path array.
-	//	straightPath - (out) Points describing the straight path.
-	//  straightPathFlags - (out, opt) Flags describing each point type, see dtStraightPathFlags.
-	//  straightPathRefs - (out, opt) References to polygons at point locations.
-	//  straightPathCount - (out) Number of points in the path.
-	//	maxStraightPath - (in) The max number of points the straight path array can hold. Must be at least 1.
-	dtStatus findStraightPath(const float* startPos, const float* endPos,
-							  const dtPolyRef* path, const int pathSize,
-							  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
-							  int* straightPathCount, const int maxStraightPath) const;
-	
-	// Moves from startPos to endPos constrained to the navmesh.
-	// If the endPos is reachable, the resultPos will be endPos,
-	// or else the resultPos will be the nearest point in navmesh.
-	// Note: The resulting point is not projected to the ground, use getPolyHeight() to get height.
-	// Note: The algorithm is optimized for small delta movement and small number of polygons. 
-	// Params:
-	//  startRef - (in) ref to the polygon where startPos lies.
-	//  startPos[3] - (in) start position of the mover.
-	//  endPos[3] - (in) desired end position of the mover.
-	//  filter - (in) path polygon filter.
-	//  resultPos[3] - (out) new position of the mover.
-	//  visited - (out) array of visited polygons.
-	//  visitedCount - (out) Number of entries in the visited array.
-	//  maxVisitedSize - (in) max number of polygons in the visited array.
-	dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
-							  const dtQueryFilter* filter,
-							  float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const;
-	
-	// Casts 'walkability' ray along the navmesh surface from startPos towards the endPos.
-	// Params:
-	//	startRef - (in) ref to the polygon where the start lies.
-	//	startPos[3] - (in) start position of the query.
-	//	endPos[3] - (in) end position of the query.
-	//	t - (out) hit parameter along the segment, FLT_MAX if no hit.
-	//	hitNormal[3] - (out) normal of the nearest hit.
-	//  filter - (in) path polygon filter.
-	//  path - (out,opt) visited path polygons.
-	//  pathCount - (out,opt) Number of polygons visited.
-	//  maxPath - (in) max number of polygons in the path array.
-	dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
-					 const dtQueryFilter* filter,
-					 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const;
-	
-	// Returns distance to nearest wall from the specified location.
-	// Params:
-	//	startRef - (in) ref to the polygon where the center lies.
-	//	centerPos[3] - (in) center if the query circle.
-	//	maxRadius - (in) max search radius.
-	//  filter - (in) path polygon filter.
-	//  hitDist - (out) distance to nearest wall from the test location.
-	//	hitPos[3] - (out) location of the nearest hit.
-	//	hitNormal[3] - (out) normal of the nearest hit.
-	dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
-								const dtQueryFilter* filter,
-								float* hitDist, float* hitPos, float* hitNormal) const;
-	
+
+	///@}
+	/// @name Dijkstra Search Functions
+	/// @{ 
+
 	/// Finds the polygons along the navigation graph that touch the specified circle.
 	///  @param[in]		startRef		The reference id of the polygon where the search starts.
 	///  @param[in]		centerPos		The center of the search circle. [(x, y, z)]
@@ -308,6 +249,33 @@ public:
 								  dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
 								  int* resultCount, const int maxResult) const;
 	
+	/// @}
+	/// @name Local Query Functions
+	///@{
+
+	/// Finds the polygon nearest to the specified center point.
+	///  @param[in]		center		The center of the search box. [(x, y, z)]
+	///  @param[in]		extents		The search distance along each axis. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	nearestRef	The reference id of the nearest polygon.
+	///  @param[out]	nearestPt	The nearest point on the polygon. [opt] [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus findNearestPoly(const float* center, const float* extents,
+							 const dtQueryFilter* filter,
+							 dtPolyRef* nearestRef, float* nearestPt) const;
+	
+	/// Finds polygons that overlap the search box.
+	///  @param[in]		center		The center of the search box. [(x, y, z)]
+	///  @param[in]		extents		The search distance along each axis. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	polys		The reference ids of the polygons that overlap the query box.
+	///  @param[out]	polyCount	The number of polygons in the search result.
+	///  @param[in]		maxPolys	The maximum number of polygons the search result can hold.
+	/// @returns The status flags for the query.
+	dtStatus queryPolygons(const float* center, const float* extents,
+						   const dtQueryFilter* filter,
+						   dtPolyRef* polys, int* polyCount, const int maxPolys) const;
+
 	/// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position.
 	///  @param[in]		startRef		The reference id of the polygon where the search starts.
 	///  @param[in]		centerPos		The center of the query circle. [(x, y, z)]
@@ -323,23 +291,96 @@ public:
 									const dtQueryFilter* filter,
 									dtPolyRef* resultRef, dtPolyRef* resultParent,
 									int* resultCount, const int maxResult) const;
+
+	/// Moves from the start to the end position constrained to the navigation mesh.
+	///  @param[in]		startRef		The reference id of the start polygon.
+	///  @param[in]		startPos		A position of the mover within the start polygon. [(x, y, x)]
+	///  @param[in]		endPos			The desired end position of the mover. [(x, y, z)]
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	resultPos		The result position of the mover. [(x, y, z)]
+	///  @param[out]	visited			The reference ids of the polygons visited during the move.
+	///  @param[out]	visitedCount	The number of polygons visited during the move.
+	///  @param[in]		maxVisitedSize	The maximum number of polygons the @p visited array can hold.
+	/// @returns The status flags for the query.
+	dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
+							  const dtQueryFilter* filter,
+							  float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const;
+	
+	/// Casts a 'walkability' ray along the surface of the navigation mesh from 
+	/// the start position toward the end position.
+	///  @param[in]		startRef	The reference id of the start polygon.
+	///  @param[in]		startPos	A position within the start polygon representing 
+	///  							the start of the ray. [(x, y, z)]
+	///  @param[in]		endPos		The position to cast the ray toward. [(x, y, z)]
+	///  @param[out]	t			The hit parameter. (FLT_MAX if no wall hit.)
+	///  @param[out]	hitNormal	The normal of the nearest wall hit. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	path		The reference ids of the visited polygons. [opt]
+	///  @param[out]	pathCount	The number of visited polygons. [opt]
+	///  @param[in]		maxPath		The maximum number of polygons the @p path array can hold.
+	/// @returns The status flags for the query.
+	dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+					 const dtQueryFilter* filter,
+					 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const;
+	
+	/// Finds the distance from the specified position to the nearest polygon wall.
+	///  @param[in]		startRef		The reference id of the polygon containing @p centerPos.
+	///  @param[in]		centerPos		The center of the search circle. [(x, y, z)]
+	///  @param[in]		maxRadius		The radius of the search circle.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	hitDist			The distance to the nearest wall from @p centerPos.
+	///  @param[out]	hitPos			The nearest position on the wall that was hit. [(x, y, z)]
+	///  @param[out]	hitNormal		The normalized ray formed from the wall point to the 
+	///  								source point. [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
+								const dtQueryFilter* filter,
+								float* hitDist, float* hitPos, float* hitNormal) const;
 	
 	/// Returns the segments for the specified polygon, optionally including portals.
-	// Params:
-	//  ref - (in) ref to the polygon.
-	//  filter - (in) path polygon filter.
-	//  segments[6*maxSegments] - (out) wall segments (2 endpoints per segment).
-	//  segmentCount - (out) number of wall segments.
-	//  maxSegments - (in) max number of segments that can be stored in 'segments'.
+	///  @param[in]		ref				The reference id of the polygon.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	segmentVerts	The segments. [(ax, ay, az, bx, by, bz) * segmentCount]
+	///  @param[out]	segmentRefs		The reference ids of each segment's neighbor polygon. 
+	///  								Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] 
+	///  @param[out]	segmentCount	The number of segments returned.
+	///  @param[in]		maxSegments		The maximum number of segments the result arrays can hold.
+	/// @returns The status flags for the query.
 	dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
-								 float* segments, int* segmentCount, const int maxSegments) const;
+								 float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
+								 const int maxSegments) const;
+
+	/// Returns random location on navmesh.
+	/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[in]		frand			Function returning a random number [0..1).
+	///  @param[out]	randomRef		The reference id of the random location.
+	///  @param[out]	randomPt		The random location. 
+	/// @returns The status flags for the query.
+	dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
+							 dtPolyRef* randomRef, float* randomPt) const;
+
+	/// Returns random location on navmesh within the reach of specified location.
+	/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
+	/// The location is not exactly constrained by the circle, but it limits the visited polygons.
+	///  @param[in]		startRef		The reference id of the polygon where the search starts.
+	///  @param[in]		centerPos		The center of the search circle. [(x, y, z)]
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[in]		frand			Function returning a random number [0..1).
+	///  @param[out]	randomRef		The reference id of the random location.
+	///  @param[out]	randomPt		The random location. [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius,
+										 const dtQueryFilter* filter, float (*frand)(),
+										 dtPolyRef* randomRef, float* randomPt) const;
 	
 	/// Finds the closest point on the specified polygon.
 	///  @param[in]		ref			The reference id of the polygon.
 	///  @param[in]		pos			The position to check. [(x, y, z)]
 	///  @param[out]	closest		The closest point on the polygon. [(x, y, z)]
+	///  @param[out]	posOverPoly	True of the position is over the polygon.
 	/// @returns The status flags for the query.
-	dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const;
+	dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
 	
 	/// Returns a point on the boundary closest to the source point if the source point is outside the 
 	/// polygon's xz-bounds.
@@ -349,15 +390,6 @@ public:
 	/// @returns The status flags for the query.
 	dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const;
 	
-	// Returns start and end location of an off-mesh link polygon.
-	// Params:
-	//	prevRef - (in) ref to the polygon before the link (used to select direction).
-	//	polyRef - (in) ref to the off-mesh link polygon.
-	//	startPos[3] - (out) start point of the link.
-	//	endPos[3] - (out) end point of the link.
-	// Returns: true if link is found.
-	dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const;
-	
 	/// Gets the height of the polygon at the provided position using the height detail. (Most accurate.)
 	///  @param[in]		ref			The reference id of the polygon.
 	///  @param[in]		pos			A position within the xz-bounds of the polygon. [(x, y, z)]
@@ -365,6 +397,15 @@ public:
 	/// @returns The status flags for the query.
 	dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const;
 
+	/// @}
+	/// @name Miscellaneous Functions
+	/// @{
+
+	/// Returns true if the polygon reference is valid and passes the filter restrictions.
+	///  @param[in]		ref			The polygon reference to check.
+	///  @param[in]		filter		The filter to apply.
+	bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const;
+
 	/// Returns true if the polygon reference is in the closed list. 
 	///  @param[in]		ref		The reference id of the polygon to check.
 	/// @returns True if the polygon is in closed list.
@@ -374,6 +415,12 @@ public:
 	/// @returns The node pool.
 	class dtNodePool* getNodePool() const { return m_nodePool; }
 	
+	/// Gets the navigation mesh the query object is using.
+	/// @return The navigation mesh the query object is using.
+	const dtNavMesh* getAttachedNavMesh() const { return m_nav; }
+
+	/// @}
+	
 private:
 	
 	/// Returns neighbour tile based on side.
@@ -382,12 +429,7 @@ private:
 	/// Queries polygons within a tile.
 	int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter,
 							dtPolyRef* polys, const int maxPolys) const;
-	/// Find nearest polygon within a tile.
-	dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
-									const dtQueryFilter* filter, float* nearestPt) const;
-	/// Returns closest point on polygon.
-	dtStatus closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const;
-	
+
 	/// Returns portal points between two polygons.
 	dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
 							 unsigned char& fromType, unsigned char& toType) const;
@@ -401,6 +443,16 @@ private:
 							 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
 							 float* mid) const;
 	
+	// Appends vertex to a straight path
+	dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
+						  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+						  int* straightPathCount, const int maxStraightPath) const;
+
+	// Appends intermediate portal points to a straight path.
+	dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
+						   float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+						   int* straightPathCount, const int maxStraightPath, const int options) const;
+	
 	const dtNavMesh* m_nav;				///< Pointer to navmesh data.
 
 	struct dtQueryData
diff --git a/dep/recastnavigation/Detour/DetourNode.cpp b/dep/recastnavigation/Detour/DetourNode.cpp
index 03c6e370aac..4c8215e20d0 100644
--- a/dep/recastnavigation/Detour/DetourNode.cpp
+++ b/dep/recastnavigation/Detour/DetourNode.cpp
@@ -47,15 +47,15 @@ dtNodePool::dtNodePool(int maxNodes, int hashSize) :
 	dtAssert(m_maxNodes > 0);
 
 	m_nodes = (dtNode*)dtAlloc(sizeof(dtNode)*m_maxNodes, DT_ALLOC_PERM);
-	m_next = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_maxNodes, DT_ALLOC_PERM);
-	m_first = (unsigned short*)dtAlloc(sizeof(unsigned short)*hashSize, DT_ALLOC_PERM);
+	m_next = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*m_maxNodes, DT_ALLOC_PERM);
+	m_first = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*hashSize, DT_ALLOC_PERM);
 
 	dtAssert(m_nodes);
 	dtAssert(m_next);
 	dtAssert(m_first);
 
-	memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize);
-	memset(m_next, 0xff, sizeof(unsigned short)*m_maxNodes);
+	memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize);
+	memset(m_next, 0xff, sizeof(dtNodeIndex)*m_maxNodes);
 }
 
 dtNodePool::~dtNodePool()
@@ -67,14 +67,14 @@ dtNodePool::~dtNodePool()
 
 void dtNodePool::clear()
 {
-	memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize);
+	memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize);
 	m_nodeCount = 0;
 }
 
 dtNode* dtNodePool::findNode(dtPolyRef id)
 {
 	unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
-	unsigned short i = m_first[bucket];
+	dtNodeIndex i = m_first[bucket];
 	while (i != DT_NULL_IDX)
 	{
 		if (m_nodes[i].id == id)
@@ -87,7 +87,7 @@ dtNode* dtNodePool::findNode(dtPolyRef id)
 dtNode* dtNodePool::getNode(dtPolyRef id)
 {
 	unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
-	unsigned short i = m_first[bucket];
+	dtNodeIndex i = m_first[bucket];
 	dtNode* node = 0;
 	while (i != DT_NULL_IDX)
 	{
@@ -99,7 +99,7 @@ dtNode* dtNodePool::getNode(dtPolyRef id)
 	if (m_nodeCount >= m_maxNodes)
 		return 0;
 	
-	i = (unsigned short)m_nodeCount;
+	i = (dtNodeIndex)m_nodeCount;
 	m_nodeCount++;
 	
 	// Init node
diff --git a/dep/recastnavigation/Detour/DetourNode.h b/dep/recastnavigation/Detour/DetourNode.h
index aec26d2fc57..b68c922d038 100644
--- a/dep/recastnavigation/Detour/DetourNode.h
+++ b/dep/recastnavigation/Detour/DetourNode.h
@@ -27,7 +27,8 @@ enum dtNodeFlags
 	DT_NODE_CLOSED = 0x02,
 };
 
-static const unsigned short DT_NULL_IDX = 0xffff;
+typedef unsigned short dtNodeIndex;
+static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0;
 
 struct dtNode
 {
@@ -72,21 +73,21 @@ public:
 	{
 		return sizeof(*this) +
 			sizeof(dtNode)*m_maxNodes +
-			sizeof(unsigned short)*m_maxNodes +
-			sizeof(unsigned short)*m_hashSize;
+			sizeof(dtNodeIndex)*m_maxNodes +
+			sizeof(dtNodeIndex)*m_hashSize;
 	}
 	
 	inline int getMaxNodes() const { return m_maxNodes; }
 	
 	inline int getHashSize() const { return m_hashSize; }
-	inline unsigned short getFirst(int bucket) const { return m_first[bucket]; }
-	inline unsigned short getNext(int i) const { return m_next[i]; }
+	inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; }
+	inline dtNodeIndex getNext(int i) const { return m_next[i]; }
 	
 private:
 	
 	dtNode* m_nodes;
-	unsigned short* m_first;
-	unsigned short* m_next;
+	dtNodeIndex* m_first;
+	dtNodeIndex* m_next;
 	const int m_maxNodes;
 	const int m_hashSize;
 	int m_nodeCount;
diff --git a/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp b/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp
index a255c9b3fd1..d3f90b7ab17 100644
--- a/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp
+++ b/dep/recastnavigation/Detour/DetourObstacleAvoidance.cpp
@@ -25,6 +25,7 @@
 #include <float.h>
 #include <new>
 
+static const float DT_PI = 3.14159265f;
 
 static int sweepCircleCircle(const float* c0, const float r0, const float* v,
 							 const float* c1, const float r1,
@@ -206,12 +207,6 @@ void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr)
 
 
 dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() :
-	m_velBias(0.0f),
-	m_weightDesVel(0.0f),
-	m_weightCurVel(0.0f),
-	m_weightSide(0.0f),
-	m_weightToi(0.0f),
-	m_horizTime(0.0f),
 	m_maxCircles(0),
 	m_circles(0),
 	m_ncircles(0),
@@ -318,11 +313,11 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
 
 float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
 											  const float* pos, const float rad,
-											  const float vmax, const float* vel, const float* dvel,
+											  const float* vel, const float* dvel,
 											  dtObstacleAvoidanceDebugData* debug)
 {
 	// Find min time of impact and exit amongst all obstacles.
-	float tmin = m_horizTime;
+	float tmin = m_params.horizTime;
 	float side = 0;
 	int nside = 0;
 	
@@ -395,11 +390,10 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
 	if (nside)
 		side /= nside;
 	
-	const float ivmax = 1.0f / vmax;
-	const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax);
-	const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax);
-	const float spen = m_weightSide * side;
-	const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime));
+	const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
+	const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
+	const float spen = m_params.weightSide * side;
+	const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
 	
 	const float penalty = vpen + vcpen + spen + tpen;
 	
@@ -410,28 +404,34 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
 	return penalty;
 }
 
-void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax,
-												  const float* vel, const float* dvel,
-												  float* nvel, const int gsize,
-												  dtObstacleAvoidanceDebugData* debug)
+int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax,
+												 const float* vel, const float* dvel, float* nvel,
+												 const dtObstacleAvoidanceParams* params,
+												 dtObstacleAvoidanceDebugData* debug)
 {
 	prepare(pos, dvel);
 	
+	memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
+	m_invHorizTime = 1.0f / m_params.horizTime;
+	m_vmax = vmax;
+	m_invVmax = 1.0f / vmax;
+	
 	dtVset(nvel, 0,0,0);
 	
 	if (debug)
 		debug->reset();
 
-	const float cvx = dvel[0] * m_velBias;
-	const float cvz = dvel[2] * m_velBias;
-	const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1);
-	const float half = (gsize-1)*cs*0.5f;
+	const float cvx = dvel[0] * m_params.velBias;
+	const float cvz = dvel[2] * m_params.velBias;
+	const float cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1);
+	const float half = (m_params.gridSize-1)*cs*0.5f;
 		
 	float minPenalty = FLT_MAX;
+	int ns = 0;
 		
-	for (int y = 0; y < gsize; ++y)
+	for (int y = 0; y < m_params.gridSize; ++y)
 	{
-		for (int x = 0; x < gsize; ++x)
+		for (int x = 0; x < m_params.gridSize; ++x)
 		{
 			float vcand[3];
 			vcand[0] = cvx + x*cs - half;
@@ -440,7 +440,8 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float
 			
 			if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
 			
-			const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug);
+			const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug);
+			ns++;
 			if (penalty < minPenalty)
 			{
 				minPenalty = penalty;
@@ -448,31 +449,38 @@ void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float
 			}
 		}
 	}
+	
+	return ns;
 }
 
 
-static const float DT_PI = 3.14159265f;
-
-void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
-													  const float* vel, const float* dvel, float* nvel,
-													  const int ndivs, const int nrings, const int depth,
-													  dtObstacleAvoidanceDebugData* debug)
+int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
+													 const float* vel, const float* dvel, float* nvel,
+													 const dtObstacleAvoidanceParams* params,
+													 dtObstacleAvoidanceDebugData* debug)
 {
 	prepare(pos, dvel);
 	
+	memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
+	m_invHorizTime = 1.0f / m_params.horizTime;
+	m_vmax = vmax;
+	m_invVmax = 1.0f / vmax;
+	
 	dtVset(nvel, 0,0,0);
 	
 	if (debug)
 		debug->reset();
-	
+
 	// Build sampling pattern aligned to desired velocity.
-	static const int MAX_PATTERN_DIVS = 32;
-	static const int MAX_PATTERN_RINGS = 4;
-	float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2];
+	float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
 	int npat = 0;
 
-	const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS);
-	const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS);
+	const int ndivs = (int)m_params.adaptiveDivs;
+	const int nrings= (int)m_params.adaptiveRings;
+	const int depth = (int)m_params.adaptiveDepth;
+	
+	const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
+	const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
 	const float da = (1.0f/nd) * DT_PI*2;
 	const float dang = atan2f(dvel[2], dvel[0]);
 	
@@ -483,21 +491,22 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl
 	
 	for (int j = 0; j < nr; ++j)
 	{
-		const float rad = (float)(nr-j)/(float)nr;
+		const float r = (float)(nr-j)/(float)nr;
 		float a = dang + (j&1)*0.5f*da;
 		for (int i = 0; i < nd; ++i)
 		{
-			pat[npat*2+0] = cosf(a)*rad;
-			pat[npat*2+1] = sinf(a)*rad;
+			pat[npat*2+0] = cosf(a)*r;
+			pat[npat*2+1] = sinf(a)*r;
 			npat++;
 			a += da;
 		}
 	}
 
 	// Start sampling.
-	float cr = vmax * (1.0f-m_velBias);
+	float cr = vmax * (1.0f - m_params.velBias);
 	float res[3];
-	dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias);
+	dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
+	int ns = 0;
 
 	for (int k = 0; k < depth; ++k)
 	{
@@ -514,7 +523,8 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl
 			
 			if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
 			
-			const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug);
+			const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
+			ns++;
 			if (penalty < minPenalty)
 			{
 				minPenalty = penalty;
@@ -528,5 +538,7 @@ void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const fl
 	}	
 	
 	dtVcopy(nvel, res);
+	
+	return ns;
 }
 
diff --git a/dep/recastnavigation/Detour/DetourObstacleAvoidance.h b/dep/recastnavigation/Detour/DetourObstacleAvoidance.h
index 4a7187a7998..8ff6211e867 100644
--- a/dep/recastnavigation/Detour/DetourObstacleAvoidance.h
+++ b/dep/recastnavigation/Detour/DetourObstacleAvoidance.h
@@ -21,21 +21,19 @@
 
 struct dtObstacleCircle
 {
-	float p[3];				// Position of the obstacle
-	float vel[3];			// Velocity of the obstacle
-	float dvel[3];			// Velocity of the obstacle
-	float rad;				// Radius of the obstacle
-	float dp[3], np[3];		// Use for side selection during sampling.
+	float p[3];				///< Position of the obstacle
+	float vel[3];			///< Velocity of the obstacle
+	float dvel[3];			///< Velocity of the obstacle
+	float rad;				///< Radius of the obstacle
+	float dp[3], np[3];		///< Use for side selection during sampling.
 };
 
 struct dtObstacleSegment
 {
-	float p[3], q[3];		// End points of the obstacle segment
+	float p[3], q[3];		///< End points of the obstacle segment
 	bool touch;
 };
 
-static const int RVO_SAMPLE_RAD = 15;
-static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100;
 
 class dtObstacleAvoidanceDebugData
 {
@@ -75,6 +73,23 @@ dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData();
 void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr);
 
 
+static const int DT_MAX_PATTERN_DIVS = 32;	///< Max numver of adaptive divs.
+static const int DT_MAX_PATTERN_RINGS = 4;	///< Max number of adaptive rings.
+
+struct dtObstacleAvoidanceParams
+{
+	float velBias;
+	float weightDesVel;
+	float weightCurVel;
+	float weightSide;
+	float weightToi;
+	float horizTime;
+	unsigned char gridSize;	///< grid
+	unsigned char adaptiveDivs;	///< adaptive
+	unsigned char adaptiveRings;	///< adaptive
+	unsigned char adaptiveDepth;	///< adaptive
+};
+
 class dtObstacleAvoidanceQuery
 {
 public:
@@ -90,22 +105,15 @@ public:
 				   
 	void addSegment(const float* p, const float* q);
 
-	inline void setVelocitySelectionBias(float v) { m_velBias = v; }
-	inline void setDesiredVelocityWeight(float w) { m_weightDesVel = w; }
-	inline void setCurrentVelocityWeight(float w) { m_weightCurVel = w; }
-	inline void setPreferredSideWeight(float w) { m_weightSide = w; }
-	inline void setCollisionTimeWeight(float w) { m_weightToi = w; }
-	inline void setTimeHorizon(float t) { m_horizTime = t; }
-
-	void sampleVelocityGrid(const float* pos, const float rad, const float vmax,
-							const float* vel, const float* dvel, float* nvel,
-							const int gsize,
-							dtObstacleAvoidanceDebugData* debug = 0);
-
-	void sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
-								const float* vel, const float* dvel, float* nvel,
-								const int ndivs, const int nrings, const int depth, 
-								dtObstacleAvoidanceDebugData* debug = 0);
+	int sampleVelocityGrid(const float* pos, const float rad, const float vmax,
+						   const float* vel, const float* dvel, float* nvel,
+						   const dtObstacleAvoidanceParams* params,
+						   dtObstacleAvoidanceDebugData* debug = 0);
+
+	int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
+							   const float* vel, const float* dvel, float* nvel,
+							   const dtObstacleAvoidanceParams* params, 
+							   dtObstacleAvoidanceDebugData* debug = 0);
 	
 	inline int getObstacleCircleCount() const { return m_ncircles; }
 	const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; }
@@ -119,19 +127,17 @@ private:
 
 	float processSample(const float* vcand, const float cs,
 						const float* pos, const float rad,
-						const float vmax, const float* vel, const float* dvel,
+						const float* vel, const float* dvel,
 						dtObstacleAvoidanceDebugData* debug);
 
 	dtObstacleCircle* insertCircle(const float dist);
 	dtObstacleSegment* insertSegment(const float dist);
 
-	float m_velBias;
-	float m_weightDesVel;
-	float m_weightCurVel;
-	float m_weightSide;
-	float m_weightToi;
-	float m_horizTime;
-	
+	dtObstacleAvoidanceParams m_params;
+	float m_invHorizTime;
+	float m_vmax;
+	float m_invVmax;
+
 	int m_maxCircles;
 	dtObstacleCircle* m_circles;
 	int m_ncircles;
@@ -145,4 +151,4 @@ dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery();
 void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr);
 
 
-#endif // DETOUROBSTACLEAVOIDANCE_H
\ No newline at end of file
+#endif // DETOUROBSTACLEAVOIDANCE_H
diff --git a/dep/recastnavigation/Recast/CMakeLists.txt b/dep/recastnavigation/Recast/CMakeLists.txt
index e5e30294238..10028f8535f 100644
--- a/dep/recastnavigation/Recast/CMakeLists.txt
+++ b/dep/recastnavigation/Recast/CMakeLists.txt
@@ -14,6 +14,7 @@ set(Recast_STAT_SRCS
     RecastArea.cpp 
     RecastContour.cpp 
     RecastFilter.cpp 
+	RecastLayers.cpp
     RecastMesh.cpp 
     RecastMeshDetail.cpp 
     RecastRasterization.cpp 
diff --git a/dep/recastnavigation/Recast/Recast.cpp b/dep/recastnavigation/Recast/Recast.cpp
index 922c9163c13..b9d86036c3f 100644
--- a/dep/recastnavigation/Recast/Recast.cpp
+++ b/dep/recastnavigation/Recast/Recast.cpp
@@ -109,6 +109,28 @@ void rcFreeCompactHeightfield(rcCompactHeightfield* chf)
 	rcFree(chf);
 }
 
+
+rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
+{
+	rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM);
+	memset(lset, 0, sizeof(rcHeightfieldLayerSet));
+	return lset;
+}
+
+void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset)
+{
+	if (!lset) return;
+	for (int i = 0; i < lset->nlayers; ++i)
+	{
+		rcFree(lset->layers[i].heights);
+		rcFree(lset->layers[i].areas);
+		rcFree(lset->layers[i].cons);
+	}
+	rcFree(lset->layers);
+	rcFree(lset);
+}
+
+
 rcContourSet* rcAllocContourSet()
 {
 	rcContourSet* cset = (rcContourSet*)rcAlloc(sizeof(rcContourSet), RC_ALLOC_PERM);
@@ -186,12 +208,11 @@ void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int*
 /// See the #rcConfig documentation for more information on the configuration parameters.
 /// 
 /// @see rcAllocHeightfield, rcHeightfield 
-bool rcCreateHeightfield(rcContext* /*ctx*/, rcHeightfield& hf, int width, int height,
+bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height,
 						 const float* bmin, const float* bmax,
 						 float cs, float ch)
 {
-	// TODO: VC complains about unref formal variable, figure out a way to handle this better.
-//	rcAssert(ctx);
+	rcIgnoreUnused(ctx);
 	
 	hf.width = width;
 	hf.height = height;
@@ -223,13 +244,12 @@ static void calcTriNormal(const float* v0, const float* v1, const float* v2, flo
 /// See the #rcConfig documentation for more information on the configuration parameters.
 /// 
 /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
-void rcMarkWalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle,
+void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
 							 const float* verts, int /*nv*/,
 							 const int* tris, int nt,
 							 unsigned char* areas)
 {
-	// TODO: VC complains about unref formal variable, figure out a way to handle this better.
-//	rcAssert(ctx);
+	rcIgnoreUnused(ctx);
 	
 	const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
 
@@ -253,13 +273,12 @@ void rcMarkWalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle,
 /// See the #rcConfig documentation for more information on the configuration parameters.
 /// 
 /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
-void rcClearUnwalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle,
+void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
 								const float* verts, int /*nv*/,
 								const int* tris, int nt,
 								unsigned char* areas)
 {
-	// TODO: VC complains about unref formal variable, figure out a way to handle this better.
-//	rcAssert(ctx);
+	rcIgnoreUnused(ctx);
 	
 	const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
 	
@@ -275,10 +294,9 @@ void rcClearUnwalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAng
 	}
 }
 
-int rcGetHeightFieldSpanCount(rcContext* /*ctx*/, rcHeightfield& hf)
+int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf)
 {
-	// TODO: VC complains about unref formal variable, figure out a way to handle this better.
-//	rcAssert(ctx);
+	rcIgnoreUnused(ctx);
 	
 	const int w = hf.width;
 	const int h = hf.height;
@@ -417,13 +435,13 @@ bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const i
 						if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
 						{
 							// Mark direction as walkable.
-							const int idx = k - (int)nc.index;
-							if (idx < 0 || idx > MAX_LAYERS)
+							const int lidx = k - (int)nc.index;
+							if (lidx < 0 || lidx > MAX_LAYERS)
 							{
-								tooHighNeighbour = rcMax(tooHighNeighbour, idx);
+								tooHighNeighbour = rcMax(tooHighNeighbour, lidx);
 								continue;
 							}
-							rcSetCon(s, dir, idx);
+							rcSetCon(s, dir, lidx);
 							break;
 						}
 					}
diff --git a/dep/recastnavigation/Recast/Recast.h b/dep/recastnavigation/Recast/Recast.h
index 57e98ea444a..3f4ae96d1c9 100644
--- a/dep/recastnavigation/Recast/Recast.h
+++ b/dep/recastnavigation/Recast/Recast.h
@@ -65,6 +65,8 @@ enum rcTimerLabel
 	RC_TIMER_ERODE_AREA,
 	/// The time to mark a box area. (See: #rcMarkBoxArea)
 	RC_TIMER_MARK_BOX_AREA,
+	/// The time to mark a cylinder area. (See: #rcMarkCylinderArea)
+	RC_TIMER_MARK_CYLINDER_AREA,
 	/// The time to mark a convex polygon area. (See: #rcMarkConvexPolyArea)
 	RC_TIMER_MARK_CONVEXPOLY_AREA,
 	/// The total time to build the distance field. (See: #rcBuildDistanceField)
@@ -83,6 +85,8 @@ enum rcTimerLabel
 	RC_TIMER_BUILD_REGIONS_FLOOD,
 	/// The time to filter out small regions. (See: #rcBuildRegions, #rcBuildRegionsMonotone)
 	RC_TIMER_BUILD_REGIONS_FILTER,
+	/// The time to build heightfield layers. (See: #rcBuildHeightfieldLayers)
+	RC_TIMER_BUILD_LAYERS, 
 	/// The time to build the polygon mesh detail. (See: #rcBuildPolyMeshDetail)
 	RC_TIMER_BUILD_POLYMESHDETAIL,
 	/// The time to merge polygon mesh details. (See: #rcMergePolyMeshDetails)
@@ -215,7 +219,7 @@ struct rcConfig
 	int maxEdgeLen;
 	
 	/// The maximum distance a simplfied contour's border edges should deviate 
-	/// the original raw contour. [Limit: >=0] [Units: wu]
+	/// the original raw contour. [Limit: >=0] [Units: vx]
 	float maxSimplificationError;
 	
 	/// The minimum number of cells allowed to form isolated island areas. [Limit: >=0] [Units: vx] 
@@ -280,9 +284,6 @@ struct rcHeightfield
 	rcSpan* freelist;	///< The next free span.
 };
 
-rcHeightfield* rcAllocHeightfield();
-void rcFreeHeightField(rcHeightfield* hf);
-
 /// Provides information on the content of a cell column in a compact heightfield. 
 struct rcCompactCell
 {
@@ -308,6 +309,7 @@ struct rcCompactHeightfield
 	int spanCount;				///< The number of spans in the heightfield.
 	int walkableHeight;			///< The walkable height used during the build of the field.  (See: rcConfig::walkableHeight)
 	int walkableClimb;			///< The walkable climb used during the build of the field. (See: rcConfig::walkableClimb)
+	int borderSize;				///< The AABB border size used during the build of the field. (See: rcConfig::borderSize)
 	unsigned short maxDistance;	///< The maximum distance value of any span within the field. 
 	unsigned short maxRegions;	///< The maximum region id of any span within the field. 
 	float bmin[3];				///< The minimum bounds in world space. [(x, y, z)]
@@ -320,9 +322,35 @@ struct rcCompactHeightfield
 	unsigned char* areas;		///< Array containing area id data. [Size: #spanCount]
 };
 
-rcCompactHeightfield* rcAllocCompactHeightfield();
-void rcFreeCompactHeightfield(rcCompactHeightfield* chf);
+/// Represents a heightfield layer within a layer set.
+/// @see rcHeightfieldLayerSet
+struct rcHeightfieldLayer
+{
+	float bmin[3];				///< The minimum bounds in world space. [(x, y, z)]
+	float bmax[3];				///< The maximum bounds in world space. [(x, y, z)]
+	float cs;					///< The size of each cell. (On the xz-plane.)
+	float ch;					///< The height of each cell. (The minimum increment along the y-axis.)
+	int width;					///< The width of the heightfield. (Along the x-axis in cell units.)
+	int height;					///< The height of the heightfield. (Along the z-axis in cell units.)
+	int minx;					///< The minimum x-bounds of usable data.
+	int maxx;					///< The maximum x-bounds of usable data.
+	int miny;					///< The minimum y-bounds of usable data. (Along the z-axis.)
+	int maxy;					///< The maximum y-bounds of usable data. (Along the z-axis.)
+	int hmin;					///< The minimum height bounds of usable data. (Along the y-axis.)
+	int hmax;					///< The maximum height bounds of usable data. (Along the y-axis.)
+	unsigned char* heights;		///< The heightfield. [Size: (width - borderSize*2) * (h - borderSize*2)]
+	unsigned char* areas;		///< Area ids. [Size: Same as #heights]
+	unsigned char* cons;		///< Packed neighbor connection information. [Size: Same as #heights]
+};
 
+/// Represents a set of heightfield layers.
+/// @ingroup recast
+/// @see rcAllocHeightfieldLayerSet, rcFreeHeightfieldLayerSet 
+struct rcHeightfieldLayerSet
+{
+	rcHeightfieldLayer* layers;			///< The layers in the set. [Size: #nlayers]
+	int nlayers;						///< The number of layers in the set.
+};
 
 /// Represents a simple, non-overlapping contour in field space.
 struct rcContour
@@ -345,12 +373,11 @@ struct rcContourSet
 	float bmax[3];		///< The maximum bounds in world space. [(x, y, z)]
 	float cs;			///< The size of each cell. (On the xz-plane.)
 	float ch;			///< The height of each cell. (The minimum increment along the y-axis.)
+	int width;			///< The width of the set. (Along the x-axis in cell units.) 
+	int height;			///< The height of the set. (Along the z-axis in cell units.) 
+	int borderSize;		///< The AABB border size used to generate the source data from which the contours were derived.
 };
 
-rcContourSet* rcAllocContourSet();
-void rcFreeContourSet(rcContourSet* cset);
-
-
 /// Represents a polygon mesh suitable for use in building a navigation mesh. 
 /// @ingroup recast
 struct rcPolyMesh
@@ -368,12 +395,9 @@ struct rcPolyMesh
 	float bmax[3];			///< The maximum bounds in world space. [(x, y, z)]
 	float cs;				///< The size of each cell. (On the xz-plane.)
 	float ch;				///< The height of each cell. (The minimum increment along the y-axis.)
+	int borderSize;			///< The AABB border size used to generate the source data from which the mesh was derived.
 };
 
-rcPolyMesh* rcAllocPolyMesh();
-void rcFreePolyMesh(rcPolyMesh* pmesh);
-
-
 /// Contains triangle meshes that represent detailed height data associated 
 /// with the polygons in its associated polygon mesh object.
 /// @ingroup recast
@@ -387,6 +411,71 @@ struct rcPolyMeshDetail
 	int ntris;				///< The number of triangles in #tris.
 };
 
+/// @name Allocation Functions
+/// Functions used to allocate and de-allocate Recast objects.
+/// @see rcAllocSetCustom
+/// @{
+
+/// Allocates a heightfield object using the Recast allocator.
+///  @return A heightfield that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcCreateHeightfield, rcFreeHeightField
+rcHeightfield* rcAllocHeightfield();
+
+/// Frees the specified heightfield object using the Recast allocator.
+///  @param[in]		hf	A heightfield allocated using #rcAllocHeightfield
+///  @ingroup recast
+///  @see rcAllocHeightfield
+void rcFreeHeightField(rcHeightfield* hf);
+
+/// Allocates a compact heightfield object using the Recast allocator.
+///  @return A compact heightfield that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildCompactHeightfield, rcFreeCompactHeightfield
+rcCompactHeightfield* rcAllocCompactHeightfield();
+
+/// Frees the specified compact heightfield object using the Recast allocator.
+///  @param[in]		chf		A compact heightfield allocated using #rcAllocCompactHeightfield
+///  @ingroup recast
+///  @see rcAllocCompactHeightfield
+void rcFreeCompactHeightfield(rcCompactHeightfield* chf);
+
+/// Allocates a heightfield layer set using the Recast allocator.
+///  @return A heightfield layer set that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildHeightfieldLayers, rcFreeHeightfieldLayerSet
+rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet();
+
+/// Frees the specified heightfield layer set using the Recast allocator.
+///  @param[in]		lset	A heightfield layer set allocated using #rcAllocHeightfieldLayerSet
+///  @ingroup recast
+///  @see rcAllocHeightfieldLayerSet
+void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset);
+
+/// Allocates a contour set object using the Recast allocator.
+///  @return A contour set that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildContours, rcFreeContourSet
+rcContourSet* rcAllocContourSet();
+
+/// Frees the specified contour set using the Recast allocator.
+///  @param[in]		cset	A contour set allocated using #rcAllocContourSet
+///  @ingroup recast
+///  @see rcAllocContourSet
+void rcFreeContourSet(rcContourSet* cset);
+
+/// Allocates a polygon mesh object using the Recast allocator.
+///  @return A polygon mesh that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildPolyMesh, rcFreePolyMesh
+rcPolyMesh* rcAllocPolyMesh();
+
+/// Frees the specified polygon mesh using the Recast allocator.
+///  @param[in]		pmesh	A polygon mesh allocated using #rcAllocPolyMesh
+///  @ingroup recast
+///  @see rcAllocPolyMesh
+void rcFreePolyMesh(rcPolyMesh* pmesh);
+
 /// Allocates a detail mesh object using the Recast allocator.
 ///  @return A detail mesh that is ready for initialization, or null on failure.
 ///  @ingroup recast
@@ -457,35 +546,14 @@ static const unsigned char RC_WALKABLE_AREA = 63;
 /// to another span. (Has no neighbor.)
 static const int RC_NOT_CONNECTED = 0x3f;
 
-// Compact span neighbour helpers.
-inline void rcSetCon(rcCompactSpan& s, int dir, int i)
-{
-	const unsigned int shift = (unsigned int)dir*6;
-	unsigned int con = s.con;
-	s.con = (con & ~(0x3f << shift)) | (((unsigned int)i & 0x3f) << shift);
-}
-
-inline int rcGetCon(const rcCompactSpan& s, int dir)
-{
-	const unsigned int shift = (unsigned int)dir*6;
-	return (s.con >> shift) & 0x3f;
-}
-
-inline int rcGetDirOffsetX(int dir)
-{
-	const int offset[4] = { -1, 0, 1, 0, };
-	return offset[dir&0x03];
-}
-
-inline int rcGetDirOffsetY(int dir)
-{
-	const int offset[4] = { 0, 1, 0, -1 };
-	return offset[dir&0x03];
-}
-
 /// @name General helper functions
 /// @{
 
+/// Used to ignore a function parameter.  VS complains about unused parameters
+/// and this silences the warning.
+///  @param [in] _ Unused parameter
+template<class T> void rcIgnoreUnused(const T&) { }
+
 /// Swaps the values of the two parameters.
 ///  @param[in,out]	a	Value A
 ///  @param[in,out]	b	Value B
@@ -647,13 +715,6 @@ inline void rcVnormalize(float* v)
 	v[2] *= d;
 }
 
-inline bool rcVequal(const float* p0, const float* p1)
-{
-	static const float thr = rcSqr(1.0f/16384.0f);
-	const float d = rcVdistSqr(p0, p1);
-	return d < thr;
-}
-
 /// @}
 /// @name Heightfield Functions
 /// @see rcHeightfield
@@ -718,18 +779,20 @@ void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
 								const int* tris, int nt, unsigned char* areas); 
 
 /// Adds a span to the specified heightfield.
-// The span addition can set to favor flags. If the span is merged to
-// another span and the new smax is within 'flagMergeThr' units away
-// from the existing span the span flags are merged and stored.
-// Params:
-//	solid - (in) heightfield where the spans is added to
-//  x,y - (in) location on the heightfield where the span is added
-//  smin,smax - (in) spans min/max height
-//  flags - (in) span flags (zero or WALKABLE)
-//  flagMergeThr - (in) merge threshold.
-void rcAddSpan(rcContext* ctx, rcHeightfield& solid, const int x, const int y,
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in,out]	hf				An initialized heightfield.
+///  @param[in]		x				The width index where the span is to be added.
+///  								[Limits: 0 <= value < rcHeightfield::width]
+///  @param[in]		y				The height index where the span is to be added.
+///  								[Limits: 0 <= value < rcHeightfield::height]
+///  @param[in]		smin			The minimum height of the span. [Limit: < @p smax] [Units: vx]
+///  @param[in]		smax			The maximum height of the span. [Limit: <= #RC_SPAN_MAX_HEIGHT] [Units: vx]
+///  @param[in]		area			The area id of the span. [Limit: <= #RC_WALKABLE_AREA)
+///  @param[in]		flagMergeThr	The merge theshold. [Limit: >= 0] [Units: vx]
+void rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y,
 			   const unsigned short smin, const unsigned short smax,
-			   const unsigned short area, const int flagMergeThr);
+			   const unsigned char area, const int flagMergeThr);
 
 /// Rasterizes a triangle into the specified heightfield.
 ///  @ingroup recast
@@ -877,6 +940,28 @@ void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
 						  const float hmin, const float hmax, unsigned char areaId,
 						  rcCompactHeightfield& chf);
 
+/// Helper function to offset voncex polygons for rcMarkConvexPolyArea.
+///  @ingroup recast
+///  @param[in]		verts		The vertices of the polygon [Form: (x, y, z) * @p nverts]
+///  @param[in]		nverts		The number of vertices in the polygon.
+///  @param[out]	outVerts	The offset vertices (should hold up to 2 * @p nverts) [Form: (x, y, z) * return value]
+///  @param[in]		maxOutVerts	The max number of vertices that can be stored to @p outVerts.
+///  @returns Number of vertices in the offset polygon or 0 if too few vertices in @p outVerts.
+int rcOffsetPoly(const float* verts, const int nverts, const float offset,
+				 float* outVerts, const int maxOutVerts);
+
+/// Applies the area id to all spans within the specified cylinder.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		pos		The center of the base of the cylinder. [Form: (x, y, z)] 
+///  @param[in]		r		The radius of the cylinder.
+///  @param[in]		h		The height of the cylinder.
+///  @param[in]		areaId	The area id to apply. [Limit: <= #RC_WALKABLE_AREA]
+///  @param[in,out]	chf	A populated compact heightfield.
+void rcMarkCylinderArea(rcContext* ctx, const float* pos,
+						const float r, const float h, unsigned char areaId,
+						rcCompactHeightfield& chf);
+
 /// Builds the distance field for the specified compact heightfield. 
 ///  @ingroup recast
 ///  @param[in,out]	ctx		The build context to use during the operation.
@@ -912,6 +997,68 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
 bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf,
 							const int borderSize, const int minRegionArea, const int mergeRegionArea);
 
+
+/// Sets the neighbor connection data for the specified direction.
+///  @param[in]		s		The span to update.
+///  @param[in]		dir		The direction to set. [Limits: 0 <= value < 4]
+///  @param[in]		i		The index of the neighbor span.
+inline void rcSetCon(rcCompactSpan& s, int dir, int i)
+{
+	const unsigned int shift = (unsigned int)dir*6;
+	unsigned int con = s.con;
+	s.con = (con & ~(0x3f << shift)) | (((unsigned int)i & 0x3f) << shift);
+}
+
+/// Gets neighbor connection data for the specified direction.
+///  @param[in]		s		The span to check.
+///  @param[in]		dir		The direction to check. [Limits: 0 <= value < 4]
+///  @return The neighbor connection data for the specified direction,
+///  	or #RC_NOT_CONNECTED if there is no connection.
+inline int rcGetCon(const rcCompactSpan& s, int dir)
+{
+	const unsigned int shift = (unsigned int)dir*6;
+	return (s.con >> shift) & 0x3f;
+}
+
+/// Gets the standard width (x-axis) offset for the specified direction.
+///  @param[in]		dir		The direction. [Limits: 0 <= value < 4]
+///  @return The width offset to apply to the current cell position to move
+///  	in the direction.
+inline int rcGetDirOffsetX(int dir)
+{
+	const int offset[4] = { -1, 0, 1, 0, };
+	return offset[dir&0x03];
+}
+
+/// Gets the standard height (z-axis) offset for the specified direction.
+///  @param[in]		dir		The direction. [Limits: 0 <= value < 4]
+///  @return The height offset to apply to the current cell position to move
+///  	in the direction.
+inline int rcGetDirOffsetY(int dir)
+{
+	const int offset[4] = { 0, 1, 0, -1 };
+	return offset[dir&0x03];
+}
+
+/// @}
+/// @name Layer, Contour, Polymesh, and Detail Mesh Functions
+/// @see rcHeightfieldLayer, rcContourSet, rcPolyMesh, rcPolyMeshDetail
+/// @{
+
+/// Builds a layer set from the specified compact heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx			The build context to use during the operation.
+///  @param[in]		chf			A fully built compact heightfield.
+///  @param[in]		borderSize	The size of the non-navigable border around the heightfield. [Limit: >=0] 
+///  							[Units: vx]
+///  @param[in]		walkableHeight	Minimum floor to 'ceiling' height that will still allow the floor area 
+///  							to be considered walkable. [Limit: >= 3] [Units: vx]
+///  @param[out]	lset		The resulting layer set. (Must be pre-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, 
+							  const int borderSize, const int walkableHeight,
+							  rcHeightfieldLayerSet& lset);
+
 /// Builds a contour set from the region outlines in the provided compact heightfield.
 ///  @ingroup recast
 ///  @param[in,out]	ctx			The build context to use during the operation.
@@ -927,13 +1074,15 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 					 const float maxError, const int maxEdgeLen,
 					 rcContourSet& cset, const int flags = RC_CONTOUR_TESS_WALL_EDGES);
 
-// Builds connected convex polygon mesh from contour polygons.
-// Params:
-//	cset - (in) contour set.
-//	nvp - (in) maximum number of vertices per polygon.
-//	mesh - (out) poly mesh.
-// Returns false if operation ran out of memory.
-bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& mesh);
+/// Builds a polygon mesh from the provided contours.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		cset	A fully built contour set.
+///  @param[in]		nvp		The maximum number of vertices allowed for polygons generated during the 
+///  						contour to polygon conversion process. [Limit: >= 3] 
+///  @param[out]	mesh	The resulting polygon mesh. (Must be re-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh);
 
 /// Merges multiple polygon meshes into a single mesh.
 ///  @ingroup recast
@@ -958,6 +1107,14 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa
 						   const float sampleDist, const float sampleMaxError,
 						   rcPolyMeshDetail& dmesh);
 
+/// Copies the poly mesh data from src to dst.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		src		The source mesh to copy from.
+///  @param[out]	dst		The resulting detail mesh. (Must be pre-allocated, must be empty mesh.)
+///  @returns True if the operation completed successfully.
+bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst);
+
 /// Merges multiple detail meshes into a single detail mesh.
 ///  @ingroup recast
 ///  @param[in,out]	ctx		The build context to use during the operation.
diff --git a/dep/recastnavigation/Recast/RecastArea.cpp b/dep/recastnavigation/Recast/RecastArea.cpp
index 4e7b79d301d..1a338cd9b8c 100644
--- a/dep/recastnavigation/Recast/RecastArea.cpp
+++ b/dep/recastnavigation/Recast/RecastArea.cpp
@@ -61,14 +61,26 @@ bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf)
 			const rcCompactCell& c = chf.cells[x+y*w];
 			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
 			{
-				if (chf.areas[i] != RC_NULL_AREA)
+				if (chf.areas[i] == RC_NULL_AREA)
+				{
+					dist[i] = 0;
+				}
+				else
 				{
 					const rcCompactSpan& s = chf.spans[i];
 					int nc = 0;
 					for (int dir = 0; dir < 4; ++dir)
 					{
 						if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
-							nc++;
+						{
+							const int nx = x + rcGetDirOffsetX(dir);
+							const int ny = y + rcGetDirOffsetY(dir);
+							const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir);
+							if (chf.areas[nidx] != RC_NULL_AREA)
+							{
+								nc++;
+							}
+						}
 					}
 					// At least one missing neighbour.
 					if (nc != 4)
@@ -339,7 +351,8 @@ void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigne
 				rcCompactSpan& s = chf.spans[i];
 				if ((int)s.y >= miny && (int)s.y <= maxy)
 				{
-					chf.areas[i] = areaId;
+					if (chf.areas[i] != RC_NULL_AREA)
+						chf.areas[i] = areaId;
 				}
 			}
 		}
@@ -418,6 +431,8 @@ void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
 			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
 			{
 				rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA)
+					continue;
 				if ((int)s.y >= miny && (int)s.y <= maxy)
 				{
 					float p[3];
@@ -436,3 +451,152 @@ void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
 
 	ctx->stopTimer(RC_TIMER_MARK_CONVEXPOLY_AREA);
 }
+
+int rcOffsetPoly(const float* verts, const int nverts, const float offset,
+				 float* outVerts, const int maxOutVerts)
+{
+	const float	MITER_LIMIT = 1.20f;
+
+	int n = 0;
+
+	for (int i = 0; i < nverts; i++)
+	{
+		const int a = (i+nverts-1) % nverts;
+		const int b = i;
+		const int c = (i+1) % nverts;
+		const float* va = &verts[a*3];
+		const float* vb = &verts[b*3];
+		const float* vc = &verts[c*3];
+		float dx0 = vb[0] - va[0];
+		float dy0 = vb[2] - va[2];
+		float d0 = dx0*dx0 + dy0*dy0;
+		if (d0 > 1e-6f)
+		{
+			d0 = 1.0f/rcSqrt(d0);
+			dx0 *= d0;
+			dy0 *= d0;
+		}
+		float dx1 = vc[0] - vb[0];
+		float dy1 = vc[2] - vb[2];
+		float d1 = dx1*dx1 + dy1*dy1;
+		if (d1 > 1e-6f)
+		{
+			d1 = 1.0f/rcSqrt(d1);
+			dx1 *= d1;
+			dy1 *= d1;
+		}
+		const float dlx0 = -dy0;
+		const float dly0 = dx0;
+		const float dlx1 = -dy1;
+		const float dly1 = dx1;
+		float cross = dx1*dy0 - dx0*dy1;
+		float dmx = (dlx0 + dlx1) * 0.5f;
+		float dmy = (dly0 + dly1) * 0.5f;
+		float dmr2 = dmx*dmx + dmy*dmy;
+		bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f;
+		if (dmr2 > 1e-6f)
+		{
+			const float scale = 1.0f / dmr2;
+			dmx *= scale;
+			dmy *= scale;
+		}
+
+		if (bevel && cross < 0.0f)
+		{
+			if (n+2 >= maxOutVerts)
+				return 0;
+			float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f;
+			outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset;
+			n++;
+			outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset;
+			n++;
+		}
+		else
+		{
+			if (n+1 >= maxOutVerts)
+				return 0;
+			outVerts[n*3+0] = vb[0] - dmx*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] - dmy*offset;
+			n++;
+		}
+	}
+	
+	return n;
+}
+
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkCylinderArea(rcContext* ctx, const float* pos,
+						const float r, const float h, unsigned char areaId,
+						rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	ctx->startTimer(RC_TIMER_MARK_CYLINDER_AREA);
+	
+	float bmin[3], bmax[3];
+	bmin[0] = pos[0] - r;
+	bmin[1] = pos[1];
+	bmin[2] = pos[2] - r;
+	bmax[0] = pos[0] + r;
+	bmax[1] = pos[1] + h;
+	bmax[2] = pos[2] + r;
+	const float r2 = r*r;
+	
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+	
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				
+				if (chf.areas[i] == RC_NULL_AREA)
+					continue;
+				
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; 
+					const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; 
+					const float dx = sx - pos[0];
+					const float dz = sz - pos[2];
+					
+					if (dx*dx + dz*dz < r2)
+					{
+						chf.areas[i] = areaId;
+					}
+				}
+			}
+		}
+	}
+	
+	ctx->stopTimer(RC_TIMER_MARK_CYLINDER_AREA);
+}
diff --git a/dep/recastnavigation/Recast/RecastContour.cpp b/dep/recastnavigation/Recast/RecastContour.cpp
index 8500e97f08d..5c324bcedfe 100644
--- a/dep/recastnavigation/Recast/RecastContour.cpp
+++ b/dep/recastnavigation/Recast/RecastContour.cpp
@@ -420,15 +420,13 @@ static void simplifyContour(rcIntArray& points, rcIntArray& simplified,
 					// Round based on the segments in lexilogical order so that the
 					// max tesselation is consistent regardles in which direction
 					// segments are traversed.
-					if (bx > ax || (bx == ax && bz > az))
+					const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
+					if (n > 1)
 					{
-						const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
-						maxi = (ai + n/2) % pn;
-					}
-					else
-					{
-						const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
-						maxi = (ai + (n+1)/2) % pn;
+						if (bx > ax || (bx == ax && bz > az))
+							maxi = (ai + n/2) % pn;
+						else
+							maxi = (ai + (n+1)/2) % pn;
 					}
 				}
 			}
@@ -466,7 +464,7 @@ static void simplifyContour(rcIntArray& points, rcIntArray& simplified,
 		// and the neighbour region is take from the next raw point.
 		const int ai = (simplified[i*4+3]+1) % pn;
 		const int bi = simplified[i*4+3];
-		simplified[i*4+3] = (points[ai*4+3] & RC_CONTOUR_REG_MASK) | (points[bi*4+3] & RC_BORDER_VERTEX);
+		simplified[i*4+3] = (points[ai*4+3] & (RC_CONTOUR_REG_MASK|RC_AREA_BORDER)) | (points[bi*4+3] & RC_BORDER_VERTEX);
 	}
 	
 }
@@ -613,13 +611,26 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 	
 	const int w = chf.width;
 	const int h = chf.height;
+	const int borderSize = chf.borderSize;
 	
 	ctx->startTimer(RC_TIMER_BUILD_CONTOURS);
 	
 	rcVcopy(cset.bmin, chf.bmin);
 	rcVcopy(cset.bmax, chf.bmax);
+	if (borderSize > 0)
+	{
+		// If the heightfield was build with bordersize, remove the offset.
+		const float pad = borderSize*chf.cs;
+		cset.bmin[0] += pad;
+		cset.bmin[2] += pad;
+		cset.bmax[0] -= pad;
+		cset.bmax[2] -= pad;
+	}
 	cset.cs = chf.cs;
 	cset.ch = chf.ch;
+	cset.width = chf.width - chf.borderSize*2;
+	cset.height = chf.height - chf.borderSize*2;
+	cset.borderSize = chf.borderSize;
 	
 	int maxContours = rcMax((int)chf.maxRegions, 8);
 	cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM);
@@ -671,8 +682,6 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 	
 	ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
 	
-	ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
-	
 	rcIntArray verts(256);
 	rcIntArray simplified(64);
 	
@@ -695,10 +704,17 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 				
 				verts.resize(0);
 				simplified.resize(0);
+
+				ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
 				walkContour(x, y, i, chf, flags, verts);
+				ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
+
+				ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
 				simplifyContour(verts, simplified, maxError, maxEdgeLen, buildFlags);
 				removeDegenerateSegments(simplified);
+				ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
 				
+
 				// Store region->contour remap info.
 				// Create contour.
 				if (simplified.size()/4 >= 3)
@@ -733,6 +749,16 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 						return false;
 					}
 					memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4);
+					if (borderSize > 0)
+					{
+						// If the heightfield was build with bordersize, remove the offset.
+						for (int j = 0; j < cont->nverts; ++j)
+						{
+							int* v = &cont->verts[j*4];
+							v[0] -= borderSize;
+							v[2] -= borderSize;
+						}
+					}
 					
 					cont->nrverts = verts.size()/4;
 					cont->rverts = (int*)rcAlloc(sizeof(int)*cont->nrverts*4, RC_ALLOC_PERM);
@@ -742,6 +768,16 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 						return false;
 					}
 					memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4);
+					if (borderSize > 0)
+					{
+						// If the heightfield was build with bordersize, remove the offset.
+						for (int j = 0; j < cont->nrverts; ++j)
+						{
+							int* v = &cont->rverts[j*4];
+							v[0] -= borderSize;
+							v[2] -= borderSize;
+						}
+					}
 					
 /*					cont->cx = cont->cy = cont->cz = 0;
 					for (int i = 0; i < cont->nverts; ++i)
@@ -809,8 +845,6 @@ bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
 		}
 	}
 	
-	ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
-	
 	ctx->stopTimer(RC_TIMER_BUILD_CONTOURS);
 	
 	return true;
diff --git a/dep/recastnavigation/Recast/RecastFilter.cpp b/dep/recastnavigation/Recast/RecastFilter.cpp
index 42d9f2c755b..bf985c362c9 100644
--- a/dep/recastnavigation/Recast/RecastFilter.cpp
+++ b/dep/recastnavigation/Recast/RecastFilter.cpp
@@ -48,6 +48,7 @@ void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb
 		{
 			rcSpan* ps = 0;
 			bool previousWalkable = false;
+			unsigned char previousArea = RC_NULL_AREA;
 			
 			for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next)
 			{
@@ -57,11 +58,12 @@ void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb
 				if (!walkable && previousWalkable)
 				{
 					if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb)
-						s->area = RC_NULL_AREA;
+						s->area = previousArea;
 				}
 				// Copy walkable flag so that it cannot propagate
 				// past multiple non-walkable objects.
 				previousWalkable = walkable;
+				previousArea = s->area;
 			}
 		}
 	}
diff --git a/dep/recastnavigation/Recast/RecastLayers.cpp b/dep/recastnavigation/Recast/RecastLayers.cpp
index 5ea6cb79d16..204f72e8cb2 100644
--- a/dep/recastnavigation/Recast/RecastLayers.cpp
+++ b/dep/recastnavigation/Recast/RecastLayers.cpp
@@ -325,7 +325,7 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf,
 					continue;
 				// Skip if the height range would become too large.
 				const int ymin = rcMin(root.ymin, regn.ymin);
-				const int ymax = rcMax(root.ymax, regn.ymax); // Edited by TC
+				const int ymax = rcMax(root.ymax, regn.ymax);
 				if ((ymax - ymin) >= 255)
 					 continue;
 
@@ -373,7 +373,7 @@ bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf,
 					continue;
 				// Skip if the height range would become too large.
 				const int ymin = rcMin(ri.ymin, rj.ymin);
-				const int ymax = rcMax(ri.ymax, rj.ymax);  // Edited by TC
+				const int ymax = rcMax(ri.ymax, rj.ymax);
 				if ((ymax - ymin) >= 255)
 				  continue;
 						  
diff --git a/dep/recastnavigation/Recast/RecastMesh.cpp b/dep/recastnavigation/Recast/RecastMesh.cpp
index 088b67aafd3..8af609b79fb 100644
--- a/dep/recastnavigation/Recast/RecastMesh.cpp
+++ b/dep/recastnavigation/Recast/RecastMesh.cpp
@@ -59,6 +59,7 @@ static bool buildMeshAdjacency(unsigned short* polys, const int npolys,
 		unsigned short* t = &polys[i*vertsPerPoly*2];
 		for (int j = 0; j < vertsPerPoly; ++j)
 		{
+			if (t[j] == RC_MESH_NULL_IDX) break;
 			unsigned short v0 = t[j];
 			unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1];
 			if (v0 < v1)
@@ -83,6 +84,7 @@ static bool buildMeshAdjacency(unsigned short* polys, const int npolys,
 		unsigned short* t = &polys[i*vertsPerPoly*2];
 		for (int j = 0; j < vertsPerPoly; ++j)
 		{
+			if (t[j] == RC_MESH_NULL_IDX) break;
 			unsigned short v0 = t[j];
 			unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1];
 			if (v0 > v1)
@@ -195,7 +197,7 @@ inline bool collinear(const int* a, const int* b, const int* c)
 //	Returns true iff ab properly intersects cd: they share
 //	a point interior to both segments.  The properness of the
 //	intersection is ensured by using strict leftness.
-bool intersectProp(const int* a, const int* b, const int* c, const int* d)
+static bool intersectProp(const int* a, const int* b, const int* c, const int* d)
 {
 	// Eliminate improper cases.
 	if (collinear(a,b,c) || collinear(a,b,d) ||
@@ -548,9 +550,9 @@ static bool canRemoveVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned sho
 					
 				// Check if the edge exists
 				bool exists = false;
-				for (int k = 0; k < nedges; ++k)
+				for (int m = 0; m < nedges; ++m)
 				{
-					int* e = &edges[k*3];
+					int* e = &edges[m*3];
 					if (e[1] == b)
 					{
 						// Exists, increment vertex share count.
@@ -659,7 +661,8 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short
 			}
 			// Remove the polygon.
 			unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2];
-			memcpy(p,p2,sizeof(unsigned short)*nvp);
+			if (p != p2)
+				memcpy(p,p2,sizeof(unsigned short)*nvp);
 			memset(p+nvp,0xff,sizeof(unsigned short)*nvp);
 			mesh.regs[i] = mesh.regs[mesh.npolys-1];
 			mesh.areas[i] = mesh.areas[mesh.npolys-1];
@@ -859,7 +862,9 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short
 				unsigned short* pa = &polys[bestPa*nvp];
 				unsigned short* pb = &polys[bestPb*nvp];
 				mergePolys(pa, pb, bestEa, bestEb, tmpPoly, nvp);
-				memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp);
+				unsigned short* last = &polys[(npolys-1)*nvp];
+				if (pb != last)
+					memcpy(pb, last, sizeof(unsigned short)*nvp);
 				pregs[bestPb] = pregs[npolys-1];
 				pareas[bestPb] = pareas[npolys-1];
 				npolys--;
@@ -893,8 +898,13 @@ static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short
 	return true;
 }
 
-
-bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& mesh)
+/// @par
+///
+/// @note If the mesh data is to be used to construct a Detour navigation mesh, then the upper 
+/// limit must be retricted to <= #DT_VERTS_PER_POLYGON.
+///
+/// @see rcAllocPolyMesh, rcContourSet, rcPolyMesh, rcConfig
+bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh)
 {
 	rcAssert(ctx);
 	
@@ -904,6 +914,7 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me
 	rcVcopy(mesh.bmax, cset.bmax);
 	mesh.cs = cset.cs;
 	mesh.ch = cset.ch;
+	mesh.borderSize = cset.borderSize;
 	
 	int maxVertices = 0;
 	int maxTris = 0;
@@ -926,7 +937,7 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me
 	rcScopedDelete<unsigned char> vflags = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxVertices, RC_ALLOC_TEMP);
 	if (!vflags)
 	{
-		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices);
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'vflags' (%d).", maxVertices);
 		return false;
 	}
 	memset(vflags, 0, maxVertices);
@@ -937,7 +948,7 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me
 		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices);
 		return false;
 	}
-	mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris*nvp*2*2, RC_ALLOC_PERM);
+	mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris*nvp*2, RC_ALLOC_PERM);
 	if (!mesh.polys)
 	{
 		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2);
@@ -1097,7 +1108,9 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me
 					unsigned short* pa = &polys[bestPa*nvp];
 					unsigned short* pb = &polys[bestPb*nvp];
 					mergePolys(pa, pb, bestEa, bestEb, tmpPoly, nvp);
-					memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp);
+					unsigned short* lastPoly = &polys[(npolys-1)*nvp];
+					if (pb != lastPoly)
+						memcpy(pb, lastPoly, sizeof(unsigned short)*nvp);
 					npolys--;
 				}
 				else
@@ -1155,6 +1168,37 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me
 		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed.");
 		return false;
 	}
+	
+	// Find portal edges
+	if (mesh.borderSize > 0)
+	{
+		const int w = cset.width;
+		const int h = cset.height;
+		for (int i = 0; i < mesh.npolys; ++i)
+		{
+			unsigned short* p = &mesh.polys[i*2*nvp];
+			for (int j = 0; j < nvp; ++j)
+			{
+				if (p[j] == RC_MESH_NULL_IDX) break;
+				// Skip connected edges.
+				if (p[nvp+j] != RC_MESH_NULL_IDX)
+					continue;
+				int nj = j+1;
+				if (nj >= nvp || p[nj] == RC_MESH_NULL_IDX) nj = 0;
+				const unsigned short* va = &mesh.verts[p[j]*3];
+				const unsigned short* vb = &mesh.verts[p[nj]*3];
+
+				if ((int)va[0] == 0 && (int)vb[0] == 0)
+					p[nvp+j] = 0x8000 | 0;
+				else if ((int)va[2] == h && (int)vb[2] == h)
+					p[nvp+j] = 0x8000 | 1;
+				else if ((int)va[0] == w && (int)vb[0] == w)
+					p[nvp+j] = 0x8000 | 2;
+				else if ((int)va[2] == 0 && (int)vb[2] == 0)
+					p[nvp+j] = 0x8000 | 3;
+			}
+		}
+	}
 
 	// Just allocate the mesh flags array. The user is resposible to fill it.
 	mesh.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*mesh.npolys, RC_ALLOC_PERM);
@@ -1167,11 +1211,11 @@ bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, int nvp, rcPolyMesh& me
 	
 	if (mesh.nverts > 0xffff)
 	{
-		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff);
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff);
 	}
 	if (mesh.npolys > 0xffff)
 	{
-		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff);
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff);
 	}
 	
 	ctx->stopTimer(RC_TIMER_BUILD_POLYMESH);
@@ -1271,7 +1315,7 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r
 		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh);
 		return false;
 	}
-	memset(nextVert, 0, sizeof(int)*maxVerts);
+	memset(vremap, 0, sizeof(unsigned short)*maxVertsPerMesh);
 	
 	for (int i = 0; i < nmeshes; ++i)
 	{
@@ -1280,6 +1324,12 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r
 		const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f);
 		const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f);
 		
+		bool isMinX = (ox == 0);
+		bool isMinZ = (oz == 0);
+		bool isMaxX = ((unsigned short)floorf((mesh.bmax[0] - pmesh->bmax[0]) / mesh.cs + 0.5f)) == 0;
+		bool isMaxZ = ((unsigned short)floorf((mesh.bmax[2] - pmesh->bmax[2]) / mesh.cs + 0.5f)) == 0;
+		bool isOnBorder = (isMinX || isMinZ || isMaxX || isMaxZ);
+
 		for (int j = 0; j < pmesh->nverts; ++j)
 		{
 			unsigned short* v = &pmesh->verts[j*3];
@@ -1300,6 +1350,36 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r
 				if (src[k] == RC_MESH_NULL_IDX) break;
 				tgt[k] = vremap[src[k]];
 			}
+
+			if (isOnBorder)
+			{
+				for (int k = mesh.nvp; k < mesh.nvp * 2; ++k)
+				{
+					if (src[k] & 0x8000 && src[k] != 0xffff)
+					{
+						unsigned short dir = src[k] & 0xf;
+						switch (dir)
+						{
+							case 0: // Portal x-
+								if (isMinX)
+									tgt[k] = src[k];
+								break;
+							case 1: // Portal z+
+								if (isMaxZ)
+									tgt[k] = src[k];
+								break;
+							case 2: // Portal x+
+								if (isMaxX)
+									tgt[k] = src[k];
+								break;
+							case 3: // Portal z-
+								if (isMinZ)
+									tgt[k] = src[k];
+								break;
+						}
+					}
+				}
+			}
 		}
 	}
 
@@ -1323,3 +1403,67 @@ bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, r
 	
 	return true;
 }
+
+bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst)
+{
+	rcAssert(ctx);
+	
+	// Destination must be empty.
+	rcAssert(dst.verts == 0);
+	rcAssert(dst.polys == 0);
+	rcAssert(dst.regs == 0);
+	rcAssert(dst.areas == 0);
+	rcAssert(dst.flags == 0);
+	
+	dst.nverts = src.nverts;
+	dst.npolys = src.npolys;
+	dst.maxpolys = src.npolys;
+	dst.nvp = src.nvp;
+	rcVcopy(dst.bmin, src.bmin);
+	rcVcopy(dst.bmax, src.bmax);
+	dst.cs = src.cs;
+	dst.ch = src.ch;
+	dst.borderSize = src.borderSize;
+	
+	dst.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.nverts*3, RC_ALLOC_PERM);
+	if (!dst.verts)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.verts' (%d).", src.nverts*3);
+		return false;
+	}
+	memcpy(dst.verts, src.verts, sizeof(unsigned short)*src.nverts*3);
+	
+	dst.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys*2*src.nvp, RC_ALLOC_PERM);
+	if (!dst.polys)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.polys' (%d).", src.npolys*2*src.nvp);
+		return false;
+	}
+	memcpy(dst.polys, src.polys, sizeof(unsigned short)*src.npolys*2*src.nvp);
+	
+	dst.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM);
+	if (!dst.regs)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.regs' (%d).", src.npolys);
+		return false;
+	}
+	memcpy(dst.regs, src.regs, sizeof(unsigned short)*src.npolys);
+	
+	dst.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*src.npolys, RC_ALLOC_PERM);
+	if (!dst.areas)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.areas' (%d).", src.npolys);
+		return false;
+	}
+	memcpy(dst.areas, src.areas, sizeof(unsigned char)*src.npolys);
+	
+	dst.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM);
+	if (!dst.flags)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.flags' (%d).", src.npolys);
+		return false;
+	}
+	memcpy(dst.flags, src.flags, sizeof(unsigned char)*src.npolys);
+	
+	return true;
+}
diff --git a/dep/recastnavigation/Recast/RecastMeshDetail.cpp b/dep/recastnavigation/Recast/RecastMeshDetail.cpp
index 77a085c5c2b..8325b883707 100644
--- a/dep/recastnavigation/Recast/RecastMeshDetail.cpp
+++ b/dep/recastnavigation/Recast/RecastMeshDetail.cpp
@@ -200,8 +200,8 @@ static unsigned short getHeight(const float fx, const float fy, const float fz,
 {
 	int ix = (int)floorf(fx*ics + 0.01f);
 	int iz = (int)floorf(fz*ics + 0.01f);
-	ix = rcClamp(ix-hp.xmin, 0, hp.width);
-	iz = rcClamp(iz-hp.ymin, 0, hp.height);
+	ix = rcClamp(ix-hp.xmin, 0, hp.width - 1);
+	iz = rcClamp(iz-hp.ymin, 0, hp.height - 1);
 	unsigned short h = hp.data[ix+iz*hp.width];
 	if (h == RC_UNSET_HEIGHT)
 	{
@@ -267,11 +267,11 @@ static int addEdge(rcContext* ctx, int* edges, int& nedges, const int maxEdges,
 	int e = findEdge(edges, nedges, s, t);
 	if (e == UNDEF)
 	{
-		int* e = &edges[nedges*4];
-		e[0] = s;
-		e[1] = t;
-		e[2] = l;
-		e[3] = r;
+		int* edge = &edges[nedges*4];
+		edge[0] = s;
+		edge[1] = t;
+		edge[2] = l;
+		edge[3] = r;
 		return nedges++;
 	}
 	else
@@ -583,10 +583,10 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin,
 				int maxi = -1;
 				for (int m = a+1; m < b; ++m)
 				{
-					float d = distancePtSeg(&edge[m*3],va,vb);
-					if (d > maxd)
+					float dev = distancePtSeg(&edge[m*3],va,vb);
+					if (dev > maxd)
 					{
-						maxd = d;
+						maxd = dev;
 						maxi = m;
 					}
 				}
@@ -741,9 +741,10 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin,
 	return true;
 }
 
-static void getHeightData(const rcCompactHeightfield& chf,
+
+static void getHeightDataSeedsFromVertices(const rcCompactHeightfield& chf,
 						  const unsigned short* poly, const int npoly,
-						  const unsigned short* verts,
+						  const unsigned short* verts, const int bs,
 						  rcHeightPatch& hp, rcIntArray& stack)
 {
 	// Floodfill the heightfield to get 2D height data,
@@ -775,7 +776,7 @@ static void getHeightData(const rcCompactHeightfield& chf,
 				az < hp.ymin || az >= hp.ymin+hp.height)
 				continue;
 			
-			const rcCompactCell& c = chf.cells[ax+az*chf.width];
+			const rcCompactCell& c = chf.cells[(ax+bs)+(az+bs)*chf.width];
 			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
 			{
 				const rcCompactSpan& s = chf.spans[i];
@@ -847,7 +848,7 @@ static void getHeightData(const rcCompactHeightfield& chf,
 			if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0)
 				continue;
 			
-			const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir);
+			const int ai = (int)chf.cells[(ax+bs)+(ay+bs)*chf.width].index + rcGetCon(cs, dir);
 
 			int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width;
 			hp.data[idx] = 1;
@@ -869,8 +870,83 @@ static void getHeightData(const rcCompactHeightfield& chf,
 		int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width;
 		const rcCompactSpan& cs = chf.spans[ci];
 		hp.data[idx] = cs.y;
+
+		// getHeightData seeds are given in coordinates with borders 
+		stack[i+0] += bs;
+		stack[i+1] += bs;
 	}
 	
+}
+
+
+
+static void getHeightData(const rcCompactHeightfield& chf,
+						  const unsigned short* poly, const int npoly,
+						  const unsigned short* verts, const int bs,
+						  rcHeightPatch& hp, rcIntArray& stack,
+						  int region)
+{
+	// Note: Reads to the compact heightfield are offset by border size (bs)
+	// since border size offset is already removed from the polymesh vertices.
+
+	stack.resize(0);
+	memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height);
+	
+	bool empty = true;
+
+	// Copy the height from the same region, and mark region borders
+	// as seed points to fill the rest.
+	for (int hy = 0; hy < hp.height; hy++)
+	{
+		int y = hp.ymin + hy + bs;
+		for (int hx = 0; hx < hp.width; hx++)
+		{
+			int x = hp.xmin + hx + bs;
+			const rcCompactCell& c = chf.cells[x+y*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (s.reg == region)
+				{
+					// Store height
+					hp.data[hx + hy*hp.width] = s.y;
+					empty = false;
+
+					// If any of the neighbours is not in same region,
+					// add the current location as flood fill start
+					bool border = false;
+					for (int dir = 0; dir < 4; ++dir)
+					{
+						if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+						{
+							const int ax = x + rcGetDirOffsetX(dir);
+							const int ay = y + rcGetDirOffsetY(dir);
+							const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
+							const rcCompactSpan& as = chf.spans[ai];
+							if (as.reg != region)
+							{
+								border = true;
+								break;
+							}
+						}
+					}
+					if (border)
+					{
+						stack.push(x);
+						stack.push(y);
+						stack.push(i);
+					}
+					break;
+				}
+			}
+		}
+	}	
+
+	// if the polygon does not contian any points from the current region (rare, but happens)
+	// then use the cells closest to the polygon vertices as seeds to fill the height field
+	if (empty)
+		getHeightDataSeedsFromVertices(chf, poly, npoly, verts, bs, hp, stack);
+	
 	static const int RETRACT_SIZE = 256;
 	int head = 0;
 	
@@ -895,26 +971,25 @@ static void getHeightData(const rcCompactHeightfield& chf,
 			
 			const int ax = cx + rcGetDirOffsetX(dir);
 			const int ay = cy + rcGetDirOffsetY(dir);
+			const int hx = ax - hp.xmin - bs;
+			const int hy = ay - hp.ymin - bs;
 			
-			if (ax < hp.xmin || ax >= (hp.xmin+hp.width) ||
-				ay < hp.ymin || ay >= (hp.ymin+hp.height))
+			if (hx < 0 || hx >= hp.width || hy < 0 || hy >= hp.height)
 				continue;
 			
-			if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != RC_UNSET_HEIGHT)
+			if (hp.data[hx + hy*hp.width] != RC_UNSET_HEIGHT)
 				continue;
 			
-			const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir);
-			
+			const int ai = (int)chf.cells[ax + ay*chf.width].index + rcGetCon(cs, dir);
 			const rcCompactSpan& as = chf.spans[ai];
-			int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width;
-			hp.data[idx] = as.y;
+
+			hp.data[hx + hy*hp.width] = as.y;
 
 			stack.push(ax);
 			stack.push(ay);
 			stack.push(ai);
 		}
 	}
-	
 }
 
 static unsigned char getEdgeFlags(const float* va, const float* vb,
@@ -961,6 +1036,7 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa
 	const float cs = mesh.cs;
 	const float ch = mesh.ch;
 	const float* orig = mesh.bmin;
+	const int borderSize = mesh.borderSize;
 	
 	rcIntArray edges(64);
 	rcIntArray tris(512);
@@ -1071,7 +1147,7 @@ bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompa
 		hp.ymin = bounds[i*4+2];
 		hp.width = bounds[i*4+1]-bounds[i*4+0];
 		hp.height = bounds[i*4+3]-bounds[i*4+2];
-		getHeightData(chf, p, npoly, mesh.verts, hp, stack);
+		getHeightData(chf, p, npoly, mesh.verts, borderSize, hp, stack, mesh.regs[i]);
 		
 		// Build detail mesh.
 		int nverts = 0;
@@ -1241,4 +1317,3 @@ bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int
 	
 	return true;
 }
-
diff --git a/dep/recastnavigation/Recast/RecastRasterization.cpp b/dep/recastnavigation/Recast/RecastRasterization.cpp
index d2bb7c98f18..45a7d35bf3e 100644
--- a/dep/recastnavigation/Recast/RecastRasterization.cpp
+++ b/dep/recastnavigation/Recast/RecastRasterization.cpp
@@ -95,7 +95,7 @@ static void addSpan(rcHeightfield& hf, const int x, const int y,
 	s->area = area;
 	s->next = 0;
 	
-	// Empty cell, add he first span.
+	// Empty cell, add the first span.
 	if (!hf.spans[idx])
 	{
 		hf.spans[idx] = s;
@@ -169,36 +169,64 @@ void rcAddSpan(rcContext* /*ctx*/, rcHeightfield& hf, const int x, const int y,
 	addSpan(hf, x,y, smin, smax, area, flagMergeThr);
 }
 
-static int clipPoly(const float* in, int n, float* out, float pnx, float pnz, float pd)
+// divides a convex polygons into two convex polygons on both sides of a line
+static void dividePoly(const float* in, int nin,
+					  float* out1, int* nout1,
+					  float* out2, int* nout2,
+					  float x, int axis)
 {
 	float d[12];
-	for (int i = 0; i < n; ++i)
-		d[i] = pnx*in[i*3+0] + pnz*in[i*3+2] + pd;
-	
-	int m = 0;
-	for (int i = 0, j = n-1; i < n; j=i, ++i)
+	for (int i = 0; i < nin; ++i)
+		d[i] = x - in[i*3+axis];
+
+	int m = 0, n = 0;
+	for (int i = 0, j = nin-1; i < nin; j=i, ++i)
 	{
 		bool ina = d[j] >= 0;
 		bool inb = d[i] >= 0;
 		if (ina != inb)
 		{
 			float s = d[j] / (d[j] - d[i]);
-			out[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
-			out[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
-			out[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
+			out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
+			out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
+			out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
+			rcVcopy(out2 + n*3, out1 + m*3);
 			m++;
+			n++;
+			// add the i'th point to the right polygon. Do NOT add points that are on the dividing line
+			// since these were already added above
+			if (d[i] > 0)
+			{
+				rcVcopy(out1 + m*3, in + i*3);
+				m++;
+			}
+			else if (d[i] < 0)
+			{
+				rcVcopy(out2 + n*3, in + i*3);
+				n++;
+			}
 		}
-		if (inb)
+		else // same side
 		{
-			out[m*3+0] = in[i*3+0];
-			out[m*3+1] = in[i*3+1];
-			out[m*3+2] = in[i*3+2];
-			m++;
+			// add the i'th point to the right polygon. Addition is done even for points on the dividing line
+			if (d[i] >= 0)
+			{
+				rcVcopy(out1 + m*3, in + i*3);
+				m++;
+				if (d[i] != 0)
+					continue;
+			}
+			rcVcopy(out2 + n*3, in + i*3);
+			n++;
 		}
 	}
-	return m;
+
+	*nout1 = m;
+	*nout2 = n;
 }
 
+
+
 static void rasterizeTri(const float* v0, const float* v1, const float* v2,
 						 const unsigned char area, rcHeightfield& hf,
 						 const float* bmin, const float* bmax,
@@ -222,48 +250,57 @@ static void rasterizeTri(const float* v0, const float* v1, const float* v2,
 	if (!overlapBounds(bmin, bmax, tmin, tmax))
 		return;
 	
-	// Calculate the footpring of the triangle on the grid.
-	int x0 = (int)((tmin[0] - bmin[0])*ics);
+	// Calculate the footprint of the triangle on the grid's y-axis
 	int y0 = (int)((tmin[2] - bmin[2])*ics);
-	int x1 = (int)((tmax[0] - bmin[0])*ics);
 	int y1 = (int)((tmax[2] - bmin[2])*ics);
-	x0 = rcClamp(x0, 0, w-1);
 	y0 = rcClamp(y0, 0, h-1);
-	x1 = rcClamp(x1, 0, w-1);
 	y1 = rcClamp(y1, 0, h-1);
 	
 	// Clip the triangle into all grid cells it touches.
-	float in[7*3], out[7*3], inrow[7*3];
+	float buf[7*3*4];
+	float *in = buf, *inrow = buf+7*3, *p1 = inrow+7*3, *p2 = p1+7*3;
+
+	rcVcopy(&in[0], v0);
+	rcVcopy(&in[1*3], v1);
+	rcVcopy(&in[2*3], v2);
+	int nvrow, nvIn = 3;
 	
 	for (int y = y0; y <= y1; ++y)
 	{
-		// Clip polygon to row.
-		rcVcopy(&in[0], v0);
-		rcVcopy(&in[1*3], v1);
-		rcVcopy(&in[2*3], v2);
-		int nvrow = 3;
+		// Clip polygon to row. Store the remaining polygon as well
 		const float cz = bmin[2] + y*cs;
-		nvrow = clipPoly(in, nvrow, out, 0, 1, -cz);
-		if (nvrow < 3) continue;
-		nvrow = clipPoly(out, nvrow, inrow, 0, -1, cz+cs);
+		dividePoly(in, nvIn, inrow, &nvrow, p1, &nvIn, cz+cs, 2);
+		rcSwap(in, p1);
 		if (nvrow < 3) continue;
 		
+		// find the horizontal bounds in the row
+		float minX = inrow[0], maxX = inrow[0];
+		for (int i=1; i<nvrow; ++i)
+		{
+			if (minX > inrow[i*3])	minX = inrow[i*3];
+			if (maxX < inrow[i*3])	maxX = inrow[i*3];
+		}
+		int x0 = (int)((minX - bmin[0])*ics);
+		int x1 = (int)((maxX - bmin[0])*ics);
+		x0 = rcClamp(x0, 0, w-1);
+		x1 = rcClamp(x1, 0, w-1);
+
+		int nv, nv2 = nvrow;
+
 		for (int x = x0; x <= x1; ++x)
 		{
-			// Clip polygon to column.
-			int nv = nvrow;
+			// Clip polygon to column. store the remaining polygon as well
 			const float cx = bmin[0] + x*cs;
-			nv = clipPoly(inrow, nv, out, 1, 0, -cx);
-			if (nv < 3) continue;
-			nv = clipPoly(out, nv, in, -1, 0, cx+cs);
+			dividePoly(inrow, nv2, p1, &nv, p2, &nv2, cx+cs, 0);
+			rcSwap(inrow, p2);
 			if (nv < 3) continue;
 			
 			// Calculate min and max of the span.
-			float smin = in[1], smax = in[1];
+			float smin = p1[1], smax = p1[1];
 			for (int i = 1; i < nv; ++i)
 			{
-				smin = rcMin(smin, in[i*3+1]);
-				smax = rcMax(smax, in[i*3+1]);
+				smin = rcMin(smin, p1[i*3+1]);
+				smax = rcMax(smax, p1[i*3+1]);
 			}
 			smin -= bmin[1];
 			smax -= bmin[1];
diff --git a/dep/recastnavigation/Recast/RecastRegion.cpp b/dep/recastnavigation/Recast/RecastRegion.cpp
index 2da99abb41b..589fac29203 100644
--- a/dep/recastnavigation/Recast/RecastRegion.cpp
+++ b/dep/recastnavigation/Recast/RecastRegion.cpp
@@ -283,8 +283,13 @@ static bool floodRegion(int x, int y, int i,
 				if (chf.areas[ai] != area)
 					continue;
 				unsigned short nr = srcReg[ai];
+				if (nr & RC_BORDER_REG) // Do not take borders into account.
+					continue;
 				if (nr != 0 && nr != r)
+				{
 					ar = nr;
+					break;
+				}
 				
 				const rcCompactSpan& as = chf.spans[ai];
 				
@@ -296,9 +301,12 @@ static bool floodRegion(int x, int y, int i,
 					const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
 					if (chf.areas[ai2] != area)
 						continue;
-					unsigned short nr = srcReg[ai2];
-					if (nr != 0 && nr != r)
-						ar = nr;
+					unsigned short nr2 = srcReg[ai2];
+					if (nr2 != 0 && nr2 != r)
+					{
+						ar = nr2;
+						break;
+					}
 				}				
 			}
 		}
@@ -319,16 +327,13 @@ static bool floodRegion(int x, int y, int i,
 				const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir);
 				if (chf.areas[ai] != area)
 					continue;
-				if (chf.dist[ai] >= lev)
+				if (chf.dist[ai] >= lev && srcReg[ai] == 0)
 				{
-					if (srcReg[ai] == 0)
-					{
-						srcReg[ai] = r;
-						srcDist[ai] = 0;
-						stack.push(ax);
-						stack.push(ay);
-						stack.push(ai);
-					}
+					srcReg[ai] = r;
+					srcDist[ai] = 0;
+					stack.push(ax);
+					stack.push(ay);
+					stack.push(ai);
 				}
 			}
 		}
@@ -341,30 +346,44 @@ static unsigned short* expandRegions(int maxIter, unsigned short level,
 									 rcCompactHeightfield& chf,
 									 unsigned short* srcReg, unsigned short* srcDist,
 									 unsigned short* dstReg, unsigned short* dstDist, 
-									 rcIntArray& stack)
+									 rcIntArray& stack,
+									 bool fillStack)
 {
 	const int w = chf.width;
 	const int h = chf.height;
 
-	// Find cells revealed by the raised level.
-	stack.resize(0);
-	for (int y = 0; y < h; ++y)
+	if (fillStack)
 	{
-		for (int x = 0; x < w; ++x)
+		// Find cells revealed by the raised level.
+		stack.resize(0);
+		for (int y = 0; y < h; ++y)
 		{
-			const rcCompactCell& c = chf.cells[x+y*w];
-			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			for (int x = 0; x < w; ++x)
 			{
-				if (chf.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA)
+				const rcCompactCell& c = chf.cells[x+y*w];
+				for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
 				{
-					stack.push(x);
-					stack.push(y);
-					stack.push(i);
+					if (chf.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA)
+					{
+						stack.push(x);
+						stack.push(y);
+						stack.push(i);
+					}
 				}
 			}
 		}
 	}
-	
+	else // use cells in the input stack
+	{
+		// mark all cells which already have a region
+		for (int j=0; j<stack.size(); j+=3)
+		{
+			int i = stack[j+2];
+			if (srcReg[i] != 0)
+				stack[j+2] = -1;
+		}
+	}
+
 	int iter = 0;
 	while (stack.size() > 0)
 	{
@@ -435,6 +454,61 @@ static unsigned short* expandRegions(int maxIter, unsigned short level,
 }
 
 
+
+static void sortCellsByLevel(unsigned short startLevel,
+							  rcCompactHeightfield& chf,
+							  unsigned short* srcReg,
+							  unsigned int nbStacks, rcIntArray* stacks,
+							  unsigned short loglevelsPerStack) // the levels per stack (2 in our case) as a bit shift
+{
+	const int w = chf.width;
+	const int h = chf.height;
+	startLevel = startLevel >> loglevelsPerStack;
+
+	for (unsigned int j=0; j<nbStacks; ++j)
+		stacks[j].resize(0);
+
+	// put all cells in the level range into the appropriate stacks
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (chf.areas[i] == RC_NULL_AREA || srcReg[i] != 0)
+					continue;
+
+				int level = chf.dist[i] >> loglevelsPerStack;
+				int sId = startLevel - level;
+				if (sId >= (int)nbStacks)
+					continue;
+				if (sId < 0)
+					sId = 0;
+
+				stacks[sId].push(x);
+				stacks[sId].push(y);
+				stacks[sId].push(i);
+			}
+		}
+	}
+}
+
+
+static void appendStacks(rcIntArray& srcStack, rcIntArray& dstStack,
+						 unsigned short* srcReg)
+{
+	for (int j=0; j<srcStack.size(); j+=3)
+	{
+		int i = srcStack[j+2];
+		if ((i < 0) || (srcReg[i] != 0))
+			continue;
+		dstStack.push(srcStack[j]);
+		dstStack.push(srcStack[j+1]);
+		dstStack.push(srcStack[j+2]);
+	}
+}
+
 struct rcRegion
 {
 	inline rcRegion(unsigned short i) :
@@ -679,17 +753,17 @@ static void walkContour(int x, int y, int i, int dir,
 	// Remove adjacent duplicates.
 	if (cont.size() > 1)
 	{
-		for (int i = 0; i < cont.size(); )
+		for (int j = 0; j < cont.size(); )
 		{
-			int ni = (i+1) % cont.size();
-			if (cont[i] == cont[ni])
+			int nj = (j+1) % cont.size();
+			if (cont[j] == cont[nj])
 			{
-				for (int j = i; j < cont.size()-1; ++j)
-					cont[j] = cont[j+1];
+				for (int k = j; k < cont.size()-1; ++k)
+					cont[k] = cont[k+1];
 				cont.pop();
 			}
 			else
-				++i;
+				++j;
 		}
 	}
 }
@@ -807,14 +881,14 @@ static bool filterSmallRegions(rcContext* ctx, int minRegionArea, int mergeRegio
 					connectsToBorder = true;
 					continue;
 				}
-				rcRegion& nreg = regions[creg.connections[j]];
-				if (nreg.visited)
+				rcRegion& neireg = regions[creg.connections[j]];
+				if (neireg.visited)
 					continue;
-				if (nreg.id == 0 || (nreg.id & RC_BORDER_REG))
+				if (neireg.id == 0 || (neireg.id & RC_BORDER_REG))
 					continue;
 				// Visit
-				stack.push(nreg.id);
-				nreg.visited = true;
+				stack.push(neireg.id);
+				neireg.visited = true;
 			}
 		}
 		
@@ -1087,6 +1161,8 @@ bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf,
 		paintRectRegion(w-bw, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++;
 		paintRectRegion(0, w, 0, bh, id|RC_BORDER_REG, chf, srcReg); id++;
 		paintRectRegion(0, w, h-bh, h, id|RC_BORDER_REG, chf, srcReg); id++;
+		
+		chf.borderSize = borderSize;
 	}
 	
 	rcIntArray prev(256);
@@ -1235,7 +1311,13 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
 	}
 	
 	ctx->startTimer(RC_TIMER_BUILD_REGIONS_WATERSHED);
-	
+
+	const int LOG_NB_STACKS = 3;
+	const int NB_STACKS = 1 << LOG_NB_STACKS;
+	rcIntArray lvlStacks[NB_STACKS];
+	for (int i=0; i<NB_STACKS; ++i)
+		lvlStacks[i].resize(1024);
+
 	rcIntArray stack(1024);
 	rcIntArray visited(1024);
 	
@@ -1256,20 +1338,39 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
 //	const int expandIters = 4 + walkableRadius * 2;
 	const int expandIters = 8;
 
-	// Mark border regions.
-	paintRectRegion(0, borderSize, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
-	paintRectRegion(w-borderSize, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
-	paintRectRegion(0, w, 0, borderSize, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
-	paintRectRegion(0, w, h-borderSize, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+	if (borderSize > 0)
+	{
+		// Make sure border will not overflow.
+		const int bw = rcMin(w, borderSize);
+		const int bh = rcMin(h, borderSize);
+		// Paint regions
+		paintRectRegion(0, bw, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+		paintRectRegion(w-bw, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+		paintRectRegion(0, w, 0, bh, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+		paintRectRegion(0, w, h-bh, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+
+		chf.borderSize = borderSize;
+	}
 	
+	int sId = -1;
 	while (level > 0)
 	{
 		level = level >= 2 ? level-2 : 0;
-		
+		sId = (sId+1) & (NB_STACKS-1);
+
+//		ctx->startTimer(RC_TIMER_DIVIDE_TO_LEVELS);
+
+		if (sId == 0)
+			sortCellsByLevel(level, chf, srcReg, NB_STACKS, lvlStacks, 1);
+		else 
+			appendStacks(lvlStacks[sId-1], lvlStacks[sId], srcReg); // copy left overs from last level
+
+//		ctx->stopTimer(RC_TIMER_DIVIDE_TO_LEVELS);
+
 		ctx->startTimer(RC_TIMER_BUILD_REGIONS_EXPAND);
 		
 		// Expand current regions until no empty connected cells found.
-		if (expandRegions(expandIters, level, chf, srcReg, srcDist, dstReg, dstDist, stack) != srcReg)
+		if (expandRegions(expandIters, level, chf, srcReg, srcDist, dstReg, dstDist, lvlStacks[sId], false) != srcReg)
 		{
 			rcSwap(srcReg, dstReg);
 			rcSwap(srcDist, dstDist);
@@ -1280,18 +1381,15 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
 		ctx->startTimer(RC_TIMER_BUILD_REGIONS_FLOOD);
 		
 		// Mark new regions with IDs.
-		for (int y = 0; y < h; ++y)
+		for (int j=0; j<lvlStacks[sId].size(); j+=3)
 		{
-			for (int x = 0; x < w; ++x)
+			int x = lvlStacks[sId][j];
+			int y = lvlStacks[sId][j+1];
+			int i = lvlStacks[sId][j+2];
+			if (i >= 0 && srcReg[i] == 0)
 			{
-				const rcCompactCell& c = chf.cells[x+y*w];
-				for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
-				{
-					if (chf.dist[i] < level || srcReg[i] != 0 || chf.areas[i] == RC_NULL_AREA)
-						continue;
-					if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack))
-						regionId++;
-				}
+				if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack))
+					regionId++;
 			}
 		}
 		
@@ -1299,7 +1397,7 @@ bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
 	}
 	
 	// Expand current regions until no empty connected cells found.
-	if (expandRegions(expandIters*8, 0, chf, srcReg, srcDist, dstReg, dstDist, stack) != srcReg)
+	if (expandRegions(expandIters*8, 0, chf, srcReg, srcDist, dstReg, dstDist, stack, true) != srcReg)
 	{
 		rcSwap(srcReg, dstReg);
 		rcSwap(srcDist, dstDist);
diff --git a/src/server/game/Miscellaneous/SharedDefines.h b/src/server/game/Miscellaneous/SharedDefines.h
index 7cf4f9fa192..b52e640afc7 100644
--- a/src/server/game/Miscellaneous/SharedDefines.h
+++ b/src/server/game/Miscellaneous/SharedDefines.h
@@ -3550,7 +3550,7 @@ enum PartyResult
 };
 
 const uint32 MMAP_MAGIC = 0x4d4d4150; // 'MMAP'
-#define MMAP_VERSION 3
+#define MMAP_VERSION 5
 
 struct MmapTileHeader
 {
diff --git a/src/server/game/Movement/PathGenerator.cpp b/src/server/game/Movement/PathGenerator.cpp
index 1f36b7c3106..2cda9b21c20 100644
--- a/src/server/game/Movement/PathGenerator.cpp
+++ b/src/server/game/Movement/PathGenerator.cpp
@@ -99,7 +99,7 @@ dtPolyRef PathGenerator::GetPathPolyByPosition(dtPolyRef const* polyPath, uint32
     for (uint32 i = 0; i < polyPathSize; ++i)
     {
         float closestPoint[VERTEX_SIZE];
-        if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint)))
+        if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint, NULL)))
             continue;
 
         float d = dtVdist2DSqr(point, closestPoint);
@@ -230,7 +230,7 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
         {
             float closestPoint[VERTEX_SIZE];
             // we may want to use closestPointOnPolyBoundary instead
-            if (dtStatusSucceed(_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint)))
+            if (dtStatusSucceed(_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, NULL)))
             {
                 dtVcopy(endPoint, closestPoint);
                 SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1]));
@@ -321,13 +321,13 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
 
         // we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
         float suffixEndPoint[VERTEX_SIZE];
-        if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint)))
+        if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL)))
         {
             // we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
             // try to recover by using prev polyref
             --prefixPolyLength;
             suffixStartPoly = _pathPolyRefs[prefixPolyLength-1];
-            if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint)))
+            if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL)))
             {
                 // suffixStartPoly is still invalid, error state
                 BuildShortcut();
@@ -581,7 +581,7 @@ bool PathGenerator::HaveTile(const G3D::Vector3& p) const
     if (tx < 0 || ty < 0)
         return false;
 
-    return (_navMesh->getTileAt(tx, ty) != NULL);
+    return (_navMesh->getTileAt(tx, ty, 0) != NULL);
 }
 
 uint32 PathGenerator::FixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath, dtPolyRef const* visited, uint32 nvisited)
diff --git a/src/tools/mmaps_generator/MapBuilder.cpp b/src/tools/mmaps_generator/MapBuilder.cpp
index 5524b994175..d60924a83f9 100644
--- a/src/tools/mmaps_generator/MapBuilder.cpp
+++ b/src/tools/mmaps_generator/MapBuilder.cpp
@@ -36,7 +36,7 @@ namespace DisableMgr
 }
 
 #define MMAP_MAGIC 0x4d4d4150   // 'MMAP'
-#define MMAP_VERSION 3
+#define MMAP_VERSION 5
 
 struct MmapTileHeader
 {
@@ -701,14 +701,6 @@ namespace MMAP
         delete[] dmmerge;
         delete[] tiles;
 
-        // remove padding for extraction
-        for (int i = 0; i < iv.polyMesh->nverts; ++i)
-        {
-            unsigned short* v = &iv.polyMesh->verts[i * 3];
-            v[0] -= (unsigned short)config.borderSize;
-            v[2] -= (unsigned short)config.borderSize;
-        }
-
         // set polygons as walkable
         // TODO: special flags for DYNAMIC polygons, ie surfaces that can be turned on and off
         for (int i = 0; i < iv.polyMesh->npolys; ++i)
@@ -747,9 +739,8 @@ namespace MMAP
         rcVcopy(params.bmax, bmax);
         params.cs = config.cs;
         params.ch = config.ch;
-        params.tileSize = VERTEX_PER_MAP;
-        /*params.tileLayer = 0;
-        params.buildBvTree = true;*/
+        params.tileLayer = 0;
+        params.buildBvTree = true;
 
         // will hold final navmesh
         unsigned char* navData = NULL;