//
// Copyright (c) 2009 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 <stdio.h>
#include "DetourNavMesh.h"
#include "DetourNode.h"
#include "DetourCommon.h"


inline int opposite(int side) { return (side+4) & 0x7; }

inline bool overlapBoxes(const float* amin, const float* amax,
						 const float* bmin, const float* bmax)
{
	bool overlap = true;
	overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
	overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
	overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
	return overlap;
}

inline bool overlapRects(const float* amin, const float* amax,
						 const float* bmin, const float* bmax)
{
	bool overlap = true;
	overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
	overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
	return overlap;
}

static void calcRect(const float* va, const float* vb,
					 float* bmin, float* bmax,
					 int side, float padx, float pady)
{
	if (side == 0 || side == 4)
	{
		bmin[0] = min(va[2],vb[2]) + padx;
		bmin[1] = min(va[1],vb[1]) - pady;
		bmax[0] = max(va[2],vb[2]) - padx;
		bmax[1] = max(va[1],vb[1]) + pady;
	}
	else if (side == 2 || side == 6)
	{
		bmin[0] = min(va[0],vb[0]) + padx;
		bmin[1] = min(va[1],vb[1]) - pady;
		bmax[0] = max(va[0],vb[0]) - padx;
		bmax[1] = max(va[1],vb[1]) + pady;
	}
}

inline int computeTileHash(int x, int y, const int mask)
{
	const unsigned int h1 = 0x8da6b343; // Large multiplicative constants;
	const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes
	unsigned int n = h1 * x + h2 * y;
	return (int)(n & mask);
}

inline unsigned int allocLink(dtMeshTile* tile)
{
	if (tile->linksFreeList == DT_NULL_LINK)
		return DT_NULL_LINK;
	unsigned int link = tile->linksFreeList;
	tile->linksFreeList = tile->links[link].next;
	return link;
}

inline void freeLink(dtMeshTile* tile, unsigned int link)
{
	tile->links[link].next = tile->linksFreeList;
	tile->linksFreeList = link;
}


inline bool passFilter(dtQueryFilter* filter, unsigned short flags)
{
	return (flags & filter->includeFlags) != 0 && (flags & filter->excludeFlags) == 0;
}



//////////////////////////////////////////////////////////////////////////////////////////
dtNavMesh::dtNavMesh() :
	m_tileWidth(0),
	m_tileHeight(0),
	m_maxTiles(0),
	m_tileLutSize(0),
	m_tileLutMask(0),
	m_posLookup(0),
	m_nextFree(0),
	m_tiles(0),
	m_saltBits(0),
	m_tileBits(0),
	m_polyBits(0),
	m_nodePool(0),
	m_openList(0)
{
	m_orig[0] = 0;
	m_orig[1] = 0;
	m_orig[2] = 0;
	
	for (int i = 0; i < DT_MAX_AREAS; ++i)
		m_areaCost[i] = 1.0f;
}

dtNavMesh::~dtNavMesh()
{
	for (int i = 0; i < m_maxTiles; ++i)
	{
		if (m_tiles[i].data && m_tiles[i].ownsData)
		{
			delete [] m_tiles[i].data;
			m_tiles[i].data = 0;
			m_tiles[i].dataSize = 0;
		}
	}
	delete m_nodePool;
	delete m_openList;
	delete [] m_posLookup;
	delete [] m_tiles;
}
		
bool dtNavMesh::init(const float* orig, float tileWidth, float tileHeight,
					 int maxTiles, int maxPolys, int maxNodes)
{
	vcopy(m_orig, orig);
	m_tileWidth = tileWidth;
	m_tileHeight = tileHeight;
	
	// Init tiles
	m_maxTiles = maxTiles;
	m_tileLutSize = nextPow2(maxTiles/4);
	if (!m_tileLutSize) m_tileLutSize = 1;
	m_tileLutMask = m_tileLutSize-1;
	
	m_tiles = new dtMeshTile[m_maxTiles];
	if (!m_tiles)
		return false;
	m_posLookup = new dtMeshTile*[m_tileLutSize];
	if (!m_posLookup)
		return false;
	memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles);
	memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize);
	m_nextFree = 0;
	for (int i = m_maxTiles-1; i >= 0; --i)
	{
		m_tiles[i].next = m_nextFree;
		m_nextFree = &m_tiles[i];
	}

	if (!m_nodePool)
	{
		m_nodePool = new dtNodePool(maxNodes, nextPow2(maxNodes/4));
		if (!m_nodePool)
			return false;
	}
	
	if (!m_openList)
	{
		m_openList = new dtNodeQueue(maxNodes);
		if (!m_openList)
			return false;
	}
	
	// Init ID generator values.
	m_tileBits = max((unsigned int)1,ilog2(nextPow2((unsigned int)maxTiles)));
	m_polyBits = max((unsigned int)1,ilog2(nextPow2((unsigned int)maxPolys)));
	m_saltBits = 32 - m_tileBits - m_polyBits;
	if (m_saltBits < 10)
		return false;
	
	return true;
}

bool dtNavMesh::init(unsigned char* data, int dataSize, bool ownsData, int maxNodes)
{
	// Make sure the data is in right format.
	dtMeshHeader* header = (dtMeshHeader*)data;
	if (header->magic != DT_NAVMESH_MAGIC)
		return false;
	if (header->version != DT_NAVMESH_VERSION)
		return false;

	const float w = header->bmax[0] - header->bmin[0];
	const float h = header->bmax[2] - header->bmin[2];
	if (!init(header->bmin, w, h, 1, header->polyCount, maxNodes))
		return false;

	return addTileAt(0,0, data, dataSize, ownsData);
}

//////////////////////////////////////////////////////////////////////////////////////////
int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
								   dtMeshTile* tile, int side,
								   dtPolyRef* con, float* conarea, int maxcon)
{
	if (!tile) return 0;
	
	float amin[2], amax[2];
	calcRect(va,vb, amin,amax, side, 0.01f, tile->header->walkableClimb);

	// Remove links pointing to 'side' and compact the links array. 
	float bmin[2], bmax[2];
	unsigned short m = DT_EXT_LINK | (unsigned short)side;
	int n = 0;
	
	dtPolyRef base = getTileId(tile);
	
	for (int i = 0; i < tile->header->polyCount; ++i)
	{
		dtPoly* poly = &tile->polys[i];
		const int nv = poly->vertCount;
		for (int j = 0; j < nv; ++j)
		{
			// Skip edges which do not point to the right side.
			if (poly->neis[j] != m) continue;
			// Check if the segments touch.
			const float* vc = &tile->verts[poly->verts[j]*3];
			const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3];
			calcRect(vc,vd, bmin,bmax, side, 0.01f, tile->header->walkableClimb);
			if (!overlapRects(amin,amax, bmin,bmax)) continue;
			// Add return value.
			if (n < maxcon)
			{
				conarea[n*2+0] = max(amin[0], bmin[0]);
				conarea[n*2+1] = min(amax[0], bmax[0]);
				con[n] = base | (unsigned int)i;
				n++;
			}
			break;
		}
	}
	return n;
}

void dtNavMesh::unconnectExtLinks(dtMeshTile* tile, int side)
{
	if (!tile) return;

	for (int i = 0; i < tile->header->polyCount; ++i)
	{
		dtPoly* poly = &tile->polys[i];
		unsigned int j = poly->firstLink;
		unsigned int pj = DT_NULL_LINK;
		while (j != DT_NULL_LINK)
		{
			if (tile->links[j].side == side)
			{
				// Revove link.
				unsigned int nj = tile->links[j].next;
				if (pj == DT_NULL_LINK)
					poly->firstLink = nj;
				else
					tile->links[pj].next = nj;
				freeLink(tile, j);
				j = nj;
			}
			else
			{
				// Advance
				pj = j;
				j = tile->links[j].next;
			}
		}
	}
}

void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side)
{
	if (!tile) return;
	
	// Connect border links.
	for (int i = 0; i < tile->header->polyCount; ++i)
	{
		dtPoly* poly = &tile->polys[i];

		// Create new links.
		unsigned short m = DT_EXT_LINK | (unsigned short)side;
		const int nv = poly->vertCount;
		for (int j = 0; j < nv; ++j)
		{
			// Skip edges which do not point to the right side.
			if (poly->neis[j] != m) 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, opposite(side), nei,neia,4);
			for (int k = 0; k < nnei; ++k)
			{
				unsigned int idx = allocLink(tile);
				if (idx != DT_NULL_LINK)
				{
					dtLink* link = &tile->links[idx];
					link->ref = nei[k];
					link->edge = (unsigned char)j;
					link->side = (unsigned char)side;
					
					link->next = poly->firstLink;
					poly->firstLink = idx;

					// Compress portal limits to a byte value.
					if (side == 0 || side == 4)
					{
						const float lmin = min(va[2], vb[2]);
						const float lmax = max(va[2], vb[2]);
						link->bmin = (unsigned char)(clamp((neia[k*2+0]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f);
						link->bmax = (unsigned char)(clamp((neia[k*2+1]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f);
					}
					else if (side == 2 || side == 6)
					{
						const float lmin = min(va[0], vb[0]);
						const float lmax = max(va[0], vb[0]);
						link->bmin = (unsigned char)(clamp((neia[k*2+0]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f);
						link->bmax = (unsigned char)(clamp((neia[k*2+1]-lmin)/(lmax-lmin), 0.0f, 1.0f)*255.0f);
					}
				}
			}
		}
	}
}

void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side)
{
	if (!tile) return;
	
	// Connect off-mesh links.
	// We are interested on links which land from target tile to this tile.
	const unsigned char oppositeSide = (unsigned char)opposite(side);
	dtQueryFilter defaultFilter;
	
	for (int i = 0; i < target->header->offMeshConCount; ++i)
	{
		dtOffMeshConnection* targetCon = &target->offMeshCons[i];
		if (targetCon->side != oppositeSide)
			continue;
		
		dtPoly* targetPoly = &target->polys[targetCon->poly];
		
		const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad };
		
		// Find polygon to connect to.
		const float* p = &targetCon->pos[3];
		float nearestPt[3];
		dtPolyRef ref = findNearestPolyInTile(tile, p, ext, &defaultFilter, nearestPt);
		if (!ref) continue;
		// findNearestPoly may return too optimistic results, further check to make sure. 
		if (sqr(nearestPt[0]-p[0])+sqr(nearestPt[2]-p[2]) > sqr(targetCon->rad))
			continue;
		// Make sure the location is on current mesh.
		float* v = &target->verts[targetPoly->verts[1]*3];
		vcopy(v, nearestPt);
				
		// Link off-mesh connection to target poly.
		unsigned int idx = allocLink(target);
		if (idx != DT_NULL_LINK)
		{
			dtLink* link = &target->links[idx];
			link->ref = ref;
			link->edge = (unsigned char)1;
			link->side = oppositeSide;
			link->bmin = link->bmax = 0;
			// Add to linked list.
			link->next = targetPoly->firstLink;
			targetPoly->firstLink = idx;
		}
		
		// 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 short landPolyIdx = decodePolyIdPoly(ref);
				dtPoly* landPoly = &tile->polys[landPolyIdx];
				dtLink* link = &tile->links[idx];
				link->ref = getTileId(target) | (unsigned int)(targetCon->poly);
				link->edge = 0;
				link->side = side;
				link->bmin = link->bmax = 0;
				// Add to linked list.
				link->next = landPoly->firstLink;
				landPoly->firstLink = idx;
			}
		}
	}

}

void dtNavMesh::connectIntLinks(dtMeshTile* tile)
{
	if (!tile) return;

	dtPolyRef base = getTileId(tile);

	for (int i = 0; i < tile->header->polyCount; ++i)
	{
		dtPoly* poly = &tile->polys[i];
		poly->firstLink = DT_NULL_LINK;

		if (poly->type == DT_POLYTYPE_OFFMESH_CONNECTION)
			continue;
			
		// Build edge links backwards so that the links will be
		// in the linked list from lowest index to highest.
		for (int j = poly->vertCount-1; j >= 0; --j)
		{
			// Skip hard and non-internal edges.
			if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue;

			unsigned int idx = allocLink(tile);
			if (idx != DT_NULL_LINK)
			{
				dtLink* link = &tile->links[idx];
				link->ref = base | (unsigned int)(poly->neis[j]-1);
				link->edge = (unsigned char)j;
				link->side = 0xff;
				link->bmin = link->bmax = 0;
				// Add to linked list.
				link->next = poly->firstLink;
				poly->firstLink = idx;
			}
		}			
	}
}

void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile)
{
	if (!tile) return;
	
	dtPolyRef base = getTileId(tile);
	
	// Find Off-mesh connection end points.
	for (int i = 0; i < tile->header->offMeshConCount; ++i)
	{
		dtOffMeshConnection* con = &tile->offMeshCons[i];
		dtPoly* poly = &tile->polys[con->poly];
		dtQueryFilter defaultFilter;
	
		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;

			if (side == 0xff)
			{
				// Find polygon to connect to.
				const float* p = &con->pos[j*3];
				float nearestPt[3];
				dtPolyRef ref = findNearestPolyInTile(tile, p, ext, &defaultFilter, nearestPt);
				if (!ref) continue;
				// findNearestPoly may return too optimistic results, further check to make sure. 
				if (sqr(nearestPt[0]-p[0])+sqr(nearestPt[2]-p[2]) > sqr(con->rad))
					continue;
				// Make sure the location is on current mesh.
				float* v = &tile->verts[poly->verts[j]*3];
				vcopy(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;
				}

				// 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)
					{
						unsigned short landPolyIdx = decodePolyIdPoly(ref);
						dtPoly* landPoly = &tile->polys[landPolyIdx];
						dtLink* link = &tile->links[idx];
						link->ref = base | (unsigned int)(con->poly);
						link->edge = 0;
						link->side = 0xff;
						link->bmin = link->bmax = 0;
						// Add to linked list.
						link->next = landPoly->firstLink;
						landPoly->firstLink = idx;
					}
				}
				
			}
		}
	}
}

bool dtNavMesh::addTileAt(int x, int y, unsigned char* data, int dataSize, bool ownsData)
{
	if (getTileAt(x,y))
		return false;
	// Make sure there is enough space for new tile.
	if (!m_nextFree)
		return false;
	// Make sure the data is in right format.
	dtMeshHeader* header = (dtMeshHeader*)data;
	if (header->magic != DT_NAVMESH_MAGIC)
		return false;
	if (header->version != DT_NAVMESH_VERSION)
		return false;
		
	// Allocate a tile.
	dtMeshTile* tile = m_nextFree;
	m_nextFree = tile->next;
	tile->next = 0;

	// Insert tile into the position lut.
	int h = computeTileHash(x,y,m_tileLutMask);
	tile->next = m_posLookup[h];
	m_posLookup[h] = tile;
	
	// Patch header pointers.
	const int headerSize = align4(sizeof(dtMeshHeader));
	const int vertsSize = align4(sizeof(float)*3*header->vertCount);
	const int polysSize = align4(sizeof(dtPoly)*header->polyCount);
	const int linksSize = align4(sizeof(dtLink)*(header->maxLinkCount));
	const int detailMeshesSize = align4(sizeof(dtPolyDetail)*header->detailMeshCount);
	const int detailVertsSize = align4(sizeof(float)*3*header->detailVertCount);
	const int detailTrisSize = align4(sizeof(unsigned char)*4*header->detailTriCount);
	const int bvtreeSize = align4(sizeof(dtBVNode)*header->bvNodeCount);
	const int offMeshLinksSize = align4(sizeof(dtOffMeshConnection)*header->offMeshConCount);
	
	unsigned char* d = data + headerSize;
	tile->verts = (float*)d; d += vertsSize;
	tile->polys = (dtPoly*)d; d += polysSize;
	tile->links = (dtLink*)d; d += linksSize;
	tile->detailMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
	tile->detailVerts = (float*)d; d += detailVertsSize;
	tile->detailTris = (unsigned char*)d; d += detailTrisSize;
	tile->bvTree = (dtBVNode*)d; d += bvtreeSize;
	tile->offMeshCons = (dtOffMeshConnection*)d; d += offMeshLinksSize;

	// Build links freelist
	tile->linksFreeList = 0;
	tile->links[header->maxLinkCount-1].next = DT_NULL_LINK;
	for (int i = 0; i < header->maxLinkCount-1; ++i)
		tile->links[i].next = i+1;

	// Init tile.
	tile->header = header;
	tile->x = x;
	tile->y = y;
	tile->data = data;
	tile->dataSize = dataSize;
	tile->ownsData = ownsData;

	connectIntLinks(tile);
	connectIntOffMeshLinks(tile);

	// Create connections connections.
	for (int i = 0; i < 8; ++i)
	{
		dtMeshTile* nei = getNeighbourTileAt(x,y,i);
		if (nei)
		{
			connectExtLinks(tile, nei, i);
			connectExtLinks(nei, tile, opposite(i));
			connectExtOffMeshLinks(tile, nei, i);
			connectExtOffMeshLinks(nei, tile, opposite(i));
		}
	}
	
	return true;
}

dtMeshTile* dtNavMesh::getTileAt(int x, int y)
{
	// Find tile based on hash.
	int h = computeTileHash(x,y,m_tileLutMask);
	dtMeshTile* tile = m_posLookup[h];
	while (tile)
	{
		if (tile->x == x && tile->y == y)
			return tile;
		tile = tile->next;
	}
	return 0;
}

int dtNavMesh::getMaxTiles() const
{
	return m_maxTiles;
}

dtMeshTile* dtNavMesh::getTile(int i)
{
	return &m_tiles[i];
}

const dtMeshTile* dtNavMesh::getTile(int i) const
{
	return &m_tiles[i];
}

const dtMeshTile* dtNavMesh::getTileByRef(dtPolyRef ref, int* polyIndex) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return 0;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
	if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0;
	if (polyIndex) *polyIndex = (int)ip;
	return &m_tiles[it];
}

dtMeshTile* dtNavMesh::getNeighbourTileAt(int x, int y, int side)
{
	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;
	};
	return getTileAt(x,y);
}

bool dtNavMesh::removeTileAt(int x, int y, unsigned char** data, int* dataSize)
{
	// Remove tile from hash lookup.
	int h = computeTileHash(x,y,m_tileLutMask);
	dtMeshTile* prev = 0;
	dtMeshTile* tile = m_posLookup[h];
	while (tile)
	{
		if (tile->x == x && tile->y == y)
		{
			if (prev)
				prev->next = tile->next;
			else
				m_posLookup[h] = tile->next;
			break;
		}
		prev = tile;
		tile = tile->next;
	}
	if (!tile)
		return false;
	
	// Remove connections to neighbour tiles.
	for (int i = 0; i < 8; ++i)
	{
		dtMeshTile* nei = getNeighbourTileAt(x,y,i);
		if (!nei) continue;
		unconnectExtLinks(nei, opposite(i));
	}
	
	
	// Reset tile.
	if (tile->ownsData)
	{
		// Owns data
		delete [] tile->data;
		tile->data = 0;
		tile->dataSize = 0;
		if (data) *data = 0;
		if (dataSize) *dataSize = 0;
	}
	else
	{
		if (data) *data = tile->data;
		if (dataSize) *dataSize = tile->dataSize;
	}
	tile->header = 0;
	tile->linksFreeList = 0;
	tile->polys = 0;
	tile->verts = 0;
	tile->links = 0;
	tile->detailMeshes = 0;
	tile->detailVerts = 0;
	tile->detailTris = 0;
	tile->bvTree = 0;
	tile->offMeshCons = 0;
	
	tile->x = tile->y = 0;
	tile->salt++;

	// Add to free list.
	tile->next = m_nextFree;
	m_nextFree = tile;

	return true;
}

dtPolyRef dtNavMesh::getTileId(const dtMeshTile* tile) const
{
	if (!tile) return 0;
	const unsigned int it = tile - m_tiles;
	return encodePolyId(tile->salt, it, 0);
}

//////////////////////////////////////////////////////////////////////////////////////////
bool dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return false;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
	const dtMeshHeader* header = m_tiles[it].header;
	if (ip >= (unsigned int)header->polyCount) return false;
	
	return closestPointOnPolyInTile(&m_tiles[it], ip, pos, closest);
}

bool dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, const float* pos, float* closest) const
{
	const dtPoly* poly = &tile->polys[ip];
	
	float closestDistSqr = FLT_MAX;
	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 pt[3];
		closestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
		float d = vdistSqr(pos, pt);
		if (d < closestDistSqr)
		{
			vcopy(closest, pt);
			closestDistSqr = d;
		}
	}
	
	return true;
}

bool dtNavMesh::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return false;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
	const dtMeshTile* tile = &m_tiles[it];
	
	if (ip >= (unsigned int)tile->header->polyCount) return false;
	const dtPoly* poly = &tile->polys[ip];

	// 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)
	{
		vcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
		nv++;
	}		
	
	bool inside = distancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
	if (inside)
	{
		// Point is inside the polygon, return the point.
		vcopy(closest, pos);
	}
	else
	{
		// Point is outside the polygon, clamp 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];
		vlerp(closest, va, vb, edget[imin]);
	}

	return true;
}

// Returns start and end location of an off-mesh link polygon.
bool dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const
{
	unsigned int salt, it, ip;

	// Get current polygon
	decodePolyId(polyRef, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return false;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
	const dtMeshTile* tile = &m_tiles[it];
	if (ip >= (unsigned int)tile->header->polyCount) return false;
	const dtPoly* poly = &tile->polys[ip];

	// Make sure that the current poly is indeed off-mesh link.
	if (poly->type != DT_POLYTYPE_OFFMESH_CONNECTION)
		return false;

	// Figure out which way to hand out the vertices.
	int idx0 = 0, idx1 = 1;
	
	// Find link that points to first vertex.
	for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
	{
		if (tile->links[i].edge == 0)
		{
			if (tile->links[i].ref != prevRef)
			{
				idx0 = 1;
				idx1 = 0;
			}
			break;
		}
	}
	
	vcopy(startPos, &tile->verts[poly->verts[idx0]*3]);
	vcopy(endPos, &tile->verts[poly->verts[idx1]*3]);

	return true;
}


bool dtNavMesh::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return false;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
	const dtMeshTile* tile = &m_tiles[it];
	
	if (ip >= (unsigned int)tile->header->polyCount) return false;
	const dtPoly* poly = &tile->polys[ip];
	
	if (poly->type == 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 = vdist(pos, v0);
		const float d1 = vdist(pos, v1);
		const float u = d0 / (d0+d1);
		if (height)
			*height = v0[1] + (v1[1] - v0[1]) * u;
		return true;
	}
	else
	{
		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 (closestHeightPointTriangle(pos, v[0], v[1], v[2], h))
			{
				if (height)
					*height = h;
				return true;
			}
		}
	}
	
	return false;
}

void dtNavMesh::setAreaCost(const int area, float cost)
{
	if (area >= 0 && area < DT_MAX_AREAS)
		m_areaCost[area] = cost;
}

float dtNavMesh::getAreaCost(const int area) const
{
	if (area >= 0 && area < DT_MAX_AREAS)
		return m_areaCost[area];
	return -1;
}

dtPolyRef dtNavMesh::findNearestPoly(const float* center, const float* extents, dtQueryFilter* filter, float* nearestPt)
{
	// Get nearby polygons from proximity grid.
	dtPolyRef polys[128];
	int polyCount = queryPolygons(center, extents, 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];
		float closestPtPoly[3];
		if (!closestPointOnPoly(ref, center, closestPtPoly))
			continue;
		float d = vdistSqr(center, closestPtPoly);
		if (d < nearestDistanceSqr)
		{
			if (nearestPt)
				vcopy(nearestPt, closestPtPoly);
			nearestDistanceSqr = d;
			nearest = ref;
		}
	}
	
	return nearest;
}

dtPolyRef dtNavMesh::findNearestPolyInTile(dtMeshTile* tile, const float* center, const float* extents,
										   dtQueryFilter* filter, float* nearestPt)
{
	float bmin[3], bmax[3];
	vsub(bmin, center, extents);
	vadd(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];
		float closestPtPoly[3];
		if (!closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly))
			continue;
		float d = vdistSqr(center, closestPtPoly);
		if (d < nearestDistanceSqr)
		{
			if (nearestPt)
				vcopy(nearestPt, closestPtPoly);
			nearestDistanceSqr = d;
			nearest = ref;
		}
	}
	
	return nearest;
}

int dtNavMesh::queryPolygonsInTile(dtMeshTile* tile, const float* qmin, const float* qmax,
								   dtQueryFilter* filter,
								   dtPolyRef* polys, const int maxPolys)
{
	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];
		// Clamp query box to world box.
		float minx = clamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
		float miny = clamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
		float minz = clamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
		float maxx = clamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
		float maxy = clamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
		float maxz = clamp(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
		dtPolyRef base = getTileId(tile);
		int n = 0;
		while (node < end)
		{
			bool overlap = checkOverlapBox(bmin, bmax, node->bmin, node->bmax);
			bool isLeafNode = node->i >= 0;
			
			if (isLeafNode && overlap)
			{
				if (passFilter(filter, tile->polys[node->i].flags))
				{
					if (n < maxPolys)
						polys[n++] = base | (dtPolyRef)node->i;
				}
			}
			
			if (overlap || isLeafNode)
				node++;
			else
			{
				const int escapeIndex = -node->i;
				node += escapeIndex;
			}
		}
	
		return n;
	}
	else
	{
		float bmin[3], bmax[3];
		int n = 0;
		dtPolyRef base = getTileId(tile);
		for (int i = 0; i < tile->header->polyCount; ++i)
		{
			// Calc polygon bounds.
			dtPoly* p = &tile->polys[i];
			const float* v = &tile->verts[p->verts[0]*3];
			vcopy(bmin, v);
			vcopy(bmax, v);
			for (int j = 1; j < p->vertCount; ++j)
			{
				v = &tile->verts[p->verts[j]*3];
				vmin(bmin, v);
				vmax(bmax, v);
			}
			if (overlapBoxes(qmin,qmax, bmin,bmax))
			{
				if (passFilter(filter, p->flags))
				{
					if (n < maxPolys)
						polys[n++] = base | (dtPolyRef)i;
				}
			}
		}
		return n;
	}
}

int dtNavMesh::queryPolygons(const float* center, const float* extents, dtQueryFilter* filter,
							 dtPolyRef* polys, const int maxPolys)
{
	float bmin[3], bmax[3];
	vsub(bmin, center, extents);
	vadd(bmax, center, extents);
	
	// Find tiles the query touches.
	const int minx = (int)((bmin[0]-m_orig[0]) / m_tileWidth);
	const int maxx = (int)((bmax[0]-m_orig[0]) / m_tileWidth);
	const int miny = (int)((bmin[2]-m_orig[2]) / m_tileHeight);
	const int maxy = (int)((bmax[2]-m_orig[2]) / m_tileHeight);

	int n = 0;
	for (int y = miny; y <= maxy; ++y)
	{
		for (int x = minx; x <= maxx; ++x)
		{
			dtMeshTile* tile = getTileAt(x,y);
			if (!tile) continue;
			n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n);
			if (n >= maxPolys) return n;
		}
	}

	return n;
}

int dtNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
						const float* startPos, const float* endPos,
						dtQueryFilter* filter,
						dtPolyRef* path, const int maxPathSize)
{
	if (!startRef || !endRef)
		return 0;
	
	if (!maxPathSize)
		return 0;
	
	if (!getPolyByRef(startRef) || !getPolyByRef(endRef))
		return 0;
	
	if (startRef == endRef)
	{
		path[0] = startRef;
		return 1;
	}
	
	if (!m_nodePool || !m_openList)
		return 0;
		
	m_nodePool->clear();
	m_openList->clear();
	
	static const float H_SCALE = 0.999f;	// Heuristic scale.
	
	dtNode* startNode = m_nodePool->getNode(startRef);
	startNode->pidx = 0;
	startNode->cost = 0;
	startNode->total = vdist(startPos, endPos) * H_SCALE;
	startNode->id = startRef;
	startNode->flags = DT_NODE_OPEN;
	m_openList->push(startNode);
	
	dtNode* lastBestNode = startNode;
	float lastBestNodeCost = startNode->total;

	unsigned int it, ip;
	
	while (!m_openList->empty())
	{
		dtNode* bestNode = m_openList->pop();
		// Remove node from open list and put it in closed list.
		bestNode->flags &= ~DT_NODE_OPEN;
		bestNode->flags |= DT_NODE_CLOSED;

		// Reached the goal, stop searching.
		if (bestNode->id == endRef)
		{
			lastBestNode = bestNode;
			break;
		}

		float previousEdgeMidPoint[3];

		// Get current poly and tile.
		// The API input has been cheked already, skip checking internal data.
		const dtPolyRef bestRef = bestNode->id;
		it = decodePolyIdTile(bestRef);
		ip = decodePolyIdPoly(bestRef);
		const dtMeshTile* bestTile = &m_tiles[it];
		const dtPoly* bestPoly = &bestTile->polys[ip];

		// 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)
		{
			it = decodePolyIdTile(parentRef);
			ip = decodePolyIdPoly(parentRef);
			parentTile = &m_tiles[it];
			parentPoly = &parentTile->polys[ip];

			getEdgeMidPoint(parentRef, parentPoly, parentTile,
							bestRef, bestPoly, bestTile, previousEdgeMidPoint);
		}
		else
		{
			vcopy(previousEdgeMidPoint, startPos);
		}
		
		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 == bestRef)
				continue;

			// Get neighbour poly and tile.
			// The API input has been cheked already, skip checking internal data.
			it = decodePolyIdTile(neighbourRef);
			ip = decodePolyIdPoly(neighbourRef);
			const dtMeshTile* neighbourTile = &m_tiles[it];
			const dtPoly* neighbourPoly = &neighbourTile->polys[ip];

			if (!passFilter(filter, neighbourPoly->flags))
				continue;

			dtNode newNode;
			newNode.pidx = m_nodePool->getNodeIdx(bestNode);
			newNode.id = neighbourRef;

			// Calculate cost.
			float edgeMidPoint[3];
			
			getEdgeMidPoint(bestRef, bestPoly, bestTile,
							neighbourRef, neighbourPoly, neighbourTile, edgeMidPoint);
			
			// Special case for last node.
			float h = 0;
			if (neighbourRef == endRef)
			{
				// Cost
				newNode.cost = bestNode->cost +
								vdist(previousEdgeMidPoint,edgeMidPoint) * m_areaCost[bestPoly->area] +
								vdist(edgeMidPoint, endPos) * m_areaCost[neighbourPoly->area];
				// Heuristic
				h = 0;
			}
			else
			{
				// Cost
				newNode.cost = bestNode->cost +
								vdist(previousEdgeMidPoint,edgeMidPoint) * m_areaCost[bestPoly->area];
				// Heuristic
				h = vdist(edgeMidPoint,endPos)*H_SCALE;
			}
			newNode.total = newNode.cost + h;
			
			dtNode* actualNode = m_nodePool->getNode(newNode.id);
			if (!actualNode)
				continue;

			// The node is already in open list and the new result is worse, skip.
			if ((actualNode->flags & DT_NODE_OPEN) && newNode.total >= actualNode->total)
				continue;
			// The node is already visited and process, and the new result is worse, skip.
			if ((actualNode->flags & DT_NODE_CLOSED) && newNode.total >= actualNode->total)
				continue;

			// Add or update the node.
			actualNode->flags &= ~DT_NODE_CLOSED;
			actualNode->pidx = newNode.pidx;
			actualNode->cost = newNode.cost;
			actualNode->total = newNode.total;

			// Update nearest node to target so far.
			if (h < lastBestNodeCost)
			{
				lastBestNodeCost = h;
				lastBestNode = actualNode;
			}
				
			if (actualNode->flags & DT_NODE_OPEN)
			{
				// Already in open, update node location.
				m_openList->modify(actualNode);
			}
			else
			{
				// Put the node in open list.
				actualNode->flags |= DT_NODE_OPEN;
				m_openList->push(actualNode);
			}
		}
	}
	
	// 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 < maxPathSize);
	
	return n;
}

int dtNavMesh::findStraightPath(const float* startPos, const float* endPos,
								const dtPolyRef* path, const int pathSize,
								float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
								const int maxStraightPathSize)
{
	if (!maxStraightPathSize)
		return 0;
	
	if (!path[0])
		return 0;
	
	int straightPathSize = 0;
	
	// TODO: Should this be callers responsibility?
	float closestStartPos[3];
	if (!closestPointOnPolyBoundary(path[0], startPos, closestStartPos))
		return 0;
	
	// Add start point.
	vcopy(&straightPath[straightPathSize*3], closestStartPos);
	if (straightPathFlags)
		straightPathFlags[straightPathSize] = DT_STRAIGHTPATH_START;
	if (straightPathRefs)
		straightPathRefs[straightPathSize] = path[0];
	straightPathSize++;
	if (straightPathSize >= maxStraightPathSize)
		return straightPathSize;
	
	float closestEndPos[3];
	if (!closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos))
		return 0;
	
	if (pathSize > 1)
	{
		float portalApex[3], portalLeft[3], portalRight[3];
		vcopy(portalApex, closestStartPos);
		vcopy(portalLeft, portalApex);
		vcopy(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))
				{
					if (!closestPointOnPolyBoundary(path[i], endPos, closestEndPos))
						return 0;
					
					vcopy(&straightPath[straightPathSize*3], closestEndPos);
					if (straightPathFlags)
						straightPathFlags[straightPathSize] = 0;
					if (straightPathRefs)
						straightPathRefs[straightPathSize] = path[i];
					straightPathSize++;
					
					return straightPathSize;
				}
				
				// If starting really close the portal, advance.
				if (i == 0)
				{
					float t;
					if (distancePtSegSqr2D(portalApex, left, right, t) < (0.001*0.001f))
						continue;
				}
			}
			else
			{
				// End of the path.
				vcopy(left, closestEndPos);
				vcopy(right, closestEndPos);

				fromType = toType = DT_POLYTYPE_GROUND;
			}
			
			// Right vertex.
			if (vequal(portalApex, portalRight))
			{
				vcopy(portalRight, right);
				rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
				rightPolyType = toType;
				rightIndex = i;
			}
			else
			{
				if (triArea2D(portalApex, portalRight, right) <= 0.0f)
				{
					if (triArea2D(portalApex, portalLeft, right) > 0.0f)
					{
						vcopy(portalRight, right);
						rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
						rightPolyType = toType;
						rightIndex = i;
					}
					else
					{
						vcopy(portalApex, portalLeft);
						apexIndex = leftIndex;
						
						unsigned char flags = (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) ? DT_STRAIGHTPATH_OFFMESH_CONNECTION : 0;
						dtPolyRef ref = leftPolyRef;
						
						if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex))
						{
							vcopy(&straightPath[straightPathSize*3], portalApex);
							if (straightPathFlags)
								straightPathFlags[straightPathSize] = flags;
							if (straightPathRefs)
								straightPathRefs[straightPathSize] = ref;
								
							straightPathSize++;
							if (straightPathSize >= maxStraightPathSize)
								return straightPathSize;
						}
						else
						{
							// The vertices are equal, update flags and poly.
							if (straightPathFlags)
								straightPathFlags[straightPathSize-1] = flags;
							if (straightPathRefs)
								straightPathRefs[straightPathSize-1] = ref;
						}
						
						vcopy(portalLeft, portalApex);
						vcopy(portalRight, portalApex);
						leftIndex = apexIndex;
						rightIndex = apexIndex;
						
						// Restart
						i = apexIndex;
						
						continue;
					}
				}
			}
			
			// Left vertex.
			if (vequal(portalApex, portalLeft))
			{
				vcopy(portalLeft, left);
				leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
				leftPolyType = toType;
				leftIndex = i;
			}
			else
			{
				if (triArea2D(portalApex, portalLeft, left) >= 0.0f)
				{
					if (triArea2D(portalApex, portalRight, left) < 0.0f)
					{
						vcopy(portalLeft, left);
						leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
						leftPolyType = toType;
						leftIndex = i;
					}
					else
					{
						vcopy(portalApex, portalRight);
						apexIndex = rightIndex;

						unsigned char flags = (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) ? DT_STRAIGHTPATH_OFFMESH_CONNECTION : 0;
						dtPolyRef ref = rightPolyRef;
						
						if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex))
						{
							vcopy(&straightPath[straightPathSize*3], portalApex);
							if (straightPathFlags)
								straightPathFlags[straightPathSize] = flags;
							if (straightPathRefs)
								straightPathRefs[straightPathSize] = ref;
							
							straightPathSize++;
							if (straightPathSize >= maxStraightPathSize)
								return straightPathSize;
						}
						else
						{
							// The vertices are equal, update flags and poly.
							if (straightPathFlags)
								straightPathFlags[straightPathSize-1] = flags;
							if (straightPathRefs)
								straightPathRefs[straightPathSize-1] = ref;
						}
						
						vcopy(portalLeft, portalApex);
						vcopy(portalRight, portalApex);
						leftIndex = apexIndex;
						rightIndex = apexIndex;
						
						// Restart
						i = apexIndex;
						
						continue;
					}
				}
			}
		}
	}
	
	// Add end point.
	vcopy(&straightPath[straightPathSize*3], closestEndPos);
	if (straightPathFlags)
		straightPathFlags[straightPathSize] = DT_STRAIGHTPATH_END;
	if (straightPathRefs)
		straightPathRefs[straightPathSize] = 0;
	
	straightPathSize++;
	
	return straightPathSize;
}

// Moves towards end position a long the path corridor.
// Returns: Index to the result path polygon.
int dtNavMesh::moveAlongPathCorridor(const float* startPos, const float* endPos, float* resultPos,
									 const dtPolyRef* path, const int pathSize)
{
	if (!pathSize)
		return 0;
	
	float verts[DT_VERTS_PER_POLYGON*3];	
	float edged[DT_VERTS_PER_POLYGON];
	float edget[DT_VERTS_PER_POLYGON];
	int n = 0;
	
	static const float SLOP = 0.01f;

	vcopy(resultPos, startPos);
	
	while (n < pathSize)
	{
		// Get current polygon and poly vertices.
		unsigned int salt, it, ip;
		decodePolyId(path[n], salt, it, ip);
		if (it >= (unsigned int)m_maxTiles) return n;
		if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return n;
		if (ip >= (unsigned int)m_tiles[it].header->polyCount) return n;
		const dtMeshTile* tile = &m_tiles[it];
		const dtPoly* poly = &tile->polys[ip];
		
		// In case of Off-Mesh link, just snap to the end location and advance over it.
		if (poly->type == DT_POLYTYPE_OFFMESH_CONNECTION)
		{
			if (n+1 < pathSize)
			{
				float left[3], right[3];
				unsigned char fromType, toType;
				if (!getPortalPoints(path[n], path[n+1], left, right, fromType, toType))
					return n;
				vcopy(resultPos, endPos);
			}
			return n+1;
		}
		
		// Collect vertices.
		int nv = 0;
		for (int i = 0; i < (int)poly->vertCount; ++i)
		{
			vcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
			nv++;
		}

		bool inside = distancePtPolyEdgesSqr(endPos, verts, nv, edged, edget);
		if (inside)
		{
			// The end point is inside the current polygon.
			vcopy(resultPos, endPos);
			return n;
		}

		// Constraint the point on the polygon boundary.
		// This results sliding movement.
		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];
		vlerp(resultPos, va, vb, edget[imin]);
		
		// Check to see if the point is on the portal edge to the next polygon.
		if (n+1 >= pathSize)
			return n;
		// TODO: optimize
		float left[3], right[3];
		unsigned char fromType, toType;
		if (!getPortalPoints(path[n], path[n+1], left, right, fromType, toType))
			return n;
		// If the clamped point is close to the next portal edge, advance to next poly.
		float t;
		float d = distancePtSegSqr2D(resultPos, left, right, t);
		if (d > SLOP*SLOP)
			return n;
		// Advance to next polygon.
		n++;
	}
	
	return n;
}

bool dtNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
								unsigned char& fromType, unsigned char& toType) const
{
	unsigned int salt, it, ip;
	decodePolyId(from, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return false;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
	const dtMeshTile* fromTile = &m_tiles[it];
	if (ip >= (unsigned int)fromTile->header->polyCount) return false;
	const dtPoly* fromPoly = &fromTile->polys[ip];
	fromType = fromPoly->type;

	decodePolyId(to, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return false;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
	const dtMeshTile* toTile = &m_tiles[it];
	if (ip >= (unsigned int)toTile->header->polyCount) return false;
	const dtPoly* toPoly = &toTile->polys[ip];
	toType = toPoly->type;

	return getPortalPoints(from, fromPoly, fromTile,
						   to, toPoly, toTile,
						   left, right);
}

// Returns portal points between two polygons.
bool dtNavMesh::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 false;
	
	// Handle off-mesh connections.
	if (fromPoly->type == 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;
				vcopy(left, &fromTile->verts[fromPoly->verts[v]*3]);
				vcopy(right, &fromTile->verts[fromPoly->verts[v]*3]);
				return true;
			}
		}
		return false;
	}

	if (toPoly->type == 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;
				vcopy(left, &toTile->verts[toPoly->verts[v]*3]);
				vcopy(right, &toTile->verts[toPoly->verts[v]*3]);
				return true;
			}
		}
		return false;
	}
		
	// Find portal vertices.
	const int v0 = fromPoly->verts[link->edge];
	const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
	vcopy(left, &fromTile->verts[v0*3]);
	vcopy(right, &fromTile->verts[v1*3]);
	
	// If the link is at tile boundary, clamp the vertices to
	// the link width.
	if (link->side == 0 || link->side == 4)
	{
		// Unpack portal limits.
		const float smin = min(left[2],right[2]);
		const float smax = max(left[2],right[2]);
		const float s = (smax-smin) / 255.0f;
		const float lmin = smin + link->bmin*s;
		const float lmax = smin + link->bmax*s;
		left[2] = max(left[2],lmin);
		left[2] = min(left[2],lmax);
		right[2] = max(right[2],lmin);
		right[2] = min(right[2],lmax);
	}
	else if (link->side == 2 || link->side == 6)
	{
		// Unpack portal limits.
		const float smin = min(left[0],right[0]);
		const float smax = max(left[0],right[0]);
		const float s = (smax-smin) / 255.0f;
		const float lmin = smin + link->bmin*s;
		const float lmax = smin + link->bmax*s;
		left[0] = max(left[0],lmin);
		left[0] = min(left[0],lmax);
		right[0] = max(right[0],lmin);
		right[0] = min(right[0],lmax);
	}
	
	return true;
}

// Returns edge mid point between two polygons.
bool dtNavMesh::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 false;
	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 true;
}

bool dtNavMesh::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))
		return false;
	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 true;
}

void dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags)
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return;
	dtMeshTile* tile = &m_tiles[it];
	if (ip >= (unsigned int)tile->header->polyCount) return;
	dtPoly* poly = &tile->polys[ip];
	// Change flags.
	poly->flags = flags;
}

unsigned short dtNavMesh::getPolyFlags(dtPolyRef ref) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return 0;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
	const dtMeshTile* tile = &m_tiles[it];
	if (ip >= (unsigned int)tile->header->polyCount) return 0;
	const dtPoly* poly = &tile->polys[ip];
	return poly->flags;
}

void dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area)
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return;
	dtMeshTile* tile = &m_tiles[it];
	if (ip >= (unsigned int)tile->header->polyCount) return;
	dtPoly* poly = &tile->polys[ip];
	poly->area = area;
}

unsigned char dtNavMesh::getPolyArea(dtPolyRef ref) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return 0;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
	const dtMeshTile* tile = &m_tiles[it];
	if (ip >= (unsigned int)tile->header->polyCount) return 0;
	const dtPoly* poly = &tile->polys[ip];
	return poly->area;
}

int dtNavMesh::raycast(dtPolyRef centerRef, const float* startPos, const float* endPos, dtQueryFilter* filter,
					   float& t, float* hitNormal, dtPolyRef* path, const int pathSize)
{
	t = 0;
	
	if (!centerRef || !getPolyByRef(centerRef))
		return 0;
	
	dtPolyRef curRef = centerRef;
	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.
		unsigned int it = decodePolyIdTile(curRef);
		unsigned int ip = decodePolyIdPoly(curRef);
		const dtMeshTile* tile = &m_tiles[it];
		const dtPoly* poly = &tile->polys[ip];

		// Collect vertices.
		int nv = 0;
		for (int i = 0; i < (int)poly->vertCount; ++i)
		{
			vcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
			nv++;
		}		
		
		float tmin, tmax;
		int segMin, segMax;
		if (!intersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
		{
			// Could not hit the polygon, keep the old t and report hit.
			return n;
		}
		// Keep track of furthest t so far.
		if (tmax > t)
			t = tmax;

		if (n < pathSize)
			path[n++] = curRef;
		
		// 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];
			if ((int)link->edge == segMax)
			{
				// If the link is internal, just return the ref.
				if (link->side == 0xff)
				{
					nextRef = link->ref;
					break;
				}
				
				// If the link is at tile boundary,
				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 smin = min(left[2],right[2]);
					const float smax = max(left[2],right[2]);
					const float s = (smax-smin) / 255.0f;
					const float lmin = smin + link->bmin*s;
					const float lmax = smin + link->bmax*s;
					// 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 smin = min(left[0],right[0]);
					const float smax = max(left[0],right[0]);
					const float s = (smax-smin) / 255.0f;
					const float lmin = smin + link->bmin*s;
					const float lmax = smin + link->bmax*s;
					// Find X intersection.
					float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
					if (x >= lmin && x <= lmax)
					{
						nextRef = link->ref;
						break;
					}
				}
			}
		}
		
		if (!nextRef || !passFilter(filter, getPolyFlags(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;
			vnormalize(hitNormal);
			
			return n;
		}
		
		// No hit, advance to neighbour polygon.
		curRef = nextRef;
	}
	
	return n;
}

int dtNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius, dtQueryFilter* filter,
									dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
									const int maxResult)
{
	if (!centerRef) return 0;
	if (!getPolyByRef(centerRef)) return 0;
	if (!m_nodePool || !m_openList) return 0;
	
	m_nodePool->clear();
	m_openList->clear();
	
	dtNode* startNode = m_nodePool->getNode(centerRef);
	startNode->pidx = 0;
	startNode->cost = 0;
	startNode->total = 0;
	startNode->id = centerRef;
	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 = sqr(radius);

	unsigned int it, ip;
	
	while (!m_openList->empty())
	{
		dtNode* bestNode = m_openList->pop();

		float previousEdgeMidPoint[3];

		// Get poly and tile.
		// The API input has been cheked already, skip checking internal data.
		const dtPolyRef bestRef = bestNode->id;
		it = decodePolyIdTile(bestRef);
		ip = decodePolyIdPoly(bestRef);
		const dtMeshTile* bestTile = &m_tiles[it];
		const dtPoly* bestPoly = &bestTile->polys[ip];

		// 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)
		{
			it = decodePolyIdTile(parentRef);
			ip = decodePolyIdPoly(parentRef);
			parentTile = &m_tiles[it];
			parentPoly = &parentTile->polys[ip];
			
			getEdgeMidPoint(parentRef, parentPoly, parentTile,
							bestRef, bestPoly, bestTile, previousEdgeMidPoint);
		}
		else
		{
			vcopy(previousEdgeMidPoint, centerPos);
		}
		
		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;

			// 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 = distancePtSegSqr2D(centerPos, va, vb, tseg);
			
			// If the circle is not touching the next polygon, skip it.
			if (distSqr > radiusSqr)
				continue;

			// Expand to neighbour
			it = decodePolyIdTile(neighbourRef);
			ip = decodePolyIdPoly(neighbourRef);
			const dtMeshTile* neighbourTile = &m_tiles[it];
			const dtPoly* neighbourPoly = &neighbourTile->polys[ip];
			
			if (!passFilter(filter, neighbourPoly->flags))
				continue;
			
			dtNode newNode;
			newNode.pidx = m_nodePool->getNodeIdx(bestNode);
			newNode.id = neighbourRef;

			// Cost
			float edgeMidPoint[3];
			getEdgeMidPoint(bestRef, bestPoly, bestTile,
							neighbourRef, neighbourPoly, neighbourTile, edgeMidPoint);
			
			newNode.total = bestNode->total + vdist(previousEdgeMidPoint, edgeMidPoint);
			
			dtNode* actualNode = m_nodePool->getNode(newNode.id);
			if (!actualNode)
				continue;
			
			if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) &&
				!((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total))
			{
				actualNode->flags &= ~DT_NODE_CLOSED;
				actualNode->pidx = newNode.pidx;
				actualNode->total = newNode.total;
				
				if (actualNode->flags & DT_NODE_OPEN)
				{
					m_openList->modify(actualNode);
				}
				else
				{
					if (n < maxResult)
					{
						if (resultRef)
							resultRef[n] = actualNode->id;
						if (resultParent)
							resultParent[n] = m_nodePool->getNodeAtIdx(actualNode->pidx)->id;
						if (resultCost)
							resultCost[n] = actualNode->total;
						++n;
					}
					actualNode->flags = DT_NODE_OPEN;
					m_openList->push(actualNode);
				}
			}
		}
	}
	
	return n;
}

float dtNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* centerPos, float maxRadius, dtQueryFilter* filter,
									float* hitPos, float* hitNormal)
{
	if (!centerRef) return 0;
	if (!getPolyByRef(centerRef)) return 0;
	if (!m_nodePool || !m_openList) return 0;
	
	m_nodePool->clear();
	m_openList->clear();
	
	dtNode* startNode = m_nodePool->getNode(centerRef);
	startNode->pidx = 0;
	startNode->cost = 0;
	startNode->total = 0;
	startNode->id = centerRef;
	startNode->flags = DT_NODE_OPEN;
	m_openList->push(startNode);
	
	float radiusSqr = sqr(maxRadius);
	
	unsigned int it, ip;
	
	while (!m_openList->empty())
	{
		dtNode* bestNode = m_openList->pop();
		
		float previousEdgeMidPoint[3];
		
		// Get poly and tile.
		// The API input has been cheked already, skip checking internal data.
		const dtPolyRef bestRef = bestNode->id;
		it = decodePolyIdTile(bestRef);
		ip = decodePolyIdPoly(bestRef);
		const dtMeshTile* bestTile = &m_tiles[it];
		const dtPoly* bestPoly = &bestTile->polys[ip];
		
		// 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)
		{
			it = decodePolyIdTile(parentRef);
			ip = decodePolyIdPoly(parentRef);
			parentTile = &m_tiles[it];
			parentPoly = &parentTile->polys[ip];
			
			getEdgeMidPoint(parentRef, parentPoly, parentTile,
							bestRef, bestPoly, bestTile, previousEdgeMidPoint);
		}
		else
		{
			vcopy(previousEdgeMidPoint, centerPos);
		}
		
		// 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 && passFilter(filter, getPolyFlags(link->ref)))
							solid = false;
						break;
					}
				}
				if (!solid) continue;
			}
			else if (bestPoly->neis[j] && passFilter(filter, bestTile->polys[bestPoly->neis[j]].flags))
			{
				// Internal edge
				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 = distancePtSegSqr2D(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;
			
			// 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 = distancePtSegSqr2D(centerPos, va, vb, tseg);
			
			// If the circle is not touching the next polygon, skip it.
			if (distSqr > radiusSqr)
				continue;
			
			// Expand to neighbour.
			it = decodePolyIdTile(neighbourRef);
			ip = decodePolyIdPoly(neighbourRef);
			const dtMeshTile* neighbourTile = &m_tiles[it];
			const dtPoly* neighbourPoly = &neighbourTile->polys[ip];
			
			if (!passFilter(filter, neighbourPoly->flags))
				continue;
			
			dtNode newNode;
			newNode.pidx = m_nodePool->getNodeIdx(bestNode);
			newNode.id = neighbourRef;
			
			// Cost
			float edgeMidPoint[3];
			getEdgeMidPoint(bestRef, bestPoly, bestTile,
							neighbourRef, neighbourPoly, neighbourTile, edgeMidPoint);

			newNode.total = bestNode->total + vdist(previousEdgeMidPoint, edgeMidPoint);
			
			dtNode* actualNode = m_nodePool->getNode(newNode.id);
			if (!actualNode)
				continue;
			
			if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) &&
				!((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total))
			{
				actualNode->flags &= ~DT_NODE_CLOSED;
				actualNode->pidx = newNode.pidx;
				actualNode->total = newNode.total;
				
				if (actualNode->flags & DT_NODE_OPEN)
				{
					m_openList->modify(actualNode);
				}
				else
				{
					actualNode->flags = DT_NODE_OPEN;
					m_openList->push(actualNode);
				}
			}
		}
	}
	
	// Calc hit normal.
	vsub(hitNormal, centerPos, hitPos);
	vnormalize(hitNormal);
	
	return sqrtf(radiusSqr);
}

const dtPoly* dtNavMesh::getPolyByRef(dtPolyRef ref) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return 0;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
	if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0;
	return &m_tiles[it].polys[ip];
}

const float* dtNavMesh::getPolyVertsByRef(dtPolyRef ref) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return 0;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
	if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0;
	return m_tiles[it].verts;
}

const dtLink* dtNavMesh::getPolyLinksByRef(dtPolyRef ref) const
{
	unsigned int salt, it, ip;
	decodePolyId(ref, salt, it, ip);
	if (it >= (unsigned int)m_maxTiles) return 0;
	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
	if (ip >= (unsigned int)m_tiles[it].header->polyCount) return 0;
	return m_tiles[it].links;
}

bool dtNavMesh::isInClosedList(dtPolyRef ref) const
{
	if (!m_nodePool) return false;
	const dtNode* node = m_nodePool->findNode(ref);
	return node && node->flags & DT_NODE_CLOSED;
}