Update Files

lib/haxerecast/recastnavigation/Detour/Source/DetourAlloc.cpp

@@ -0,0 +1,50 @@
+//
+// 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 <stdlib.h>
+#include "DetourAlloc.h"
+
+static void *dtAllocDefault(size_t size, dtAllocHint)
+{
+	return malloc(size);
+}
+
+static void dtFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static dtAllocFunc* sAllocFunc = dtAllocDefault;
+static dtFreeFunc* sFreeFunc = dtFreeDefault;
+
+void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc)
+{
+	sAllocFunc = allocFunc ? allocFunc : dtAllocDefault;
+	sFreeFunc = freeFunc ? freeFunc : dtFreeDefault;
+}
+
+void* dtAlloc(size_t size, dtAllocHint hint)
+{
+	return sAllocFunc(size, hint);
+}
+
+void dtFree(void* ptr)
+{
+	if (ptr)
+		sFreeFunc(ptr);
+}

lib/haxerecast/recastnavigation/Detour/Source/DetourAssert.cpp

@@ -0,0 +1,35 @@
+//
+// 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 "DetourAssert.h"
+
+#ifndef NDEBUG
+
+static dtAssertFailFunc* sAssertFailFunc = 0;
+
+void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc)
+{
+	sAssertFailFunc = assertFailFunc;
+}
+
+dtAssertFailFunc* dtAssertFailGetCustom()
+{
+	return sAssertFailFunc;
+}
+
+#endif

lib/haxerecast/recastnavigation/Detour/Source/DetourCommon.cpp

@@ -0,0 +1,387 @@
+//
+// 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 "DetourCommon.h"
+#include "DetourMath.h"
+
+//////////////////////////////////////////////////////////////////////////////////////////
+
+void dtClosestPtPointTriangle(float* closest, const float* p,
+							  const float* a, const float* b, const float* c)
+{
+	// Check if P in vertex region outside A
+	float ab[3], ac[3], ap[3];
+	dtVsub(ab, b, a);
+	dtVsub(ac, c, a);
+	dtVsub(ap, p, a);
+	float d1 = dtVdot(ab, ap);
+	float d2 = dtVdot(ac, ap);
+	if (d1 <= 0.0f && d2 <= 0.0f)
+	{
+		// barycentric coordinates (1,0,0)
+		dtVcopy(closest, a);
+		return;
+	}
+	
+	// Check if P in vertex region outside B
+	float bp[3];
+	dtVsub(bp, p, b);
+	float d3 = dtVdot(ab, bp);
+	float d4 = dtVdot(ac, bp);
+	if (d3 >= 0.0f && d4 <= d3)
+	{
+		// barycentric coordinates (0,1,0)
+		dtVcopy(closest, b);
+		return;
+	}
+	
+	// Check if P in edge region of AB, if so return projection of P onto AB
+	float vc = d1*d4 - d3*d2;
+	if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
+	{
+		// barycentric coordinates (1-v,v,0)
+		float v = d1 / (d1 - d3);
+		closest[0] = a[0] + v * ab[0];
+		closest[1] = a[1] + v * ab[1];
+		closest[2] = a[2] + v * ab[2];
+		return;
+	}
+	
+	// Check if P in vertex region outside C
+	float cp[3];
+	dtVsub(cp, p, c);
+	float d5 = dtVdot(ab, cp);
+	float d6 = dtVdot(ac, cp);
+	if (d6 >= 0.0f && d5 <= d6)
+	{
+		// barycentric coordinates (0,0,1)
+		dtVcopy(closest, c);
+		return;
+	}
+	
+	// Check if P in edge region of AC, if so return projection of P onto AC
+	float vb = d5*d2 - d1*d6;
+	if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
+	{
+		// barycentric coordinates (1-w,0,w)
+		float w = d2 / (d2 - d6);
+		closest[0] = a[0] + w * ac[0];
+		closest[1] = a[1] + w * ac[1];
+		closest[2] = a[2] + w * ac[2];
+		return;
+	}
+	
+	// Check if P in edge region of BC, if so return projection of P onto BC
+	float va = d3*d6 - d5*d4;
+	if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
+	{
+		// barycentric coordinates (0,1-w,w)
+		float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		closest[0] = b[0] + w * (c[0] - b[0]);
+		closest[1] = b[1] + w * (c[1] - b[1]);
+		closest[2] = b[2] + w * (c[2] - b[2]);
+		return;
+	}
+	
+	// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+	float denom = 1.0f / (va + vb + vc);
+	float v = vb * denom;
+	float w = vc * denom;
+	closest[0] = a[0] + ab[0] * v + ac[0] * w;
+	closest[1] = a[1] + ab[1] * v + ac[1] * w;
+	closest[2] = a[2] + ab[2] * v + ac[2] * w;
+}
+
+bool dtIntersectSegmentPoly2D(const float* p0, const float* p1,
+							  const float* verts, int nverts,
+							  float& tmin, float& tmax,
+							  int& segMin, int& segMax)
+{
+	static const float EPS = 0.00000001f;
+	
+	tmin = 0;
+	tmax = 1;
+	segMin = -1;
+	segMax = -1;
+	
+	float dir[3];
+	dtVsub(dir, p1, p0);
+	
+	for (int i = 0, j = nverts-1; i < nverts; j=i++)
+	{
+		float edge[3], diff[3];
+		dtVsub(edge, &verts[i*3], &verts[j*3]);
+		dtVsub(diff, p0, &verts[j*3]);
+		const float n = dtVperp2D(edge, diff);
+		const float d = dtVperp2D(dir, edge);
+		if (fabsf(d) < EPS)
+		{
+			// S is nearly parallel to this edge
+			if (n < 0)
+				return false;
+			else
+				continue;
+		}
+		const float t = n / d;
+		if (d < 0)
+		{
+			// segment S is entering across this edge
+			if (t > tmin)
+			{
+				tmin = t;
+				segMin = j;
+				// S enters after leaving polygon
+				if (tmin > tmax)
+					return false;
+			}
+		}
+		else
+		{
+			// segment S is leaving across this edge
+			if (t < tmax)
+			{
+				tmax = t;
+				segMax = j;
+				// S leaves before entering polygon
+				if (tmax < tmin)
+					return false;
+			}
+		}
+	}
+	
+	return true;
+}
+
+float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t)
+{
+	float pqx = q[0] - p[0];
+	float pqz = q[2] - p[2];
+	float dx = pt[0] - p[0];
+	float dz = pt[2] - p[2];
+	float d = pqx*pqx + pqz*pqz;
+	t = pqx*dx + pqz*dz;
+	if (d > 0) t /= d;
+	if (t < 0) t = 0;
+	else if (t > 1) t = 1;
+	dx = p[0] + t*pqx - pt[0];
+	dz = p[2] + t*pqz - pt[2];
+	return dx*dx + dz*dz;
+}
+
+void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts)
+{
+	tc[0] = 0.0f;
+	tc[1] = 0.0f;
+	tc[2] = 0.0f;
+	for (int j = 0; j < nidx; ++j)
+	{
+		const float* v = &verts[idx[j]*3];
+		tc[0] += v[0];
+		tc[1] += v[1];
+		tc[2] += v[2];
+	}
+	const float s = 1.0f / nidx;
+	tc[0] *= s;
+	tc[1] *= s;
+	tc[2] *= s;
+}
+
+bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h)
+{
+	const float EPS = 1e-6f;
+	float v0[3], v1[3], v2[3];
+
+	dtVsub(v0, c, a);
+	dtVsub(v1, b, a);
+	dtVsub(v2, p, a);
+
+	// Compute scaled barycentric coordinates
+	float denom = v0[0] * v1[2] - v0[2] * v1[0];
+	if (fabsf(denom) < EPS)
+		return false;
+
+	float u = v1[2] * v2[0] - v1[0] * v2[2];
+	float v = v0[0] * v2[2] - v0[2] * v2[0];
+
+	if (denom < 0) {
+		denom = -denom;
+		u = -u;
+		v = -v;
+	}
+
+	// If point lies inside the triangle, return interpolated ycoord.
+	if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) {
+		h = a[1] + (v0[1] * u + v1[1] * v) / denom;
+		return true;
+	}
+	return false;
+}
+
+/// @par
+///
+/// All points are projected onto the xz-plane, so the y-values are ignored.
+bool dtPointInPolygon(const float* pt, const float* verts, const int nverts)
+{
+	// TODO: Replace pnpoly with triArea2D tests?
+	int i, j;
+	bool c = false;
+	for (i = 0, j = nverts-1; i < nverts; j = i++)
+	{
+		const float* vi = &verts[i*3];
+		const float* vj = &verts[j*3];
+		if (((vi[2] > pt[2]) != (vj[2] > pt[2])) &&
+			(pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
+			c = !c;
+	}
+	return c;
+}
+
+bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts,
+							  float* ed, float* et)
+{
+	// TODO: Replace pnpoly with triArea2D tests?
+	int i, j;
+	bool c = false;
+	for (i = 0, j = nverts-1; i < nverts; j = i++)
+	{
+		const float* vi = &verts[i*3];
+		const float* vj = &verts[j*3];
+		if (((vi[2] > pt[2]) != (vj[2] > pt[2])) &&
+			(pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
+			c = !c;
+		ed[j] = dtDistancePtSegSqr2D(pt, vj, vi, et[j]);
+	}
+	return c;
+}
+
+static void projectPoly(const float* axis, const float* poly, const int npoly,
+						float& rmin, float& rmax)
+{
+	rmin = rmax = dtVdot2D(axis, &poly[0]);
+	for (int i = 1; i < npoly; ++i)
+	{
+		const float d = dtVdot2D(axis, &poly[i*3]);
+		rmin = dtMin(rmin, d);
+		rmax = dtMax(rmax, d);
+	}
+}
+
+inline bool overlapRange(const float amin, const float amax,
+						 const float bmin, const float bmax,
+						 const float eps)
+{
+	return ((amin+eps) > bmax || (amax-eps) < bmin) ? false : true;
+}
+
+/// @par
+///
+/// All vertices are projected onto the xz-plane, so the y-values are ignored.
+bool dtOverlapPolyPoly2D(const float* polya, const int npolya,
+						 const float* polyb, const int npolyb)
+{
+	const float eps = 1e-4f;
+	
+	for (int i = 0, j = npolya-1; i < npolya; j=i++)
+	{
+		const float* va = &polya[j*3];
+		const float* vb = &polya[i*3];
+		const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) };
+		float amin,amax,bmin,bmax;
+		projectPoly(n, polya, npolya, amin,amax);
+		projectPoly(n, polyb, npolyb, bmin,bmax);
+		if (!overlapRange(amin,amax, bmin,bmax, eps))
+		{
+			// Found separating axis
+			return false;
+		}
+	}
+	for (int i = 0, j = npolyb-1; i < npolyb; j=i++)
+	{
+		const float* va = &polyb[j*3];
+		const float* vb = &polyb[i*3];
+		const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) };
+		float amin,amax,bmin,bmax;
+		projectPoly(n, polya, npolya, amin,amax);
+		projectPoly(n, polyb, npolyb, bmin,bmax);
+		if (!overlapRange(amin,amax, bmin,bmax, eps))
+		{
+			// Found separating axis
+			return false;
+		}
+	}
+	return true;
+}
+
+// Returns a random point in a convex polygon.
+// Adapted from Graphics Gems article.
+void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas,
+							   const float s, const float t, float* out)
+{
+	// Calc triangle araes
+	float areasum = 0.0f;
+	for (int i = 2; i < npts; i++) {
+		areas[i] = dtTriArea2D(&pts[0], &pts[(i-1)*3], &pts[i*3]);
+		areasum += dtMax(0.001f, areas[i]);
+	}
+	// Find sub triangle weighted by area.
+	const float thr = s*areasum;
+	float acc = 0.0f;
+	float u = 1.0f;
+	int tri = npts - 1;
+	for (int i = 2; i < npts; i++) {
+		const float dacc = areas[i];
+		if (thr >= acc && thr < (acc+dacc))
+		{
+			u = (thr - acc) / dacc;
+			tri = i;
+			break;
+		}
+		acc += dacc;
+	}
+	
+	float v = dtMathSqrtf(t);
+	
+	const float a = 1 - v;
+	const float b = (1 - u) * v;
+	const float c = u * v;
+	const float* pa = &pts[0];
+	const float* pb = &pts[(tri-1)*3];
+	const float* pc = &pts[tri*3];
+	
+	out[0] = a*pa[0] + b*pb[0] + c*pc[0];
+	out[1] = a*pa[1] + b*pb[1] + c*pc[1];
+	out[2] = a*pa[2] + b*pb[2] + c*pc[2];
+}
+
+inline float vperpXZ(const float* a, const float* b) { return a[0]*b[2] - a[2]*b[0]; }
+
+bool dtIntersectSegSeg2D(const float* ap, const float* aq,
+						 const float* bp, const float* bq,
+						 float& s, float& t)
+{
+	float u[3], v[3], w[3];
+	dtVsub(u,aq,ap);
+	dtVsub(v,bq,bp);
+	dtVsub(w,ap,bp);
+	float d = vperpXZ(u,v);
+	if (fabsf(d) < 1e-6f) return false;
+	s = vperpXZ(v,w) / d;
+	t = vperpXZ(u,w) / d;
+	return true;
+}
+

lib/haxerecast/recastnavigation/Detour/Source/DetourNavMeshBuilder.cpp

@@ -0,0 +1,802 @@
+//
+// 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <float.h>
+#include "DetourNavMesh.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourNavMeshBuilder.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+
+static unsigned short MESH_NULL_IDX = 0xffff;
+
+
+struct BVItem
+{
+	unsigned short bmin[3];
+	unsigned short bmax[3];
+	int i;
+};
+
+static int compareItemX(const void* va, const void* vb)
+{
+	const BVItem* a = (const BVItem*)va;
+	const BVItem* b = (const BVItem*)vb;
+	if (a->bmin[0] < b->bmin[0])
+		return -1;
+	if (a->bmin[0] > b->bmin[0])
+		return 1;
+	return 0;
+}
+
+static int compareItemY(const void* va, const void* vb)
+{
+	const BVItem* a = (const BVItem*)va;
+	const BVItem* b = (const BVItem*)vb;
+	if (a->bmin[1] < b->bmin[1])
+		return -1;
+	if (a->bmin[1] > b->bmin[1])
+		return 1;
+	return 0;
+}
+
+static int compareItemZ(const void* va, const void* vb)
+{
+	const BVItem* a = (const BVItem*)va;
+	const BVItem* b = (const BVItem*)vb;
+	if (a->bmin[2] < b->bmin[2])
+		return -1;
+	if (a->bmin[2] > b->bmin[2])
+		return 1;
+	return 0;
+}
+
+static void calcExtends(BVItem* items, const int /*nitems*/, const int imin, const int imax,
+						unsigned short* bmin, unsigned short* bmax)
+{
+	bmin[0] = items[imin].bmin[0];
+	bmin[1] = items[imin].bmin[1];
+	bmin[2] = items[imin].bmin[2];
+	
+	bmax[0] = items[imin].bmax[0];
+	bmax[1] = items[imin].bmax[1];
+	bmax[2] = items[imin].bmax[2];
+	
+	for (int i = imin+1; i < imax; ++i)
+	{
+		const BVItem& it = items[i];
+		if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0];
+		if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1];
+		if (it.bmin[2] < bmin[2]) bmin[2] = it.bmin[2];
+		
+		if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0];
+		if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1];
+		if (it.bmax[2] > bmax[2]) bmax[2] = it.bmax[2];
+	}
+}
+
+inline int longestAxis(unsigned short x, unsigned short y, unsigned short z)
+{
+	int	axis = 0;
+	unsigned short maxVal = x;
+	if (y > maxVal)
+	{
+		axis = 1;
+		maxVal = y;
+	}
+	if (z > maxVal)
+	{
+		axis = 2;
+	}
+	return axis;
+}
+
+static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNode, dtBVNode* nodes)
+{
+	int inum = imax - imin;
+	int icur = curNode;
+	
+	dtBVNode& node = nodes[curNode++];
+	
+	if (inum == 1)
+	{
+		// Leaf
+		node.bmin[0] = items[imin].bmin[0];
+		node.bmin[1] = items[imin].bmin[1];
+		node.bmin[2] = items[imin].bmin[2];
+		
+		node.bmax[0] = items[imin].bmax[0];
+		node.bmax[1] = items[imin].bmax[1];
+		node.bmax[2] = items[imin].bmax[2];
+		
+		node.i = items[imin].i;
+	}
+	else
+	{
+		// Split
+		calcExtends(items, nitems, imin, imax, node.bmin, node.bmax);
+		
+		int	axis = longestAxis(node.bmax[0] - node.bmin[0],
+							   node.bmax[1] - node.bmin[1],
+							   node.bmax[2] - node.bmin[2]);
+		
+		if (axis == 0)
+		{
+			// Sort along x-axis
+			qsort(items+imin, inum, sizeof(BVItem), compareItemX);
+		}
+		else if (axis == 1)
+		{
+			// Sort along y-axis
+			qsort(items+imin, inum, sizeof(BVItem), compareItemY);
+		}
+		else
+		{
+			// Sort along z-axis
+			qsort(items+imin, inum, sizeof(BVItem), compareItemZ);
+		}
+		
+		int isplit = imin+inum/2;
+		
+		// Left
+		subdivide(items, nitems, imin, isplit, curNode, nodes);
+		// Right
+		subdivide(items, nitems, isplit, imax, curNode, nodes);
+		
+		int iescape = curNode - icur;
+		// Negative index means escape.
+		node.i = -iescape;
+	}
+}
+
+static int createBVTree(dtNavMeshCreateParams* params, dtBVNode* nodes, int /*nnodes*/)
+{
+	// Build tree
+	float quantFactor = 1 / params->cs;
+	BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*params->polyCount, DT_ALLOC_TEMP);
+	for (int i = 0; i < params->polyCount; i++)
+	{
+		BVItem& it = items[i];
+		it.i = i;
+		// Calc polygon bounds. Use detail meshes if available.
+		if (params->detailMeshes)
+		{
+			int vb = (int)params->detailMeshes[i*4+0];
+			int ndv = (int)params->detailMeshes[i*4+1];
+			float bmin[3];
+			float bmax[3];
+
+			const float* dv = &params->detailVerts[vb*3];
+			dtVcopy(bmin, dv);
+			dtVcopy(bmax, dv);
+
+			for (int j = 1; j < ndv; j++)
+			{
+				dtVmin(bmin, &dv[j * 3]);
+				dtVmax(bmax, &dv[j * 3]);
+			}
+
+			// BV-tree uses cs for all dimensions
+			it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0])*quantFactor), 0, 0xffff);
+			it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1])*quantFactor), 0, 0xffff);
+			it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2])*quantFactor), 0, 0xffff);
+
+			it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0])*quantFactor), 0, 0xffff);
+			it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1])*quantFactor), 0, 0xffff);
+			it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2])*quantFactor), 0, 0xffff);
+		}
+		else
+		{
+			const unsigned short* p = &params->polys[i*params->nvp * 2];
+			it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0];
+			it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1];
+			it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2];
+
+			for (int j = 1; j < params->nvp; ++j)
+			{
+				if (p[j] == MESH_NULL_IDX) break;
+				unsigned short x = params->verts[p[j] * 3 + 0];
+				unsigned short y = params->verts[p[j] * 3 + 1];
+				unsigned short z = params->verts[p[j] * 3 + 2];
+
+				if (x < it.bmin[0]) it.bmin[0] = x;
+				if (y < it.bmin[1]) it.bmin[1] = y;
+				if (z < it.bmin[2]) it.bmin[2] = z;
+
+				if (x > it.bmax[0]) it.bmax[0] = x;
+				if (y > it.bmax[1]) it.bmax[1] = y;
+				if (z > it.bmax[2]) it.bmax[2] = z;
+			}
+			// Remap y
+			it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs);
+			it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs);
+		}
+	}
+	
+	int curNode = 0;
+	subdivide(items, params->polyCount, 0, params->polyCount, curNode, nodes);
+	
+	dtFree(items);
+	
+	return curNode;
+}
+
+static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, const float* bmax)
+{
+	static const unsigned char XP = 1<<0;
+	static const unsigned char ZP = 1<<1;
+	static const unsigned char XM = 1<<2;
+	static const unsigned char ZM = 1<<3;	
+
+	unsigned char outcode = 0; 
+	outcode |= (pt[0] >= bmax[0]) ? XP : 0;
+	outcode |= (pt[2] >= bmax[2]) ? ZP : 0;
+	outcode |= (pt[0] < bmin[0])  ? XM : 0;
+	outcode |= (pt[2] < bmin[2])  ? ZM : 0;
+
+	switch (outcode)
+	{
+	case XP: return 0;
+	case XP|ZP: return 1;
+	case ZP: return 2;
+	case XM|ZP: return 3;
+	case XM: return 4;
+	case XM|ZM: return 5;
+	case ZM: return 6;
+	case XP|ZM: return 7;
+	};
+
+	return 0xff;	
+}
+
+// TODO: Better error handling.
+
+/// @par
+/// 
+/// The output data array is allocated using the detour allocator (dtAlloc()).  The method
+/// used to free the memory will be determined by how the tile is added to the navigation
+/// mesh.
+///
+/// @see dtNavMesh, dtNavMesh::addTile()
+bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize)
+{
+	if (params->nvp > DT_VERTS_PER_POLYGON)
+		return false;
+	if (params->vertCount >= 0xffff)
+		return false;
+	if (!params->vertCount || !params->verts)
+		return false;
+	if (!params->polyCount || !params->polys)
+		return false;
+
+	const int nvp = params->nvp;
+	
+	// Classify off-mesh connection points. We store only the connections
+	// whose start point is inside the tile.
+	unsigned char* offMeshConClass = 0;
+	int storedOffMeshConCount = 0;
+	int offMeshConLinkCount = 0;
+	
+	if (params->offMeshConCount > 0)
+	{
+		offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP);
+		if (!offMeshConClass)
+			return false;
+
+		// Find tight heigh bounds, used for culling out off-mesh start locations.
+		float hmin = FLT_MAX;
+		float hmax = -FLT_MAX;
+		
+		if (params->detailVerts && params->detailVertsCount)
+		{
+			for (int i = 0; i < params->detailVertsCount; ++i)
+			{
+				const float h = params->detailVerts[i*3+1];
+				hmin = dtMin(hmin,h);
+				hmax = dtMax(hmax,h);
+			}
+		}
+		else
+		{
+			for (int i = 0; i < params->vertCount; ++i)
+			{
+				const unsigned short* iv = &params->verts[i*3];
+				const float h = params->bmin[1] + iv[1] * params->ch;
+				hmin = dtMin(hmin,h);
+				hmax = dtMax(hmax,h);
+			}
+		}
+		hmin -= params->walkableClimb;
+		hmax += params->walkableClimb;
+		float bmin[3], bmax[3];
+		dtVcopy(bmin, params->bmin);
+		dtVcopy(bmax, params->bmax);
+		bmin[1] = hmin;
+		bmax[1] = hmax;
+
+		for (int i = 0; i < params->offMeshConCount; ++i)
+		{
+			const float* p0 = &params->offMeshConVerts[(i*2+0)*3];
+			const float* p1 = &params->offMeshConVerts[(i*2+1)*3];
+			offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax);
+			offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax);
+
+			// Zero out off-mesh start positions which are not even potentially touching the mesh.
+			if (offMeshConClass[i*2+0] == 0xff)
+			{
+				if (p0[1] < bmin[1] || p0[1] > bmax[1])
+					offMeshConClass[i*2+0] = 0;
+			}
+
+			// Cound how many links should be allocated for off-mesh connections.
+			if (offMeshConClass[i*2+0] == 0xff)
+				offMeshConLinkCount++;
+			if (offMeshConClass[i*2+1] == 0xff)
+				offMeshConLinkCount++;
+
+			if (offMeshConClass[i*2+0] == 0xff)
+				storedOffMeshConCount++;
+		}
+	}
+	
+	// Off-mesh connectionss are stored as polygons, adjust values.
+	const int totPolyCount = params->polyCount + storedOffMeshConCount;
+	const int totVertCount = params->vertCount + storedOffMeshConCount*2;
+	
+	// Find portal edges which are at tile borders.
+	int edgeCount = 0;
+	int portalCount = 0;
+	for (int i = 0; i < params->polyCount; ++i)
+	{
+		const unsigned short* p = &params->polys[i*2*nvp];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == MESH_NULL_IDX) break;
+			edgeCount++;
+			
+			if (p[nvp+j] & 0x8000)
+			{
+				unsigned short dir = p[nvp+j] & 0xf;
+				if (dir != 0xf)
+					portalCount++;
+			}
+		}
+	}
+
+	const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2;
+	
+	// Find unique detail vertices.
+	int uniqueDetailVertCount = 0;
+	int detailTriCount = 0;
+	if (params->detailMeshes)
+	{
+		// Has detail mesh, count unique detail vertex count and use input detail tri count.
+		detailTriCount = params->detailTriCount;
+		for (int i = 0; i < params->polyCount; ++i)
+		{
+			const unsigned short* p = &params->polys[i*nvp*2];
+			int ndv = params->detailMeshes[i*4+1];
+			int nv = 0;
+			for (int j = 0; j < nvp; ++j)
+			{
+				if (p[j] == MESH_NULL_IDX) break;
+				nv++;
+			}
+			ndv -= nv;
+			uniqueDetailVertCount += ndv;
+		}
+	}
+	else
+	{
+		// No input detail mesh, build detail mesh from nav polys.
+		uniqueDetailVertCount = 0; // No extra detail verts.
+		detailTriCount = 0;
+		for (int i = 0; i < params->polyCount; ++i)
+		{
+			const unsigned short* p = &params->polys[i*nvp*2];
+			int nv = 0;
+			for (int j = 0; j < nvp; ++j)
+			{
+				if (p[j] == MESH_NULL_IDX) break;
+				nv++;
+			}
+			detailTriCount += nv-2;
+		}
+	}
+	
+	// Calculate data size
+	const int headerSize = dtAlign4(sizeof(dtMeshHeader));
+	const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount);
+	const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount);
+	const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount);
+	const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount);
+	const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount);
+	const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount);
+	const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0;
+	const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount);
+	
+	const int dataSize = headerSize + vertsSize + polysSize + linksSize +
+						 detailMeshesSize + detailVertsSize + detailTrisSize +
+						 bvTreeSize + offMeshConsSize;
+						 
+	unsigned char* data = (unsigned char*)dtAlloc(sizeof(unsigned char)*dataSize, DT_ALLOC_PERM);
+	if (!data)
+	{
+		dtFree(offMeshConClass);
+		return false;
+	}
+	memset(data, 0, dataSize);
+	
+	unsigned char* d = data;
+
+	dtMeshHeader* header = dtGetThenAdvanceBufferPointer<dtMeshHeader>(d, headerSize);
+	float* navVerts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
+	dtPoly* navPolys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
+	d += linksSize; // Ignore links; just leave enough space for them. They'll be created on load.
+	dtPolyDetail* navDMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
+	float* navDVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
+	unsigned char* navDTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
+	dtBVNode* navBvtree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvTreeSize);
+	dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshConsSize);
+	
+	
+	// Store header
+	header->magic = DT_NAVMESH_MAGIC;
+	header->version = DT_NAVMESH_VERSION;
+	header->x = params->tileX;
+	header->y = params->tileY;
+	header->layer = params->tileLayer;
+	header->userId = params->userId;
+	header->polyCount = totPolyCount;
+	header->vertCount = totVertCount;
+	header->maxLinkCount = maxLinkCount;
+	dtVcopy(header->bmin, params->bmin);
+	dtVcopy(header->bmax, params->bmax);
+	header->detailMeshCount = params->polyCount;
+	header->detailVertCount = uniqueDetailVertCount;
+	header->detailTriCount = detailTriCount;
+	header->bvQuantFactor = 1.0f / params->cs;
+	header->offMeshBase = params->polyCount;
+	header->walkableHeight = params->walkableHeight;
+	header->walkableRadius = params->walkableRadius;
+	header->walkableClimb = params->walkableClimb;
+	header->offMeshConCount = storedOffMeshConCount;
+	header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0;
+	
+	const int offMeshVertsBase = params->vertCount;
+	const int offMeshPolyBase = params->polyCount;
+	
+	// Store vertices
+	// Mesh vertices
+	for (int i = 0; i < params->vertCount; ++i)
+	{
+		const unsigned short* iv = &params->verts[i*3];
+		float* v = &navVerts[i*3];
+		v[0] = params->bmin[0] + iv[0] * params->cs;
+		v[1] = params->bmin[1] + iv[1] * params->ch;
+		v[2] = params->bmin[2] + iv[2] * params->cs;
+	}
+	// Off-mesh link vertices.
+	int n = 0;
+	for (int i = 0; i < params->offMeshConCount; ++i)
+	{
+		// Only store connections which start from this tile.
+		if (offMeshConClass[i*2+0] == 0xff)
+		{
+			const float* linkv = &params->offMeshConVerts[i*2*3];
+			float* v = &navVerts[(offMeshVertsBase + n*2)*3];
+			dtVcopy(&v[0], &linkv[0]);
+			dtVcopy(&v[3], &linkv[3]);
+			n++;
+		}
+	}
+	
+	// Store polygons
+	// Mesh polys
+	const unsigned short* src = params->polys;
+	for (int i = 0; i < params->polyCount; ++i)
+	{
+		dtPoly* p = &navPolys[i];
+		p->vertCount = 0;
+		p->flags = params->polyFlags[i];
+		p->setArea(params->polyAreas[i]);
+		p->setType(DT_POLYTYPE_GROUND);
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (src[j] == MESH_NULL_IDX) break;
+			p->verts[j] = src[j];
+			if (src[nvp+j] & 0x8000)
+			{
+				// Border or portal edge.
+				unsigned short dir = src[nvp+j] & 0xf;
+				if (dir == 0xf) // Border
+					p->neis[j] = 0;
+				else if (dir == 0) // Portal x-
+					p->neis[j] = DT_EXT_LINK | 4;
+				else if (dir == 1) // Portal z+
+					p->neis[j] = DT_EXT_LINK | 2;
+				else if (dir == 2) // Portal x+
+					p->neis[j] = DT_EXT_LINK | 0;
+				else if (dir == 3) // Portal z-
+					p->neis[j] = DT_EXT_LINK | 6;
+			}
+			else
+			{
+				// Normal connection
+				p->neis[j] = src[nvp+j]+1;
+			}
+			
+			p->vertCount++;
+		}
+		src += nvp*2;
+	}
+	// Off-mesh connection vertices.
+	n = 0;
+	for (int i = 0; i < params->offMeshConCount; ++i)
+	{
+		// Only store connections which start from this tile.
+		if (offMeshConClass[i*2+0] == 0xff)
+		{
+			dtPoly* p = &navPolys[offMeshPolyBase+n];
+			p->vertCount = 2;
+			p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0);
+			p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1);
+			p->flags = params->offMeshConFlags[i];
+			p->setArea(params->offMeshConAreas[i]);
+			p->setType(DT_POLYTYPE_OFFMESH_CONNECTION);
+			n++;
+		}
+	}
+
+	// Store detail meshes and vertices.
+	// The nav polygon vertices are stored as the first vertices on each mesh.
+	// We compress the mesh data by skipping them and using the navmesh coordinates.
+	if (params->detailMeshes)
+	{
+		unsigned short vbase = 0;
+		for (int i = 0; i < params->polyCount; ++i)
+		{
+			dtPolyDetail& dtl = navDMeshes[i];
+			const int vb = (int)params->detailMeshes[i*4+0];
+			const int ndv = (int)params->detailMeshes[i*4+1];
+			const int nv = navPolys[i].vertCount;
+			dtl.vertBase = (unsigned int)vbase;
+			dtl.vertCount = (unsigned char)(ndv-nv);
+			dtl.triBase = (unsigned int)params->detailMeshes[i*4+2];
+			dtl.triCount = (unsigned char)params->detailMeshes[i*4+3];
+			// Copy vertices except the first 'nv' verts which are equal to nav poly verts.
+			if (ndv-nv)
+			{
+				memcpy(&navDVerts[vbase*3], &params->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv));
+				vbase += (unsigned short)(ndv-nv);
+			}
+		}
+		// Store triangles.
+		memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount);
+	}
+	else
+	{
+		// Create dummy detail mesh by triangulating polys.
+		int tbase = 0;
+		for (int i = 0; i < params->polyCount; ++i)
+		{
+			dtPolyDetail& dtl = navDMeshes[i];
+			const int nv = navPolys[i].vertCount;
+			dtl.vertBase = 0;
+			dtl.vertCount = 0;
+			dtl.triBase = (unsigned int)tbase;
+			dtl.triCount = (unsigned char)(nv-2);
+			// Triangulate polygon (local indices).
+			for (int j = 2; j < nv; ++j)
+			{
+				unsigned char* t = &navDTris[tbase*4];
+				t[0] = 0;
+				t[1] = (unsigned char)(j-1);
+				t[2] = (unsigned char)j;
+				// Bit for each edge that belongs to poly boundary.
+				t[3] = (1<<2);
+				if (j == 2) t[3] |= (1<<0);
+				if (j == nv-1) t[3] |= (1<<4);
+				tbase++;
+			}
+		}
+	}
+
+	// Store and create BVtree.
+	if (params->buildBvTree)
+	{
+		createBVTree(params, navBvtree, 2*params->polyCount);
+	}
+	
+	// Store Off-Mesh connections.
+	n = 0;
+	for (int i = 0; i < params->offMeshConCount; ++i)
+	{
+		// Only store connections which start from this tile.
+		if (offMeshConClass[i*2+0] == 0xff)
+		{
+			dtOffMeshConnection* con = &offMeshCons[n];
+			con->poly = (unsigned short)(offMeshPolyBase + n);
+			// Copy connection end-points.
+			const float* endPts = &params->offMeshConVerts[i*2*3];
+			dtVcopy(&con->pos[0], &endPts[0]);
+			dtVcopy(&con->pos[3], &endPts[3]);
+			con->rad = params->offMeshConRad[i];
+			con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0;
+			con->side = offMeshConClass[i*2+1];
+			if (params->offMeshConUserID)
+				con->userId = params->offMeshConUserID[i];
+			n++;
+		}
+	}
+		
+	dtFree(offMeshConClass);
+	
+	*outData = data;
+	*outDataSize = dataSize;
+	
+	return true;
+}
+
+bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int /*dataSize*/)
+{
+	dtMeshHeader* header = (dtMeshHeader*)data;
+	
+	int swappedMagic = DT_NAVMESH_MAGIC;
+	int swappedVersion = DT_NAVMESH_VERSION;
+	dtSwapEndian(&swappedMagic);
+	dtSwapEndian(&swappedVersion);
+	
+	if ((header->magic != DT_NAVMESH_MAGIC || header->version != DT_NAVMESH_VERSION) &&
+		(header->magic != swappedMagic || header->version != swappedVersion))
+	{
+		return false;
+	}
+		
+	dtSwapEndian(&header->magic);
+	dtSwapEndian(&header->version);
+	dtSwapEndian(&header->x);
+	dtSwapEndian(&header->y);
+	dtSwapEndian(&header->layer);
+	dtSwapEndian(&header->userId);
+	dtSwapEndian(&header->polyCount);
+	dtSwapEndian(&header->vertCount);
+	dtSwapEndian(&header->maxLinkCount);
+	dtSwapEndian(&header->detailMeshCount);
+	dtSwapEndian(&header->detailVertCount);
+	dtSwapEndian(&header->detailTriCount);
+	dtSwapEndian(&header->bvNodeCount);
+	dtSwapEndian(&header->offMeshConCount);
+	dtSwapEndian(&header->offMeshBase);
+	dtSwapEndian(&header->walkableHeight);
+	dtSwapEndian(&header->walkableRadius);
+	dtSwapEndian(&header->walkableClimb);
+	dtSwapEndian(&header->bmin[0]);
+	dtSwapEndian(&header->bmin[1]);
+	dtSwapEndian(&header->bmin[2]);
+	dtSwapEndian(&header->bmax[0]);
+	dtSwapEndian(&header->bmax[1]);
+	dtSwapEndian(&header->bmax[2]);
+	dtSwapEndian(&header->bvQuantFactor);
+
+	// Freelist index and pointers are updated when tile is added, no need to swap.
+
+	return true;
+}
+
+/// @par
+///
+/// @warning This function assumes that the header is in the correct endianess already. 
+/// Call #dtNavMeshHeaderSwapEndian() first on the data if the data is expected to be in wrong endianess 
+/// to start with. Call #dtNavMeshHeaderSwapEndian() after the data has been swapped if converting from 
+/// native to foreign endianess.
+bool dtNavMeshDataSwapEndian(unsigned char* data, const int /*dataSize*/)
+{
+	// 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;
+	
+	// Patch header pointers.
+	const int headerSize = dtAlign4(sizeof(dtMeshHeader));
+	const int vertsSize = dtAlign4(sizeof(float)*3*header->vertCount);
+	const int polysSize = dtAlign4(sizeof(dtPoly)*header->polyCount);
+	const int linksSize = dtAlign4(sizeof(dtLink)*(header->maxLinkCount));
+	const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*header->detailMeshCount);
+	const int detailVertsSize = dtAlign4(sizeof(float)*3*header->detailVertCount);
+	const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*header->detailTriCount);
+	const int bvtreeSize = dtAlign4(sizeof(dtBVNode)*header->bvNodeCount);
+	const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount);
+	
+	unsigned char* d = data + headerSize;
+	float* verts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
+	dtPoly* polys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
+	d += linksSize; // Ignore links; they technically should be endian-swapped but all their data is overwritten on load anyway.
+	//dtLink* links = dtGetThenAdvanceBufferPointer<dtLink>(d, linksSize);
+	dtPolyDetail* detailMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
+	float* detailVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
+	d += detailTrisSize; // Ignore detail tris; single bytes can't be endian-swapped.
+	//unsigned char* detailTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
+	dtBVNode* bvTree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvtreeSize);
+	dtOffMeshConnection* offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshLinksSize);
+	
+	// Vertices
+	for (int i = 0; i < header->vertCount*3; ++i)
+	{
+		dtSwapEndian(&verts[i]);
+	}
+
+	// Polys
+	for (int i = 0; i < header->polyCount; ++i)
+	{
+		dtPoly* p = &polys[i];
+		// poly->firstLink is update when tile is added, no need to swap.
+		for (int j = 0; j < DT_VERTS_PER_POLYGON; ++j)
+		{
+			dtSwapEndian(&p->verts[j]);
+			dtSwapEndian(&p->neis[j]);
+		}
+		dtSwapEndian(&p->flags);
+	}
+
+	// Links are rebuild when tile is added, no need to swap.
+
+	// Detail meshes
+	for (int i = 0; i < header->detailMeshCount; ++i)
+	{
+		dtPolyDetail* pd = &detailMeshes[i];
+		dtSwapEndian(&pd->vertBase);
+		dtSwapEndian(&pd->triBase);
+	}
+	
+	// Detail verts
+	for (int i = 0; i < header->detailVertCount*3; ++i)
+	{
+		dtSwapEndian(&detailVerts[i]);
+	}
+
+	// BV-tree
+	for (int i = 0; i < header->bvNodeCount; ++i)
+	{
+		dtBVNode* node = &bvTree[i];
+		for (int j = 0; j < 3; ++j)
+		{
+			dtSwapEndian(&node->bmin[j]);
+			dtSwapEndian(&node->bmax[j]);
+		}
+		dtSwapEndian(&node->i);
+	}
+
+	// Off-mesh Connections.
+	for (int i = 0; i < header->offMeshConCount; ++i)
+	{
+		dtOffMeshConnection* con = &offMeshCons[i];
+		for (int j = 0; j < 6; ++j)
+			dtSwapEndian(&con->pos[j]);
+		dtSwapEndian(&con->rad);
+		dtSwapEndian(&con->poly);
+	}
+	
+	return true;
+}

lib/haxerecast/recastnavigation/Detour/Source/DetourNode.cpp

@@ -0,0 +1,200 @@
+//
+// 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 "DetourNode.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+#include "DetourCommon.h"
+#include <string.h>
+
+#ifdef DT_POLYREF64
+// From Thomas Wang, https://gist.github.com/badboy/6267743
+inline unsigned int dtHashRef(dtPolyRef a)
+{
+	a = (~a) + (a << 18); // a = (a << 18) - a - 1;
+	a = a ^ (a >> 31);
+	a = a * 21; // a = (a + (a << 2)) + (a << 4);
+	a = a ^ (a >> 11);
+	a = a + (a << 6);
+	a = a ^ (a >> 22);
+	return (unsigned int)a;
+}
+#else
+inline unsigned int dtHashRef(dtPolyRef a)
+{
+	a += ~(a<<15);
+	a ^=  (a>>10);
+	a +=  (a<<3);
+	a ^=  (a>>6);
+	a += ~(a<<11);
+	a ^=  (a>>16);
+	return (unsigned int)a;
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////////////
+dtNodePool::dtNodePool(int maxNodes, int hashSize) :
+	m_nodes(0),
+	m_first(0),
+	m_next(0),
+	m_maxNodes(maxNodes),
+	m_hashSize(hashSize),
+	m_nodeCount(0)
+{
+	dtAssert(dtNextPow2(m_hashSize) == (unsigned int)m_hashSize);
+	// pidx is special as 0 means "none" and 1 is the first node. For that reason
+	// we have 1 fewer nodes available than the number of values it can contain.
+	dtAssert(m_maxNodes > 0 && m_maxNodes <= DT_NULL_IDX && m_maxNodes <= (1 << DT_NODE_PARENT_BITS) - 1);
+
+	m_nodes = (dtNode*)dtAlloc(sizeof(dtNode)*m_maxNodes, DT_ALLOC_PERM);
+	m_next = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*m_maxNodes, DT_ALLOC_PERM);
+	m_first = (dtNodeIndex*)dtAlloc(sizeof(dtNodeIndex)*hashSize, DT_ALLOC_PERM);
+
+	dtAssert(m_nodes);
+	dtAssert(m_next);
+	dtAssert(m_first);
+
+	memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize);
+	memset(m_next, 0xff, sizeof(dtNodeIndex)*m_maxNodes);
+}
+
+dtNodePool::~dtNodePool()
+{
+	dtFree(m_nodes);
+	dtFree(m_next);
+	dtFree(m_first);
+}
+
+void dtNodePool::clear()
+{
+	memset(m_first, 0xff, sizeof(dtNodeIndex)*m_hashSize);
+	m_nodeCount = 0;
+}
+
+unsigned int dtNodePool::findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes)
+{
+	int n = 0;
+	unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
+	dtNodeIndex i = m_first[bucket];
+	while (i != DT_NULL_IDX)
+	{
+		if (m_nodes[i].id == id)
+		{
+			if (n >= maxNodes)
+				return n;
+			nodes[n++] = &m_nodes[i];
+		}
+		i = m_next[i];
+	}
+
+	return n;
+}
+
+dtNode* dtNodePool::findNode(dtPolyRef id, unsigned char state)
+{
+	unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
+	dtNodeIndex i = m_first[bucket];
+	while (i != DT_NULL_IDX)
+	{
+		if (m_nodes[i].id == id && m_nodes[i].state == state)
+			return &m_nodes[i];
+		i = m_next[i];
+	}
+	return 0;
+}
+
+dtNode* dtNodePool::getNode(dtPolyRef id, unsigned char state)
+{
+	unsigned int bucket = dtHashRef(id) & (m_hashSize-1);
+	dtNodeIndex i = m_first[bucket];
+	dtNode* node = 0;
+	while (i != DT_NULL_IDX)
+	{
+		if (m_nodes[i].id == id && m_nodes[i].state == state)
+			return &m_nodes[i];
+		i = m_next[i];
+	}
+	
+	if (m_nodeCount >= m_maxNodes)
+		return 0;
+	
+	i = (dtNodeIndex)m_nodeCount;
+	m_nodeCount++;
+	
+	// Init node
+	node = &m_nodes[i];
+	node->pidx = 0;
+	node->cost = 0;
+	node->total = 0;
+	node->id = id;
+	node->state = state;
+	node->flags = 0;
+	
+	m_next[i] = m_first[bucket];
+	m_first[bucket] = i;
+	
+	return node;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+dtNodeQueue::dtNodeQueue(int n) :
+	m_heap(0),
+	m_capacity(n),
+	m_size(0)
+{
+	dtAssert(m_capacity > 0);
+	
+	m_heap = (dtNode**)dtAlloc(sizeof(dtNode*)*(m_capacity+1), DT_ALLOC_PERM);
+	dtAssert(m_heap);
+}
+
+dtNodeQueue::~dtNodeQueue()
+{
+	dtFree(m_heap);
+}
+
+void dtNodeQueue::bubbleUp(int i, dtNode* node)
+{
+	int parent = (i-1)/2;
+	// note: (index > 0) means there is a parent
+	while ((i > 0) && (m_heap[parent]->total > node->total))
+	{
+		m_heap[i] = m_heap[parent];
+		i = parent;
+		parent = (i-1)/2;
+	}
+	m_heap[i] = node;
+}
+
+void dtNodeQueue::trickleDown(int i, dtNode* node)
+{
+	int child = (i*2)+1;
+	while (child < m_size)
+	{
+		if (((child+1) < m_size) && 
+			(m_heap[child]->total > m_heap[child+1]->total))
+		{
+			child++;
+		}
+		m_heap[i] = m_heap[child];
+		i = child;
+		child = (i*2)+1;
+	}
+	bubbleUp(i, node);
+}