Merge branch 'master' of https://github.com/azerothcore/azerothcore-wotlk into dir-restructure

1 files changed, 0 insertions, 2578 deletions

diff --git a/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp b/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp
deleted file mode 100644
index 33ee87fb95..0000000000
--- a/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp
+++ /dev/null
@@ -1,2578 +0,0 @@
-//
-// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
-//
-// This software is provided 'as-is', without any express or implied
-// warranty.  In no event will the authors be held liable for any damages
-// arising from the use of this software.
-// Permission is granted to anyone to use this software for any purpose,
-// including commercial applications, and to alter it and redistribute it
-// freely, subject to the following restrictions:
-// 1. The origin of this software must not be misrepresented; you must not
-//    claim that you wrote the original software. If you use this software
-//    in a product, an acknowledgment in the product documentation would be
-//    appreciated but is not required.
-// 2. Altered source versions must be plainly marked as such, and must not be
-//    misrepresented as being the original software.
-// 3. This notice may not be removed or altered from any source distribution.
-//
-
-#include <math.h>
-#include <float.h>
-#include <string.h>
-#include "DetourNavMeshQuery.h"
-#include "DetourNavMesh.h"
-#include "DetourNode.h"
-#include "DetourCommon.h"
-#include "DetourAlloc.h"
-#include "DetourAssert.h"
-#include <new>
-
-
-dtQueryFilter::dtQueryFilter() :
-	m_includeFlags(0xffff),
-	m_excludeFlags(0)
-{
-	for (int i = 0; i < DT_MAX_AREAS; ++i)
-		m_areaCost[i] = 1.0f;
-}
-
-#ifdef DT_VIRTUAL_QUERYFILTER
-bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
-							   const dtMeshTile* /*tile*/,
-							   const dtPoly* poly) const
-{
-	return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
-}
-
-float dtQueryFilter::getCost(const float* pa, const float* pb,
-							 const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
-							 const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
-							 const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
-{
-	return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
-}
-#else
-inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
-									  const dtMeshTile* /*tile*/,
-									  const dtPoly* poly) const
-{
-	return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
-}
-
-inline float dtQueryFilter::getCost(const float* pa, const float* pb,
-									const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
-									const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
-									const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
-{
-	return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
-}
-#endif	
-	
-static const float H_SCALE = 2.0f; // Search heuristic scale.
-
-
-dtNavMeshQuery* dtAllocNavMeshQuery()
-{
-	void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM);
-	if (!mem) return 0;
-	return new(mem) dtNavMeshQuery;
-}
-
-void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh)
-{
-	if (!navmesh) return;
-	navmesh->~dtNavMeshQuery();
-	dtFree(navmesh);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////
-dtNavMeshQuery::dtNavMeshQuery() :
-	m_nav(0),
-	m_tinyNodePool(0),
-	m_nodePool(0),
-	m_openList(0)
-{
-	memset(&m_query, 0, sizeof(dtQueryData));
-}
-
-dtNavMeshQuery::~dtNavMeshQuery()
-{
-	if (m_tinyNodePool)
-		m_tinyNodePool->~dtNodePool();
-	if (m_nodePool)
-		m_nodePool->~dtNodePool();
-	if (m_openList)
-		m_openList->~dtNodeQueue();
-	dtFree(m_tinyNodePool);
-	dtFree(m_nodePool);
-	dtFree(m_openList);
-}
-
-dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
-{
-	m_nav = nav;
-	
-	if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
-	{
-		if (m_nodePool)
-		{
-			m_nodePool->~dtNodePool();
-			dtFree(m_nodePool);
-			m_nodePool = 0;
-		}
-		m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4));
-		if (!m_nodePool)
-			return DT_FAILURE_OUT_OF_MEMORY;
-	}
-	else
-	{
-		m_nodePool->clear();
-	}
-	
-	if (!m_tinyNodePool)
-	{
-		m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32);
-		if (!m_tinyNodePool)
-			return DT_FAILURE_OUT_OF_MEMORY;
-	}
-	else
-	{
-		m_tinyNodePool->clear();
-	}
-	
-	// TODO: check the open list size too.
-	if (!m_openList || m_openList->getCapacity() < maxNodes)
-	{
-		if (m_openList)
-		{
-			m_openList->~dtNodeQueue();
-			dtFree(m_openList);
-			m_openList = 0;
-		}
-		m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes);
-		if (!m_openList)
-			return DT_FAILURE_OUT_OF_MEMORY;
-	}
-	else
-	{
-		m_openList->clear();
-	}
-	
-	return DT_SUCCESS;
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////
-dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const
-{
-	dtAssert(m_nav);
-	const dtMeshTile* tile = 0;
-	const dtPoly* poly = 0;
-	if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS)
-		return DT_FAILURE;
-	if (!tile) return DT_FAILURE;
-
-    if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-        return DT_FAILURE;
-
-	if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS)
-		return DT_FAILURE;
-	return DT_SUCCESS;
-}
-
-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;
-}
-
-dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const
-{
-	dtAssert(m_nav);
-	
-	const dtMeshTile* tile = 0;
-	const dtPoly* poly = 0;
-	if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS)
-		return DT_FAILURE;
-	
-	// Collect vertices.
-	float verts[DT_VERTS_PER_POLYGON*3];	
-	float edged[DT_VERTS_PER_POLYGON];
-	float edget[DT_VERTS_PER_POLYGON];
-	int nv = 0;
-	for (int i = 0; i < (int)poly->vertCount; ++i)
-	{
-		dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
-		nv++;
-	}		
-	
-	bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
-	if (inside)
-	{
-		// Point is inside the polygon, return the point.
-		dtVcopy(closest, pos);
-	}
-	else
-	{
-		// 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]);
-	}
-	
-	return DT_SUCCESS;
-}
-
-
-dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
-{
-	dtAssert(m_nav);
-
-	const dtMeshTile* tile = 0;
-	const dtPoly* poly = 0;
-	if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS)
-		return DT_FAILURE;
-	
-	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);
-		if (height)
-			*height = v0[1] + (v1[1] - v0[1]) * u;
-		return DT_SUCCESS;
-	}
-	else
-	{
-		const unsigned int ip = (unsigned int)(poly - tile->polys);
-		const dtPolyDetail* pd = &tile->detailMeshes[ip];
-		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))
-			{
-				if (height)
-					*height = h;
-				return DT_SUCCESS;
-			}
-		}
-	}
-	
-	return DT_FAILURE;
-}
-
-dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents,
-										 const dtQueryFilter* filter,
-										 dtPolyRef* nearestRef, float* nearestPt) const
-{
-	dtAssert(m_nav);
-
-	*nearestRef = 0;
-	
-	// Get nearby polygons from proximity grid.
-	dtPolyRef polys[128];
-	int polyCount = 0;
-	if (queryPolygons(center, extents, filter, polys, &polyCount, 128) != DT_SUCCESS)
-		return DT_FAILURE;
-	
-	// 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];
-		float closestPtPoly[3];
-		if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS)
-			continue;
-		float d = dtVdistSqr(center, closestPtPoly);
-		if (d < nearestDistanceSqr)
-		{
-			if (nearestPt)
-				dtVcopy(nearestPt, closestPtPoly);
-			nearestDistanceSqr = d;
-			nearest = ref;
-		}
-	}
-	
-	if (nearestRef)
-		*nearestRef = nearest;
-	
-	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
-{
-	dtAssert(m_nav);
-
-	if (tile->bvTree)
-	{
-		const dtBVNode* node = &tile->bvTree[0];
-		const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
-		const float* tbmin = tile->header->bmin;
-		const float* tbmax = tile->header->bmax;
-		const float qfac = tile->header->bvQuantFactor;
-		
-		// Calculate quantized box
-		unsigned short bmin[3], bmax[3];
-		// dtClamp query box to world box.
-		float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
-		float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
-		float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
-		float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
-		float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
-		float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
-		// Quantize
-		bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
-		bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
-		bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
-		bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
-		bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
-		bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
-		
-		// Traverse tree
-		const dtPolyRef base = m_nav->getPolyRefBase(tile);
-		int n = 0;
-		while (node < end)
-		{
-			const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
-			const bool isLeafNode = node->i >= 0;
-			
-			if (isLeafNode && overlap)
-			{
-				dtPolyRef ref = base | (dtPolyRef)node->i;
-				if (filter->passFilter(ref, tile, &tile->polys[node->i]))
-				{
-					if (n < maxPolys)
-						polys[n++] = ref;
-				}
-			}
-			
-			if (overlap || isLeafNode)
-				node++;
-			else
-			{
-				const int escapeIndex = -node->i;
-				node += escapeIndex;
-			}
-		}
-		
-		return n;
-	}
-	else
-	{
-		float bmin[3], bmax[3];
-		int n = 0;
-		const dtPolyRef base = m_nav->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;
-
-			const float* v = &tile->verts[p->verts[0]*3];
-			dtVcopy(bmin, v);
-			dtVcopy(bmax, v);
-			for (int j = 1; j < p->vertCount; ++j)
-			{
-				v = &tile->verts[p->verts[j]*3];
-				dtVmin(bmin, v);
-				dtVmax(bmax, v);
-			}
-			if (dtOverlapBounds(qmin,qmax, bmin,bmax))
-			{
-				const dtPolyRef ref = base | (dtPolyRef)i;
-				if (filter->passFilter(ref, tile, p))
-				{
-					if (n < maxPolys)
-						polys[n++] = ref;
-				}
-			}
-		}
-		return n;
-	}
-}
-
-dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents,
-									   const dtQueryFilter* filter,
-									   dtPolyRef* polys, int* polyCount, const int maxPolys) const
-{
-	dtAssert(m_nav);
-	
-	float bmin[3], bmax[3];
-	dtVsub(bmin, center, extents);
-	dtVadd(bmax, center, extents);
-	
-	// Find tiles the query touches.
-	int minx, miny, maxx, maxy;
-	m_nav->calcTileLoc(bmin, &minx, &miny);
-	m_nav->calcTileLoc(bmax, &maxx, &maxy);
-
-	int n = 0;
-
-	/// pussywizard: additional checks as in PathGenerator::HaveTile
-	if (minx < 0) minx = 0; if (miny < 0) miny = 0; // min can be negative because we subtract extents (few lines above)
-	if (maxx < 0 || maxy < 0 || maxx-minx >= 64 /*MAX_NUMBER_OF_GRIDS*/ || maxy-miny >= 64) // max should never be negative
-	{
-		*polyCount = n;
-		return DT_SUCCESS;
-	}
-
-	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)
-			{
-				*polyCount = n;
-				return DT_SUCCESS;
-			}
-		}
-	}
-	*polyCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
-								  const float* startPos, const float* endPos,
-								  const dtQueryFilter* filter,
-								  dtPolyRef* path, int* pathCount, const int maxPath) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_nodePool);
-	dtAssert(m_openList);
-	
-	*pathCount = 0;
-	
-	if (!startRef || !endRef)
-		return DT_FAILURE;
-	
-	if (!maxPath)
-		return DT_FAILURE;
-	
-	// Validate input
-	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
-		return DT_FAILURE;
-	
-	if (startRef == endRef)
-	{
-		path[0] = startRef;
-		*pathCount = 1;
-		return DT_SUCCESS;
-	}
-	
-	m_nodePool->clear();
-	m_openList->clear();
-	
-	dtNode* startNode = m_nodePool->getNode(startRef);
-	dtVcopy(startNode->pos, startPos);
-	startNode->pidx = 0;
-	startNode->cost = 0;
-	startNode->total = dtVdist(startPos, endPos) * H_SCALE;
-	startNode->id = startRef;
-	startNode->flags = DT_NODE_OPEN;
-	m_openList->push(startNode);
-	
-	dtNode* lastBestNode = startNode;
-	float lastBestNodeCost = startNode->total;
-	
-	while (!m_openList->empty())
-	{
-		// Remove node from open list and put it in closed list.
-		dtNode* bestNode = m_openList->pop();
-		bestNode->flags &= ~DT_NODE_OPEN;
-		bestNode->flags |= DT_NODE_CLOSED;
-		
-		// Reached the goal, stop searching.
-		if (bestNode->id == endRef)
-		{
-			lastBestNode = bestNode;
-			break;
-		}
-		
-		// Get current 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);
-		
-		// 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)
-		{
-			dtPolyRef neighbourRef = bestTile->links[i].ref;
-			
-			// Skip invalid ids and do not expand back to where we came from.
-			if (!neighbourRef || neighbourRef == parentRef)
-				continue;
-			
-			// Get neighbour poly and tile.
-			// The API input has been cheked already, skip checking internal data.
-			const dtMeshTile* neighbourTile = 0;
-			const dtPoly* neighbourPoly = 0;
-			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);			
-			
-			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
-				continue;
-
-			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
-			if (!neighbourNode)
-				continue;
-			
-			// If the node is visited the first time, calculate node position.
-			if (neighbourNode->flags == 0)
-			{
-				getEdgeMidPoint(bestRef, bestPoly, bestTile,
-								neighbourRef, neighbourPoly, neighbourTile,
-								neighbourNode->pos);
-			}
-
-			// Calculate cost and heuristic.
-			float cost = 0;
-			float heuristic = 0;
-			
-			// Special case for last node.
-			if (neighbourRef == endRef)
-			{
-				// Cost
-				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
-													  parentRef, parentTile, parentPoly,
-													  bestRef, bestTile, bestPoly,
-													  neighbourRef, neighbourTile, neighbourPoly);
-				const float endCost = filter->getCost(neighbourNode->pos, endPos,
-													  bestRef, bestTile, bestPoly,
-													  neighbourRef, neighbourTile, neighbourPoly,
-													  0, 0, 0);
-				
-				cost = bestNode->cost + curCost + endCost;
-				heuristic = 0;
-			}
-			else
-			{
-				// Cost
-				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
-													  parentRef, parentTile, parentPoly,
-													  bestRef, bestTile, bestPoly,
-													  neighbourRef, neighbourTile, neighbourPoly);
-				cost = bestNode->cost + curCost;
-				heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
-			}
-
-			const float total = cost + heuristic;
-			
-			// The node is already in open list and the new result is worse, skip.
-			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
-				continue;
-			// The node is already visited and process, and the new result is worse, skip.
-			if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
-				continue;
-			
-			// Add or update the node.
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
-			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
-			neighbourNode->cost = cost;
-			neighbourNode->total = total;
-			
-			if (neighbourNode->flags & DT_NODE_OPEN)
-			{
-				// Already in open, update node location.
-				m_openList->modify(neighbourNode);
-			}
-			else
-			{
-				// Put the node in open list.
-				neighbourNode->flags |= DT_NODE_OPEN;
-				m_openList->push(neighbourNode);
-			}
-			
-			// Update nearest node to target so far.
-			if (heuristic < lastBestNodeCost)
-			{
-				lastBestNodeCost = heuristic;
-				lastBestNode = neighbourNode;
-			}
-		}
-	}
-	
-	// Reverse the path.
-	dtNode* prev = 0;
-	dtNode* node = lastBestNode;
-	do
-	{
-		dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
-		node->pidx = m_nodePool->getNodeIdx(prev);
-		prev = node;
-		node = next;
-	}
-	while (node);
-	
-	// Store path
-	node = prev;
-	int n = 0;
-	do
-	{
-		path[n++] = node->id;
-		node = m_nodePool->getNodeAtIdx(node->pidx);
-	}
-	while (node && n < maxPath);
-	
-	*pathCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
-											const float* startPos, const float* endPos,
-											const dtQueryFilter* filter)
-{
-	dtAssert(m_nav);
-	dtAssert(m_nodePool);
-	dtAssert(m_openList);
-
-	// Init path state.
-	memset(&m_query, 0, sizeof(dtQueryData));
-	m_query.status = DT_FAILURE;
-	m_query.startRef = startRef;
-	m_query.endRef = endRef;
-	dtVcopy(m_query.startPos, startPos);
-	dtVcopy(m_query.endPos, endPos);
-	m_query.filter = filter;
-	
-	if (!startRef || !endRef)
-		return DT_FAILURE;
-	
-	// Validate input
-	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
-		return DT_FAILURE;
-
-	if (startRef == endRef)
-	{
-		m_query.status = DT_SUCCESS;
-		return DT_SUCCESS;
-	}
-	
-	m_nodePool->clear();
-	m_openList->clear();
-	
-	dtNode* startNode = m_nodePool->getNode(startRef);
-	dtVcopy(startNode->pos, startPos);
-	startNode->pidx = 0;
-	startNode->cost = 0;
-	startNode->total = dtVdist(startPos, endPos) * H_SCALE;
-	startNode->id = startRef;
-	startNode->flags = DT_NODE_OPEN;
-	m_openList->push(startNode);
-	
-	m_query.status = DT_IN_PROGRESS;
-	m_query.lastBestNode = startNode;
-	m_query.lastBestNodeCost = startNode->total;
-	
-	return m_query.status;
-}
-	
-dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
-{
-	if (m_query.status!= DT_IN_PROGRESS)
-		return m_query.status;
-
-	// Make sure the request is still valid.
-	if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef))
-	{
-		m_query.status = DT_FAILURE;
-		return DT_FAILURE;
-	}
-		
-	int iter = 0;
-	while (iter < maxIter && !m_openList->empty())
-	{
-		iter++;
-		
-		// Remove node from open list and put it in closed list.
-		dtNode* bestNode = m_openList->pop();
-		bestNode->flags &= ~DT_NODE_OPEN;
-		bestNode->flags |= DT_NODE_CLOSED;
-		
-		// Reached the goal, stop searching.
-		if (bestNode->id == m_query.endRef)
-		{
-			m_query.lastBestNode = bestNode;
-			m_query.status = DT_SUCCESS;
-			return m_query.status;
-		}
-		
-		// Get current 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;
-		if (m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly) != DT_SUCCESS)
-		{
-			// The polygon has disappeared during the sliced query, fail.
-			m_query.status = DT_FAILURE;
-			return m_query.status;
-		}
-		
-		// 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)
-		{
-			if (m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly) != DT_SUCCESS)
-			{
-				// The polygon has disappeared during the sliced query, fail.
-				m_query.status = DT_FAILURE;
-				return m_query.status;
-			}
-		}
-		
-		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
-		{
-			dtPolyRef neighbourRef = bestTile->links[i].ref;
-			
-			// Skip invalid ids and do not expand back to where we came from.
-			if (!neighbourRef || neighbourRef == parentRef)
-				continue;
-			
-			// Get neighbour poly and tile.
-			// The API input has been cheked already, skip checking internal data.
-			const dtMeshTile* neighbourTile = 0;
-			const dtPoly* neighbourPoly = 0;
-			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);			
-			
-			if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
-				continue;
-			
-			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
-			if (!neighbourNode)
-				continue;
-			
-			// If the node is visited the first time, calculate node position.
-			if (neighbourNode->flags == 0)
-			{
-				getEdgeMidPoint(bestRef, bestPoly, bestTile,
-								neighbourRef, neighbourPoly, neighbourTile,
-								neighbourNode->pos);
-			}
-			
-			// Calculate cost and heuristic.
-			float cost = 0;
-			float heuristic = 0;
-			
-			// Special case for last node.
-			if (neighbourRef == m_query.endRef)
-			{
-				// Cost
-				const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
-															  parentRef, parentTile, parentPoly,
-															  bestRef, bestTile, bestPoly,
-															  neighbourRef, neighbourTile, neighbourPoly);
-				const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
-															  bestRef, bestTile, bestPoly,
-															  neighbourRef, neighbourTile, neighbourPoly,
-															  0, 0, 0);
-				
-				cost = bestNode->cost + curCost + endCost;
-				heuristic = 0;
-			}
-			else
-			{
-				// Cost
-				const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
-															  parentRef, parentTile, parentPoly,
-															  bestRef, bestTile, bestPoly,
-															  neighbourRef, neighbourTile, neighbourPoly);
-				cost = bestNode->cost + curCost;
-				heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
-			}
-			
-			const float total = cost + heuristic;
-			
-			// The node is already in open list and the new result is worse, skip.
-			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
-				continue;
-			// The node is already visited and process, and the new result is worse, skip.
-			if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
-				continue;
-			
-			// Add or update the node.
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
-			neighbourNode->id = neighbourRef;
-			neighbourNode->flags &= ~DT_NODE_CLOSED;
-			neighbourNode->cost = cost;
-			neighbourNode->total = total;
-			
-			if (neighbourNode->flags & DT_NODE_OPEN)
-			{
-				// Already in open, update node location.
-				m_openList->modify(neighbourNode);
-			}
-			else
-			{
-				// Put the node in open list.
-				neighbourNode->flags |= DT_NODE_OPEN;
-				m_openList->push(neighbourNode);
-			}
-			
-			// Update nearest node to target so far.
-			if (heuristic < m_query.lastBestNodeCost)
-			{
-				m_query.lastBestNodeCost = heuristic;
-				m_query.lastBestNode = neighbourNode;
-			}
-		}
-	}
-	
-	// Exhausted all nodes, but could not find path.
-	if (m_openList->empty())
-		m_query.status = DT_SUCCESS;
-
-	return m_query.status;
-}
-
-dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath)
-{
-	*pathCount = 0;
-	
-	if (m_query.status != DT_SUCCESS)
-	{
-		// Reset query.
-		memset(&m_query, 0, sizeof(dtQueryData));
-		return DT_FAILURE;
-	}
-
-	int n = 0;
-
-	if (m_query.startRef == m_query.endRef)
-	{
-		// Special case: the search starts and ends at same poly.
-		path[n++] = m_query.startRef;
-	}
-	else
-	{
-		// Reverse the path.
-		dtAssert(m_query.lastBestNode);
-		dtNode* prev = 0;
-		dtNode* node = m_query.lastBestNode;
-		do
-		{
-			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
-			node->pidx = m_nodePool->getNodeIdx(prev);
-			prev = node;
-			node = next;
-		}
-		while (node);
-		
-		// Store path
-		node = prev;
-		do
-		{
-			path[n++] = node->id;
-			node = m_nodePool->getNodeAtIdx(node->pidx);
-		}
-		while (node && n < maxPath);
-	}
-	
-	// Reset query.
-	memset(&m_query, 0, sizeof(dtQueryData));
-	
-	*pathCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
-													   dtPolyRef* path, int* pathCount, const int maxPath)
-{
-	*pathCount = 0;
-	
-	if (existingSize == 0)
-	{
-		return DT_FAILURE;
-	}
-	
-	if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS)
-	{
-		// Reset query.
-		memset(&m_query, 0, sizeof(dtQueryData));
-		return DT_FAILURE;
-	}
-	
-	int n = 0;
-	
-	if (m_query.startRef == m_query.endRef)
-	{
-		// Special case: the search starts and ends at same poly.
-		path[n++] = m_query.startRef;
-	}
-	else
-	{
-		// Find furthest existing node that was visited.
-		dtNode* prev = 0;
-		dtNode* node = 0;
-		for (int i = existingSize-1; i >= 0; --i)
-		{
-			node = m_nodePool->findNode(existing[i]);
-			if (node)
-				break;
-		}
-		
-		if (!node)
-		{
-			return DT_FAILURE;
-		}
-		
-		// Reverse the path.
-		do
-		{
-			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
-			node->pidx = m_nodePool->getNodeIdx(prev);
-			prev = node;
-			node = next;
-		}
-		while (node);
-		
-		// Store path
-		node = prev;
-		do
-		{
-			path[n++] = node->id;
-			node = m_nodePool->getNodeAtIdx(node->pidx);
-		}
-		while (node && n < maxPath);
-	}
-	
-	// Reset query.
-	memset(&m_query, 0, sizeof(dtQueryData));
-	
-	*pathCount = n;
-	
-	return DT_SUCCESS;
-}
-
-
-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
-{
-	dtAssert(m_nav);
-	
-	*straightPathCount = 0;
-	
-	if (!maxStraightPath)
-		return DT_FAILURE;
-	
-	if (!path[0])
-		return DT_FAILURE;
-	
-	int n = 0;
-	
-	// TODO: Should this be callers responsibility?
-	float closestStartPos[3];
-	if (closestPointOnPolyBoundary(path[0], startPos, closestStartPos) != DT_SUCCESS)
-		return DT_FAILURE;
-	
-	// 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;
-	
-	if (pathSize > 1)
-	{
-		float portalApex[3], portalLeft[3], portalRight[3];
-		dtVcopy(portalApex, closestStartPos);
-		dtVcopy(portalLeft, portalApex);
-		dtVcopy(portalRight, portalApex);
-		int apexIndex = 0;
-		int leftIndex = 0;
-		int rightIndex = 0;
-		
-		unsigned char leftPolyType = 0;
-		unsigned char rightPolyType = 0;
-		
-		dtPolyRef leftPolyRef = path[0];
-		dtPolyRef rightPolyRef = path[0];
-		
-		for (int i = 0; i < pathSize; ++i)
-		{
-			float left[3], right[3];
-			unsigned char fromType, toType;
-			
-			if (i+1 < pathSize)
-			{
-				// Next portal.
-				if (getPortalPoints(path[i], path[i+1], left, right, fromType, toType) != DT_SUCCESS)
-				{
-					if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS)
-						return DT_FAILURE;
-					
-					dtVcopy(&straightPath[n*3], closestEndPos);
-					if (straightPathFlags)
-						straightPathFlags[n] = 0;
-					if (straightPathRefs)
-						straightPathRefs[n] = path[i];
-					n++;
-					
-					return DT_SUCCESS;
-				}
-				
-				// If starting really close the portal, advance.
-				if (i == 0)
-				{
-					float t;
-					if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f))
-						continue;
-				}
-			}
-			else
-			{
-				// End of the path.
-				dtVcopy(left, closestEndPos);
-				dtVcopy(right, closestEndPos);
-				
-				fromType = toType = DT_POLYTYPE_GROUND;
-			}
-			
-			// Right vertex.
-			if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
-			{
-				if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
-				{
-					dtVcopy(portalRight, right);
-					rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
-					rightPolyType = toType;
-					rightIndex = i;
-				}
-				else
-				{
-					dtVcopy(portalApex, portalLeft);
-					apexIndex = leftIndex;
-					
-					unsigned char flags = 0;
-					if (!leftPolyRef)
-						flags = DT_STRAIGHTPATH_END;
-					else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
-						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;
-					}
-					
-					dtVcopy(portalLeft, portalApex);
-					dtVcopy(portalRight, portalApex);
-					leftIndex = apexIndex;
-					rightIndex = apexIndex;
-					
-					// Restart
-					i = apexIndex;
-					
-					continue;
-				}
-			}
-			
-			// Left vertex.
-			if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
-			{
-				if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
-				{
-					dtVcopy(portalLeft, left);
-					leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
-					leftPolyType = toType;
-					leftIndex = i;
-				}
-				else
-				{
-					dtVcopy(portalApex, portalRight);
-					apexIndex = rightIndex;
-					
-					unsigned char flags = 0;
-					if (!rightPolyRef)
-						flags = DT_STRAIGHTPATH_END;
-					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;
-					}
-					
-					dtVcopy(portalLeft, portalApex);
-					dtVcopy(portalRight, portalApex);
-					leftIndex = apexIndex;
-					rightIndex = apexIndex;
-					
-					// Restart
-					i = apexIndex;
-					
-					continue;
-				}
-			}
-		}
-	}
-	
-	// 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;
-}
-
-dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
-										  const dtQueryFilter* filter,
-										  float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_tinyNodePool);
-
-	*visitedCount = 0;
-	
-	// Validate input
-	if (!startRef) return DT_FAILURE;
-	if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE;
-	
-	static const int MAX_STACK = 48;
-	dtNode* stack[MAX_STACK];
-	int nstack = 0;
-	
-	m_tinyNodePool->clear();
-	
-	dtNode* startNode = m_tinyNodePool->getNode(startRef);
-	startNode->pidx = 0;
-	startNode->cost = 0;
-	startNode->total = 0;
-	startNode->id = startRef;
-	startNode->flags = DT_NODE_CLOSED;
-	stack[nstack++] = startNode;
-	
-	float bestPos[3];
-	float bestDist = FLT_MAX;
-	dtNode* bestNode = 0;
-	dtVcopy(bestPos, startPos);
-	
-	// Search constraints
-	float searchPos[3], searchRadSqr;
-	dtVlerp(searchPos, startPos, endPos, 0.5f);
-	searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);
-	
-	float verts[DT_VERTS_PER_POLYGON*3];
-	
-	while (nstack)
-	{
-		// Pop front.
-		dtNode* curNode = stack[0];
-		for (int i = 0; i < nstack-1; ++i)
-			stack[i] = stack[i+1];
-		nstack--;
-		
-		// Get poly and tile.
-		// The API input has been cheked already, skip checking internal data.
-		const dtPolyRef curRef = curNode->id;
-		const dtMeshTile* curTile = 0;
-		const dtPoly* curPoly = 0;
-		m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);			
-		
-		// Collect vertices.
-		const int nverts = curPoly->vertCount;
-		for (int i = 0; i < nverts; ++i)
-			dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]);
-		
-		// If target is inside the poly, stop search.
-		if (dtPointInPolygon(endPos, verts, nverts))
-		{
-			bestNode = curNode;
-			dtVcopy(bestPos, endPos);
-			break;
-		}
-		
-		// Find wall edges and find nearest point inside the walls.
-		for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++)
-		{
-			// Find links to neighbours.
-			static const int MAX_NEIS = 8;
-			int nneis = 0;
-			dtPolyRef neis[MAX_NEIS];
-			
-			if (curPoly->neis[j] & DT_EXT_LINK)
-			{
-				// Tile border.
-				for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
-				{
-					const dtLink* link = &curTile->links[k];
-					if (link->edge == j)
-					{
-						if (link->ref != 0)
-						{
-							const dtMeshTile* neiTile = 0;
-							const dtPoly* neiPoly = 0;
-							m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
-							if (filter->passFilter(link->ref, neiTile, neiPoly))
-							{
-								if (nneis < MAX_NEIS)
-									neis[nneis++] = link->ref;
-							}
-						}
-					}
-				}
-			}
-			else if (curPoly->neis[j])
-			{
-				const unsigned int idx = (unsigned int)(curPoly->neis[j]-1);
-				const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx;
-				if (filter->passFilter(ref, curTile, &curTile->polys[idx]))
-				{
-					// Internal edge, encode id.
-					neis[nneis++] = ref;
-				}
-			}
-			
-			if (!nneis)
-			{
-				// Wall edge, calc distance.
-				const float* vj = &verts[j*3];
-				const float* vi = &verts[i*3];
-				float tseg;
-				const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg);
-				if (distSqr < bestDist)
-				{
-                    // Update nearest distance.
-					dtVlerp(bestPos, vj,vi, tseg);
-					bestDist = distSqr;
-					bestNode = curNode;
-				}
-			}
-			else
-			{
-				for (int k = 0; k < nneis; ++k)
-				{
-					// Skip if no node can be allocated.
-					dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]);
-					if (!neighbourNode)
-						continue;
-					// Skip if already visited.
-					if (neighbourNode->flags & DT_NODE_CLOSED)
-						continue;
-					
-					// Skip the link if it is too far from search constraint.
-					// TODO: Maybe should use getPortalPoints(), but this one is way faster.
-					const float* vj = &verts[j*3];
-					const float* vi = &verts[i*3];
-					float tseg;
-					float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg);
-					if (distSqr > searchRadSqr)
-						continue;
-					
-					// Mark as the node as visited and push to queue.
-					if (nstack < MAX_STACK)
-					{
-						neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
-						neighbourNode->flags |= DT_NODE_CLOSED;
-						stack[nstack++] = neighbourNode;
-					}
-				}
-			}
-		}
-	}
-	
-	int n = 0;
-	if (bestNode)
-	{
-		// Reverse the path.
-		dtNode* prev = 0;
-		dtNode* node = bestNode;
-		do
-		{
-			dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx);
-			node->pidx = m_tinyNodePool->getNodeIdx(prev);
-			prev = node;
-			node = next;
-		}
-		while (node);
-		
-		// Store result
-		node = prev;
-		do
-		{
-			visited[n++] = node->id;
-			node = m_tinyNodePool->getNodeAtIdx(node->pidx);
-		}
-		while (node && n < maxVisitedSize);
-	}
-	
-	dtVcopy(resultPos, bestPos);
-	
-	*visitedCount = n;
-	
-	return DT_SUCCESS;
-}
-
-
-dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
-										 unsigned char& fromType, unsigned char& toType) const
-{
-	dtAssert(m_nav);
-	
-	const dtMeshTile* fromTile = 0;
-	const dtPoly* fromPoly = 0;
-	if (m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly) != DT_SUCCESS)
-		return DT_FAILURE;
-	fromType = fromPoly->getType();
-
-	const dtMeshTile* toTile = 0;
-	const dtPoly* toPoly = 0;
-	if (m_nav->getTileAndPolyByRef(to, &toTile, &toPoly) != DT_SUCCESS)
-		return DT_FAILURE;
-	toType = toPoly->getType();
-		
-	return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
-}
-
-// Returns portal points between two polygons.
-dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
-										 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
-										 float* left, float* right) const
-{
-	// Find the link that points to the 'to' polygon.
-	const dtLink* link = 0;
-	for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
-	{
-		if (fromTile->links[i].ref == to)
-		{
-			link = &fromTile->links[i];
-			break;
-		}
-	}
-	if (!link)
-		return DT_FAILURE;
-	
-	// Handle off-mesh connections.
-	if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-	{
-		// Find link that points to first vertex.
-		for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
-		{
-			if (fromTile->links[i].ref == to)
-			{
-				const int v = fromTile->links[i].edge;
-				dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]);
-				dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]);
-				return DT_SUCCESS;
-			}
-		}
-		return DT_FAILURE;
-	}
-	
-	if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-	{
-		for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
-		{
-			if (toTile->links[i].ref == from)
-			{
-				const int v = toTile->links[i].edge;
-				dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]);
-				dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]);
-				return DT_SUCCESS;
-			}
-		}
-		return DT_FAILURE;
-	}
-	
-	// Find portal vertices.
-	const int v0 = fromPoly->verts[link->edge];
-	const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
-	dtVcopy(left, &fromTile->verts[v0*3]);
-	dtVcopy(right, &fromTile->verts[v1*3]);
-	
-	// If the link is at tile boundary, dtClamp the vertices to
-	// the link width.
-	if (link->side != 0xff)
-	{
-		// Unpack portal limits.
-		if (link->bmin != 0 || link->bmax != 255)
-		{
-			const float s = 1.0f/255.0f;
-			const float tmin = link->bmin*s;
-			const float tmax = link->bmax*s;
-			dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin);
-			dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax);
-		}
-	}
-	
-	return DT_SUCCESS;
-}
-
-// Returns edge mid point between two polygons.
-dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const
-{
-	float left[3], right[3];
-	unsigned char fromType, toType;
-	if (!getPortalPoints(from, to, left,right, fromType, toType)) return DT_FAILURE;
-	mid[0] = (left[0]+right[0])*0.5f;
-	mid[1] = (left[1]+right[1])*0.5f;
-	mid[2] = (left[2]+right[2])*0.5f;
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
-										 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
-										 float* mid) const
-{
-	float left[3], right[3];
-	if (getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right) != DT_SUCCESS)
-		return DT_FAILURE;
-	mid[0] = (left[0]+right[0])*0.5f;
-	mid[1] = (left[1]+right[1])*0.5f;
-	mid[2] = (left[2]+right[2])*0.5f;
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
-								 const dtQueryFilter* filter,
-								 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const
-{
-	dtAssert(m_nav);
-	
-	*t = 0;
-	if (pathCount)
-		*pathCount = 0;
-	
-	// Validate input
-	if (!startRef || !m_nav->isValidPolyRef(startRef))
-		return DT_FAILURE;
-	
-	dtPolyRef curRef = startRef;
-	float verts[DT_VERTS_PER_POLYGON*3];	
-	int n = 0;
-	
-	hitNormal[0] = 0;
-	hitNormal[1] = 0;
-	hitNormal[2] = 0;
-	
-	while (curRef)
-	{
-		// Cast ray against current polygon.
-		
-		// The API input has been cheked already, skip checking internal data.
-		const dtMeshTile* tile = 0;
-		const dtPoly* poly = 0;
-		m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
-		
-		// Collect vertices.
-		int nv = 0;
-		for (int i = 0; i < (int)poly->vertCount; ++i)
-		{
-			dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
-			nv++;
-		}		
-		
-		float tmin, tmax;
-		int segMin, segMax;
-		if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
-		{
-			// Could not hit the polygon, keep the old t and report hit.
-			if (pathCount)
-				*pathCount = n;
-			return DT_SUCCESS;
-		}
-		// Keep track of furthest t so far.
-		if (tmax > *t)
-			*t = tmax;
-		
-		// Store visited polygons.
-		if (n < maxPath)
-			path[n++] = curRef;
-		
-		// Ray end is completely inside the polygon.
-		if (segMax == -1)
-		{
-			*t = FLT_MAX;
-			if (pathCount)
-				*pathCount = n;
-			return DT_SUCCESS;
-		}
-		
-		// Follow neighbours.
-		dtPolyRef nextRef = 0;
-		
-		for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
-		{
-			const dtLink* link = &tile->links[i];
-			
-			// Find link which contains this edge.
-			if ((int)link->edge != segMax)
-				continue;
-			
-			// Get pointer to the next polygon.
-			const dtMeshTile* nextTile = 0;
-			const dtPoly* nextPoly = 0;
-			m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
-			
-			// Skip off-mesh connections.
-			if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-				continue;
-			
-			// Skip links based on filter.
-			if (!filter->passFilter(link->ref, nextTile, nextPoly))
-				continue;
-			
-			// If the link is internal, just return the ref.
-			if (link->side == 0xff)
-			{
-				nextRef = link->ref;
-				break;
-			}
-			
-			// If the link is at tile boundary,
-			
-			// Check if the link spans the whole edge, and accept.
-			if (link->bmin == 0 && link->bmax == 255)
-			{
-				nextRef = link->ref;
-				break;
-			}
-			
-			// Check for partial edge links.
-			const int v0 = poly->verts[link->edge];
-			const int v1 = poly->verts[(link->edge+1) % poly->vertCount];
-			const float* left = &tile->verts[v0*3];
-			const float* right = &tile->verts[v1*3];
-			
-			// Check that the intersection lies inside the link portal.
-			if (link->side == 0 || link->side == 4)
-			{
-				// Calculate link size.
-				const float s = 1.0f/255.0f;
-				float lmin = left[2] + (right[2] - left[2])*(link->bmin*s);
-				float lmax = left[2] + (right[2] - left[2])*(link->bmax*s);
-				if (lmin > lmax) dtSwap(lmin, lmax);
-				
-				// Find Z intersection.
-				float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
-				if (z >= lmin && z <= lmax)
-				{
-					nextRef = link->ref;
-					break;
-				}
-			}
-			else if (link->side == 2 || link->side == 6)
-			{
-				// Calculate link size.
-				const float s = 1.0f/255.0f;
-				float lmin = left[0] + (right[0] - left[0])*(link->bmin*s);
-				float lmax = left[0] + (right[0] - left[0])*(link->bmax*s);
-				if (lmin > lmax) dtSwap(lmin, lmax);
-				
-				// Find X intersection.
-				float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
-				if (x >= lmin && x <= lmax)
-				{
-					nextRef = link->ref;
-					break;
-				}
-			}
-		}
-		
-		if (!nextRef)
-		{
-			// No neighbour, we hit a wall.
-			
-			// Calculate hit normal.
-			const int a = segMax;
-			const int b = segMax+1 < nv ? segMax+1 : 0;
-			const float* va = &verts[a*3];
-			const float* vb = &verts[b*3];
-			const float dx = vb[0] - va[0];
-			const float dz = vb[2] - va[2];
-			hitNormal[0] = dz;
-			hitNormal[1] = 0;
-			hitNormal[2] = -dx;
-			dtVnormalize(hitNormal);
-			
-			if (pathCount)
-				*pathCount = n;
-			return DT_SUCCESS;
-		}
-		
-		// No hit, advance to neighbour polygon.
-		curRef = nextRef;
-	}
-	
-	if (pathCount)
-		*pathCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
-											   const dtQueryFilter* filter,
-											   dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
-											   int* resultCount, const int maxResult) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_nodePool);
-	dtAssert(m_openList);
-
-	*resultCount = 0;
-	
-	// Validate input
-	if (!startRef) return DT_FAILURE;
-	if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE;
-	
-	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);
-	
-	int n = 0;
-	if (n < maxResult)
-	{
-		if (resultRef)
-			resultRef[n] = startNode->id;
-		if (resultParent)
-			resultParent[n] = 0;
-		if (resultCost)
-			resultCost[n] = 0;
-		++n;
-	}
-	
-	const float radiusSqr = dtSqr(radius);
-	
-	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);
-		
-		// 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)
-				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 &= ~DT_NODE_CLOSED;
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
-			neighbourNode->total = total;
-			
-			if (neighbourNode->flags & DT_NODE_OPEN)
-			{
-				m_openList->modify(neighbourNode);
-			}
-			else
-			{
-				if (n < maxResult)
-				{
-					if (resultRef)
-						resultRef[n] = neighbourNode->id;
-					if (resultParent)
-						resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id;
-					if (resultCost)
-						resultCost[n] = neighbourNode->total;
-					++n;
-				}
-				neighbourNode->flags = DT_NODE_OPEN;
-				m_openList->push(neighbourNode);
-			}
-		}
-	}
-	
-	*resultCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
-											  const dtQueryFilter* filter,
-											  dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
-											  int* resultCount, const int maxResult) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_nodePool);
-	dtAssert(m_openList);
-	
-	*resultCount = 0;
-	
-	// Validate input
-	if (!startRef) return DT_FAILURE;
-	if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE;
-	
-	m_nodePool->clear();
-	m_openList->clear();
-	
-	float centerPos[3] = {0,0,0};
-	for (int i = 0; i < nverts; ++i)
-		dtVadd(centerPos,centerPos,&verts[i*3]);
-	dtVscale(centerPos,centerPos,1.0f/nverts);
-
-	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);
-	
-	int n = 0;
-	if (n < maxResult)
-	{
-		if (resultRef)
-			resultRef[n] = startNode->id;
-		if (resultParent)
-			resultParent[n] = 0;
-		if (resultCost)
-			resultCost[n] = 0;
-		++n;
-	}
-	
-	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);
-		
-		// 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 poly is not touching the edge to the next polygon, skip the connection it.
-			float tmin, tmax;
-			int segMin, segMax;
-			if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax))
-				continue;
-			if (tmin > 1.0f || tmax < 0.0f)
-				continue;
-			
-			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
-			if (!neighbourNode)
-				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 &= ~DT_NODE_CLOSED;
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
-			neighbourNode->total = total;
-			
-			if (neighbourNode->flags & DT_NODE_OPEN)
-			{
-				m_openList->modify(neighbourNode);
-			}
-			else
-			{
-				if (n < maxResult)
-				{
-					if (resultRef)
-						resultRef[n] = neighbourNode->id;
-					if (resultParent)
-						resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id;
-					if (resultCost)
-						resultCost[n] = neighbourNode->total;
-					++n;
-				}
-				neighbourNode->flags = DT_NODE_OPEN;
-				m_openList->push(neighbourNode);
-			}
-		}
-	}
-	
-	*resultCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
-												const dtQueryFilter* filter,
-												dtPolyRef* resultRef, dtPolyRef* resultParent,
-												int* resultCount, const int maxResult) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_tinyNodePool);
-	
-	*resultCount = 0;
-
-	// Validate input
-	if (!startRef) return DT_FAILURE;
-	if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE;
-	
-	static const int MAX_STACK = 48;
-	dtNode* stack[MAX_STACK];
-	int nstack = 0;
-	
-	m_tinyNodePool->clear();
-	
-	dtNode* startNode = m_tinyNodePool->getNode(startRef);
-	startNode->pidx = 0;
-	startNode->id = startRef;
-	startNode->flags = DT_NODE_CLOSED;
-	stack[nstack++] = startNode;
-	
-	const float radiusSqr = dtSqr(radius);
-	
-	float pa[DT_VERTS_PER_POLYGON*3];
-	float pb[DT_VERTS_PER_POLYGON*3];
-	
-	int n = 0;
-	if (n < maxResult)
-	{
-		resultRef[n] = startNode->id;
-		if (resultParent)
-			resultParent[n] = 0;
-		++n;
-	}
-	
-	while (nstack)
-	{
-		// Pop front.
-		dtNode* curNode = stack[0];
-		for (int i = 0; i < nstack-1; ++i)
-			stack[i] = stack[i+1];
-		nstack--;
-		
-		// Get poly and tile.
-		// The API input has been cheked already, skip checking internal data.
-		const dtPolyRef curRef = curNode->id;
-		const dtMeshTile* curTile = 0;
-		const dtPoly* curPoly = 0;
-		m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
-		
-		for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
-		{
-			const dtLink* link = &curTile->links[i];
-			dtPolyRef neighbourRef = link->ref;
-			// Skip invalid neighbours.
-			if (!neighbourRef)
-				continue;
-			
-			// Skip if cannot alloca more nodes.
-			dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
-			if (!neighbourNode)
-				continue;
-			// Skip visited.
-			if (neighbourNode->flags & DT_NODE_CLOSED)
-				continue;
-			
-			// Expand to neighbour
-			const dtMeshTile* neighbourTile = 0;
-			const dtPoly* neighbourPoly = 0;
-			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-			
-			// Skip off-mesh connections.
-			if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-				continue;
-			
-			// 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(curRef, curPoly, curTile, 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;
-			
-			// Mark node visited, this is done before the overlap test so that
-			// we will not visit the poly again if the test fails.
-			neighbourNode->flags |= DT_NODE_CLOSED;
-			neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
-			
-			// Check that the polygon does not collide with existing polygons.
-			
-			// Collect vertices of the neighbour poly.
-			const int npa = neighbourPoly->vertCount;
-			for (int k = 0; k < npa; ++k)
-				dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]);
-			
-			bool overlap = false;
-			for (int j = 0; j < n; ++j)
-			{
-				dtPolyRef pastRef = resultRef[j];
-				
-				// Connected polys do not overlap.
-				bool connected = false;
-				for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
-				{
-					if (curTile->links[k].ref == pastRef)
-					{
-						connected = true;
-						break;
-					}
-				}
-				if (connected)
-					continue;
-				
-				// Potentially overlapping.
-				const dtMeshTile* pastTile = 0;
-				const dtPoly* pastPoly = 0;
-				m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);
-				
-				// Get vertices and test overlap
-				const int npb = pastPoly->vertCount;
-				for (int k = 0; k < npb; ++k)
-					dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]);
-				
-				if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
-				{
-					overlap = true;
-					break;
-				}
-			}
-			if (overlap)
-				continue;
-			
-			// This poly is fine, store and advance to the poly.
-			if (n < maxResult)
-			{
-				resultRef[n] = neighbourRef;
-				if (resultParent)
-					resultParent[n] = curRef;
-				++n;
-			}
-			
-			if (nstack < MAX_STACK)
-			{
-				stack[nstack++] = neighbourNode;
-			}
-		}
-	}
-	
-	*resultCount = n;
-	
-	return DT_SUCCESS;
-}
-
-
-struct dtSegInterval
-{
-	short tmin, tmax;
-};
-
-static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
-						   const short tmin, const short tmax)
-{
-	if (nints+1 > maxInts) return;
-	// Find insertion point.
-	int idx = 0;
-	while (idx < nints)
-	{
-		if (tmax <= ints[idx].tmin)
-			break;
-		idx++;
-	}
-	// Move current results.
-	if (nints-idx)
-		memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
-	// Store
-	ints[idx].tmin = tmin;
-	ints[idx].tmax = tmax;
-	nints++;
-}
-
-dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
-											 float* segments, int* segmentCount, const int maxSegments) const
-{
-	dtAssert(m_nav);
-	
-	*segmentCount = 0;
-	
-	const dtMeshTile* tile = 0;
-	const dtPoly* poly = 0;
-	if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS)
-		return DT_FAILURE;
-	
-	int n = 0;
-	static const int MAX_INTERVAL = 16;
-	dtSegInterval ints[MAX_INTERVAL];
-	int nints;
-	
-	for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
-	{
-		// Skip non-solid edges.
-		nints = 0;
-		if (poly->neis[j] & DT_EXT_LINK)
-		{
-			// Tile border.
-			for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
-			{
-				const dtLink* link = &tile->links[k];
-				if (link->edge == j)
-				{
-					if (link->ref != 0)
-					{
-						const dtMeshTile* neiTile = 0;
-						const dtPoly* neiPoly = 0;
-						m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
-						if (filter->passFilter(link->ref, neiTile, neiPoly))
-						{
-							insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax);
-						}
-					}
-				}
-			}
-		}
-		else if (poly->neis[j])
-		{
-			// 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]))
-				continue;
-		}
-		
-		// Add sentinels
-		insertInterval(ints, nints, MAX_INTERVAL, -1, 0);
-		insertInterval(ints, nints, MAX_INTERVAL, 255, 256);
-		
-		// Store segment.
-		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)
-			{
-				if (n < maxSegments)
-				{
-					float* seg = &segments[n*6];
-					n++;
-					dtVcopy(seg+0, vj);
-					dtVcopy(seg+3, vi);
-				}
-			}
-			else
-			{
-				const float tmin = imin/255.0f; 
-				const float tmax = imax/255.0f; 
-				if (n < maxSegments)
-				{
-					float* seg = &segments[n*6];
-					n++;
-					dtVlerp(seg+0, vj,vi, tmin);
-					dtVlerp(seg+3, vj,vi, tmax);
-				}
-			}
-		}
-	}
-	
-	*segmentCount = n;
-	
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
-											const dtQueryFilter* filter,
-											float* hitDist, float* hitPos, float* hitNormal) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_nodePool);
-	dtAssert(m_openList);
-	
-	// Validate input
-	if (!startRef) return DT_FAILURE;
-	if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE;
-	
-	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);
-	
-	float radiusSqr = dtSqr(maxRadius);
-	
-	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);
-		
-		// 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);
-		
-		// Hit test walls.
-		for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++)
-		{
-			// Skip non-solid edges.
-			if (bestPoly->neis[j] & DT_EXT_LINK)
-			{
-				// Tile border.
-				bool solid = true;
-				for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next)
-				{
-					const dtLink* link = &bestTile->links[k];
-					if (link->edge == j)
-					{
-						if (link->ref != 0)
-						{
-							const dtMeshTile* neiTile = 0;
-							const dtPoly* neiPoly = 0;
-							m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
-							if (filter->passFilter(link->ref, neiTile, neiPoly))
-								solid = false;
-						}
-						break;
-					}
-				}
-				if (!solid) continue;
-			}
-			else if (bestPoly->neis[j])
-			{
-				// Internal edge
-				const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1);
-				const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx;
-				if (filter->passFilter(ref, bestTile, &bestTile->polys[idx]))
-					continue;
-			}
-			
-			// Calc distance to the edge.
-			const float* vj = &bestTile->verts[bestPoly->verts[j]*3];
-			const float* vi = &bestTile->verts[bestPoly->verts[i]*3];
-			float tseg;
-			float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);
-			
-			// Edge is too far, skip.
-			if (distSqr > radiusSqr)
-				continue;
-			
-			// Hit wall, update radius.
-			radiusSqr = distSqr;
-			// Calculate hit pos.
-			hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
-			hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
-			hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
-		}
-		
-		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);
-			
-			// Skip off-mesh connections.
-			if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-				continue;
-			
-			// Calc distance to the edge.
-			const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3];
-			const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3];
-			float tseg;
-			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
-			
-			// If the circle is not touching the next polygon, skip it.
-			if (distSqr > radiusSqr)
-				continue;
-			
-			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
-				continue;
-
-			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
-			if (!neighbourNode)
-				continue;
-			
-			if (neighbourNode->flags & DT_NODE_CLOSED)
-				continue;
-			
-			// Cost
-			if (neighbourNode->flags == 0)
-			{
-				getEdgeMidPoint(bestRef, bestPoly, bestTile,
-								neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
-			}
-			
-			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 &= ~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);
-			}
-		}
-	}
-	
-	// Calc hit normal.
-	dtVsub(hitNormal, centerPos, hitPos);
-	dtVnormalize(hitNormal);
-	
-	*hitDist = sqrtf(radiusSqr);
-	
-	return DT_SUCCESS;
-}
-
-bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
-{
-	if (!m_nodePool) return false;
-	const dtNode* node = m_nodePool->findNode(ref);
-	return node && node->flags & DT_NODE_CLOSED;
-}