makepanda for recastdetour integration

makepanda/makepanda.py

@@ -95,6 +95,7 @@ PkgListSet(["PYTHON", "DIRECT",                        # Python support
   "PANDAPARTICLESYSTEM",                               # Built in particle system
   "CONTRIB",                                           # Experimental
   "SSE2", "NEON",                                      # Compiler features
+  "RECASTDETOUR",				       # Recast Detour Toolset
 ])
 
 CheckPandaSourceTree()
@@ -2149,6 +2150,9 @@ def CompileAnything(target, inputs, opts, progress = None):
         if (infile.endswith(".cxx")):
             ProgressOutput(progress, "Building C++ object", target)
             return CompileCxx(target, infile, opts)
+        if (infile.endswith(".cpp")):
+            ProgressOutput(progress, "Building C++ object", target)
+            return CompileCxx(target, infile, opts)
         elif (infile.endswith(".c")):
             ProgressOutput(progress, "Building C object", target)
             return CompileCxx(target, infile, opts)
@@ -2636,6 +2640,8 @@ if not PkgSkip("DIRECT"):
     panda_modules.append('direct')
 if not PkgSkip("VISION"):
     panda_modules.append('vision')
+if not PkgSkip("RECASTDETOUR"):
+    panda_modules.append('recastdetour')
 if not PkgSkip("SKEL"):
     panda_modules.append('skel')
 if not PkgSkip("EGG"):
@@ -3041,6 +3047,8 @@ CopyAllHeaders('panda/src/text')
 CopyAllHeaders('panda/src/grutil')
 if (PkgSkip("VISION")==0):
     CopyAllHeaders('panda/src/vision')
+if (PkgSkip("RECASTDETOUR")==0):
+    CopyAllHeaders('panda/src/recastdetour')
 if (PkgSkip("FFMPEG")==0):
     CopyAllHeaders('panda/src/ffmpeg')
 CopyAllHeaders('panda/src/tform')
@@ -4117,6 +4125,95 @@ if (PkgSkip("VISION") == 0):
   PyTargetAdd('vision.pyd', input='libp3interrogatedb.dll')
   PyTargetAdd('vision.pyd', input=COMMON_PANDA_LIBS)
 
+#
+# DIRECTORY: panda/src/recastdetour/
+#
+
+if (PkgSkip("RECASTDETOUR") == 0):
+  OPTS=['DIR:panda/src/recastdetour', 'BUILDING:RECASTDETOUR']
+  TargetAdd('p3recastdetour_composite1.obj', opts=OPTS, input='p3recastdetour_composite1.cxx')
+  TargetAdd('p3recastdetour_DebugDraw.obj', opts=OPTS,  input='DebugDraw.cpp')
+  TargetAdd('p3recastdetour_DetourAlloc.obj', opts=OPTS,  input='DetourAlloc.cpp')
+  TargetAdd('p3recastdetour_DetourAssert.obj', opts=OPTS,  input='DetourAssert.cpp')
+  TargetAdd('p3recastdetour_DetourCommon.obj', opts=OPTS,  input='DetourCommon.cpp')
+  TargetAdd('p3recastdetour_DetourCrowd.obj', opts=OPTS,  input='DetourCrowd.cpp')
+  TargetAdd('p3recastdetour_DetourDebugDraw.obj', opts=OPTS,  input='DetourDebugDraw.cpp')
+  TargetAdd('p3recastdetour_DetourLocalBoundary.obj', opts=OPTS,  input='DetourLocalBoundary.cpp')
+  TargetAdd('p3recastdetour_DetourNavMesh.obj', opts=OPTS,  input='DetourNavMesh.cpp')
+  TargetAdd('p3recastdetour_DetourNavMeshBuilder.obj', opts=OPTS,  input='DetourNavMeshBuilder.cpp')
+  TargetAdd('p3recastdetour_DetourNavMeshQuery.obj', opts=OPTS,  input='DetourNavMeshQuery.cpp')
+  TargetAdd('p3recastdetour_DetourNode.obj', opts=OPTS,  input='DetourNode.cpp')
+  TargetAdd('p3recastdetour_DetourObstacleAvoidance.obj', opts=OPTS,  input='DetourObstacleAvoidance.cpp')
+  TargetAdd('p3recastdetour_DetourPathCorridor.obj', opts=OPTS,  input='DetourPathCorridor.cpp')
+  TargetAdd('p3recastdetour_DetourPathQueue.obj', opts=OPTS,  input='DetourPathQueue.cpp')
+  TargetAdd('p3recastdetour_DetourProximityGrid.obj', opts=OPTS,  input='DetourProximityGrid.cpp')
+  TargetAdd('p3recastdetour_DetourTileCache.obj', opts=OPTS,  input='DetourTileCache.cpp')
+  TargetAdd('p3recastdetour_DetourTileCacheBuilder.obj', opts=OPTS,  input='DetourTileCacheBuilder.cpp')
+  TargetAdd('p3recastdetour_Recast.obj', opts=OPTS,  input='Recast.cpp')
+  TargetAdd('p3recastdetour_RecastAlloc.obj', opts=OPTS,  input='RecastAlloc.cpp')
+  TargetAdd('p3recastdetour_RecastArea.obj', opts=OPTS,  input='RecastArea.cpp')
+  TargetAdd('p3recastdetour_RecastAssert.obj', opts=OPTS,  input='RecastAssert.cpp')
+  TargetAdd('p3recastdetour_RecastContour.obj', opts=OPTS,  input='RecastContour.cpp')
+  TargetAdd('p3recastdetour_RecastDebugDraw.obj', opts=OPTS,  input='RecastDebugDraw.cpp')
+  TargetAdd('p3recastdetour_RecastDump.obj', opts=OPTS,  input='RecastDump.cpp')
+  TargetAdd('p3recastdetour_RecastFilter.obj', opts=OPTS,  input='RecastFilter.cpp')
+  TargetAdd('p3recastdetour_RecastLayers.obj', opts=OPTS,  input='RecastLayers.cpp')
+  TargetAdd('p3recastdetour_RecastMesh.obj', opts=OPTS,  input='RecastMesh.cpp')
+  TargetAdd('p3recastdetour_RecastMeshDetail.obj', opts=OPTS,  input='RecastMeshDetail.cpp')
+  TargetAdd('p3recastdetour_RecastRasterization.obj', opts=OPTS,  input='RecastRasterization.cpp')
+  TargetAdd('p3recastdetour_RecastRegion.obj', opts=OPTS,  input='RecastRegion.cpp')
+
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_composite1.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DebugDraw.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourAlloc.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourAssert.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourCommon.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourCrowd.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourDebugDraw.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourLocalBoundary.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourNavMesh.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourNavMeshBuilder.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourNavMeshQuery.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourNode.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourObstacleAvoidance.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourPathCorridor.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourPathQueue.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourProximityGrid.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourTileCache.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_DetourTileCacheBuilder.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_Recast.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastAlloc.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastArea.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastAssert.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastContour.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastDebugDraw.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastDump.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastFilter.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastLayers.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastMesh.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastMeshDetail.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastRasterization.obj')
+  TargetAdd('libp3recastdetour.dll', input='p3recastdetour_RecastRegion.obj')
+  TargetAdd('libp3recastdetour.dll', input=COMMON_PANDA_LIBS)
+  TargetAdd('libp3recastdetour.dll', opts=OPTS)
+
+  OPTS=['DIR:panda/src/recastdetour']
+  IGATEFILES = ["config_recastdetour.h", "p3recastdetour_composite1.cxx"]
+  TargetAdd('libp3recastdetour.in', opts=OPTS, input=IGATEFILES)
+  TargetAdd('libp3recastdetour.in', opts=['IMOD:panda3d.recastdetour', 'ILIB:libp3recastdetour', 'SRCDIR:panda/src/recastdetour'])
+
+
+  PyTargetAdd('recastdetour_module.obj', input='libp3recastdetour.in')
+  PyTargetAdd('recastdetour_module.obj', opts=OPTS)
+  PyTargetAdd('recastdetour_module.obj', opts=['IMOD:panda3d.recastdetour', 'ILIB:recastdetour', 'IMPORT:panda3d.core'])
+
+  PyTargetAdd('recastdetour.pyd', input='recastdetour_module.obj')
+  PyTargetAdd('recastdetour.pyd', input='libp3recastdetour_igate.obj')
+  PyTargetAdd('recastdetour.pyd', input='libp3recastdetour.dll')
+  PyTargetAdd('recastdetour.pyd', input='libp3interrogatedb.dll')
+  PyTargetAdd('recastdetour.pyd', input=COMMON_PANDA_LIBS)
+
+
 #
 # DIRECTORY: panda/src/p3skel
 #

makepanda/makepanda.vcproj

@@ -648,6 +648,71 @@
 				<File RelativePath="..\panda\src\vision\webcamVideo.cxx"></File>
 				<File RelativePath="..\panda\src\vision\arToolKit.cxx"></File>
 			</Filter>
+			<Filter Name="recastdetour">
+				<File RelativePath="..\panda\src\recastdetour\InputGeom.cxx"></File>
+				<File RelativePath="..\panda\src\recastdetour\InputGeom.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\MeshLoaderObj.cxx"></File>
+				<File RelativePath="..\panda\src\recastdetour\MeshLoaderObj.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\NavMeshSample.cxx"></File>
+				<File RelativePath="..\panda\src\recastdetour\NavMeshSample.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DebugDraw.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DebugDraw.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourAlloc.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourAlloc.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourAssert.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourAssert.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourCommon.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourCommon.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourCrowd.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourCrowd.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourDebugDraw.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourDebugDraw.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourLocalBoundary.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourLocalBoundary.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourMath.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNavMesh.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNavMesh.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNavMeshBuilder.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNavMeshBuilder.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNavMeshQuery.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNavMeshQuery.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNode.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourNode.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourObstacleAvoidance.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourObstacleAvoidance.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourPathCorridor.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourPathCorridor.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourPathQueue.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourPathQueue.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourProximityGrid.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourProximityGrid.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourStatus.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourTileCache.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourTileCache.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourTileCacheBuilder.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\DetourTileCacheBuilder.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\Recast.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\Recast.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastAlloc.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastAlloc.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastArea.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastAssert.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastAssert.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastContour.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastDebugDraw.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastDebugDraw.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastDump.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastDump.h"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastFilter.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastLayers.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastMesh.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastMeshDetail.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastRasterization.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\RecastRegion.cpp"></File>
+				<File RelativePath="..\panda\src\recastdetour\recastdetour_composite1.cxx"></File>
+				<File RelativePath="..\panda\src\recastdetour\config_recastdetour.cxx"></File>
+				<File RelativePath="..\panda\src\recastdetour\config_recastdetour.h"></File>
+			</Filter>
 			<Filter Name="gobj">
 				<File RelativePath="..\panda\src\gobj\geomEnums.cxx"></File>
 				<File RelativePath="..\panda\src\gobj\geomContext.h"></File>

makepanda/makepandacore.py

@@ -3253,6 +3253,7 @@ def CalcLocation(fn, ipath):
 
     if (fn == "AndroidManifest.xml"): return OUTPUTDIR+"/"+fn
     if (fn.endswith(".cxx")): return CxxFindSource(fn, ipath)
+    if (fn.endswith(".cpp")): return CxxFindSource(fn, ipath)
     if (fn.endswith(".I")):   return CxxFindSource(fn, ipath)
     if (fn.endswith(".h")):   return CxxFindSource(fn, ipath)
     if (fn.endswith(".c")):   return CxxFindSource(fn, ipath)

panda/src/pandabase/pandasymbols.h

@@ -579,6 +579,14 @@
   #define EXPTP_VISION IMPORT_TEMPL
 #endif
 
+#ifdef BUILDING_RECASTDETOUR
+  #define EXPCL_RECASTDETOUR EXPORT_CLASS
+  #define EXPTP_RECASTDETOUR EXPORT_TEMPL
+#else
+  #define EXPCL_RECASTDETOUR IMPORT_CLASS
+  #define EXPTP_RECASTDETOUR IMPORT_TEMPL
+#endif
+
 #ifdef BUILDING_VRPN
   #define EXPCL_VRPN EXPORT_CLASS
   #define EXPTP_VRPN EXPORT_TEMPL

panda/src/recastdetour/DebugDraw.cpp

@@ -0,0 +1,612 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <string.h>
+#include "DebugDraw.h"
+#include "DetourMath.h"
+#include "DetourNavMesh.h"
+
+
+duDebugDraw::~duDebugDraw()
+{
+	// Empty
+}
+
+unsigned int duDebugDraw::areaToCol(unsigned int area)
+{
+	if (area == 0)
+	{
+		// Treat zero area type as default.
+		return duRGBA(0, 192, 255, 255);
+	}
+	else
+	{
+		return duIntToCol(area, 255);
+	}
+}
+
+inline int bit(int a, int b)
+{
+	return (a & (1 << b)) >> b;
+}
+
+unsigned int duIntToCol(int i, int a)
+{
+	int	r = bit(i, 1) + bit(i, 3) * 2 + 1;
+	int	g = bit(i, 2) + bit(i, 4) * 2 + 1;
+	int	b = bit(i, 0) + bit(i, 5) * 2 + 1;
+	return duRGBA(r*63,g*63,b*63,a);
+}
+
+void duIntToCol(int i, float* col)
+{
+	int	r = bit(i, 0) + bit(i, 3) * 2 + 1;
+	int	g = bit(i, 1) + bit(i, 4) * 2 + 1;
+	int	b = bit(i, 2) + bit(i, 5) * 2 + 1;
+	col[0] = 1 - r*63.0f/255.0f;
+	col[1] = 1 - g*63.0f/255.0f;
+	col[2] = 1 - b*63.0f/255.0f;
+}
+
+void duCalcBoxColors(unsigned int* colors, unsigned int colTop, unsigned int colSide)
+{
+	if (!colors) return;
+	
+	colors[0] = duMultCol(colTop, 250);
+	colors[1] = duMultCol(colSide, 140);
+	colors[2] = duMultCol(colSide, 165);
+	colors[3] = duMultCol(colSide, 217);
+	colors[4] = duMultCol(colSide, 165);
+	colors[5] = duMultCol(colSide, 217);
+}
+
+void duDebugDrawCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+							 float maxx, float maxy, float maxz, unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	duAppendCylinderWire(dd, minx,miny,minz, maxx,maxy,maxz, col);
+	dd->end();
+}
+
+void duDebugDrawBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+						float maxx, float maxy, float maxz, unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	duAppendBoxWire(dd, minx,miny,minz, maxx,maxy,maxz, col);
+	dd->end();
+}
+
+void duDebugDrawArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+					const float x1, const float y1, const float z1, const float h,
+					const float as0, const float as1, unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	duAppendArc(dd, x0,y0,z0, x1,y1,z1, h, as0, as1, col);
+	dd->end();
+}
+
+void duDebugDrawArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+					  const float x1, const float y1, const float z1,
+					  const float as0, const float as1, unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	duAppendArrow(dd, x0,y0,z0, x1,y1,z1, as0, as1, col);
+	dd->end();
+}
+
+void duDebugDrawCircle(struct duDebugDraw* dd, const float x, const float y, const float z,
+					   const float r, unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	duAppendCircle(dd, x,y,z, r, col);
+	dd->end();
+}
+
+void duDebugDrawCross(struct duDebugDraw* dd, const float x, const float y, const float z,
+					  const float size, unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	duAppendCross(dd, x,y,z, size, col);
+	dd->end();
+}
+
+void duDebugDrawBox(struct duDebugDraw* dd, float minx, float miny, float minz,
+					float maxx, float maxy, float maxz, const unsigned int* fcol)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_QUADS);
+	duAppendBox(dd, minx,miny,minz, maxx,maxy,maxz, fcol);
+	dd->end();
+}
+
+void duDebugDrawCylinder(struct duDebugDraw* dd, float minx, float miny, float minz,
+						 float maxx, float maxy, float maxz, unsigned int col)
+{
+	if (!dd) return;
+	
+	dd->begin(DU_DRAW_TRIS);
+	duAppendCylinder(dd, minx,miny,minz, maxx,maxy,maxz, col);
+	dd->end();
+}
+
+void duDebugDrawGridXZ(struct duDebugDraw* dd, const float ox, const float oy, const float oz,
+					   const int w, const int h, const float size,
+					   const unsigned int col, const float lineWidth)
+{
+	if (!dd) return;
+
+	dd->begin(DU_DRAW_LINES, lineWidth);
+	for (int i = 0; i <= h; ++i)
+	{
+		dd->vertex(ox,oy,oz+i*size, col);
+		dd->vertex(ox+w*size,oy,oz+i*size, col);
+	}
+	for (int i = 0; i <= w; ++i)
+	{
+		dd->vertex(ox+i*size,oy,oz, col);
+		dd->vertex(ox+i*size,oy,oz+h*size, col);
+	}
+	dd->end();
+}
+		 
+
+void duAppendCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+						  float maxx, float maxy, float maxz, unsigned int col)
+{
+	if (!dd) return;
+
+	static const int NUM_SEG = 16;
+	static float dir[NUM_SEG*2];
+	static bool init = false;
+	if (!init)
+	{
+		init = true;
+		for (int i = 0; i < NUM_SEG; ++i)
+		{
+			const float a = (float)i/(float)NUM_SEG*DU_PI*2;
+			dir[i*2] = dtMathCosf(a);
+			dir[i*2+1] = dtMathSinf(a);
+		}
+	}
+	
+	const float cx = (maxx + minx)/2;
+	const float cz = (maxz + minz)/2;
+	const float rx = (maxx - minx)/2;
+	const float rz = (maxz - minz)/2;
+	
+	for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++)
+	{
+		dd->vertex(cx+dir[j*2+0]*rx, miny, cz+dir[j*2+1]*rz, col);
+		dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col);
+		dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col);
+		dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col);
+	}
+	for (int i = 0; i < NUM_SEG; i += NUM_SEG/4)
+	{
+		dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col);
+		dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col);
+	}
+}
+
+void duAppendBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+					 float maxx, float maxy, float maxz, unsigned int col)
+{
+	if (!dd) return;
+	// Top
+	dd->vertex(minx, miny, minz, col);
+	dd->vertex(maxx, miny, minz, col);
+	dd->vertex(maxx, miny, minz, col);
+	dd->vertex(maxx, miny, maxz, col);
+	dd->vertex(maxx, miny, maxz, col);
+	dd->vertex(minx, miny, maxz, col);
+	dd->vertex(minx, miny, maxz, col);
+	dd->vertex(minx, miny, minz, col);
+	
+	// bottom
+	dd->vertex(minx, maxy, minz, col);
+	dd->vertex(maxx, maxy, minz, col);
+	dd->vertex(maxx, maxy, minz, col);
+	dd->vertex(maxx, maxy, maxz, col);
+	dd->vertex(maxx, maxy, maxz, col);
+	dd->vertex(minx, maxy, maxz, col);
+	dd->vertex(minx, maxy, maxz, col);
+	dd->vertex(minx, maxy, minz, col);
+	
+	// Sides
+	dd->vertex(minx, miny, minz, col);
+	dd->vertex(minx, maxy, minz, col);
+	dd->vertex(maxx, miny, minz, col);
+	dd->vertex(maxx, maxy, minz, col);
+	dd->vertex(maxx, miny, maxz, col);
+	dd->vertex(maxx, maxy, maxz, col);
+	dd->vertex(minx, miny, maxz, col);
+	dd->vertex(minx, maxy, maxz, col);
+}
+
+void duAppendBoxPoints(struct duDebugDraw* dd, float minx, float miny, float minz,
+					   float maxx, float maxy, float maxz, unsigned int col)
+{
+	if (!dd) return;
+	// Top
+	dd->vertex(minx, miny, minz, col);
+	dd->vertex(maxx, miny, minz, col);
+	dd->vertex(maxx, miny, minz, col);
+	dd->vertex(maxx, miny, maxz, col);
+	dd->vertex(maxx, miny, maxz, col);
+	dd->vertex(minx, miny, maxz, col);
+	dd->vertex(minx, miny, maxz, col);
+	dd->vertex(minx, miny, minz, col);
+	
+	// bottom
+	dd->vertex(minx, maxy, minz, col);
+	dd->vertex(maxx, maxy, minz, col);
+	dd->vertex(maxx, maxy, minz, col);
+	dd->vertex(maxx, maxy, maxz, col);
+	dd->vertex(maxx, maxy, maxz, col);
+	dd->vertex(minx, maxy, maxz, col);
+	dd->vertex(minx, maxy, maxz, col);
+	dd->vertex(minx, maxy, minz, col);
+}
+
+void duAppendBox(struct duDebugDraw* dd, float minx, float miny, float minz,
+				 float maxx, float maxy, float maxz, const unsigned int* fcol)
+{
+	if (!dd) return;
+	const float verts[8*3] =
+	{
+		minx, miny, minz,
+		maxx, miny, minz,
+		maxx, miny, maxz,
+		minx, miny, maxz,
+		minx, maxy, minz,
+		maxx, maxy, minz,
+		maxx, maxy, maxz,
+		minx, maxy, maxz,
+	};
+	static const unsigned char inds[6*4] =
+	{
+		7, 6, 5, 4,
+		0, 1, 2, 3,
+		1, 5, 6, 2,
+		3, 7, 4, 0,
+		2, 6, 7, 3,
+		0, 4, 5, 1,
+	};
+	
+	const unsigned char* in = inds;
+	for (int i = 0; i < 6; ++i)
+	{
+		dd->vertex(&verts[*in*3], fcol[i]); in++;
+		dd->vertex(&verts[*in*3], fcol[i]); in++;
+		dd->vertex(&verts[*in*3], fcol[i]); in++;
+		dd->vertex(&verts[*in*3], fcol[i]); in++;
+	}
+}
+
+void duAppendCylinder(struct duDebugDraw* dd, float minx, float miny, float minz,
+					  float maxx, float maxy, float maxz, unsigned int col)
+{
+	if (!dd) return;
+	
+	static const int NUM_SEG = 16;
+	static float dir[NUM_SEG*2];
+	static bool init = false;
+	if (!init)
+	{
+		init = true;
+		for (int i = 0; i < NUM_SEG; ++i)
+		{
+			const float a = (float)i/(float)NUM_SEG*DU_PI*2;
+			dir[i*2] = cosf(a);
+			dir[i*2+1] = sinf(a);
+		}
+	}
+	
+	unsigned int col2 = duMultCol(col, 160);
+	
+	const float cx = (maxx + minx)/2;
+	const float cz = (maxz + minz)/2;
+	const float rx = (maxx - minx)/2;
+	const float rz = (maxz - minz)/2;
+
+	for (int i = 2; i < NUM_SEG; ++i)
+	{
+		const int a = 0, b = i-1, c = i;
+		dd->vertex(cx+dir[a*2+0]*rx, miny, cz+dir[a*2+1]*rz, col2);
+		dd->vertex(cx+dir[b*2+0]*rx, miny, cz+dir[b*2+1]*rz, col2);
+		dd->vertex(cx+dir[c*2+0]*rx, miny, cz+dir[c*2+1]*rz, col2);
+	}
+	for (int i = 2; i < NUM_SEG; ++i)
+	{
+		const int a = 0, b = i, c = i-1;
+		dd->vertex(cx+dir[a*2+0]*rx, maxy, cz+dir[a*2+1]*rz, col);
+		dd->vertex(cx+dir[b*2+0]*rx, maxy, cz+dir[b*2+1]*rz, col);
+		dd->vertex(cx+dir[c*2+0]*rx, maxy, cz+dir[c*2+1]*rz, col);
+	}
+	for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++)
+	{
+		dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col2);
+		dd->vertex(cx+dir[j*2+0]*rx, miny, cz+dir[j*2+1]*rz, col2);
+		dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col);
+
+		dd->vertex(cx+dir[i*2+0]*rx, miny, cz+dir[i*2+1]*rz, col2);
+		dd->vertex(cx+dir[j*2+0]*rx, maxy, cz+dir[j*2+1]*rz, col);
+		dd->vertex(cx+dir[i*2+0]*rx, maxy, cz+dir[i*2+1]*rz, col);
+	}
+}
+
+
+inline void evalArc(const float x0, const float y0, const float z0,
+					const float dx, const float dy, const float dz,
+					const float h, const float u, float* res)
+{
+	res[0] = x0 + dx * u;
+	res[1] = y0 + dy * u + h * (1-(u*2-1)*(u*2-1));
+	res[2] = z0 + dz * u;
+}
+
+
+inline void vcross(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[1]*v2[2] - v1[2]*v2[1];
+	dest[1] = v1[2]*v2[0] - v1[0]*v2[2];
+	dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; 
+}
+
+inline void vnormalize(float* v)
+{
+	float d = 1.0f / sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
+	v[0] *= d;
+	v[1] *= d;
+	v[2] *= d;
+}
+
+inline void vsub(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[0]-v2[0];
+	dest[1] = v1[1]-v2[1];
+	dest[2] = v1[2]-v2[2];
+}
+
+inline float vdistSqr(const float* v1, const float* v2)
+{
+	const float x = v1[0]-v2[0];
+	const float y = v1[1]-v2[1];
+	const float z = v1[2]-v2[2];
+	return x*x + y*y + z*z;
+}
+
+
+void appendArrowHead(struct duDebugDraw* dd, const float* p, const float* q,
+					 const float s, unsigned int col)
+{
+	const float eps = 0.001f;
+	if (!dd) return;
+	if (vdistSqr(p,q) < eps*eps) return;
+	float ax[3], ay[3] = {0,1,0}, az[3];
+	vsub(az, q, p);
+	vnormalize(az);
+	vcross(ax, ay, az);
+	vcross(ay, az, ax);
+	vnormalize(ay);
+
+	dd->vertex(p, col);
+//	dd->vertex(p[0]+az[0]*s+ay[0]*s/2, p[1]+az[1]*s+ay[1]*s/2, p[2]+az[2]*s+ay[2]*s/2, col);
+	dd->vertex(p[0]+az[0]*s+ax[0]*s/3, p[1]+az[1]*s+ax[1]*s/3, p[2]+az[2]*s+ax[2]*s/3, col);
+
+	dd->vertex(p, col);
+//	dd->vertex(p[0]+az[0]*s-ay[0]*s/2, p[1]+az[1]*s-ay[1]*s/2, p[2]+az[2]*s-ay[2]*s/2, col);
+	dd->vertex(p[0]+az[0]*s-ax[0]*s/3, p[1]+az[1]*s-ax[1]*s/3, p[2]+az[2]*s-ax[2]*s/3, col);
+	
+}
+
+void duAppendArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+				 const float x1, const float y1, const float z1, const float h,
+				 const float as0, const float as1, unsigned int col)
+{
+	if (!dd) return;
+	static const int NUM_ARC_PTS = 8;
+	static const float PAD = 0.05f;
+	static const float ARC_PTS_SCALE = (1.0f-PAD*2) / (float)NUM_ARC_PTS;
+	const float dx = x1 - x0;
+	const float dy = y1 - y0;
+	const float dz = z1 - z0;
+	const float len = sqrtf(dx*dx + dy*dy + dz*dz);
+	float prev[3];
+	evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD, prev);
+	for (int i = 1; i <= NUM_ARC_PTS; ++i)
+	{
+		const float u = PAD + i * ARC_PTS_SCALE;
+		float pt[3];
+		evalArc(x0,y0,z0, dx,dy,dz, len*h, u, pt);
+		dd->vertex(prev[0],prev[1],prev[2], col);
+		dd->vertex(pt[0],pt[1],pt[2], col);
+		prev[0] = pt[0]; prev[1] = pt[1]; prev[2] = pt[2];
+	}
+	
+	// End arrows
+	if (as0 > 0.001f)
+	{
+		float p[3], q[3];
+		evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD, p);
+		evalArc(x0,y0,z0, dx,dy,dz, len*h, PAD+0.05f, q);
+		appendArrowHead(dd, p, q, as0, col);
+	}
+
+	if (as1 > 0.001f)
+	{
+		float p[3], q[3];
+		evalArc(x0,y0,z0, dx,dy,dz, len*h, 1-PAD, p);
+		evalArc(x0,y0,z0, dx,dy,dz, len*h, 1-(PAD+0.05f), q);
+		appendArrowHead(dd, p, q, as1, col);
+	}
+}
+
+void duAppendArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+				   const float x1, const float y1, const float z1,
+				   const float as0, const float as1, unsigned int col)
+{
+	if (!dd) return;
+
+	dd->vertex(x0,y0,z0, col);
+	dd->vertex(x1,y1,z1, col);
+	
+	// End arrows
+	const float p[3] = {x0,y0,z0}, q[3] = {x1,y1,z1};
+	if (as0 > 0.001f)
+		appendArrowHead(dd, p, q, as0, col);
+	if (as1 > 0.001f)
+		appendArrowHead(dd, q, p, as1, col);
+}
+
+void duAppendCircle(struct duDebugDraw* dd, const float x, const float y, const float z,
+					const float r, unsigned int col)
+{
+	if (!dd) return;
+	static const int NUM_SEG = 40;
+	static float dir[40*2];
+	static bool init = false;
+	if (!init)
+	{
+		init = true;
+		for (int i = 0; i < NUM_SEG; ++i)
+		{
+			const float a = (float)i/(float)NUM_SEG*DU_PI*2;
+			dir[i*2] = cosf(a);
+			dir[i*2+1] = sinf(a);
+		}
+	}
+	
+	for (int i = 0, j = NUM_SEG-1; i < NUM_SEG; j = i++)
+	{
+		dd->vertex(x+dir[j*2+0]*r, y, z+dir[j*2+1]*r, col);
+		dd->vertex(x+dir[i*2+0]*r, y, z+dir[i*2+1]*r, col);
+	}
+}
+
+void duAppendCross(struct duDebugDraw* dd, const float x, const float y, const float z,
+				   const float s, unsigned int col)
+{
+	if (!dd) return;
+	dd->vertex(x-s,y,z, col);
+	dd->vertex(x+s,y,z, col);
+	dd->vertex(x,y-s,z, col);
+	dd->vertex(x,y+s,z, col);
+	dd->vertex(x,y,z-s, col);
+	dd->vertex(x,y,z+s, col);
+}
+
+duDisplayList::duDisplayList(int cap) :
+	m_pos(0),
+	m_color(0),
+	m_size(0),
+	m_cap(0),
+	m_depthMask(true),
+	m_prim(DU_DRAW_LINES),
+	m_primSize(1.0f)
+{
+	if (cap < 8)
+		cap = 8;
+	resize(cap);
+}
+
+duDisplayList::~duDisplayList()
+{
+	delete [] m_pos;
+	delete [] m_color;
+}
+
+void duDisplayList::resize(int cap)
+{
+	float* newPos = new float[cap*3];
+	if (m_size)
+		memcpy(newPos, m_pos, sizeof(float)*3*m_size);
+	delete [] m_pos;
+	m_pos = newPos;
+
+	unsigned int* newColor = new unsigned int[cap];
+	if (m_size)
+		memcpy(newColor, m_color, sizeof(unsigned int)*m_size);
+	delete [] m_color;
+	m_color = newColor;
+	
+	m_cap = cap;
+}
+
+void duDisplayList::clear()
+{
+	m_size = 0;
+}
+
+void duDisplayList::depthMask(bool state)
+{
+	m_depthMask = state;
+}
+
+void duDisplayList::begin(duDebugDrawPrimitives prim, float size)
+{
+	clear();
+	m_prim = prim;
+	m_primSize = size;
+}
+
+void duDisplayList::vertex(const float x, const float y, const float z, unsigned int color)
+{
+	if (m_size+1 >= m_cap)
+		resize(m_cap*2);
+	float* p = &m_pos[m_size*3];
+	p[0] = x;
+	p[1] = y;
+	p[2] = z;
+	m_color[m_size] = color;
+	m_size++;
+}
+
+void duDisplayList::vertex(const float* pos, unsigned int color)
+{
+	vertex(pos[0],pos[1],pos[2],color);
+}
+
+void duDisplayList::end()
+{
+}
+
+void duDisplayList::draw(struct duDebugDraw* dd)
+{
+	if (!dd) return;
+	if (!m_size) return;
+	dd->depthMask(m_depthMask);
+	dd->begin(m_prim, m_primSize);
+	for (int i = 0; i < m_size; ++i)
+		dd->vertex(&m_pos[i*3], m_color[i]);
+	dd->end();
+}

panda/src/recastdetour/DebugDraw.h

@@ -0,0 +1,223 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DEBUGDRAW_H
+#define DEBUGDRAW_H
+
+// Some math headers don't have PI defined.
+static const float DU_PI = 3.14159265f;
+
+enum duDebugDrawPrimitives
+{
+	DU_DRAW_POINTS,
+	DU_DRAW_LINES,
+	DU_DRAW_TRIS,
+	DU_DRAW_QUADS,	
+};
+
+/// Abstract debug draw interface.
+struct duDebugDraw
+{
+	virtual ~duDebugDraw() = 0;
+	
+	virtual void depthMask(bool state) = 0;
+
+	virtual void texture(bool state) = 0;
+
+	/// Begin drawing primitives.
+	///  @param prim [in] primitive type to draw, one of rcDebugDrawPrimitives.
+	///  @param size [in] size of a primitive, applies to point size and line width only.
+	virtual void begin(duDebugDrawPrimitives prim, float size = 1.0f) = 0;
+
+	/// Submit a vertex
+	///  @param pos [in] position of the verts.
+	///  @param color [in] color of the verts.
+	virtual void vertex(const float* pos, unsigned int color) = 0;
+
+	/// Submit a vertex
+	///  @param x,y,z [in] position of the verts.
+	///  @param color [in] color of the verts.
+	virtual void vertex(const float x, const float y, const float z, unsigned int color) = 0;
+
+	/// Submit a vertex
+	///  @param pos [in] position of the verts.
+	///  @param color [in] color of the verts.
+	virtual void vertex(const float* pos, unsigned int color, const float* uv) = 0;
+	
+	/// Submit a vertex
+	///  @param x,y,z [in] position of the verts.
+	///  @param color [in] color of the verts.
+	virtual void vertex(const float x, const float y, const float z, unsigned int color, const float u, const float v) = 0;
+	
+	/// End drawing primitives.
+	virtual void end() = 0;
+
+	/// Compute a color for given area.
+	virtual unsigned int areaToCol(unsigned int area);
+};
+
+inline unsigned int duRGBA(int r, int g, int b, int a)
+{
+	return ((unsigned int)r) | ((unsigned int)g << 8) | ((unsigned int)b << 16) | ((unsigned int)a << 24);
+}
+
+inline unsigned int duRGBAf(float fr, float fg, float fb, float fa)
+{
+	unsigned char r = (unsigned char)(fr*255.0f);
+	unsigned char g = (unsigned char)(fg*255.0f);
+	unsigned char b = (unsigned char)(fb*255.0f);
+	unsigned char a = (unsigned char)(fa*255.0f);
+	return duRGBA(r,g,b,a);
+}
+
+unsigned int duIntToCol(int i, int a);
+void duIntToCol(int i, float* col);
+
+inline unsigned int duMultCol(const unsigned int col, const unsigned int d)
+{
+	const unsigned int r = col & 0xff;
+	const unsigned int g = (col >> 8) & 0xff;
+	const unsigned int b = (col >> 16) & 0xff;
+	const unsigned int a = (col >> 24) & 0xff;
+	return duRGBA((r*d) >> 8, (g*d) >> 8, (b*d) >> 8, a);
+}
+
+inline unsigned int duDarkenCol(unsigned int col)
+{
+	return ((col >> 1) & 0x007f7f7f) | (col & 0xff000000);
+}
+
+inline unsigned int duLerpCol(unsigned int ca, unsigned int cb, unsigned int u)
+{
+	const unsigned int ra = ca & 0xff;
+	const unsigned int ga = (ca >> 8) & 0xff;
+	const unsigned int ba = (ca >> 16) & 0xff;
+	const unsigned int aa = (ca >> 24) & 0xff;
+	const unsigned int rb = cb & 0xff;
+	const unsigned int gb = (cb >> 8) & 0xff;
+	const unsigned int bb = (cb >> 16) & 0xff;
+	const unsigned int ab = (cb >> 24) & 0xff;
+	
+	unsigned int r = (ra*(255-u) + rb*u)/255;
+	unsigned int g = (ga*(255-u) + gb*u)/255;
+	unsigned int b = (ba*(255-u) + bb*u)/255;
+	unsigned int a = (aa*(255-u) + ab*u)/255;
+	return duRGBA(r,g,b,a);
+}
+
+inline unsigned int duTransCol(unsigned int c, unsigned int a)
+{
+	return (a<<24) | (c & 0x00ffffff);
+}
+
+
+void duCalcBoxColors(unsigned int* colors, unsigned int colTop, unsigned int colSide);
+
+void duDebugDrawCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+							 float maxx, float maxy, float maxz, unsigned int col, const float lineWidth);
+
+void duDebugDrawBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+						float maxx, float maxy, float maxz, unsigned int col, const float lineWidth);
+
+void duDebugDrawArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+					const float x1, const float y1, const float z1, const float h,
+					const float as0, const float as1, unsigned int col, const float lineWidth);
+
+void duDebugDrawArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+					  const float x1, const float y1, const float z1,
+					  const float as0, const float as1, unsigned int col, const float lineWidth);
+
+void duDebugDrawCircle(struct duDebugDraw* dd, const float x, const float y, const float z,
+					   const float r, unsigned int col, const float lineWidth);
+
+void duDebugDrawCross(struct duDebugDraw* dd, const float x, const float y, const float z,
+					  const float size, unsigned int col, const float lineWidth);
+
+void duDebugDrawBox(struct duDebugDraw* dd, float minx, float miny, float minz,
+					float maxx, float maxy, float maxz, const unsigned int* fcol);
+
+void duDebugDrawCylinder(struct duDebugDraw* dd, float minx, float miny, float minz,
+						 float maxx, float maxy, float maxz, unsigned int col);
+
+void duDebugDrawGridXZ(struct duDebugDraw* dd, const float ox, const float oy, const float oz,
+					   const int w, const int h, const float size,
+					   const unsigned int col, const float lineWidth);
+
+
+// Versions without begin/end, can be used to draw multiple primitives.
+void duAppendCylinderWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+						  float maxx, float maxy, float maxz, unsigned int col);
+
+void duAppendBoxWire(struct duDebugDraw* dd, float minx, float miny, float minz,
+					 float maxx, float maxy, float maxz, unsigned int col);
+
+void duAppendBoxPoints(struct duDebugDraw* dd, float minx, float miny, float minz,
+					   float maxx, float maxy, float maxz, unsigned int col);
+
+void duAppendArc(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+				 const float x1, const float y1, const float z1, const float h,
+				 const float as0, const float as1, unsigned int col);
+
+void duAppendArrow(struct duDebugDraw* dd, const float x0, const float y0, const float z0,
+				   const float x1, const float y1, const float z1,
+				   const float as0, const float as1, unsigned int col);
+
+void duAppendCircle(struct duDebugDraw* dd, const float x, const float y, const float z,
+					const float r, unsigned int col);
+
+void duAppendCross(struct duDebugDraw* dd, const float x, const float y, const float z,
+				   const float size, unsigned int col);
+
+void duAppendBox(struct duDebugDraw* dd, float minx, float miny, float minz,
+				 float maxx, float maxy, float maxz, const unsigned int* fcol);
+
+void duAppendCylinder(struct duDebugDraw* dd, float minx, float miny, float minz,
+					  float maxx, float maxy, float maxz, unsigned int col);
+
+
+class duDisplayList : public duDebugDraw
+{
+	float* m_pos;
+	unsigned int* m_color;
+	int m_size;
+	int m_cap;
+
+	bool m_depthMask;
+	duDebugDrawPrimitives m_prim;
+	float m_primSize;
+	
+	void resize(int cap);
+	
+public:
+	duDisplayList(int cap = 512);
+	~duDisplayList();
+	virtual void depthMask(bool state);
+	virtual void begin(duDebugDrawPrimitives prim, float size = 1.0f);
+	virtual void vertex(const float x, const float y, const float z, unsigned int color);
+	virtual void vertex(const float* pos, unsigned int color);
+	virtual void end();
+	void clear();
+	void draw(struct duDebugDraw* dd);
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	duDisplayList(const duDisplayList&);
+	duDisplayList& operator=(const duDisplayList&);
+};
+
+
+#endif // DEBUGDRAW_H

panda/src/recastdetour/DetourAlloc.cpp

@@ -0,0 +1,50 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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);
+}

panda/src/recastdetour/DetourAlloc.h

@@ -0,0 +1,61 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURALLOCATOR_H
+#define DETOURALLOCATOR_H
+
+#include <stddef.h>
+
+/// Provides hint values to the memory allocator on how long the
+/// memory is expected to be used.
+enum dtAllocHint
+{
+	DT_ALLOC_PERM,		///< Memory persist after a function call.
+	DT_ALLOC_TEMP		///< Memory used temporarily within a function.
+};
+
+/// A memory allocation function.
+//  @param[in]		size			The size, in bytes of memory, to allocate.
+//  @param[in]		rcAllocHint	A hint to the allocator on how long the memory is expected to be in use.
+//  @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
+///  @see dtAllocSetCustom
+typedef void* (dtAllocFunc)(size_t size, dtAllocHint hint);
+
+/// A memory deallocation function.
+///  @param[in]		ptr		A pointer to a memory block previously allocated using #dtAllocFunc.
+/// @see dtAllocSetCustom
+typedef void (dtFreeFunc)(void* ptr);
+
+/// Sets the base custom allocation functions to be used by Detour.
+///  @param[in]		allocFunc	The memory allocation function to be used by #dtAlloc
+///  @param[in]		freeFunc	The memory de-allocation function to be used by #dtFree
+void dtAllocSetCustom(dtAllocFunc *allocFunc, dtFreeFunc *freeFunc);
+
+/// Allocates a memory block.
+///  @param[in]		size	The size, in bytes of memory, to allocate.
+///  @param[in]		hint	A hint to the allocator on how long the memory is expected to be in use.
+///  @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
+/// @see dtFree
+void* dtAlloc(size_t size, dtAllocHint hint);
+
+/// Deallocates a memory block.
+///  @param[in]		ptr		A pointer to a memory block previously allocated using #dtAlloc.
+/// @see dtAlloc
+void dtFree(void* ptr);
+
+#endif

panda/src/recastdetour/DetourAssert.cpp

@@ -0,0 +1,35 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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

panda/src/recastdetour/DetourAssert.h

@@ -0,0 +1,56 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURASSERT_H
+#define DETOURASSERT_H
+
+// Note: This header file's only purpose is to include define assert.
+// Feel free to change the file and include your own implementation instead.
+
+#ifdef NDEBUG
+
+// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/
+#	define dtAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false)  
+
+#else
+
+/// An assertion failure function.
+//  @param[in]		expression  asserted expression.
+//  @param[in]		file  Filename of the failed assertion.
+//  @param[in]		line  Line number of the failed assertion.
+///  @see dtAssertFailSetCustom
+typedef void (dtAssertFailFunc)(const char* expression, const char* file, int line);
+
+/// Sets the base custom assertion failure function to be used by Detour.
+///  @param[in]		assertFailFunc	The function to be invoked in case of failure of #dtAssert
+void dtAssertFailSetCustom(dtAssertFailFunc *assertFailFunc);
+
+/// Gets the base custom assertion failure function to be used by Detour.
+dtAssertFailFunc* dtAssertFailGetCustom();
+
+#	include <assert.h> 
+#	define dtAssert(expression) \
+		{ \
+			dtAssertFailFunc* failFunc = dtAssertFailGetCustom(); \
+			if(failFunc == NULL) { assert(expression); } \
+			else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \
+		}
+
+#endif
+
+#endif // DETOURASSERT_H

panda/src/recastdetour/DetourCommon.cpp

@@ -0,0 +1,387 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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;
+}
+

panda/src/recastdetour/DetourCommon.h

@@ -0,0 +1,572 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURCOMMON_H
+#define DETOURCOMMON_H
+
+#include "DetourMath.h"
+#include <stddef.h>
+
+/**
+@defgroup detour Detour
+
+Members in this module are used to create, manipulate, and query navigation 
+meshes.
+
+@note This is a summary list of members.  Use the index or search 
+feature to find minor members.
+*/
+
+/// @name General helper functions
+/// @{
+
+/// Used to ignore a function parameter.  VS complains about unused parameters
+/// and this silences the warning.
+///  @param [in] _ Unused parameter
+template<class T> void dtIgnoreUnused(const T&) { }
+
+/// Swaps the values of the two parameters.
+///  @param[in,out]	a	Value A
+///  @param[in,out]	b	Value B
+template<class T> inline void dtSwap(T& a, T& b) { T t = a; a = b; b = t; }
+
+/// Returns the minimum of two values.
+///  @param[in]		a	Value A
+///  @param[in]		b	Value B
+///  @return The minimum of the two values.
+template<class T> inline T dtMin(T a, T b) { return a < b ? a : b; }
+
+/// Returns the maximum of two values.
+///  @param[in]		a	Value A
+///  @param[in]		b	Value B
+///  @return The maximum of the two values.
+template<class T> inline T dtMax(T a, T b) { return a > b ? a : b; }
+
+/// Returns the absolute value.
+///  @param[in]		a	The value.
+///  @return The absolute value of the specified value.
+template<class T> inline T dtAbs(T a) { return a < 0 ? -a : a; }
+
+/// Returns the square of the value.
+///  @param[in]		a	The value.
+///  @return The square of the value.
+template<class T> inline T dtSqr(T a) { return a*a; }
+
+/// Clamps the value to the specified range.
+///  @param[in]		v	The value to clamp.
+///  @param[in]		mn	The minimum permitted return value.
+///  @param[in]		mx	The maximum permitted return value.
+///  @return The value, clamped to the specified range.
+template<class T> inline T dtClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); }
+
+/// @}
+/// @name Vector helper functions.
+/// @{
+
+/// Derives the cross product of two vectors. (@p v1 x @p v2)
+///  @param[out]	dest	The cross product. [(x, y, z)]
+///  @param[in]		v1		A Vector [(x, y, z)]
+///  @param[in]		v2		A vector [(x, y, z)]
+inline void dtVcross(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[1]*v2[2] - v1[2]*v2[1];
+	dest[1] = v1[2]*v2[0] - v1[0]*v2[2];
+	dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; 
+}
+
+/// Derives the dot product of two vectors. (@p v1 . @p v2)
+///  @param[in]		v1	A Vector [(x, y, z)]
+///  @param[in]		v2	A vector [(x, y, z)]
+/// @return The dot product.
+inline float dtVdot(const float* v1, const float* v2)
+{
+	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
+}
+
+/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s))
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v1		The base vector. [(x, y, z)]
+///  @param[in]		v2		The vector to scale and add to @p v1. [(x, y, z)]
+///  @param[in]		s		The amount to scale @p v2 by before adding to @p v1.
+inline void dtVmad(float* dest, const float* v1, const float* v2, const float s)
+{
+	dest[0] = v1[0]+v2[0]*s;
+	dest[1] = v1[1]+v2[1]*s;
+	dest[2] = v1[2]+v2[2]*s;
+}
+
+/// Performs a linear interpolation between two vectors. (@p v1 toward @p v2)
+///  @param[out]	dest	The result vector. [(x, y, x)]
+///  @param[in]		v1		The starting vector.
+///  @param[in]		v2		The destination vector.
+///	 @param[in]		t		The interpolation factor. [Limits: 0 <= value <= 1.0]
+inline void dtVlerp(float* dest, const float* v1, const float* v2, const float t)
+{
+	dest[0] = v1[0]+(v2[0]-v1[0])*t;
+	dest[1] = v1[1]+(v2[1]-v1[1])*t;
+	dest[2] = v1[2]+(v2[2]-v1[2])*t;
+}
+
+/// Performs a vector addition. (@p v1 + @p v2)
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v1		The base vector. [(x, y, z)]
+///  @param[in]		v2		The vector to add to @p v1. [(x, y, z)]
+inline void dtVadd(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[0]+v2[0];
+	dest[1] = v1[1]+v2[1];
+	dest[2] = v1[2]+v2[2];
+}
+
+/// Performs a vector subtraction. (@p v1 - @p v2)
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v1		The base vector. [(x, y, z)]
+///  @param[in]		v2		The vector to subtract from @p v1. [(x, y, z)]
+inline void dtVsub(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[0]-v2[0];
+	dest[1] = v1[1]-v2[1];
+	dest[2] = v1[2]-v2[2];
+}
+
+/// Scales the vector by the specified value. (@p v * @p t)
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v		The vector to scale. [(x, y, z)]
+///  @param[in]		t		The scaling factor.
+inline void dtVscale(float* dest, const float* v, const float t)
+{
+	dest[0] = v[0]*t;
+	dest[1] = v[1]*t;
+	dest[2] = v[2]*t;
+}
+
+/// Selects the minimum value of each element from the specified vectors.
+///  @param[in,out]	mn	A vector.  (Will be updated with the result.) [(x, y, z)]
+///  @param[in]	v	A vector. [(x, y, z)]
+inline void dtVmin(float* mn, const float* v)
+{
+	mn[0] = dtMin(mn[0], v[0]);
+	mn[1] = dtMin(mn[1], v[1]);
+	mn[2] = dtMin(mn[2], v[2]);
+}
+
+/// Selects the maximum value of each element from the specified vectors.
+///  @param[in,out]	mx	A vector.  (Will be updated with the result.) [(x, y, z)]
+///  @param[in]		v	A vector. [(x, y, z)]
+inline void dtVmax(float* mx, const float* v)
+{
+	mx[0] = dtMax(mx[0], v[0]);
+	mx[1] = dtMax(mx[1], v[1]);
+	mx[2] = dtMax(mx[2], v[2]);
+}
+
+/// Sets the vector elements to the specified values.
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		x		The x-value of the vector.
+///  @param[in]		y		The y-value of the vector.
+///  @param[in]		z		The z-value of the vector.
+inline void dtVset(float* dest, const float x, const float y, const float z)
+{
+	dest[0] = x; dest[1] = y; dest[2] = z;
+}
+
+/// Performs a vector copy.
+///  @param[out]	dest	The result. [(x, y, z)]
+///  @param[in]		a		The vector to copy. [(x, y, z)]
+inline void dtVcopy(float* dest, const float* a)
+{
+	dest[0] = a[0];
+	dest[1] = a[1];
+	dest[2] = a[2];
+}
+
+/// Derives the scalar length of the vector.
+///  @param[in]		v The vector. [(x, y, z)]
+/// @return The scalar length of the vector.
+inline float dtVlen(const float* v)
+{
+	return dtMathSqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+}
+
+/// Derives the square of the scalar length of the vector. (len * len)
+///  @param[in]		v The vector. [(x, y, z)]
+/// @return The square of the scalar length of the vector.
+inline float dtVlenSqr(const float* v)
+{
+	return v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
+}
+
+/// Returns the distance between two points.
+///  @param[in]		v1	A point. [(x, y, z)]
+///  @param[in]		v2	A point. [(x, y, z)]
+/// @return The distance between the two points.
+inline float dtVdist(const float* v1, const float* v2)
+{
+	const float dx = v2[0] - v1[0];
+	const float dy = v2[1] - v1[1];
+	const float dz = v2[2] - v1[2];
+	return dtMathSqrtf(dx*dx + dy*dy + dz*dz);
+}
+
+/// Returns the square of the distance between two points.
+///  @param[in]		v1	A point. [(x, y, z)]
+///  @param[in]		v2	A point. [(x, y, z)]
+/// @return The square of the distance between the two points.
+inline float dtVdistSqr(const float* v1, const float* v2)
+{
+	const float dx = v2[0] - v1[0];
+	const float dy = v2[1] - v1[1];
+	const float dz = v2[2] - v1[2];
+	return dx*dx + dy*dy + dz*dz;
+}
+
+/// Derives the distance between the specified points on the xz-plane.
+///  @param[in]		v1	A point. [(x, y, z)]
+///  @param[in]		v2	A point. [(x, y, z)]
+/// @return The distance between the point on the xz-plane.
+///
+/// The vectors are projected onto the xz-plane, so the y-values are ignored.
+inline float dtVdist2D(const float* v1, const float* v2)
+{
+	const float dx = v2[0] - v1[0];
+	const float dz = v2[2] - v1[2];
+	return dtMathSqrtf(dx*dx + dz*dz);
+}
+
+/// Derives the square of the distance between the specified points on the xz-plane.
+///  @param[in]		v1	A point. [(x, y, z)]
+///  @param[in]		v2	A point. [(x, y, z)]
+/// @return The square of the distance between the point on the xz-plane.
+inline float dtVdist2DSqr(const float* v1, const float* v2)
+{
+	const float dx = v2[0] - v1[0];
+	const float dz = v2[2] - v1[2];
+	return dx*dx + dz*dz;
+}
+
+/// Normalizes the vector.
+///  @param[in,out]	v	The vector to normalize. [(x, y, z)]
+inline void dtVnormalize(float* v)
+{
+	float d = 1.0f / dtMathSqrtf(dtSqr(v[0]) + dtSqr(v[1]) + dtSqr(v[2]));
+	v[0] *= d;
+	v[1] *= d;
+	v[2] *= d;
+}
+
+/// Performs a 'sloppy' colocation check of the specified points.
+///  @param[in]		p0	A point. [(x, y, z)]
+///  @param[in]		p1	A point. [(x, y, z)]
+/// @return True if the points are considered to be at the same location.
+///
+/// Basically, this function will return true if the specified points are 
+/// close enough to eachother to be considered colocated.
+inline bool dtVequal(const float* p0, const float* p1)
+{
+	static const float thr = dtSqr(1.0f/16384.0f);
+	const float d = dtVdistSqr(p0, p1);
+	return d < thr;
+}
+
+/// Checks that the specified vector's components are all finite.
+///  @param[in]		v	A point. [(x, y, z)]
+/// @return True if all of the point's components are finite, i.e. not NaN
+/// or any of the infinities.
+inline bool dtVisfinite(const float* v)
+{
+	bool result =
+		dtMathIsfinite(v[0]) &&
+		dtMathIsfinite(v[1]) &&
+		dtMathIsfinite(v[2]);
+
+	return result;
+}
+
+/// Checks that the specified vector's 2D components are finite.
+///  @param[in]		v	A point. [(x, y, z)]
+inline bool dtVisfinite2D(const float* v)
+{
+	bool result = dtMathIsfinite(v[0]) && dtMathIsfinite(v[2]);
+	return result;
+}
+
+/// Derives the dot product of two vectors on the xz-plane. (@p u . @p v)
+///  @param[in]		u		A vector [(x, y, z)]
+///  @param[in]		v		A vector [(x, y, z)]
+/// @return The dot product on the xz-plane.
+///
+/// The vectors are projected onto the xz-plane, so the y-values are ignored.
+inline float dtVdot2D(const float* u, const float* v)
+{
+	return u[0]*v[0] + u[2]*v[2];
+}
+
+/// Derives the xz-plane 2D perp product of the two vectors. (uz*vx - ux*vz)
+///  @param[in]		u		The LHV vector [(x, y, z)]
+///  @param[in]		v		The RHV vector [(x, y, z)]
+/// @return The dot product on the xz-plane.
+///
+/// The vectors are projected onto the xz-plane, so the y-values are ignored.
+inline float dtVperp2D(const float* u, const float* v)
+{
+	return u[2]*v[0] - u[0]*v[2];
+}
+
+/// @}
+/// @name Computational geometry helper functions.
+/// @{
+
+/// Derives the signed xz-plane area of the triangle ABC, or the relationship of line AB to point C.
+///  @param[in]		a		Vertex A. [(x, y, z)]
+///  @param[in]		b		Vertex B. [(x, y, z)]
+///  @param[in]		c		Vertex C. [(x, y, z)]
+/// @return The signed xz-plane area of the triangle.
+inline float dtTriArea2D(const float* a, const float* b, const float* c)
+{
+	const float abx = b[0] - a[0];
+	const float abz = b[2] - a[2];
+	const float acx = c[0] - a[0];
+	const float acz = c[2] - a[2];
+	return acx*abz - abx*acz;
+}
+
+/// Determines if two axis-aligned bounding boxes overlap.
+///  @param[in]		amin	Minimum bounds of box A. [(x, y, z)]
+///  @param[in]		amax	Maximum bounds of box A. [(x, y, z)]
+///  @param[in]		bmin	Minimum bounds of box B. [(x, y, z)]
+///  @param[in]		bmax	Maximum bounds of box B. [(x, y, z)]
+/// @return True if the two AABB's overlap.
+/// @see dtOverlapBounds
+inline bool dtOverlapQuantBounds(const unsigned short amin[3], const unsigned short amax[3],
+								 const unsigned short bmin[3], const unsigned short bmax[3])
+{
+	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;
+}
+
+/// Determines if two axis-aligned bounding boxes overlap.
+///  @param[in]		amin	Minimum bounds of box A. [(x, y, z)]
+///  @param[in]		amax	Maximum bounds of box A. [(x, y, z)]
+///  @param[in]		bmin	Minimum bounds of box B. [(x, y, z)]
+///  @param[in]		bmax	Maximum bounds of box B. [(x, y, z)]
+/// @return True if the two AABB's overlap.
+/// @see dtOverlapQuantBounds
+inline bool dtOverlapBounds(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;
+}
+
+/// Derives the closest point on a triangle from the specified reference point.
+///  @param[out]	closest	The closest point on the triangle.	
+///  @param[in]		p		The reference point from which to test. [(x, y, z)]
+///  @param[in]		a		Vertex A of triangle ABC. [(x, y, z)]
+///  @param[in]		b		Vertex B of triangle ABC. [(x, y, z)]
+///  @param[in]		c		Vertex C of triangle ABC. [(x, y, z)]
+void dtClosestPtPointTriangle(float* closest, const float* p,
+							  const float* a, const float* b, const float* c);
+
+/// Derives the y-axis height of the closest point on the triangle from the specified reference point.
+///  @param[in]		p		The reference point from which to test. [(x, y, z)]
+///  @param[in]		a		Vertex A of triangle ABC. [(x, y, z)]
+///  @param[in]		b		Vertex B of triangle ABC. [(x, y, z)]
+///  @param[in]		c		Vertex C of triangle ABC. [(x, y, z)]
+///  @param[out]	h		The resulting height.
+bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h);
+
+bool dtIntersectSegmentPoly2D(const float* p0, const float* p1,
+							  const float* verts, int nverts,
+							  float& tmin, float& tmax,
+							  int& segMin, int& segMax);
+
+bool dtIntersectSegSeg2D(const float* ap, const float* aq,
+						 const float* bp, const float* bq,
+						 float& s, float& t);
+
+/// Determines if the specified point is inside the convex polygon on the xz-plane.
+///  @param[in]		pt		The point to check. [(x, y, z)]
+///  @param[in]		verts	The polygon vertices. [(x, y, z) * @p nverts]
+///  @param[in]		nverts	The number of vertices. [Limit: >= 3]
+/// @return True if the point is inside the polygon.
+bool dtPointInPolygon(const float* pt, const float* verts, const int nverts);
+
+bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts,
+							float* ed, float* et);
+
+float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t);
+
+/// Derives the centroid of a convex polygon.
+///  @param[out]	tc		The centroid of the polgyon. [(x, y, z)]
+///  @param[in]		idx		The polygon indices. [(vertIndex) * @p nidx]
+///  @param[in]		nidx	The number of indices in the polygon. [Limit: >= 3]
+///  @param[in]		verts	The polygon vertices. [(x, y, z) * vertCount]
+void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts);
+
+/// Determines if the two convex polygons overlap on the xz-plane.
+///  @param[in]		polya		Polygon A vertices.	[(x, y, z) * @p npolya]
+///  @param[in]		npolya		The number of vertices in polygon A.
+///  @param[in]		polyb		Polygon B vertices.	[(x, y, z) * @p npolyb]
+///  @param[in]		npolyb		The number of vertices in polygon B.
+/// @return True if the two polygons overlap.
+bool dtOverlapPolyPoly2D(const float* polya, const int npolya,
+						 const float* polyb, const int npolyb);
+
+/// @}
+/// @name Miscellanious functions.
+/// @{
+
+inline unsigned int dtNextPow2(unsigned int v)
+{
+	v--;
+	v |= v >> 1;
+	v |= v >> 2;
+	v |= v >> 4;
+	v |= v >> 8;
+	v |= v >> 16;
+	v++;
+	return v;
+}
+
+inline unsigned int dtIlog2(unsigned int v)
+{
+	unsigned int r;
+	unsigned int shift;
+	r = (v > 0xffff) << 4; v >>= r;
+	shift = (v > 0xff) << 3; v >>= shift; r |= shift;
+	shift = (v > 0xf) << 2; v >>= shift; r |= shift;
+	shift = (v > 0x3) << 1; v >>= shift; r |= shift;
+	r |= (v >> 1);
+	return r;
+}
+
+inline int dtAlign4(int x) { return (x+3) & ~3; }
+
+inline int dtOppositeTile(int side) { return (side+4) & 0x7; }
+
+inline void dtSwapByte(unsigned char* a, unsigned char* b)
+{
+	unsigned char tmp = *a;
+	*a = *b;
+	*b = tmp;
+}
+
+inline void dtSwapEndian(unsigned short* v)
+{
+	unsigned char* x = (unsigned char*)v;
+	dtSwapByte(x+0, x+1);
+}
+
+inline void dtSwapEndian(short* v)
+{
+	unsigned char* x = (unsigned char*)v;
+	dtSwapByte(x+0, x+1);
+}
+
+inline void dtSwapEndian(unsigned int* v)
+{
+	unsigned char* x = (unsigned char*)v;
+	dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2);
+}
+
+inline void dtSwapEndian(int* v)
+{
+	unsigned char* x = (unsigned char*)v;
+	dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2);
+}
+
+inline void dtSwapEndian(float* v)
+{
+	unsigned char* x = (unsigned char*)v;
+	dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2);
+}
+
+void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas,
+							   const float s, const float t, float* out);
+
+template<typename TypeToRetrieveAs>
+TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(const unsigned char*& buffer, const size_t distanceToAdvance)
+{
+	TypeToRetrieveAs* returnPointer = reinterpret_cast<TypeToRetrieveAs*>(buffer);
+	buffer += distanceToAdvance;
+	return returnPointer;
+}
+
+template<typename TypeToRetrieveAs>
+TypeToRetrieveAs* dtGetThenAdvanceBufferPointer(unsigned char*& buffer, const size_t distanceToAdvance)
+{
+	TypeToRetrieveAs* returnPointer = reinterpret_cast<TypeToRetrieveAs*>(buffer);
+	buffer += distanceToAdvance;
+	return returnPointer;
+}
+
+
+/// @}
+
+#endif // DETOURCOMMON_H
+
+///////////////////////////////////////////////////////////////////////////
+
+// This section contains detailed documentation for members that don't have
+// a source file. It reduces clutter in the main section of the header.
+
+/**
+
+@fn float dtTriArea2D(const float* a, const float* b, const float* c)
+@par
+
+The vertices are projected onto the xz-plane, so the y-values are ignored.
+
+This is a low cost function than can be used for various purposes.  Its main purpose
+is for point/line relationship testing.
+
+In all cases: A value of zero indicates that all vertices are collinear or represent the same point.
+(On the xz-plane.)
+
+When used for point/line relationship tests, AB usually represents a line against which
+the C point is to be tested.  In this case:
+
+A positive value indicates that point C is to the left of line AB, looking from A toward B.<br/>
+A negative value indicates that point C is to the right of lineAB, looking from A toward B.
+
+When used for evaluating a triangle:
+
+The absolute value of the return value is two times the area of the triangle when it is
+projected onto the xz-plane.
+
+A positive return value indicates:
+
+<ul>
+<li>The vertices are wrapped in the normal Detour wrap direction.</li>
+<li>The triangle's 3D face normal is in the general up direction.</li>
+</ul>
+
+A negative return value indicates:
+
+<ul>
+<li>The vertices are reverse wrapped. (Wrapped opposite the normal Detour wrap direction.)</li>
+<li>The triangle's 3D face normal is in the general down direction.</li>
+</ul>
+
+*/

panda/src/recastdetour/DetourCrowd.cpp

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

panda/src/recastdetour/DetourCrowd.h

@@ -0,0 +1,460 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURCROWD_H
+#define DETOURCROWD_H
+
+#include "DetourNavMeshQuery.h"
+#include "DetourObstacleAvoidance.h"
+#include "DetourLocalBoundary.h"
+#include "DetourPathCorridor.h"
+#include "DetourProximityGrid.h"
+#include "DetourPathQueue.h"
+
+/// The maximum number of neighbors that a crowd agent can take into account
+/// for steering decisions.
+/// @ingroup crowd
+static const int DT_CROWDAGENT_MAX_NEIGHBOURS = 6;
+
+/// The maximum number of corners a crowd agent will look ahead in the path.
+/// This value is used for sizing the crowd agent corner buffers.
+/// Due to the behavior of the crowd manager, the actual number of useful
+/// corners will be one less than this number.
+/// @ingroup crowd
+static const int DT_CROWDAGENT_MAX_CORNERS = 4;
+
+/// The maximum number of crowd avoidance configurations supported by the
+/// crowd manager.
+/// @ingroup crowd
+/// @see dtObstacleAvoidanceParams, dtCrowd::setObstacleAvoidanceParams(), dtCrowd::getObstacleAvoidanceParams(),
+///		 dtCrowdAgentParams::obstacleAvoidanceType
+static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8;
+
+/// The maximum number of query filter types supported by the crowd manager.
+/// @ingroup crowd
+/// @see dtQueryFilter, dtCrowd::getFilter() dtCrowd::getEditableFilter(),
+///		dtCrowdAgentParams::queryFilterType
+static const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16;
+
+/// Provides neighbor data for agents managed by the crowd.
+/// @ingroup crowd
+/// @see dtCrowdAgent::neis, dtCrowd
+struct dtCrowdNeighbour
+{
+	int idx;		///< The index of the neighbor in the crowd.
+	float dist;		///< The distance between the current agent and the neighbor.
+};
+
+/// The type of navigation mesh polygon the agent is currently traversing.
+/// @ingroup crowd
+enum CrowdAgentState
+{
+	DT_CROWDAGENT_STATE_INVALID,		///< The agent is not in a valid state.
+	DT_CROWDAGENT_STATE_WALKING,		///< The agent is traversing a normal navigation mesh polygon.
+	DT_CROWDAGENT_STATE_OFFMESH,		///< The agent is traversing an off-mesh connection.
+};
+
+/// Configuration parameters for a crowd agent.
+/// @ingroup crowd
+struct dtCrowdAgentParams
+{
+	float radius;						///< Agent radius. [Limit: >= 0]
+	float height;						///< Agent height. [Limit: > 0]
+	float maxAcceleration;				///< Maximum allowed acceleration. [Limit: >= 0]
+	float maxSpeed;						///< Maximum allowed speed. [Limit: >= 0]
+
+	/// Defines how close a collision element must be before it is considered for steering behaviors. [Limits: > 0]
+	float collisionQueryRange;
+
+	float pathOptimizationRange;		///< The path visibility optimization range. [Limit: > 0]
+
+	/// How aggresive the agent manager should be at avoiding collisions with this agent. [Limit: >= 0]
+	float separationWeight;
+
+	/// Flags that impact steering behavior. (See: #UpdateFlags)
+	unsigned char updateFlags;
+
+	/// The index of the avoidance configuration to use for the agent. 
+	/// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
+	unsigned char obstacleAvoidanceType;	
+
+	/// The index of the query filter used by this agent.
+	unsigned char queryFilterType;
+
+	/// User defined data attached to the agent.
+	void* userData;
+};
+
+enum MoveRequestState
+{
+	DT_CROWDAGENT_TARGET_NONE = 0,
+	DT_CROWDAGENT_TARGET_FAILED,
+	DT_CROWDAGENT_TARGET_VALID,
+	DT_CROWDAGENT_TARGET_REQUESTING,
+	DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE,
+	DT_CROWDAGENT_TARGET_WAITING_FOR_PATH,
+	DT_CROWDAGENT_TARGET_VELOCITY,
+};
+
+/// Represents an agent managed by a #dtCrowd object.
+/// @ingroup crowd
+struct dtCrowdAgent
+{
+	/// True if the agent is active, false if the agent is in an unused slot in the agent pool.
+	bool active;
+
+	/// The type of mesh polygon the agent is traversing. (See: #CrowdAgentState)
+	unsigned char state;
+
+	/// True if the agent has valid path (targetState == DT_CROWDAGENT_TARGET_VALID) and the path does not lead to the requested position, else false.
+	bool partial;
+
+	/// The path corridor the agent is using.
+	dtPathCorridor corridor;
+
+	/// The local boundary data for the agent.
+	dtLocalBoundary boundary;
+	
+	/// Time since the agent's path corridor was optimized.
+	float topologyOptTime;
+	
+	/// The known neighbors of the agent.
+	dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS];
+
+	/// The number of neighbors.
+	int nneis;
+	
+	/// The desired speed.
+	float desiredSpeed;
+
+	float npos[3];		///< The current agent position. [(x, y, z)]
+	float disp[3];		///< A temporary value used to accumulate agent displacement during iterative collision resolution. [(x, y, z)]
+	float dvel[3];		///< The desired velocity of the agent. Based on the current path, calculated from scratch each frame. [(x, y, z)]
+	float nvel[3];		///< The desired velocity adjusted by obstacle avoidance, calculated from scratch each frame. [(x, y, z)]
+	float vel[3];		///< The actual velocity of the agent. The change from nvel -> vel is constrained by max acceleration. [(x, y, z)]
+
+	/// The agent's configuration parameters.
+	dtCrowdAgentParams params;
+
+	/// The local path corridor corners for the agent. (Staight path.) [(x, y, z) * #ncorners]
+	float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3];
+
+	/// The local path corridor corner flags. (See: #dtStraightPathFlags) [(flags) * #ncorners]
+	unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS];
+
+	/// The reference id of the polygon being entered at the corner. [(polyRef) * #ncorners]
+	dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS];
+
+	/// The number of corners.
+	int ncorners;
+	
+	unsigned char targetState;			///< State of the movement request.
+	dtPolyRef targetRef;				///< Target polyref of the movement request.
+	float targetPos[3];					///< Target position of the movement request (or velocity in case of DT_CROWDAGENT_TARGET_VELOCITY).
+	dtPathQueueRef targetPathqRef;		///< Path finder ref.
+	bool targetReplan;					///< Flag indicating that the current path is being replanned.
+	float targetReplanTime;				/// <Time since the agent's target was replanned.
+};
+
+struct dtCrowdAgentAnimation
+{
+	bool active;
+	float initPos[3], startPos[3], endPos[3];
+	dtPolyRef polyRef;
+	float t, tmax;
+};
+
+/// Crowd agent update flags.
+/// @ingroup crowd
+/// @see dtCrowdAgentParams::updateFlags
+enum UpdateFlags
+{
+	DT_CROWD_ANTICIPATE_TURNS = 1,
+	DT_CROWD_OBSTACLE_AVOIDANCE = 2,
+	DT_CROWD_SEPARATION = 4,
+	DT_CROWD_OPTIMIZE_VIS = 8,			///< Use #dtPathCorridor::optimizePathVisibility() to optimize the agent path.
+	DT_CROWD_OPTIMIZE_TOPO = 16,		///< Use dtPathCorridor::optimizePathTopology() to optimize the agent path.
+};
+
+struct dtCrowdAgentDebugInfo
+{
+	int idx;
+	float optStart[3], optEnd[3];
+	dtObstacleAvoidanceDebugData* vod;
+};
+
+/// Provides local steering behaviors for a group of agents. 
+/// @ingroup crowd
+class dtCrowd
+{
+	int m_maxAgents;
+	dtCrowdAgent* m_agents;
+	dtCrowdAgent** m_activeAgents;
+	dtCrowdAgentAnimation* m_agentAnims;
+	
+	dtPathQueue m_pathq;
+
+	dtObstacleAvoidanceParams m_obstacleQueryParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS];
+	dtObstacleAvoidanceQuery* m_obstacleQuery;
+	
+	dtProximityGrid* m_grid;
+	
+	dtPolyRef* m_pathResult;
+	int m_maxPathResult;
+	
+	float m_agentPlacementHalfExtents[3];
+
+	dtQueryFilter m_filters[DT_CROWD_MAX_QUERY_FILTER_TYPE];
+
+	float m_maxAgentRadius;
+
+	int m_velocitySampleCount;
+
+	dtNavMeshQuery* m_navquery;
+
+	void updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt);
+	void updateMoveRequest(const float dt);
+	void checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt);
+
+	inline int getAgentIndex(const dtCrowdAgent* agent) const  { return (int)(agent - m_agents); }
+
+	bool requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos);
+
+	void purge();
+	
+public:
+	dtCrowd();
+	~dtCrowd();
+	
+	/// Initializes the crowd.  
+	///  @param[in]		maxAgents		The maximum number of agents the crowd can manage. [Limit: >= 1]
+	///  @param[in]		maxAgentRadius	The maximum radius of any agent that will be added to the crowd. [Limit: > 0]
+	///  @param[in]		nav				The navigation mesh to use for planning.
+	/// @return True if the initialization succeeded.
+	bool init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav);
+	
+	/// Sets the shared avoidance configuration for the specified index.
+	///  @param[in]		idx		The index. [Limits: 0 <= value < #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
+	///  @param[in]		params	The new configuration.
+	void setObstacleAvoidanceParams(const int idx, const dtObstacleAvoidanceParams* params);
+
+	/// Gets the shared avoidance configuration for the specified index.
+	///  @param[in]		idx		The index of the configuration to retreive. 
+	///							[Limits:  0 <= value < #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
+	/// @return The requested configuration.
+	const dtObstacleAvoidanceParams* getObstacleAvoidanceParams(const int idx) const;
+	
+	/// Gets the specified agent from the pool.
+	///	 @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	/// @return The requested agent.
+	const dtCrowdAgent* getAgent(const int idx);
+
+	/// Gets the specified agent from the pool.
+	///	 @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	/// @return The requested agent.
+	dtCrowdAgent* getEditableAgent(const int idx);
+
+	/// The maximum number of agents that can be managed by the object.
+	/// @return The maximum number of agents.
+	int getAgentCount() const;
+	
+	/// Adds a new agent to the crowd.
+	///  @param[in]		pos		The requested position of the agent. [(x, y, z)]
+	///  @param[in]		params	The configutation of the agent.
+	/// @return The index of the agent in the agent pool. Or -1 if the agent could not be added.
+	int addAgent(const float* pos, const dtCrowdAgentParams* params);
+
+	/// Updates the specified agent's configuration.
+	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	///  @param[in]		params	The new agent configuration.
+	void updateAgentParameters(const int idx, const dtCrowdAgentParams* params);
+
+	/// Removes the agent from the crowd.
+	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	void removeAgent(const int idx);
+	
+	/// Submits a new move request for the specified agent.
+	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	///  @param[in]		ref		The position's polygon reference.
+	///  @param[in]		pos		The position within the polygon. [(x, y, z)]
+	/// @return True if the request was successfully submitted.
+	bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos);
+
+	/// Submits a new move request for the specified agent.
+	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	///  @param[in]		vel		The movement velocity. [(x, y, z)]
+	/// @return True if the request was successfully submitted.
+	bool requestMoveVelocity(const int idx, const float* vel);
+
+	/// Resets any request for the specified agent.
+	///  @param[in]		idx		The agent index. [Limits: 0 <= value < #getAgentCount()]
+	/// @return True if the request was successfully reseted.
+	bool resetMoveTarget(const int idx);
+
+	/// Gets the active agents int the agent pool.
+	///  @param[out]	agents		An array of agent pointers. [(#dtCrowdAgent *) * maxAgents]
+	///  @param[in]		maxAgents	The size of the crowd agent array.
+	/// @return The number of agents returned in @p agents.
+	int getActiveAgents(dtCrowdAgent** agents, const int maxAgents);
+
+	/// Updates the steering and positions of all agents.
+	///  @param[in]		dt		The time, in seconds, to update the simulation. [Limit: > 0]
+	///  @param[out]	debug	A debug object to load with debug information. [Opt]
+	void update(const float dt, dtCrowdAgentDebugInfo* debug);
+	
+	/// Gets the filter used by the crowd.
+	/// @return The filter used by the crowd.
+	inline const dtQueryFilter* getFilter(const int i) const { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
+	
+	/// Gets the filter used by the crowd.
+	/// @return The filter used by the crowd.
+	inline dtQueryFilter* getEditableFilter(const int i) { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
+
+	/// Gets the search halfExtents [(x, y, z)] used by the crowd for query operations. 
+	/// @return The search halfExtents used by the crowd. [(x, y, z)]
+	const float* getQueryHalfExtents() const { return m_agentPlacementHalfExtents; }
+
+	/// Same as getQueryHalfExtents. Left to maintain backwards compatibility.
+	/// @return The search halfExtents used by the crowd. [(x, y, z)]
+	const float* getQueryExtents() const { return m_agentPlacementHalfExtents; }
+	
+	/// Gets the velocity sample count.
+	/// @return The velocity sample count.
+	inline int getVelocitySampleCount() const { return m_velocitySampleCount; }
+	
+	/// Gets the crowd's proximity grid.
+	/// @return The crowd's proximity grid.
+	const dtProximityGrid* getGrid() const { return m_grid; }
+
+	/// Gets the crowd's path request queue.
+	/// @return The crowd's path request queue.
+	const dtPathQueue* getPathQueue() const { return &m_pathq; }
+
+	/// Gets the query object used by the crowd.
+	const dtNavMeshQuery* getNavMeshQuery() const { return m_navquery; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtCrowd(const dtCrowd&);
+	dtCrowd& operator=(const dtCrowd&);
+};
+
+/// Allocates a crowd object using the Detour allocator.
+/// @return A crowd object that is ready for initialization, or null on failure.
+///  @ingroup crowd
+dtCrowd* dtAllocCrowd();
+
+/// Frees the specified crowd object using the Detour allocator.
+///  @param[in]		ptr		A crowd object allocated using #dtAllocCrowd
+///  @ingroup crowd
+void dtFreeCrowd(dtCrowd* ptr);
+
+
+#endif // DETOURCROWD_H
+
+///////////////////////////////////////////////////////////////////////////
+
+// This section contains detailed documentation for members that don't have
+// a source file. It reduces clutter in the main section of the header.
+
+/**
+
+@defgroup crowd Crowd
+
+Members in this module implement local steering and dynamic avoidance features.
+
+The crowd is the big beast of the navigation features. It not only handles a 
+lot of the path management for you, but also local steering and dynamic 
+avoidance between members of the crowd. I.e. It can keep your agents from 
+running into each other.
+
+Main class: #dtCrowd
+
+The #dtNavMeshQuery and #dtPathCorridor classes provide perfectly good, easy 
+to use path planning features. But in the end they only give you points that 
+your navigation client should be moving toward. When it comes to deciding things 
+like agent velocity and steering to avoid other agents, that is up to you to 
+implement. Unless, of course, you decide to use #dtCrowd.
+
+Basically, you add an agent to the crowd, providing various configuration 
+settings such as maximum speed and acceleration. You also provide a local 
+target to more toward. The crowd manager then provides, with every update, the 
+new agent position and velocity for the frame. The movement will be 
+constrained to the navigation mesh, and steering will be applied to ensure 
+agents managed by the crowd do not collide with each other.
+
+This is very powerful feature set. But it comes with limitations.
+
+The biggest limitation is that you must give control of the agent's position 
+completely over to the crowd manager. You can update things like maximum speed 
+and acceleration. But in order for the crowd manager to do its thing, it can't 
+allow you to constantly be giving it overrides to position and velocity. So 
+you give up direct control of the agent's movement. It belongs to the crowd.
+
+The second biggest limitation revolves around the fact that the crowd manager 
+deals with local planning. So the agent's target should never be more than 
+256 polygons aways from its current position. If it is, you risk 
+your agent failing to reach its target. So you may still need to do long 
+distance planning and provide the crowd manager with intermediate targets.
+
+Other significant limitations:
+
+- All agents using the crowd manager will use the same #dtQueryFilter.
+- Crowd management is relatively expensive. The maximum agents under crowd 
+  management at any one time is between 20 and 30.  A good place to start
+  is a maximum of 25 agents for 0.5ms per frame.
+
+@note This is a summary list of members.  Use the index or search 
+feature to find minor members.
+
+@struct dtCrowdAgentParams
+@see dtCrowdAgent, dtCrowd::addAgent(), dtCrowd::updateAgentParameters()
+
+@var dtCrowdAgentParams::obstacleAvoidanceType
+@par
+
+#dtCrowd permits agents to use different avoidance configurations.  This value 
+is the index of the #dtObstacleAvoidanceParams within the crowd.
+
+@see dtObstacleAvoidanceParams, dtCrowd::setObstacleAvoidanceParams(), 
+	 dtCrowd::getObstacleAvoidanceParams()
+
+@var dtCrowdAgentParams::collisionQueryRange
+@par
+
+Collision elements include other agents and navigation mesh boundaries.
+
+This value is often based on the agent radius and/or maximum speed. E.g. radius * 8
+
+@var dtCrowdAgentParams::pathOptimizationRange
+@par
+
+Only applicalbe if #updateFlags includes the #DT_CROWD_OPTIMIZE_VIS flag.
+
+This value is often based on the agent radius. E.g. radius * 30
+
+@see dtPathCorridor::optimizePathVisibility()
+
+@var dtCrowdAgentParams::separationWeight
+@par
+
+A higher value will result in agents trying to stay farther away from each other at 
+the cost of more difficult steering in tight spaces.
+
+*/
+

panda/src/recastdetour/DetourDebugDraw.cpp

@@ -0,0 +1,864 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 "DebugDraw.h"
+#include "DetourDebugDraw.h"
+#include "DetourNavMesh.h"
+#include "DetourCommon.h"
+#include "DetourNode.h"
+
+
+static float distancePtLine2d(const float* pt, const float* p, const float* q)
+{
+	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;
+	float t = pqx*dx + pqz*dz;
+	if (d != 0) t /= d;
+	dx = p[0] + t*pqx - pt[0];
+	dz = p[2] + t*pqz - pt[2];
+	return dx*dx + dz*dz;
+}
+
+static void drawPolyBoundaries(duDebugDraw* dd, const dtMeshTile* tile,
+							   const unsigned int col, const float linew,
+							   bool inner)
+{
+	static const float thr = 0.01f*0.01f;
+
+	dd->begin(DU_DRAW_LINES, linew);
+
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		const dtPoly* p = &tile->polys[i];
+		
+		if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue;
+		
+		const dtPolyDetail* pd = &tile->detailMeshes[i];
+		
+		for (int j = 0, nj = (int)p->vertCount; j < nj; ++j)
+		{
+			unsigned int c = col;
+			if (inner)
+			{
+				if (p->neis[j] == 0) continue;
+				if (p->neis[j] & DT_EXT_LINK)
+				{
+					bool con = false;
+					for (unsigned int k = p->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
+					{
+						if (tile->links[k].edge == j)
+						{
+							con = true;
+							break;
+						}
+					}
+					if (con)
+						c = duRGBA(255,255,255,48);
+					else
+						c = duRGBA(0,0,0,48);
+				}
+				else
+					c = duRGBA(0,48,64,32);
+			}
+			else
+			{
+				if (p->neis[j] != 0) continue;
+			}
+			
+			const float* v0 = &tile->verts[p->verts[j]*3];
+			const float* v1 = &tile->verts[p->verts[(j+1) % nj]*3];
+			
+			// Draw detail mesh edges which align with the actual poly edge.
+			// This is really slow.
+			for (int k = 0; k < pd->triCount; ++k)
+			{
+				const unsigned char* t = &tile->detailTris[(pd->triBase+k)*4];
+				const float* tv[3];
+				for (int m = 0; m < 3; ++m)
+				{
+					if (t[m] < p->vertCount)
+						tv[m] = &tile->verts[p->verts[t[m]]*3];
+					else
+						tv[m] = &tile->detailVerts[(pd->vertBase+(t[m]-p->vertCount))*3];
+				}
+				for (int m = 0, n = 2; m < 3; n=m++)
+				{
+					if ((dtGetDetailTriEdgeFlags(t[3], n) & DT_DETAIL_EDGE_BOUNDARY) == 0)
+						continue;
+
+					if (distancePtLine2d(tv[n],v0,v1) < thr &&
+						distancePtLine2d(tv[m],v0,v1) < thr)
+					{
+						dd->vertex(tv[n], c);
+						dd->vertex(tv[m], c);
+					}
+				}
+			}
+		}
+	}
+	dd->end();
+}
+
+static void drawMeshTile(duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery* query,
+						 const dtMeshTile* tile, unsigned char flags)
+{
+	dtPolyRef base = mesh.getPolyRefBase(tile);
+
+	int tileNum = mesh.decodePolyIdTile(base);
+	const unsigned int tileColor = duIntToCol(tileNum, 128);
+	
+	dd->depthMask(false);
+
+	dd->begin(DU_DRAW_TRIS);
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		const dtPoly* p = &tile->polys[i];
+		if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)	// Skip off-mesh links.
+			continue;
+			
+		const dtPolyDetail* pd = &tile->detailMeshes[i];
+
+		unsigned int col;
+		if (query && query->isInClosedList(base | (dtPolyRef)i))
+			col = duRGBA(255,196,0,64);
+		else
+		{
+			if (flags & DU_DRAWNAVMESH_COLOR_TILES)
+				col = tileColor;
+			else
+				col = duTransCol(dd->areaToCol(p->getArea()), 64);
+		}
+		
+		for (int j = 0; j < pd->triCount; ++j)
+		{
+			const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
+			for (int k = 0; k < 3; ++k)
+			{
+				if (t[k] < p->vertCount)
+					dd->vertex(&tile->verts[p->verts[t[k]]*3], col);
+				else
+					dd->vertex(&tile->detailVerts[(pd->vertBase+t[k]-p->vertCount)*3], col);
+			}
+		}
+	}
+	dd->end();
+	
+	// Draw inter poly boundaries
+	drawPolyBoundaries(dd, tile, duRGBA(0,48,64,32), 1.5f, true);
+	
+	// Draw outer poly boundaries
+	drawPolyBoundaries(dd, tile, duRGBA(0,48,64,220), 2.5f, false);
+
+	if (flags & DU_DRAWNAVMESH_OFFMESHCONS)
+	{
+		dd->begin(DU_DRAW_LINES, 2.0f);
+		for (int i = 0; i < tile->header->polyCount; ++i)
+		{
+			const dtPoly* p = &tile->polys[i];
+			if (p->getType() != DT_POLYTYPE_OFFMESH_CONNECTION)	// Skip regular polys.
+				continue;
+			
+			unsigned int col, col2;
+			if (query && query->isInClosedList(base | (dtPolyRef)i))
+				col = duRGBA(255,196,0,220);
+			else
+				col = duDarkenCol(duTransCol(dd->areaToCol(p->getArea()), 220));
+
+			const dtOffMeshConnection* con = &tile->offMeshCons[i - tile->header->offMeshBase];
+			const float* va = &tile->verts[p->verts[0]*3];
+			const float* vb = &tile->verts[p->verts[1]*3];
+
+			// Check to see if start and end end-points have links.
+			bool startSet = false;
+			bool endSet = false;
+			for (unsigned int k = p->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
+			{
+				if (tile->links[k].edge == 0)
+					startSet = true;
+				if (tile->links[k].edge == 1)
+					endSet = true;
+			}
+			
+			// End points and their on-mesh locations.
+			dd->vertex(va[0],va[1],va[2], col);
+			dd->vertex(con->pos[0],con->pos[1],con->pos[2], col);
+			col2 = startSet ? col : duRGBA(220,32,16,196);
+			duAppendCircle(dd, con->pos[0],con->pos[1]+0.1f,con->pos[2], con->rad, col2);
+
+			dd->vertex(vb[0],vb[1],vb[2], col);
+			dd->vertex(con->pos[3],con->pos[4],con->pos[5], col);
+			col2 = endSet ? col : duRGBA(220,32,16,196);
+			duAppendCircle(dd, con->pos[3],con->pos[4]+0.1f,con->pos[5], con->rad, col2);
+			
+			// End point vertices.
+			dd->vertex(con->pos[0],con->pos[1],con->pos[2], duRGBA(0,48,64,196));
+			dd->vertex(con->pos[0],con->pos[1]+0.2f,con->pos[2], duRGBA(0,48,64,196));
+			
+			dd->vertex(con->pos[3],con->pos[4],con->pos[5], duRGBA(0,48,64,196));
+			dd->vertex(con->pos[3],con->pos[4]+0.2f,con->pos[5], duRGBA(0,48,64,196));
+			
+			// Connection arc.
+			duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f,
+						(con->flags & 1) ? 0.6f : 0, 0.6f, col);
+		}
+		dd->end();
+	}
+	
+	const unsigned int vcol = duRGBA(0,0,0,196);
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	for (int i = 0; i < tile->header->vertCount; ++i)
+	{
+		const float* v = &tile->verts[i*3];
+		dd->vertex(v[0], v[1], v[2], vcol);
+	}
+	dd->end();
+
+	dd->depthMask(true);
+}
+
+void duDebugDrawNavMesh(duDebugDraw* dd, const dtNavMesh& mesh, unsigned char flags)
+{
+	if (!dd) return;
+	
+	for (int i = 0; i < mesh.getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = mesh.getTile(i);
+		if (!tile->header) continue;
+		drawMeshTile(dd, mesh, 0, tile, flags);
+	}
+}
+
+void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery& query, unsigned char flags)
+{
+	if (!dd) return;
+
+	const dtNavMeshQuery* q = (flags & DU_DRAWNAVMESH_CLOSEDLIST) ? &query : 0;
+	
+	for (int i = 0; i < mesh.getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = mesh.getTile(i);
+		if (!tile->header) continue;
+		drawMeshTile(dd, mesh, q, tile, flags);
+	}
+}
+
+void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query)
+{
+	if (!dd) return;
+	
+	const dtNodePool* pool = query.getNodePool();
+	if (pool)
+	{
+		const float off = 0.5f;
+		dd->begin(DU_DRAW_POINTS, 4.0f);
+		for (int i = 0; i < pool->getHashSize(); ++i)
+		{
+			for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j))
+			{
+				const dtNode* node = pool->getNodeAtIdx(j+1);
+				if (!node) continue;
+				dd->vertex(node->pos[0],node->pos[1]+off,node->pos[2], duRGBA(255,192,0,255));
+			}
+		}
+		dd->end();
+		
+		dd->begin(DU_DRAW_LINES, 2.0f);
+		for (int i = 0; i < pool->getHashSize(); ++i)
+		{
+			for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j))
+			{
+				const dtNode* node = pool->getNodeAtIdx(j+1);
+				if (!node) continue;
+				if (!node->pidx) continue;
+				const dtNode* parent = pool->getNodeAtIdx(node->pidx);
+				if (!parent) continue;
+				dd->vertex(node->pos[0],node->pos[1]+off,node->pos[2], duRGBA(255,192,0,128));
+				dd->vertex(parent->pos[0],parent->pos[1]+off,parent->pos[2], duRGBA(255,192,0,128));
+			}
+		}
+		dd->end();
+	}
+}
+
+
+static void drawMeshTileBVTree(duDebugDraw* dd, const dtMeshTile* tile)
+{
+	// Draw BV nodes.
+	const float cs = 1.0f / tile->header->bvQuantFactor;
+	dd->begin(DU_DRAW_LINES, 1.0f);
+	for (int i = 0; i < tile->header->bvNodeCount; ++i)
+	{
+		const dtBVNode* n = &tile->bvTree[i];
+		if (n->i < 0) // Leaf indices are positive.
+			continue;
+		duAppendBoxWire(dd, tile->header->bmin[0] + n->bmin[0]*cs,
+						tile->header->bmin[1] + n->bmin[1]*cs,
+						tile->header->bmin[2] + n->bmin[2]*cs,
+						tile->header->bmin[0] + n->bmax[0]*cs,
+						tile->header->bmin[1] + n->bmax[1]*cs,
+						tile->header->bmin[2] + n->bmax[2]*cs,
+						duRGBA(255,255,255,128));
+	}
+	dd->end();
+}
+
+void duDebugDrawNavMeshBVTree(duDebugDraw* dd, const dtNavMesh& mesh)
+{
+	if (!dd) return;
+	
+	for (int i = 0; i < mesh.getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = mesh.getTile(i);
+		if (!tile->header) continue;
+		drawMeshTileBVTree(dd, tile);
+	}
+}
+
+static void drawMeshTilePortal(duDebugDraw* dd, const dtMeshTile* tile)
+{
+	// Draw portals
+	const float padx = 0.04f;
+	const float pady = tile->header->walkableClimb;
+
+	dd->begin(DU_DRAW_LINES, 2.0f);
+
+	for (int side = 0; side < 8; ++side)
+	{
+		unsigned short m = DT_EXT_LINK | (unsigned short)side;
+		
+		for (int i = 0; i < tile->header->polyCount; ++i)
+		{
+			dtPoly* poly = &tile->polys[i];
+			
+			// Create new links.
+			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];
+				
+				if (side == 0 || side == 4)
+				{
+					unsigned int col = side == 0 ? duRGBA(128,0,0,128) : duRGBA(128,0,128,128);
+
+					const float x = va[0] + ((side == 0) ? -padx : padx);
+					
+					dd->vertex(x,va[1]-pady,va[2], col);
+					dd->vertex(x,va[1]+pady,va[2], col);
+
+					dd->vertex(x,va[1]+pady,va[2], col);
+					dd->vertex(x,vb[1]+pady,vb[2], col);
+
+					dd->vertex(x,vb[1]+pady,vb[2], col);
+					dd->vertex(x,vb[1]-pady,vb[2], col);
+
+					dd->vertex(x,vb[1]-pady,vb[2], col);
+					dd->vertex(x,va[1]-pady,va[2], col);
+				}
+				else if (side == 2 || side == 6)
+				{
+					unsigned int col = side == 2 ? duRGBA(0,128,0,128) : duRGBA(0,128,128,128);
+
+					const float z = va[2] + ((side == 2) ? -padx : padx);
+					
+					dd->vertex(va[0],va[1]-pady,z, col);
+					dd->vertex(va[0],va[1]+pady,z, col);
+					
+					dd->vertex(va[0],va[1]+pady,z, col);
+					dd->vertex(vb[0],vb[1]+pady,z, col);
+					
+					dd->vertex(vb[0],vb[1]+pady,z, col);
+					dd->vertex(vb[0],vb[1]-pady,z, col);
+					
+					dd->vertex(vb[0],vb[1]-pady,z, col);
+					dd->vertex(va[0],va[1]-pady,z, col);
+				}
+
+			}
+		}
+	}
+	
+	dd->end();
+}
+
+void duDebugDrawNavMeshPortals(duDebugDraw* dd, const dtNavMesh& mesh)
+{
+	if (!dd) return;
+	
+	for (int i = 0; i < mesh.getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = mesh.getTile(i);
+		if (!tile->header) continue;
+		drawMeshTilePortal(dd, tile);
+	}
+}
+
+void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh,
+									  const unsigned short polyFlags, const unsigned int col)
+{
+	if (!dd) return;
+	
+	for (int i = 0; i < mesh.getMaxTiles(); ++i)
+	{
+		const dtMeshTile* tile = mesh.getTile(i);
+		if (!tile->header) continue;
+		dtPolyRef base = mesh.getPolyRefBase(tile);
+
+		for (int j = 0; j < tile->header->polyCount; ++j)
+		{
+			const dtPoly* p = &tile->polys[j];
+			if ((p->flags & polyFlags) == 0) continue;
+			duDebugDrawNavMeshPoly(dd, mesh, base|(dtPolyRef)j, col);
+		}
+	}
+}
+
+void duDebugDrawNavMeshPoly(duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col)
+{
+	if (!dd) return;
+	
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	if (dtStatusFailed(mesh.getTileAndPolyByRef(ref, &tile, &poly)))
+		return;
+	
+	dd->depthMask(false);
+	
+	const unsigned int c = duTransCol(col, 64);
+	const unsigned int ip = (unsigned int)(poly - tile->polys);
+
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		dtOffMeshConnection* con = &tile->offMeshCons[ip - tile->header->offMeshBase];
+
+		dd->begin(DU_DRAW_LINES, 2.0f);
+
+		// Connection arc.
+		duAppendArc(dd, con->pos[0],con->pos[1],con->pos[2], con->pos[3],con->pos[4],con->pos[5], 0.25f,
+					(con->flags & 1) ? 0.6f : 0.0f, 0.6f, c);
+		
+		dd->end();
+	}
+	else
+	{
+		const dtPolyDetail* pd = &tile->detailMeshes[ip];
+
+		dd->begin(DU_DRAW_TRIS);
+		for (int i = 0; i < pd->triCount; ++i)
+		{
+			const unsigned char* t = &tile->detailTris[(pd->triBase+i)*4];
+			for (int j = 0; j < 3; ++j)
+			{
+				if (t[j] < poly->vertCount)
+					dd->vertex(&tile->verts[poly->verts[t[j]]*3], c);
+				else
+					dd->vertex(&tile->detailVerts[(pd->vertBase+t[j]-poly->vertCount)*3], c);
+			}
+		}
+		dd->end();
+	}
+	
+	dd->depthMask(true);
+
+}
+
+static void debugDrawTileCachePortals(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	const float* bmin = layer.header->bmin;
+
+	// Portals
+	unsigned int pcol = duRGBA(255,255,255,255);
+	
+	const int segs[4*4] = {0,0,0,1, 0,1,1,1, 1,1,1,0, 1,0,0,0};
+	
+	// Layer portals
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int idx = x+y*w;
+			const int lh = (int)layer.heights[idx];
+			if (lh == 0xff) continue;
+			
+			for (int dir = 0; dir < 4; ++dir)
+			{
+				if (layer.cons[idx] & (1<<(dir+4)))
+				{
+					const int* seg = &segs[dir*4];
+					const float ax = bmin[0] + (x+seg[0])*cs;
+					const float ay = bmin[1] + (lh+2)*ch;
+					const float az = bmin[2] + (y+seg[1])*cs;
+					const float bx = bmin[0] + (x+seg[2])*cs;
+					const float by = bmin[1] + (lh+2)*ch;
+					const float bz = bmin[2] + (y+seg[3])*cs;
+					dd->vertex(ax, ay, az, pcol);
+					dd->vertex(bx, by, bz, pcol);
+				}
+			}
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	const float* bmin = layer.header->bmin;
+	const float* bmax = layer.header->bmax;
+	const int idx = layer.header->tlayer;
+	
+	unsigned int color = duIntToCol(idx+1, 255);
+	
+	// Layer bounds
+	float lbmin[3], lbmax[3];
+	lbmin[0] = bmin[0] + layer.header->minx*cs;
+	lbmin[1] = bmin[1];
+	lbmin[2] = bmin[2] + layer.header->miny*cs;
+	lbmax[0] = bmin[0] + (layer.header->maxx+1)*cs;
+	lbmax[1] = bmax[1];
+	lbmax[2] = bmin[2] + (layer.header->maxy+1)*cs;
+	duDebugDrawBoxWire(dd, lbmin[0],lbmin[1],lbmin[2], lbmax[0],lbmax[1],lbmax[2], duTransCol(color,128), 2.0f);
+	
+	// Layer height
+	dd->begin(DU_DRAW_QUADS);
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int lidx = x+y*w;
+			const int lh = (int)layer.heights[lidx];
+			if (lh == 0xff) continue;
+
+			const unsigned char area = layer.areas[lidx];
+			unsigned int col;
+			if (area == 63)
+				col = duLerpCol(color, duRGBA(0,192,255,64), 32);
+			else if (area == 0)
+				col = duLerpCol(color, duRGBA(0,0,0,64), 32);
+			else
+				col = duLerpCol(color, dd->areaToCol(area), 32);
+			
+			const float fx = bmin[0] + x*cs;
+			const float fy = bmin[1] + (lh+1)*ch;
+			const float fz = bmin[2] + y*cs;
+			
+			dd->vertex(fx, fy, fz, col);
+			dd->vertex(fx, fy, fz+cs, col);
+			dd->vertex(fx+cs, fy, fz+cs, col);
+			dd->vertex(fx+cs, fy, fz, col);
+		}
+	}
+	dd->end();
+	
+	debugDrawTileCachePortals(dd, layer, cs, ch);
+}
+
+void duDebugDrawTileCacheLayerRegions(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	const float* bmin = layer.header->bmin;
+	const float* bmax = layer.header->bmax;
+	const int idx = layer.header->tlayer;
+	
+	unsigned int color = duIntToCol(idx+1, 255);
+	
+	// Layer bounds
+	float lbmin[3], lbmax[3];
+	lbmin[0] = bmin[0] + layer.header->minx*cs;
+	lbmin[1] = bmin[1];
+	lbmin[2] = bmin[2] + layer.header->miny*cs;
+	lbmax[0] = bmin[0] + (layer.header->maxx+1)*cs;
+	lbmax[1] = bmax[1];
+	lbmax[2] = bmin[2] + (layer.header->maxy+1)*cs;
+	duDebugDrawBoxWire(dd, lbmin[0],lbmin[1],lbmin[2], lbmax[0],lbmax[1],lbmax[2], duTransCol(color,128), 2.0f);
+	
+	// Layer height
+	dd->begin(DU_DRAW_QUADS);
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int lidx = x+y*w;
+			const int lh = (int)layer.heights[lidx];
+			if (lh == 0xff) continue;
+			const unsigned char reg = layer.regs[lidx];
+			
+			unsigned int col = duLerpCol(color, duIntToCol(reg, 255), 192);
+			
+			const float fx = bmin[0] + x*cs;
+			const float fy = bmin[1] + (lh+1)*ch;
+			const float fz = bmin[2] + y*cs;
+			
+			dd->vertex(fx, fy, fz, col);
+			dd->vertex(fx, fy, fz+cs, col);
+			dd->vertex(fx+cs, fy, fz+cs, col);
+			dd->vertex(fx+cs, fy, fz, col);
+		}
+	}
+	dd->end();
+	
+	debugDrawTileCachePortals(dd, layer, cs, ch);
+}
+
+
+
+
+/*struct dtTileCacheContour
+{
+	int nverts;
+	unsigned char* verts;
+	unsigned char reg;
+	unsigned char area;
+};
+
+struct dtTileCacheContourSet
+{
+	int nconts;
+	dtTileCacheContour* conts;
+};*/
+
+void duDebugDrawTileCacheContours(duDebugDraw* dd, const struct dtTileCacheContourSet& lcset,
+								  const float* orig, const float cs, const float ch)
+{
+	if (!dd) return;
+	
+	const unsigned char a = 255;// (unsigned char)(alpha*255.0f);
+	
+	const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1};
+	
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		const dtTileCacheContour& c = lcset.conts[i];
+		unsigned int color = 0;
+		
+		color = duIntToCol(i, a);
+		
+		for (int j = 0; j < c.nverts; ++j)
+		{
+			const int k = (j+1) % c.nverts;
+			const unsigned char* va = &c.verts[j*4];
+			const unsigned char* vb = &c.verts[k*4];
+			const float ax = orig[0] + va[0]*cs;
+			const float ay = orig[1] + (va[1]+1+(i&1))*ch;
+			const float az = orig[2] + va[2]*cs;
+			const float bx = orig[0] + vb[0]*cs;
+			const float by = orig[1] + (vb[1]+1+(i&1))*ch;
+			const float bz = orig[2] + vb[2]*cs;
+			unsigned int col = color;
+			if ((va[3] & 0xf) != 0xf)
+			{
+				// Portal segment
+				col = duRGBA(255,255,255,128);
+				int d = va[3] & 0xf;
+				
+				const float cx = (ax+bx)*0.5f;
+				const float cy = (ay+by)*0.5f;
+				const float cz = (az+bz)*0.5f;
+				
+				const float dx = cx + offs[d*2+0]*2*cs;
+				const float dy = cy;
+				const float dz = cz + offs[d*2+1]*2*cs;
+				
+				dd->vertex(cx,cy,cz,duRGBA(255,0,0,255));
+				dd->vertex(dx,dy,dz,duRGBA(255,0,0,255));
+			}
+			
+			duAppendArrow(dd, ax,ay,az, bx,by,bz, 0.0f, cs*0.5f, col);
+		}
+	}
+	dd->end();
+	
+	dd->begin(DU_DRAW_POINTS, 4.0f);	
+	
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		const dtTileCacheContour& c = lcset.conts[i];
+		unsigned int color = 0;
+		
+		for (int j = 0; j < c.nverts; ++j)
+		{
+			const unsigned char* va = &c.verts[j*4];
+			
+			color = duDarkenCol(duIntToCol(i, a));
+			if (va[3] & 0x80)
+			{
+				// Border vertex
+				color = duRGBA(255,0,0,255);
+			}
+			
+			float fx = orig[0] + va[0]*cs;
+			float fy = orig[1] + (va[1]+1+(i&1))*ch;
+			float fz = orig[2] + va[2]*cs;
+			dd->vertex(fx,fy,fz, color);
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyMesh& lmesh,
+								  const float* orig, const float cs, const float ch)
+{
+	if (!dd) return;
+	
+	const int nvp = lmesh.nvp;
+	
+	const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1};
+	
+	dd->begin(DU_DRAW_TRIS);
+	
+	for (int i = 0; i < lmesh.npolys; ++i)
+	{
+		const unsigned short* p = &lmesh.polys[i*nvp*2];
+		const unsigned char area = lmesh.areas[i];
+		
+		unsigned int color;
+		if (area == DT_TILECACHE_WALKABLE_AREA)
+			color = duRGBA(0,192,255,64);
+		else if (area == DT_TILECACHE_NULL_AREA)
+			color = duRGBA(0,0,0,64);
+		else
+			color = dd->areaToCol(area);
+		
+		unsigned short vi[3];
+		for (int j = 2; j < nvp; ++j)
+		{
+			if (p[j] == DT_TILECACHE_NULL_IDX) break;
+			vi[0] = p[0];
+			vi[1] = p[j-1];
+			vi[2] = p[j];
+			for (int k = 0; k < 3; ++k)
+			{
+				const unsigned short* v = &lmesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x,y,z, color);
+			}
+		}
+	}
+	dd->end();
+	
+	// Draw neighbours edges
+	const unsigned int coln = duRGBA(0,48,64,32);
+	dd->begin(DU_DRAW_LINES, 1.5f);
+	for (int i = 0; i < lmesh.npolys; ++i)
+	{
+		const unsigned short* p = &lmesh.polys[i*nvp*2];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == DT_TILECACHE_NULL_IDX) break;
+			if (p[nvp+j] & 0x8000) continue;
+			const int nj = (j+1 >= nvp || p[j+1] == DT_TILECACHE_NULL_IDX) ? 0 : j+1; 
+			int vi[2] = {p[j], p[nj]};
+			
+			for (int k = 0; k < 2; ++k)
+			{
+				const unsigned short* v = &lmesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x, y, z, coln);
+			}
+		}
+	}
+	dd->end();
+	
+	// Draw boundary edges
+	const unsigned int colb = duRGBA(0,48,64,220);
+	dd->begin(DU_DRAW_LINES, 2.5f);
+	for (int i = 0; i < lmesh.npolys; ++i)
+	{
+		const unsigned short* p = &lmesh.polys[i*nvp*2];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == DT_TILECACHE_NULL_IDX) break;
+			if ((p[nvp+j] & 0x8000) == 0) continue;
+			const int nj = (j+1 >= nvp || p[j+1] == DT_TILECACHE_NULL_IDX) ? 0 : j+1; 
+			int vi[2] = {p[j], p[nj]};
+			
+			unsigned int col = colb;
+			if ((p[nvp+j] & 0xf) != 0xf)
+			{
+				const unsigned short* va = &lmesh.verts[vi[0]*3];
+				const unsigned short* vb = &lmesh.verts[vi[1]*3];
+				
+				const float ax = orig[0] + va[0]*cs;
+				const float ay = orig[1] + (va[1]+1+(i&1))*ch;
+				const float az = orig[2] + va[2]*cs;
+				const float bx = orig[0] + vb[0]*cs;
+				const float by = orig[1] + (vb[1]+1+(i&1))*ch;
+				const float bz = orig[2] + vb[2]*cs;
+				
+				const float cx = (ax+bx)*0.5f;
+				const float cy = (ay+by)*0.5f;
+				const float cz = (az+bz)*0.5f;
+				
+				int d = p[nvp+j] & 0xf;
+				
+				const float dx = cx + offs[d*2+0]*2*cs;
+				const float dy = cy;
+				const float dz = cz + offs[d*2+1]*2*cs;
+				
+				dd->vertex(cx,cy,cz,duRGBA(255,0,0,255));
+				dd->vertex(dx,dy,dz,duRGBA(255,0,0,255));
+				
+				col = duRGBA(255,255,255,128);
+			}
+			
+			for (int k = 0; k < 2; ++k)
+			{
+				const unsigned short* v = &lmesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x, y, z, col);
+			}
+		}
+	}
+	dd->end();
+	
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	const unsigned int colv = duRGBA(0,0,0,220);
+	for (int i = 0; i < lmesh.nverts; ++i)
+	{
+		const unsigned short* v = &lmesh.verts[i*3];
+		const float x = orig[0] + v[0]*cs;
+		const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+		const float z = orig[2] + v[2]*cs;
+		dd->vertex(x,y,z, colv);
+	}
+	dd->end();
+}
+
+
+

panda/src/recastdetour/DetourDebugDraw.h

@@ -0,0 +1,48 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURDEBUGDRAW_H
+#define DETOURDEBUGDRAW_H
+
+#include "DetourNavMesh.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourTileCacheBuilder.h"
+
+enum DrawNavMeshFlags
+{
+	DU_DRAWNAVMESH_OFFMESHCONS = 0x01,
+	DU_DRAWNAVMESH_CLOSEDLIST = 0x02,
+	DU_DRAWNAVMESH_COLOR_TILES = 0x04,
+};
+
+void duDebugDrawNavMesh(struct duDebugDraw* dd, const dtNavMesh& mesh, unsigned char flags);
+void duDebugDrawNavMeshWithClosedList(struct duDebugDraw* dd, const dtNavMesh& mesh, const dtNavMeshQuery& query, unsigned char flags);
+void duDebugDrawNavMeshNodes(struct duDebugDraw* dd, const dtNavMeshQuery& query);
+void duDebugDrawNavMeshBVTree(struct duDebugDraw* dd, const dtNavMesh& mesh);
+void duDebugDrawNavMeshPortals(struct duDebugDraw* dd, const dtNavMesh& mesh);
+void duDebugDrawNavMeshPolysWithFlags(struct duDebugDraw* dd, const dtNavMesh& mesh, const unsigned short polyFlags, const unsigned int col);
+void duDebugDrawNavMeshPoly(struct duDebugDraw* dd, const dtNavMesh& mesh, dtPolyRef ref, const unsigned int col);
+
+void duDebugDrawTileCacheLayerAreas(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch);
+void duDebugDrawTileCacheLayerRegions(struct duDebugDraw* dd, const dtTileCacheLayer& layer, const float cs, const float ch);
+void duDebugDrawTileCacheContours(duDebugDraw* dd, const struct dtTileCacheContourSet& lcset,
+								  const float* orig, const float cs, const float ch);
+void duDebugDrawTileCachePolyMesh(duDebugDraw* dd, const struct dtTileCachePolyMesh& lmesh,
+								  const float* orig, const float cs, const float ch);
+
+#endif // DETOURDEBUGDRAW_H

panda/src/recastdetour/DetourLocalBoundary.cpp

@@ -0,0 +1,137 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#include <string.h>
+#include "DetourLocalBoundary.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourCommon.h"
+#include "DetourAssert.h"
+
+
+dtLocalBoundary::dtLocalBoundary() :
+	m_nsegs(0),
+	m_npolys(0)
+{
+	dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
+}
+
+dtLocalBoundary::~dtLocalBoundary()
+{
+}
+
+void dtLocalBoundary::reset()
+{
+	dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
+	m_npolys = 0;
+	m_nsegs = 0;
+}
+
+void dtLocalBoundary::addSegment(const float dist, const float* s)
+{
+	// Insert neighbour based on the distance.
+	Segment* seg = 0;
+	if (!m_nsegs)
+	{
+		// First, trivial accept.
+		seg = &m_segs[0];
+	}
+	else if (dist >= m_segs[m_nsegs-1].d)
+	{
+		// Further than the last segment, skip.
+		if (m_nsegs >= MAX_LOCAL_SEGS)
+			return;
+		// Last, trivial accept.
+		seg = &m_segs[m_nsegs];
+	}
+	else
+	{
+		// Insert inbetween.
+		int i;
+		for (i = 0; i < m_nsegs; ++i)
+			if (dist <= m_segs[i].d)
+				break;
+		const int tgt = i+1;
+		const int n = dtMin(m_nsegs-i, MAX_LOCAL_SEGS-tgt);
+		dtAssert(tgt+n <= MAX_LOCAL_SEGS);
+		if (n > 0)
+			memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n);
+		seg = &m_segs[i];
+	}
+	
+	seg->d = dist;
+	memcpy(seg->s, s, sizeof(float)*6);
+	
+	if (m_nsegs < MAX_LOCAL_SEGS)
+		m_nsegs++;
+}
+
+void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
+							 dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*3;
+	
+	if (!ref)
+	{
+		dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
+		m_nsegs = 0;
+		m_npolys = 0;
+		return;
+	}
+	
+	dtVcopy(m_center, pos);
+	
+	// First query non-overlapping polygons.
+	navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange,
+									 filter, m_polys, 0, &m_npolys, MAX_LOCAL_POLYS);
+	
+	// Secondly, store all polygon edges.
+	m_nsegs = 0;
+	float segs[MAX_SEGS_PER_POLY*6];
+	int nsegs = 0;
+	for (int j = 0; j < m_npolys; ++j)
+	{
+		navquery->getPolyWallSegments(m_polys[j], filter, segs, 0, &nsegs, MAX_SEGS_PER_POLY);
+		for (int k = 0; k < nsegs; ++k)
+		{
+			const float* s = &segs[k*6];
+			// Skip too distant segments.
+			float tseg;
+			const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg);
+			if (distSqr > dtSqr(collisionQueryRange))
+				continue;
+			addSegment(distSqr, s);
+		}
+	}
+}
+
+bool dtLocalBoundary::isValid(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	if (!m_npolys)
+		return false;
+	
+	// Check that all polygons still pass query filter.
+	for (int i = 0; i < m_npolys; ++i)
+	{
+		if (!navquery->isValidPolyRef(m_polys[i], filter))
+			return false;
+	}
+	
+	return true;
+}
+

panda/src/recastdetour/DetourLocalBoundary.h

@@ -0,0 +1,66 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURLOCALBOUNDARY_H
+#define DETOURLOCALBOUNDARY_H
+
+#include "DetourNavMeshQuery.h"
+
+
+class dtLocalBoundary
+{
+	static const int MAX_LOCAL_SEGS = 8;
+	static const int MAX_LOCAL_POLYS = 16;
+	
+	struct Segment
+	{
+		float s[6];	///< Segment start/end
+		float d;	///< Distance for pruning.
+	};
+	
+	float m_center[3];
+	Segment m_segs[MAX_LOCAL_SEGS];
+	int m_nsegs;
+	
+	dtPolyRef m_polys[MAX_LOCAL_POLYS];
+	int m_npolys;
+
+	void addSegment(const float dist, const float* s);
+	
+public:
+	dtLocalBoundary();
+	~dtLocalBoundary();
+	
+	void reset();
+	
+	void update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
+				dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	bool isValid(dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	inline const float* getCenter() const { return m_center; }
+	inline int getSegmentCount() const { return m_nsegs; }
+	inline const float* getSegment(int i) const { return m_segs[i].s; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtLocalBoundary(const dtLocalBoundary&);
+	dtLocalBoundary& operator=(const dtLocalBoundary&);
+};
+
+#endif // DETOURLOCALBOUNDARY_H

panda/src/recastdetour/DetourMath.h

@@ -0,0 +1,24 @@
+/**
+@defgroup detour Detour
+
+Members in this module are wrappers around the standard math library
+*/
+
+#ifndef DETOURMATH_H
+#define DETOURMATH_H
+
+#include <math.h>
+// This include is required because libstdc++ has problems with isfinite
+// if cmath is included before math.h.
+#include <cmath>
+
+inline float dtMathFabsf(float x) { return fabsf(x); }
+inline float dtMathSqrtf(float x) { return sqrtf(x); }
+inline float dtMathFloorf(float x) { return floorf(x); }
+inline float dtMathCeilf(float x) { return ceilf(x); }
+inline float dtMathCosf(float x) { return cosf(x); }
+inline float dtMathSinf(float x) { return sinf(x); }
+inline float dtMathAtan2f(float y, float x) { return atan2f(y, x); }
+inline bool dtMathIsfinite(float x) { return std::isfinite(x); }
+
+#endif

panda/src/recastdetour/DetourNavMesh.cpp

@@ -0,0 +1,1584 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#include <string.h>
+#include <stdio.h>
+#include "DetourNavMesh.h"
+#include "DetourNode.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+#include <new>
+
+
+inline bool overlapSlabs(const float* amin, const float* amax,
+						 const float* bmin, const float* bmax,
+						 const float px, const float py)
+{
+	// Check for horizontal overlap.
+	// The segment is shrunken a little so that slabs which touch
+	// at end points are not connected.
+	const float minx = dtMax(amin[0]+px,bmin[0]+px);
+	const float maxx = dtMin(amax[0]-px,bmax[0]-px);
+	if (minx > maxx)
+		return false;
+	
+	// Check vertical overlap.
+	const float ad = (amax[1]-amin[1]) / (amax[0]-amin[0]);
+	const float ak = amin[1] - ad*amin[0];
+	const float bd = (bmax[1]-bmin[1]) / (bmax[0]-bmin[0]);
+	const float bk = bmin[1] - bd*bmin[0];
+	const float aminy = ad*minx + ak;
+	const float amaxy = ad*maxx + ak;
+	const float bminy = bd*minx + bk;
+	const float bmaxy = bd*maxx + bk;
+	const float dmin = bminy - aminy;
+	const float dmax = bmaxy - amaxy;
+		
+	// Crossing segments always overlap.
+	if (dmin*dmax < 0)
+		return true;
+		
+	// Check for overlap at endpoints.
+	const float thr = dtSqr(py*2);
+	if (dmin*dmin <= thr || dmax*dmax <= thr)
+		return true;
+		
+	return false;
+}
+
+static float getSlabCoord(const float* va, const int side)
+{
+	if (side == 0 || side == 4)
+		return va[0];
+	else if (side == 2 || side == 6)
+		return va[2];
+	return 0;
+}
+
+static void calcSlabEndPoints(const float* va, const float* vb, float* bmin, float* bmax, const int side)
+{
+	if (side == 0 || side == 4)
+	{
+		if (va[2] < vb[2])
+		{
+			bmin[0] = va[2];
+			bmin[1] = va[1];
+			bmax[0] = vb[2];
+			bmax[1] = vb[1];
+		}
+		else
+		{
+			bmin[0] = vb[2];
+			bmin[1] = vb[1];
+			bmax[0] = va[2];
+			bmax[1] = va[1];
+		}
+	}
+	else if (side == 2 || side == 6)
+	{
+		if (va[0] < vb[0])
+		{
+			bmin[0] = va[0];
+			bmin[1] = va[1];
+			bmax[0] = vb[0];
+			bmax[1] = vb[1];
+		}
+		else
+		{
+			bmin[0] = vb[0];
+			bmin[1] = vb[1];
+			bmax[0] = va[0];
+			bmax[1] = va[1];
+		}
+	}
+}
+
+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;
+}
+
+
+dtNavMesh* dtAllocNavMesh()
+{
+	void* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtNavMesh;
+}
+
+/// @par
+///
+/// This function will only free the memory for tiles with the #DT_TILE_FREE_DATA
+/// flag set.
+void dtFreeNavMesh(dtNavMesh* navmesh)
+{
+	if (!navmesh) return;
+	navmesh->~dtNavMesh();
+	dtFree(navmesh);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+
+/**
+@class dtNavMesh
+
+The navigation mesh consists of one or more tiles defining three primary types of structural data:
+
+A polygon mesh which defines most of the navigation graph. (See rcPolyMesh for its structure.)
+A detail mesh used for determining surface height on the polygon mesh. (See rcPolyMeshDetail for its structure.)
+Off-mesh connections, which define custom point-to-point edges within the navigation graph.
+
+The general build process is as follows:
+
+-# Create rcPolyMesh and rcPolyMeshDetail data using the Recast build pipeline.
+-# Optionally, create off-mesh connection data.
+-# Combine the source data into a dtNavMeshCreateParams structure.
+-# Create a tile data array using dtCreateNavMeshData().
+-# Allocate at dtNavMesh object and initialize it. (For single tile navigation meshes,
+   the tile data is loaded during this step.)
+-# For multi-tile navigation meshes, load the tile data using dtNavMesh::addTile().
+
+Notes:
+
+- This class is usually used in conjunction with the dtNavMeshQuery class for pathfinding.
+- Technically, all navigation meshes are tiled. A 'solo' mesh is simply a navigation mesh initialized 
+  to have only a single tile.
+- This class does not implement any asynchronous methods. So the ::dtStatus result of all methods will 
+  always contain either a success or failure flag.
+
+@see dtNavMeshQuery, dtCreateNavMeshData, dtNavMeshCreateParams, #dtAllocNavMesh, #dtFreeNavMesh
+*/
+
+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)
+{
+#ifndef DT_POLYREF64
+	m_saltBits = 0;
+	m_tileBits = 0;
+	m_polyBits = 0;
+#endif
+	memset(&m_params, 0, sizeof(dtNavMeshParams));
+	m_orig[0] = 0;
+	m_orig[1] = 0;
+	m_orig[2] = 0;
+}
+
+dtNavMesh::~dtNavMesh()
+{
+	for (int i = 0; i < m_maxTiles; ++i)
+	{
+		if (m_tiles[i].flags & DT_TILE_FREE_DATA)
+		{
+			dtFree(m_tiles[i].data);
+			m_tiles[i].data = 0;
+			m_tiles[i].dataSize = 0;
+		}
+	}
+	dtFree(m_posLookup);
+	dtFree(m_tiles);
+}
+		
+dtStatus dtNavMesh::init(const dtNavMeshParams* params)
+{
+	memcpy(&m_params, params, sizeof(dtNavMeshParams));
+	dtVcopy(m_orig, params->orig);
+	m_tileWidth = params->tileWidth;
+	m_tileHeight = params->tileHeight;
+	
+	// Init tiles
+	m_maxTiles = params->maxTiles;
+	m_tileLutSize = dtNextPow2(params->maxTiles/4);
+	if (!m_tileLutSize) m_tileLutSize = 1;
+	m_tileLutMask = m_tileLutSize-1;
+	
+	m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM);
+	if (!m_tiles)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM);
+	if (!m_posLookup)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	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].salt = 1;
+		m_tiles[i].next = m_nextFree;
+		m_nextFree = &m_tiles[i];
+	}
+	
+	// Init ID generator values.
+#ifndef DT_POLYREF64
+	m_tileBits = dtIlog2(dtNextPow2((unsigned int)params->maxTiles));
+	m_polyBits = dtIlog2(dtNextPow2((unsigned int)params->maxPolys));
+	// Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow.
+	m_saltBits = dtMin((unsigned int)31, 32 - m_tileBits - m_polyBits);
+
+	if (m_saltBits < 10)
+		return DT_FAILURE | DT_INVALID_PARAM;
+#endif
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flags)
+{
+	// Make sure the data is in right format.
+	dtMeshHeader* header = (dtMeshHeader*)data;
+	if (header->magic != DT_NAVMESH_MAGIC)
+		return DT_FAILURE | DT_WRONG_MAGIC;
+	if (header->version != DT_NAVMESH_VERSION)
+		return DT_FAILURE | DT_WRONG_VERSION;
+
+	dtNavMeshParams params;
+	dtVcopy(params.orig, header->bmin);
+	params.tileWidth = header->bmax[0] - header->bmin[0];
+	params.tileHeight = header->bmax[2] - header->bmin[2];
+	params.maxTiles = 1;
+	params.maxPolys = header->polyCount;
+	
+	dtStatus status = init(&params);
+	if (dtStatusFailed(status))
+		return status;
+
+	return addTile(data, dataSize, flags, 0, 0);
+}
+
+/// @par
+///
+/// @note The parameters are created automatically when the single tile
+/// initialization is performed.
+const dtNavMeshParams* dtNavMesh::getParams() const
+{
+	return &m_params;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
+								   const dtMeshTile* tile, int side,
+								   dtPolyRef* con, float* conarea, int maxcon) const
+{
+	if (!tile) return 0;
+	
+	float amin[2], amax[2];
+	calcSlabEndPoints(va, vb, amin, amax, side);
+	const float apos = getSlabCoord(va, side);
+
+	// Remove links pointing to 'side' and compact the links array. 
+	float bmin[2], bmax[2];
+	unsigned short m = DT_EXT_LINK | (unsigned short)side;
+	int n = 0;
+	
+	dtPolyRef base = getPolyRefBase(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;
+			
+			const float* vc = &tile->verts[poly->verts[j]*3];
+			const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3];
+			const float bpos = getSlabCoord(vc, side);
+			
+			// Segments are not close enough.
+			if (dtAbs(apos-bpos) > 0.01f)
+				continue;
+			
+			// Check if the segments touch.
+			calcSlabEndPoints(vc,vd, bmin,bmax, side);
+			
+			if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue;
+			
+			// Add return value.
+			if (n < maxcon)
+			{
+				conarea[n*2+0] = dtMax(amin[0], bmin[0]);
+				conarea[n*2+1] = dtMin(amax[0], bmax[0]);
+				con[n] = base | (dtPolyRef)i;
+				n++;
+			}
+			break;
+		}
+	}
+	return n;
+}
+
+void dtNavMesh::unconnectLinks(dtMeshTile* tile, dtMeshTile* target)
+{
+	if (!tile || !target) return;
+
+	const unsigned int targetNum = decodePolyIdTile(getTileRef(target));
+
+	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 (decodePolyIdTile(tile->links[j].ref) == targetNum)
+			{
+				// Remove 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 non-portal edges.
+			if ((poly->neis[j] & DT_EXT_LINK) == 0)
+				continue;
+			
+			const int dir = (int)(poly->neis[j] & 0xff);
+			if (side != -1 && dir != side)
+				continue;
+			
+			// Create new links
+			const float* va = &tile->verts[poly->verts[j]*3];
+			const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3];
+			dtPolyRef nei[4];
+			float neia[4*2];
+			int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(dir), 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)dir;
+					
+					link->next = poly->firstLink;
+					poly->firstLink = idx;
+
+					// Compress portal limits to a byte value.
+					if (dir == 0 || dir == 4)
+					{
+						float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]);
+						float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]);
+						if (tmin > tmax)
+							dtSwap(tmin,tmax);
+						link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
+						link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f);
+					}
+					else if (dir == 2 || dir == 6)
+					{
+						float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]);
+						float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]);
+						if (tmin > tmax)
+							dtSwap(tmin,tmax);
+						link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
+						link->bmax = (unsigned char)(dtClamp(tmax, 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 = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side);
+	
+	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];
+		// Skip off-mesh connections which start location could not be connected at all.
+		if (targetPoly->firstLink == DT_NULL_LINK)
+			continue;
+		
+		const float halfExtents[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, halfExtents, nearestPt);
+		if (!ref)
+			continue;
+		// findNearestPoly may return too optimistic results, further check to make sure. 
+		if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(targetCon->rad))
+			continue;
+		// Make sure the location is on current mesh.
+		float* v = &target->verts[targetPoly->verts[1]*3];
+		dtVcopy(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 tidx = allocLink(tile);
+			if (tidx != DT_NULL_LINK)
+			{
+				const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
+				dtPoly* landPoly = &tile->polys[landPolyIdx];
+				dtLink* link = &tile->links[tidx];
+				link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly);
+				link->edge = 0xff;
+				link->side = (unsigned char)(side == -1 ? 0xff : side);
+				link->bmin = link->bmax = 0;
+				// Add to linked list.
+				link->next = landPoly->firstLink;
+				landPoly->firstLink = tidx;
+			}
+		}
+	}
+
+}
+
+void dtNavMesh::connectIntLinks(dtMeshTile* tile)
+{
+	if (!tile) return;
+
+	dtPolyRef base = getPolyRefBase(tile);
+
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		dtPoly* poly = &tile->polys[i];
+		poly->firstLink = DT_NULL_LINK;
+
+		if (poly->getType() == 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 | (dtPolyRef)(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::baseOffMeshLinks(dtMeshTile* tile)
+{
+	if (!tile) return;
+	
+	dtPolyRef base = getPolyRefBase(tile);
+	
+	// Base off-mesh connection start points.
+	for (int i = 0; i < tile->header->offMeshConCount; ++i)
+	{
+		dtOffMeshConnection* con = &tile->offMeshCons[i];
+		dtPoly* poly = &tile->polys[con->poly];
+	
+		const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad };
+		
+		// Find polygon to connect to.
+		const float* p = &con->pos[0]; // First vertex
+		float nearestPt[3];
+		dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt);
+		if (!ref) continue;
+		// findNearestPoly may return too optimistic results, further check to make sure. 
+		if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad))
+			continue;
+		// Make sure the location is on current mesh.
+		float* v = &tile->verts[poly->verts[0]*3];
+		dtVcopy(v, nearestPt);
+
+		// Link off-mesh connection to target poly.
+		unsigned int idx = allocLink(tile);
+		if (idx != DT_NULL_LINK)
+		{
+			dtLink* link = &tile->links[idx];
+			link->ref = ref;
+			link->edge = (unsigned char)0;
+			link->side = 0xff;
+			link->bmin = link->bmax = 0;
+			// Add to linked list.
+			link->next = poly->firstLink;
+			poly->firstLink = idx;
+		}
+
+		// Start end-point is always connect back to off-mesh connection. 
+		unsigned int tidx = allocLink(tile);
+		if (tidx != DT_NULL_LINK)
+		{
+			const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
+			dtPoly* landPoly = &tile->polys[landPolyIdx];
+			dtLink* link = &tile->links[tidx];
+			link->ref = base | (dtPolyRef)(con->poly);
+			link->edge = 0xff;
+			link->side = 0xff;
+			link->bmin = link->bmax = 0;
+			// Add to linked list.
+			link->next = landPoly->firstLink;
+			landPoly->firstLink = tidx;
+		}
+	}
+}
+
+namespace
+{
+	template<bool onlyBoundary>
+	void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest)
+	{
+		const unsigned int ip = (unsigned int)(poly - tile->polys);
+		const dtPolyDetail* pd = &tile->detailMeshes[ip];
+
+		float dmin = FLT_MAX;
+		float tmin = 0;
+		const float* pmin = 0;
+		const float* pmax = 0;
+
+		for (int i = 0; i < pd->triCount; i++)
+		{
+			const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4];
+			const int ANY_BOUNDARY_EDGE =
+				(DT_DETAIL_EDGE_BOUNDARY << 0) |
+				(DT_DETAIL_EDGE_BOUNDARY << 2) |
+				(DT_DETAIL_EDGE_BOUNDARY << 4);
+			if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0)
+				continue;
+
+			const float* v[3];
+			for (int j = 0; j < 3; ++j)
+			{
+				if (tris[j] < poly->vertCount)
+					v[j] = &tile->verts[poly->verts[tris[j]] * 3];
+				else
+					v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3];
+			}
+
+			for (int k = 0, j = 2; k < 3; j = k++)
+			{
+				if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 &&
+					(onlyBoundary || tris[j] < tris[k]))
+				{
+					// Only looking at boundary edges and this is internal, or
+					// this is an inner edge that we will see again or have already seen.
+					continue;
+				}
+
+				float t;
+				float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t);
+				if (d < dmin)
+				{
+					dmin = d;
+					tmin = t;
+					pmin = v[j];
+					pmax = v[k];
+				}
+			}
+		}
+
+		dtVlerp(closest, pmin, pmax, tmin);
+	}
+}
+
+bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const
+{
+	// Off-mesh connections do not have detail polys and getting height
+	// over them does not make sense.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+		return false;
+
+	const unsigned int ip = (unsigned int)(poly - tile->polys);
+	const dtPolyDetail* pd = &tile->detailMeshes[ip];
+	
+	float verts[DT_VERTS_PER_POLYGON*3];	
+	const int nv = poly->vertCount;
+	for (int i = 0; i < nv; ++i)
+		dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
+	
+	if (!dtPointInPolygon(pos, verts, nv))
+		return false;
+
+	if (!height)
+		return true;
+	
+	// Find height at the location.
+	for (int j = 0; j < pd->triCount; ++j)
+	{
+		const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
+		const float* v[3];
+		for (int k = 0; k < 3; ++k)
+		{
+			if (t[k] < poly->vertCount)
+				v[k] = &tile->verts[poly->verts[t[k]]*3];
+			else
+				v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
+		}
+		float h;
+		if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
+		{
+			*height = h;
+			return true;
+		}
+	}
+
+	// If all triangle checks failed above (can happen with degenerate triangles
+	// or larger floating point values) the point is on an edge, so just select
+	// closest. This should almost never happen so the extra iteration here is
+	// ok.
+	float closest[3];
+	closestPointOnDetailEdges<false>(tile, poly, pos, closest);
+	*height = closest[1];
+	return true;
+}
+
+void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
+{
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	getTileAndPolyByRefUnsafe(ref, &tile, &poly);
+
+	dtVcopy(closest, pos);
+	if (getPolyHeight(tile, poly, pos, &closest[1]))
+	{
+		if (posOverPoly)
+			*posOverPoly = true;
+		return;
+	}
+
+	if (posOverPoly)
+		*posOverPoly = false;
+
+	// Off-mesh connections don't have detail polygons.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		const float* v0 = &tile->verts[poly->verts[0]*3];
+		const float* v1 = &tile->verts[poly->verts[1]*3];
+		float t;
+		dtDistancePtSegSqr2D(pos, v0, v1, t);
+		dtVlerp(closest, v0, v1, t);
+		return;
+	}
+
+	// Outside poly that is not an offmesh connection.
+	closestPointOnDetailEdges<true>(tile, poly, pos, closest);
+}
+
+dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
+										   const float* center, const float* halfExtents,
+										   float* nearestPt) const
+{
+	float bmin[3], bmax[3];
+	dtVsub(bmin, center, halfExtents);
+	dtVadd(bmax, center, halfExtents);
+	
+	// Get nearby polygons from proximity grid.
+	dtPolyRef polys[128];
+	int polyCount = queryPolygonsInTile(tile, bmin, bmax, 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];
+		float diff[3];
+		bool posOverPoly = false;
+		float d;
+		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
+
+		// If a point is directly over a polygon and closer than
+		// climb height, favor that instead of straight line nearest point.
+		dtVsub(diff, center, closestPtPoly);
+		if (posOverPoly)
+		{
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
+		}
+		
+		if (d < nearestDistanceSqr)
+		{
+			dtVcopy(nearestPt, closestPtPoly);
+			nearestDistanceSqr = d;
+			nearest = ref;
+		}
+	}
+	
+	return nearest;
+}
+
+int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
+								   dtPolyRef* polys, const int maxPolys) const
+{
+	if (tile->bvTree)
+	{
+		const dtBVNode* node = &tile->bvTree[0];
+		const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
+		const float* tbmin = tile->header->bmin;
+		const float* tbmax = tile->header->bmax;
+		const float qfac = tile->header->bvQuantFactor;
+		
+		// Calculate quantized box
+		unsigned short bmin[3], bmax[3];
+		// dtClamp query box to world box.
+		float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
+		float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
+		float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
+		float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
+		float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
+		float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
+		// Quantize
+		bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
+		bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
+		bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
+		bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
+		bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
+		bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
+		
+		// Traverse tree
+		dtPolyRef base = getPolyRefBase(tile);
+		int n = 0;
+		while (node < end)
+		{
+			const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
+			const bool isLeafNode = node->i >= 0;
+			
+			if (isLeafNode && overlap)
+			{
+				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 = getPolyRefBase(tile);
+		for (int i = 0; i < tile->header->polyCount; ++i)
+		{
+			dtPoly* p = &tile->polys[i];
+			// Do not return off-mesh connection polygons.
+			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			// Calc polygon bounds.
+			const float* v = &tile->verts[p->verts[0]*3];
+			dtVcopy(bmin, v);
+			dtVcopy(bmax, v);
+			for (int j = 1; j < p->vertCount; ++j)
+			{
+				v = &tile->verts[p->verts[j]*3];
+				dtVmin(bmin, v);
+				dtVmax(bmax, v);
+			}
+			if (dtOverlapBounds(qmin,qmax, bmin,bmax))
+			{
+				if (n < maxPolys)
+					polys[n++] = base | (dtPolyRef)i;
+			}
+		}
+		return n;
+	}
+}
+
+/// @par
+///
+/// The add operation will fail if the data is in the wrong format, the allocated tile
+/// space is full, or there is a tile already at the specified reference.
+///
+/// The lastRef parameter is used to restore a tile with the same tile
+/// reference it had previously used.  In this case the #dtPolyRef's for the
+/// tile will be restored to the same values they were before the tile was 
+/// removed.
+///
+/// The nav mesh assumes exclusive access to the data passed and will make
+/// changes to the dynamic portion of the data. For that reason the data
+/// should not be reused in other nav meshes until the tile has been successfully
+/// removed from this nav mesh.
+///
+/// @see dtCreateNavMeshData, #removeTile
+dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
+							dtTileRef lastRef, dtTileRef* result)
+{
+	// Make sure the data is in right format.
+	dtMeshHeader* header = (dtMeshHeader*)data;
+	if (header->magic != DT_NAVMESH_MAGIC)
+		return DT_FAILURE | DT_WRONG_MAGIC;
+	if (header->version != DT_NAVMESH_VERSION)
+		return DT_FAILURE | DT_WRONG_VERSION;
+		
+	// Make sure the location is free.
+	if (getTileAt(header->x, header->y, header->layer))
+		return DT_FAILURE | DT_ALREADY_OCCUPIED;
+		
+	// Allocate a tile.
+	dtMeshTile* tile = 0;
+	if (!lastRef)
+	{
+		if (m_nextFree)
+		{
+			tile = m_nextFree;
+			m_nextFree = tile->next;
+			tile->next = 0;
+		}
+	}
+	else
+	{
+		// Try to relocate the tile to specific index with same salt.
+		int tileIndex = (int)decodePolyIdTile((dtPolyRef)lastRef);
+		if (tileIndex >= m_maxTiles)
+			return DT_FAILURE | DT_OUT_OF_MEMORY;
+		// Try to find the specific tile id from the free list.
+		dtMeshTile* target = &m_tiles[tileIndex];
+		dtMeshTile* prev = 0;
+		tile = m_nextFree;
+		while (tile && tile != target)
+		{
+			prev = tile;
+			tile = tile->next;
+		}
+		// Could not find the correct location.
+		if (tile != target)
+			return DT_FAILURE | DT_OUT_OF_MEMORY;
+		// Remove from freelist
+		if (!prev)
+			m_nextFree = tile->next;
+		else
+			prev->next = tile->next;
+
+		// Restore salt.
+		tile->salt = decodePolyIdSalt((dtPolyRef)lastRef);
+	}
+
+	// Make sure we could allocate a tile.
+	if (!tile)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	// Insert tile into the position lut.
+	int h = computeTileHash(header->x, header->y, m_tileLutMask);
+	tile->next = m_posLookup[h];
+	m_posLookup[h] = tile;
+	
+	// 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;
+	tile->verts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
+	tile->polys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
+	tile->links = dtGetThenAdvanceBufferPointer<dtLink>(d, linksSize);
+	tile->detailMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
+	tile->detailVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
+	tile->detailTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
+	tile->bvTree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvtreeSize);
+	tile->offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshLinksSize);
+
+	// If there are no items in the bvtree, reset the tree pointer.
+	if (!bvtreeSize)
+		tile->bvTree = 0;
+
+	// Build links freelist
+	tile->linksFreeList = 0;
+	tile->links[header->maxLinkCount-1].next = DT_NULL_LINK;
+	for (int i = 0; i < header->maxLinkCount-1; ++i)
+		tile->links[i].next = i+1;
+
+	// Init tile.
+	tile->header = header;
+	tile->data = data;
+	tile->dataSize = dataSize;
+	tile->flags = flags;
+
+	connectIntLinks(tile);
+
+	// Base off-mesh connections to their starting polygons and connect connections inside the tile.
+	baseOffMeshLinks(tile);
+	connectExtOffMeshLinks(tile, tile, -1);
+
+	// Create connections with neighbour tiles.
+	static const int MAX_NEIS = 32;
+	dtMeshTile* neis[MAX_NEIS];
+	int nneis;
+	
+	// Connect with layers in current tile.
+	nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS);
+	for (int j = 0; j < nneis; ++j)
+	{
+		if (neis[j] == tile)
+			continue;
+	
+		connectExtLinks(tile, neis[j], -1);
+		connectExtLinks(neis[j], tile, -1);
+		connectExtOffMeshLinks(tile, neis[j], -1);
+		connectExtOffMeshLinks(neis[j], tile, -1);
+	}
+	
+	// Connect with neighbour tiles.
+	for (int i = 0; i < 8; ++i)
+	{
+		nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS);
+		for (int j = 0; j < nneis; ++j)
+		{
+			connectExtLinks(tile, neis[j], i);
+			connectExtLinks(neis[j], tile, dtOppositeTile(i));
+			connectExtOffMeshLinks(tile, neis[j], i);
+			connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i));
+		}
+	}
+	
+	if (result)
+		*result = getTileRef(tile);
+	
+	return DT_SUCCESS;
+}
+
+const dtMeshTile* dtNavMesh::getTileAt(const int x, const int y, const int layer) const
+{
+	// Find tile based on hash.
+	int h = computeTileHash(x,y,m_tileLutMask);
+	dtMeshTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y &&
+			tile->header->layer == layer)
+		{
+			return tile;
+		}
+		tile = tile->next;
+	}
+	return 0;
+}
+
+int dtNavMesh::getNeighbourTilesAt(const int x, const int y, const int side, dtMeshTile** tiles, const int maxTiles) const
+{
+	int nx = x, ny = y;
+	switch (side)
+	{
+		case 0: nx++; break;
+		case 1: nx++; ny++; break;
+		case 2: ny++; break;
+		case 3: nx--; ny++; break;
+		case 4: nx--; break;
+		case 5: nx--; ny--; break;
+		case 6: ny--; break;
+		case 7: nx++; ny--; break;
+	};
+
+	return getTilesAt(nx, ny, tiles, maxTiles);
+}
+
+int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const int maxTiles) const
+{
+	int n = 0;
+	
+	// Find tile based on hash.
+	int h = computeTileHash(x,y,m_tileLutMask);
+	dtMeshTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y)
+		{
+			if (n < maxTiles)
+				tiles[n++] = tile;
+		}
+		tile = tile->next;
+	}
+	
+	return n;
+}
+
+/// @par
+///
+/// This function will not fail if the tiles array is too small to hold the
+/// entire result set.  It will simply fill the array to capacity.
+int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile const** tiles, const int maxTiles) const
+{
+	int n = 0;
+	
+	// Find tile based on hash.
+	int h = computeTileHash(x,y,m_tileLutMask);
+	dtMeshTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y)
+		{
+			if (n < maxTiles)
+				tiles[n++] = tile;
+		}
+		tile = tile->next;
+	}
+	
+	return n;
+}
+
+
+dtTileRef dtNavMesh::getTileRefAt(const int x, const int y, const int layer) const
+{
+	// Find tile based on hash.
+	int h = computeTileHash(x,y,m_tileLutMask);
+	dtMeshTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->x == x &&
+			tile->header->y == y &&
+			tile->header->layer == layer)
+		{
+			return getTileRef(tile);
+		}
+		tile = tile->next;
+	}
+	return 0;
+}
+
+const dtMeshTile* dtNavMesh::getTileByRef(dtTileRef ref) const
+{
+	if (!ref)
+		return 0;
+	unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref);
+	unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref);
+	if ((int)tileIndex >= m_maxTiles)
+		return 0;
+	const dtMeshTile* tile = &m_tiles[tileIndex];
+	if (tile->salt != tileSalt)
+		return 0;
+	return tile;
+}
+
+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];
+}
+
+void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const
+{
+	*tx = (int)floorf((pos[0]-m_orig[0]) / m_tileWidth);
+	*ty = (int)floorf((pos[2]-m_orig[2]) / m_tileHeight);
+}
+
+dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const
+{
+	if (!ref) return DT_FAILURE;
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
+	if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
+	*tile = &m_tiles[it];
+	*poly = &m_tiles[it].polys[ip];
+	return DT_SUCCESS;
+}
+
+/// @par
+///
+/// @warning Only use this function if it is known that the provided polygon
+/// reference is valid. This function is faster than #getTileAndPolyByRef, but
+/// it does not validate the reference.
+void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const
+{
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	*tile = &m_tiles[it];
+	*poly = &m_tiles[it].polys[ip];
+}
+
+bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const
+{
+	if (!ref) return false;
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return false;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
+	if (ip >= (unsigned int)m_tiles[it].header->polyCount) return false;
+	return true;
+}
+
+/// @par
+///
+/// This function returns the data for the tile so that, if desired,
+/// it can be added back to the navigation mesh at a later point.
+///
+/// @see #addTile
+dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSize)
+{
+	if (!ref)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref);
+	unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref);
+	if ((int)tileIndex >= m_maxTiles)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	dtMeshTile* tile = &m_tiles[tileIndex];
+	if (tile->salt != tileSalt)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Remove tile from hash lookup.
+	int h = computeTileHash(tile->header->x,tile->header->y,m_tileLutMask);
+	dtMeshTile* prev = 0;
+	dtMeshTile* cur = m_posLookup[h];
+	while (cur)
+	{
+		if (cur == tile)
+		{
+			if (prev)
+				prev->next = cur->next;
+			else
+				m_posLookup[h] = cur->next;
+			break;
+		}
+		prev = cur;
+		cur = cur->next;
+	}
+	
+	// Remove connections to neighbour tiles.
+	static const int MAX_NEIS = 32;
+	dtMeshTile* neis[MAX_NEIS];
+	int nneis;
+	
+	// Disconnect from other layers in current tile.
+	nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS);
+	for (int j = 0; j < nneis; ++j)
+	{
+		if (neis[j] == tile) continue;
+		unconnectLinks(neis[j], tile);
+	}
+	
+	// Disconnect from neighbour tiles.
+	for (int i = 0; i < 8; ++i)
+	{
+		nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS);
+		for (int j = 0; j < nneis; ++j)
+			unconnectLinks(neis[j], tile);
+	}
+		
+	// Reset tile.
+	if (tile->flags & DT_TILE_FREE_DATA)
+	{
+		// Owns data
+		dtFree(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->flags = 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;
+
+	// Update salt, salt should never be zero.
+#ifdef DT_POLYREF64
+	tile->salt = (tile->salt+1) & ((1<<DT_SALT_BITS)-1);
+#else
+	tile->salt = (tile->salt+1) & ((1<<m_saltBits)-1);
+#endif
+	if (tile->salt == 0)
+		tile->salt++;
+
+	// Add to free list.
+	tile->next = m_nextFree;
+	m_nextFree = tile;
+
+	return DT_SUCCESS;
+}
+
+dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const
+{
+	if (!tile) return 0;
+	const unsigned int it = (unsigned int)(tile - m_tiles);
+	return (dtTileRef)encodePolyId(tile->salt, it, 0);
+}
+
+/// @par
+///
+/// Example use case:
+/// @code
+///
+/// const dtPolyRef base = navmesh->getPolyRefBase(tile);
+/// for (int i = 0; i < tile->header->polyCount; ++i)
+/// {
+///     const dtPoly* p = &tile->polys[i];
+///     const dtPolyRef ref = base | (dtPolyRef)i;
+///     
+///     // Use the reference to access the polygon data.
+/// }
+/// @endcode
+dtPolyRef dtNavMesh::getPolyRefBase(const dtMeshTile* tile) const
+{
+	if (!tile) return 0;
+	const unsigned int it = (unsigned int)(tile - m_tiles);
+	return encodePolyId(tile->salt, it, 0);
+}
+
+struct dtTileState
+{
+	int magic;								// Magic number, used to identify the data.
+	int version;							// Data version number.
+	dtTileRef ref;							// Tile ref at the time of storing the data.
+};
+
+struct dtPolyState
+{
+	unsigned short flags;						// Flags (see dtPolyFlags).
+	unsigned char area;							// Area ID of the polygon.
+};
+
+///  @see #storeTileState
+int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const
+{
+	if (!tile) return 0;
+	const int headerSize = dtAlign4(sizeof(dtTileState));
+	const int polyStateSize = dtAlign4(sizeof(dtPolyState) * tile->header->polyCount);
+	return headerSize + polyStateSize;
+}
+
+/// @par
+///
+/// Tile state includes non-structural data such as polygon flags, area ids, etc.
+/// @note The state data is only valid until the tile reference changes.
+/// @see #getTileStateSize, #restoreTileState
+dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const
+{
+	// Make sure there is enough space to store the state.
+	const int sizeReq = getTileStateSize(tile);
+	if (maxDataSize < sizeReq)
+		return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+		
+	dtTileState* tileState = dtGetThenAdvanceBufferPointer<dtTileState>(data, dtAlign4(sizeof(dtTileState)));
+	dtPolyState* polyStates = dtGetThenAdvanceBufferPointer<dtPolyState>(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount));
+	
+	// Store tile state.
+	tileState->magic = DT_NAVMESH_STATE_MAGIC;
+	tileState->version = DT_NAVMESH_STATE_VERSION;
+	tileState->ref = getTileRef(tile);
+	
+	// Store per poly state.
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		const dtPoly* p = &tile->polys[i];
+		dtPolyState* s = &polyStates[i];
+		s->flags = p->flags;
+		s->area = p->getArea();
+	}
+	
+	return DT_SUCCESS;
+}
+
+/// @par
+///
+/// Tile state includes non-structural data such as polygon flags, area ids, etc.
+/// @note This function does not impact the tile's #dtTileRef and #dtPolyRef's.
+/// @see #storeTileState
+dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize)
+{
+	// Make sure there is enough space to store the state.
+	const int sizeReq = getTileStateSize(tile);
+	if (maxDataSize < sizeReq)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	const dtTileState* tileState = dtGetThenAdvanceBufferPointer<const dtTileState>(data, dtAlign4(sizeof(dtTileState)));
+	const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer<const dtPolyState>(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount));
+	
+	// Check that the restore is possible.
+	if (tileState->magic != DT_NAVMESH_STATE_MAGIC)
+		return DT_FAILURE | DT_WRONG_MAGIC;
+	if (tileState->version != DT_NAVMESH_STATE_VERSION)
+		return DT_FAILURE | DT_WRONG_VERSION;
+	if (tileState->ref != getTileRef(tile))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Restore per poly state.
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		dtPoly* p = &tile->polys[i];
+		const dtPolyState* s = &polyStates[i];
+		p->flags = s->flags;
+		p->setArea(s->area);
+	}
+	
+	return DT_SUCCESS;
+}
+
+/// @par
+///
+/// Off-mesh connections are stored in the navigation mesh as special 2-vertex 
+/// polygons with a single edge. At least one of the vertices is expected to be 
+/// inside a normal polygon. So an off-mesh connection is "entered" from a 
+/// normal polygon at one of its endpoints. This is the polygon identified by 
+/// the prevRef parameter.
+dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const
+{
+	unsigned int salt, it, ip;
+
+	if (!polyRef)
+		return DT_FAILURE;
+	
+	// Get current polygon
+	decodePolyId(polyRef, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
+	const dtMeshTile* tile = &m_tiles[it];
+	if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
+	const dtPoly* poly = &tile->polys[ip];
+
+	// Make sure that the current poly is indeed off-mesh link.
+	if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION)
+		return DT_FAILURE;
+
+	// 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;
+		}
+	}
+	
+	dtVcopy(startPos, &tile->verts[poly->verts[idx0]*3]);
+	dtVcopy(endPos, &tile->verts[poly->verts[idx1]*3]);
+
+	return DT_SUCCESS;
+}
+
+
+const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) const
+{
+	unsigned int salt, it, ip;
+	
+	if (!ref)
+		return 0;
+	
+	// Get current polygon
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return 0;
+	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];
+	
+	// Make sure that the current poly is indeed off-mesh link.
+	if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION)
+		return 0;
+
+	const unsigned int idx =  ip - tile->header->offMeshBase;
+	dtAssert(idx < (unsigned int)tile->header->offMeshConCount);
+	return &tile->offMeshCons[idx];
+}
+
+
+dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags)
+{
+	if (!ref) return DT_FAILURE;
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
+	dtMeshTile* tile = &m_tiles[it];
+	if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
+	dtPoly* poly = &tile->polys[ip];
+	
+	// Change flags.
+	poly->flags = flags;
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const
+{
+	if (!ref) return DT_FAILURE;
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
+	const dtMeshTile* tile = &m_tiles[it];
+	if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
+	const dtPoly* poly = &tile->polys[ip];
+
+	*resultFlags = poly->flags;
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area)
+{
+	if (!ref) return DT_FAILURE;
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
+	dtMeshTile* tile = &m_tiles[it];
+	if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
+	dtPoly* poly = &tile->polys[ip];
+	
+	poly->setArea(area);
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const
+{
+	if (!ref) return DT_FAILURE;
+	unsigned int salt, it, ip;
+	decodePolyId(ref, salt, it, ip);
+	if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
+	if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
+	const dtMeshTile* tile = &m_tiles[it];
+	if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
+	const dtPoly* poly = &tile->polys[ip];
+	
+	*resultArea = poly->getArea();
+	
+	return DT_SUCCESS;
+}
+

panda/src/recastdetour/DetourNavMesh.h

@@ -0,0 +1,784 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURNAVMESH_H
+#define DETOURNAVMESH_H
+
+#include "DetourAlloc.h"
+#include "DetourStatus.h"
+
+// Undefine (or define in a build cofnig) the following line to use 64bit polyref.
+// Generally not needed, useful for very large worlds.
+// Note: tiles build using 32bit refs are not compatible with 64bit refs!
+//#define DT_POLYREF64 1
+
+#ifdef DT_POLYREF64
+// TODO: figure out a multiplatform version of uint64_t
+// - maybe: https://code.google.com/p/msinttypes/
+// - or: http://www.azillionmonkeys.com/qed/pstdint.h
+#include <stdint.h>
+#endif
+
+// Note: If you want to use 64-bit refs, change the types of both dtPolyRef & dtTileRef.
+// It is also recommended that you change dtHashRef() to a proper 64-bit hash.
+
+/// A handle to a polygon within a navigation mesh tile.
+/// @ingroup detour
+#ifdef DT_POLYREF64
+static const unsigned int DT_SALT_BITS = 16;
+static const unsigned int DT_TILE_BITS = 28;
+static const unsigned int DT_POLY_BITS = 20;
+typedef uint64_t dtPolyRef;
+#else
+typedef unsigned int dtPolyRef;
+#endif
+
+/// A handle to a tile within a navigation mesh.
+/// @ingroup detour
+#ifdef DT_POLYREF64
+typedef uint64_t dtTileRef;
+#else
+typedef unsigned int dtTileRef;
+#endif
+
+/// The maximum number of vertices per navigation polygon.
+/// @ingroup detour
+static const int DT_VERTS_PER_POLYGON = 6;
+
+/// @{
+/// @name Tile Serialization Constants
+/// These constants are used to detect whether a navigation tile's data
+/// and state format is compatible with the current build.
+///
+
+/// A magic number used to detect compatibility of navigation tile data.
+static const int DT_NAVMESH_MAGIC = 'D'<<24 | 'N'<<16 | 'A'<<8 | 'V';
+
+/// A version number used to detect compatibility of navigation tile data.
+static const int DT_NAVMESH_VERSION = 7;
+
+/// A magic number used to detect the compatibility of navigation tile states.
+static const int DT_NAVMESH_STATE_MAGIC = 'D'<<24 | 'N'<<16 | 'M'<<8 | 'S';
+
+/// A version number used to detect compatibility of navigation tile states.
+static const int DT_NAVMESH_STATE_VERSION = 1;
+
+/// @}
+
+/// A flag that indicates that an entity links to an external entity.
+/// (E.g. A polygon edge is a portal that links to another polygon.)
+static const unsigned short DT_EXT_LINK = 0x8000;
+
+/// A value that indicates the entity does not link to anything.
+static const unsigned int DT_NULL_LINK = 0xffffffff;
+
+/// A flag that indicates that an off-mesh connection can be traversed in both directions. (Is bidirectional.)
+static const unsigned int DT_OFFMESH_CON_BIDIR = 1;
+
+/// The maximum number of user defined area ids.
+/// @ingroup detour
+static const int DT_MAX_AREAS = 64;
+
+/// Tile flags used for various functions and fields.
+/// For an example, see dtNavMesh::addTile().
+enum dtTileFlags
+{
+	/// The navigation mesh owns the tile memory and is responsible for freeing it.
+	DT_TILE_FREE_DATA = 0x01,
+};
+
+/// Vertex flags returned by dtNavMeshQuery::findStraightPath.
+enum dtStraightPathFlags
+{
+	DT_STRAIGHTPATH_START = 0x01,				///< The vertex is the start position in the path.
+	DT_STRAIGHTPATH_END = 0x02,					///< The vertex is the end position in the path.
+	DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04,	///< The vertex is the start of an off-mesh connection.
+};
+
+/// Options for dtNavMeshQuery::findStraightPath.
+enum dtStraightPathOptions
+{
+	DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01,	///< Add a vertex at every polygon edge crossing where area changes.
+	DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02,	///< Add a vertex at every polygon edge crossing.
+};
+
+
+/// Options for dtNavMeshQuery::initSlicedFindPath and updateSlicedFindPath
+enum dtFindPathOptions
+{
+	DT_FINDPATH_ANY_ANGLE	= 0x02,		///< use raycasts during pathfind to "shortcut" (raycast still consider costs)
+};
+
+/// Options for dtNavMeshQuery::raycast
+enum dtRaycastOptions
+{
+	DT_RAYCAST_USE_COSTS = 0x01,		///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost
+};
+
+enum dtDetailTriEdgeFlags
+{
+	DT_DETAIL_EDGE_BOUNDARY = 0x01,		///< Detail triangle edge is part of the poly boundary
+};
+
+
+/// Limit raycasting during any angle pahfinding
+/// The limit is given as a multiple of the character radius
+static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f;
+
+/// Flags representing the type of a navigation mesh polygon.
+enum dtPolyTypes
+{
+	/// The polygon is a standard convex polygon that is part of the surface of the mesh.
+	DT_POLYTYPE_GROUND = 0,
+	/// The polygon is an off-mesh connection consisting of two vertices.
+	DT_POLYTYPE_OFFMESH_CONNECTION = 1,
+};
+
+
+/// Defines a polygon within a dtMeshTile object.
+/// @ingroup detour
+struct dtPoly
+{
+	/// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.)
+	unsigned int firstLink;
+
+	/// The indices of the polygon's vertices.
+	/// The actual vertices are located in dtMeshTile::verts.
+	unsigned short verts[DT_VERTS_PER_POLYGON];
+
+	/// Packed data representing neighbor polygons references and flags for each edge.
+	unsigned short neis[DT_VERTS_PER_POLYGON];
+
+	/// The user defined polygon flags.
+	unsigned short flags;
+
+	/// The number of vertices in the polygon.
+	unsigned char vertCount;
+
+	/// The bit packed area id and polygon type.
+	/// @note Use the structure's set and get methods to acess this value.
+	unsigned char areaAndtype;
+
+	/// Sets the user defined area id. [Limit: < #DT_MAX_AREAS]
+	inline void setArea(unsigned char a) { areaAndtype = (areaAndtype & 0xc0) | (a & 0x3f); }
+
+	/// Sets the polygon type. (See: #dtPolyTypes.)
+	inline void setType(unsigned char t) { areaAndtype = (areaAndtype & 0x3f) | (t << 6); }
+
+	/// Gets the user defined area id.
+	inline unsigned char getArea() const { return areaAndtype & 0x3f; }
+
+	/// Gets the polygon type. (See: #dtPolyTypes)
+	inline unsigned char getType() const { return areaAndtype >> 6; }
+};
+
+/// Defines the location of detail sub-mesh data within a dtMeshTile.
+struct dtPolyDetail
+{
+	unsigned int vertBase;			///< The offset of the vertices in the dtMeshTile::detailVerts array.
+	unsigned int triBase;			///< The offset of the triangles in the dtMeshTile::detailTris array.
+	unsigned char vertCount;		///< The number of vertices in the sub-mesh.
+	unsigned char triCount;			///< The number of triangles in the sub-mesh.
+};
+
+/// Defines a link between polygons.
+/// @note This structure is rarely if ever used by the end user.
+/// @see dtMeshTile
+struct dtLink
+{
+	dtPolyRef ref;					///< Neighbour reference. (The neighbor that is linked to.)
+	unsigned int next;				///< Index of the next link.
+	unsigned char edge;				///< Index of the polygon edge that owns this link.
+	unsigned char side;				///< If a boundary link, defines on which side the link is.
+	unsigned char bmin;				///< If a boundary link, defines the minimum sub-edge area.
+	unsigned char bmax;				///< If a boundary link, defines the maximum sub-edge area.
+};
+
+/// Bounding volume node.
+/// @note This structure is rarely if ever used by the end user.
+/// @see dtMeshTile
+struct dtBVNode
+{
+	unsigned short bmin[3];			///< Minimum bounds of the node's AABB. [(x, y, z)]
+	unsigned short bmax[3];			///< Maximum bounds of the node's AABB. [(x, y, z)]
+	int i;							///< The node's index. (Negative for escape sequence.)
+};
+
+/// Defines an navigation mesh off-mesh connection within a dtMeshTile object.
+/// An off-mesh connection is a user defined traversable connection made up to two vertices.
+struct dtOffMeshConnection
+{
+	/// The endpoints of the connection. [(ax, ay, az, bx, by, bz)]
+	float pos[6];
+
+	/// The radius of the endpoints. [Limit: >= 0]
+	float rad;		
+
+	/// The polygon reference of the connection within the tile.
+	unsigned short poly;
+
+	/// Link flags. 
+	/// @note These are not the connection's user defined flags. Those are assigned via the 
+	/// connection's dtPoly definition. These are link flags used for internal purposes.
+	unsigned char flags;
+
+	/// End point side.
+	unsigned char side;
+
+	/// The id of the offmesh connection. (User assigned when the navigation mesh is built.)
+	unsigned int userId;
+};
+
+/// Provides high level information related to a dtMeshTile object.
+/// @ingroup detour
+struct dtMeshHeader
+{
+	int magic;				///< Tile magic number. (Used to identify the data format.)
+	int version;			///< Tile data format version number.
+	int x;					///< The x-position of the tile within the dtNavMesh tile grid. (x, y, layer)
+	int y;					///< The y-position of the tile within the dtNavMesh tile grid. (x, y, layer)
+	int layer;				///< The layer of the tile within the dtNavMesh tile grid. (x, y, layer)
+	unsigned int userId;	///< The user defined id of the tile.
+	int polyCount;			///< The number of polygons in the tile.
+	int vertCount;			///< The number of vertices in the tile.
+	int maxLinkCount;		///< The number of allocated links.
+	int detailMeshCount;	///< The number of sub-meshes in the detail mesh.
+	
+	/// The number of unique vertices in the detail mesh. (In addition to the polygon vertices.)
+	int detailVertCount;
+	
+	int detailTriCount;			///< The number of triangles in the detail mesh.
+	int bvNodeCount;			///< The number of bounding volume nodes. (Zero if bounding volumes are disabled.)
+	int offMeshConCount;		///< The number of off-mesh connections.
+	int offMeshBase;			///< The index of the first polygon which is an off-mesh connection.
+	float walkableHeight;		///< The height of the agents using the tile.
+	float walkableRadius;		///< The radius of the agents using the tile.
+	float walkableClimb;		///< The maximum climb height of the agents using the tile.
+	float bmin[3];				///< The minimum bounds of the tile's AABB. [(x, y, z)]
+	float bmax[3];				///< The maximum bounds of the tile's AABB. [(x, y, z)]
+	
+	/// The bounding volume quantization factor. 
+	float bvQuantFactor;
+};
+
+/// Defines a navigation mesh tile.
+/// @ingroup detour
+struct dtMeshTile
+{
+	unsigned int salt;					///< Counter describing modifications to the tile.
+
+	unsigned int linksFreeList;			///< Index to the next free link.
+	dtMeshHeader* header;				///< The tile header.
+	dtPoly* polys;						///< The tile polygons. [Size: dtMeshHeader::polyCount]
+	float* verts;						///< The tile vertices. [Size: dtMeshHeader::vertCount]
+	dtLink* links;						///< The tile links. [Size: dtMeshHeader::maxLinkCount]
+	dtPolyDetail* detailMeshes;			///< The tile's detail sub-meshes. [Size: dtMeshHeader::detailMeshCount]
+	
+	/// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount]
+	float* detailVerts;	
+
+	/// The detail mesh's triangles. [(vertA, vertB, vertC, triFlags) * dtMeshHeader::detailTriCount].
+	/// See dtDetailTriEdgeFlags and dtGetDetailTriEdgeFlags.
+	unsigned char* detailTris;	
+
+	/// The tile bounding volume nodes. [Size: dtMeshHeader::bvNodeCount]
+	/// (Will be null if bounding volumes are disabled.)
+	dtBVNode* bvTree;
+
+	dtOffMeshConnection* offMeshCons;		///< The tile off-mesh connections. [Size: dtMeshHeader::offMeshConCount]
+		
+	unsigned char* data;					///< The tile data. (Not directly accessed under normal situations.)
+	int dataSize;							///< Size of the tile data.
+	int flags;								///< Tile flags. (See: #dtTileFlags)
+	dtMeshTile* next;						///< The next free tile, or the next tile in the spatial grid.
+private:
+	dtMeshTile(const dtMeshTile&);
+	dtMeshTile& operator=(const dtMeshTile&);
+};
+
+/// Get flags for edge in detail triangle.
+/// @param	triFlags[in]		The flags for the triangle (last component of detail vertices above).
+/// @param	edgeIndex[in]		The index of the first vertex of the edge. For instance, if 0,
+///								returns flags for edge AB.
+inline int dtGetDetailTriEdgeFlags(unsigned char triFlags, int edgeIndex)
+{
+	return (triFlags >> (edgeIndex * 2)) & 0x3;
+}
+
+/// Configuration parameters used to define multi-tile navigation meshes.
+/// The values are used to allocate space during the initialization of a navigation mesh.
+/// @see dtNavMesh::init()
+/// @ingroup detour
+struct dtNavMeshParams
+{
+	float orig[3];					///< The world space origin of the navigation mesh's tile space. [(x, y, z)]
+	float tileWidth;				///< The width of each tile. (Along the x-axis.)
+	float tileHeight;				///< The height of each tile. (Along the z-axis.)
+	int maxTiles;					///< The maximum number of tiles the navigation mesh can contain.
+	int maxPolys;					///< The maximum number of polygons each tile can contain.
+};
+
+/// A navigation mesh based on tiles of convex polygons.
+/// @ingroup detour
+class dtNavMesh
+{
+public:
+	dtNavMesh();
+	~dtNavMesh();
+
+	/// @{
+	/// @name Initialization and Tile Management
+
+	/// Initializes the navigation mesh for tiled use.
+	///  @param[in]	params		Initialization parameters.
+	/// @return The status flags for the operation.
+	dtStatus init(const dtNavMeshParams* params);
+
+	/// Initializes the navigation mesh for single tile use.
+	///  @param[in]	data		Data of the new tile. (See: #dtCreateNavMeshData)
+	///  @param[in]	dataSize	The data size of the new tile.
+	///  @param[in]	flags		The tile flags. (See: #dtTileFlags)
+	/// @return The status flags for the operation.
+	///  @see dtCreateNavMeshData
+	dtStatus init(unsigned char* data, const int dataSize, const int flags);
+	
+	/// The navigation mesh initialization params.
+	const dtNavMeshParams* getParams() const;
+
+	/// Adds a tile to the navigation mesh.
+	///  @param[in]		data		Data for the new tile mesh. (See: #dtCreateNavMeshData)
+	///  @param[in]		dataSize	Data size of the new tile mesh.
+	///  @param[in]		flags		Tile flags. (See: #dtTileFlags)
+	///  @param[in]		lastRef		The desired reference for the tile. (When reloading a tile.) [opt] [Default: 0]
+	///  @param[out]	result		The tile reference. (If the tile was succesfully added.) [opt]
+	/// @return The status flags for the operation.
+	dtStatus addTile(unsigned char* data, int dataSize, int flags, dtTileRef lastRef, dtTileRef* result);
+	
+	/// Removes the specified tile from the navigation mesh.
+	///  @param[in]		ref			The reference of the tile to remove.
+	///  @param[out]	data		Data associated with deleted tile.
+	///  @param[out]	dataSize	Size of the data associated with deleted tile.
+	/// @return The status flags for the operation.
+	dtStatus removeTile(dtTileRef ref, unsigned char** data, int* dataSize);
+
+	/// @}
+
+	/// @{
+	/// @name Query Functions
+
+	/// Calculates the tile grid location for the specified world position.
+	///  @param[in]	pos  The world position for the query. [(x, y, z)]
+	///  @param[out]	tx		The tile's x-location. (x, y)
+	///  @param[out]	ty		The tile's y-location. (x, y)
+	void calcTileLoc(const float* pos, int* tx, int* ty) const;
+
+	/// Gets the tile at the specified grid location.
+	///  @param[in]	x		The tile's x-location. (x, y, layer)
+	///  @param[in]	y		The tile's y-location. (x, y, layer)
+	///  @param[in]	layer	The tile's layer. (x, y, layer)
+	/// @return The tile, or null if the tile does not exist.
+	const dtMeshTile* getTileAt(const int x, const int y, const int layer) const;
+
+	/// Gets all tiles at the specified grid location. (All layers.)
+	///  @param[in]		x			The tile's x-location. (x, y)
+	///  @param[in]		y			The tile's y-location. (x, y)
+	///  @param[out]	tiles		A pointer to an array of tiles that will hold the result.
+	///  @param[in]		maxTiles	The maximum tiles the tiles parameter can hold.
+	/// @return The number of tiles returned in the tiles array.
+	int getTilesAt(const int x, const int y,
+				   dtMeshTile const** tiles, const int maxTiles) const;
+	
+	/// Gets the tile reference for the tile at specified grid location.
+	///  @param[in]	x		The tile's x-location. (x, y, layer)
+	///  @param[in]	y		The tile's y-location. (x, y, layer)
+	///  @param[in]	layer	The tile's layer. (x, y, layer)
+	/// @return The tile reference of the tile, or 0 if there is none.
+	dtTileRef getTileRefAt(int x, int y, int layer) const;
+
+	/// Gets the tile reference for the specified tile.
+	///  @param[in]	tile	The tile.
+	/// @return The tile reference of the tile.
+	dtTileRef getTileRef(const dtMeshTile* tile) const;
+
+	/// Gets the tile for the specified tile reference.
+	///  @param[in]	ref		The tile reference of the tile to retrieve.
+	/// @return The tile for the specified reference, or null if the 
+	///		reference is invalid.
+	const dtMeshTile* getTileByRef(dtTileRef ref) const;
+	
+	/// The maximum number of tiles supported by the navigation mesh.
+	/// @return The maximum number of tiles supported by the navigation mesh.
+	int getMaxTiles() const;
+	
+	/// Gets the tile at the specified index.
+	///  @param[in]	i		The tile index. [Limit: 0 >= index < #getMaxTiles()]
+	/// @return The tile at the specified index.
+	const dtMeshTile* getTile(int i) const;
+
+	/// Gets the tile and polygon for the specified polygon reference.
+	///  @param[in]		ref		The reference for the a polygon.
+	///  @param[out]	tile	The tile containing the polygon.
+	///  @param[out]	poly	The polygon.
+	/// @return The status flags for the operation.
+	dtStatus getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const;
+	
+	/// Returns the tile and polygon for the specified polygon reference.
+	///  @param[in]		ref		A known valid reference for a polygon.
+	///  @param[out]	tile	The tile containing the polygon.
+	///  @param[out]	poly	The polygon.
+	void getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const;
+
+	/// Checks the validity of a polygon reference.
+	///  @param[in]	ref		The polygon reference to check.
+	/// @return True if polygon reference is valid for the navigation mesh.
+	bool isValidPolyRef(dtPolyRef ref) const;
+	
+	/// Gets the polygon reference for the tile's base polygon.
+	///  @param[in]	tile		The tile.
+	/// @return The polygon reference for the base polygon in the specified tile.
+	dtPolyRef getPolyRefBase(const dtMeshTile* tile) const;
+	
+	/// Gets the endpoints for an off-mesh connection, ordered by "direction of travel".
+	///  @param[in]		prevRef		The reference of the polygon before the connection.
+	///  @param[in]		polyRef		The reference of the off-mesh connection polygon.
+	///  @param[out]	startPos	The start position of the off-mesh connection. [(x, y, z)]
+	///  @param[out]	endPos		The end position of the off-mesh connection. [(x, y, z)]
+	/// @return The status flags for the operation.
+	dtStatus getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const;
+
+	/// Gets the specified off-mesh connection.
+	///  @param[in]	ref		The polygon reference of the off-mesh connection.
+	/// @return The specified off-mesh connection, or null if the polygon reference is not valid.
+	const dtOffMeshConnection* getOffMeshConnectionByRef(dtPolyRef ref) const;
+	
+	/// @}
+
+	/// @{
+	/// @name State Management
+	/// These functions do not effect #dtTileRef or #dtPolyRef's. 
+
+	/// Sets the user defined flags for the specified polygon.
+	///  @param[in]	ref		The polygon reference.
+	///  @param[in]	flags	The new flags for the polygon.
+	/// @return The status flags for the operation.
+	dtStatus setPolyFlags(dtPolyRef ref, unsigned short flags);
+
+	/// Gets the user defined flags for the specified polygon.
+	///  @param[in]		ref				The polygon reference.
+	///  @param[out]	resultFlags		The polygon flags.
+	/// @return The status flags for the operation.
+	dtStatus getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const;
+
+	/// Sets the user defined area for the specified polygon.
+	///  @param[in]	ref		The polygon reference.
+	///  @param[in]	area	The new area id for the polygon. [Limit: < #DT_MAX_AREAS]
+	/// @return The status flags for the operation.
+	dtStatus setPolyArea(dtPolyRef ref, unsigned char area);
+
+	/// Gets the user defined area for the specified polygon.
+	///  @param[in]		ref			The polygon reference.
+	///  @param[out]	resultArea	The area id for the polygon.
+	/// @return The status flags for the operation.
+	dtStatus getPolyArea(dtPolyRef ref, unsigned char* resultArea) const;
+
+	/// Gets the size of the buffer required by #storeTileState to store the specified tile's state.
+	///  @param[in]	tile	The tile.
+	/// @return The size of the buffer required to store the state.
+	int getTileStateSize(const dtMeshTile* tile) const;
+	
+	/// Stores the non-structural state of the tile in the specified buffer. (Flags, area ids, etc.)
+	///  @param[in]		tile			The tile.
+	///  @param[out]	data			The buffer to store the tile's state in.
+	///  @param[in]		maxDataSize		The size of the data buffer. [Limit: >= #getTileStateSize]
+	/// @return The status flags for the operation.
+	dtStatus storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const;
+	
+	/// Restores the state of the tile.
+	///  @param[in]	tile			The tile.
+	///  @param[in]	data			The new state. (Obtained from #storeTileState.)
+	///  @param[in]	maxDataSize		The size of the state within the data buffer.
+	/// @return The status flags for the operation.
+	dtStatus restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize);
+	
+	/// @}
+
+	/// @{
+	/// @name Encoding and Decoding
+	/// These functions are generally meant for internal use only.
+
+	/// Derives a standard polygon reference.
+	///  @note This function is generally meant for internal use only.
+	///  @param[in]	salt	The tile's salt value.
+	///  @param[in]	it		The index of the tile.
+	///  @param[in]	ip		The index of the polygon within the tile.
+	inline dtPolyRef encodePolyId(unsigned int salt, unsigned int it, unsigned int ip) const
+	{
+#ifdef DT_POLYREF64
+		return ((dtPolyRef)salt << (DT_POLY_BITS+DT_TILE_BITS)) | ((dtPolyRef)it << DT_POLY_BITS) | (dtPolyRef)ip;
+#else
+		return ((dtPolyRef)salt << (m_polyBits+m_tileBits)) | ((dtPolyRef)it << m_polyBits) | (dtPolyRef)ip;
+#endif
+	}
+	
+	/// Decodes a standard polygon reference.
+	///  @note This function is generally meant for internal use only.
+	///  @param[in]	ref   The polygon reference to decode.
+	///  @param[out]	salt	The tile's salt value.
+	///  @param[out]	it		The index of the tile.
+	///  @param[out]	ip		The index of the polygon within the tile.
+	///  @see #encodePolyId
+	inline void decodePolyId(dtPolyRef ref, unsigned int& salt, unsigned int& it, unsigned int& ip) const
+	{
+#ifdef DT_POLYREF64
+		const dtPolyRef saltMask = ((dtPolyRef)1<<DT_SALT_BITS)-1;
+		const dtPolyRef tileMask = ((dtPolyRef)1<<DT_TILE_BITS)-1;
+		const dtPolyRef polyMask = ((dtPolyRef)1<<DT_POLY_BITS)-1;
+		salt = (unsigned int)((ref >> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask);
+		it = (unsigned int)((ref >> DT_POLY_BITS) & tileMask);
+		ip = (unsigned int)(ref & polyMask);
+#else
+		const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1;
+		const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1;
+		const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1;
+		salt = (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask);
+		it = (unsigned int)((ref >> m_polyBits) & tileMask);
+		ip = (unsigned int)(ref & polyMask);
+#endif
+	}
+
+	/// Extracts a tile's salt value from the specified polygon reference.
+	///  @note This function is generally meant for internal use only.
+	///  @param[in]	ref		The polygon reference.
+	///  @see #encodePolyId
+	inline unsigned int decodePolyIdSalt(dtPolyRef ref) const
+	{
+#ifdef DT_POLYREF64
+		const dtPolyRef saltMask = ((dtPolyRef)1<<DT_SALT_BITS)-1;
+		return (unsigned int)((ref >> (DT_POLY_BITS+DT_TILE_BITS)) & saltMask);
+#else
+		const dtPolyRef saltMask = ((dtPolyRef)1<<m_saltBits)-1;
+		return (unsigned int)((ref >> (m_polyBits+m_tileBits)) & saltMask);
+#endif
+	}
+	
+	/// Extracts the tile's index from the specified polygon reference.
+	///  @note This function is generally meant for internal use only.
+	///  @param[in]	ref		The polygon reference.
+	///  @see #encodePolyId
+	inline unsigned int decodePolyIdTile(dtPolyRef ref) const
+	{
+#ifdef DT_POLYREF64
+		const dtPolyRef tileMask = ((dtPolyRef)1<<DT_TILE_BITS)-1;
+		return (unsigned int)((ref >> DT_POLY_BITS) & tileMask);
+#else
+		const dtPolyRef tileMask = ((dtPolyRef)1<<m_tileBits)-1;
+		return (unsigned int)((ref >> m_polyBits) & tileMask);
+#endif
+	}
+	
+	/// Extracts the polygon's index (within its tile) from the specified polygon reference.
+	///  @note This function is generally meant for internal use only.
+	///  @param[in]	ref		The polygon reference.
+	///  @see #encodePolyId
+	inline unsigned int decodePolyIdPoly(dtPolyRef ref) const
+	{
+#ifdef DT_POLYREF64
+		const dtPolyRef polyMask = ((dtPolyRef)1<<DT_POLY_BITS)-1;
+		return (unsigned int)(ref & polyMask);
+#else
+		const dtPolyRef polyMask = ((dtPolyRef)1<<m_polyBits)-1;
+		return (unsigned int)(ref & polyMask);
+#endif
+	}
+
+	/// @}
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtNavMesh(const dtNavMesh&);
+	dtNavMesh& operator=(const dtNavMesh&);
+
+	/// Returns pointer to tile in the tile array.
+	dtMeshTile* getTile(int i);
+
+	/// Returns neighbour tile based on side.
+	int getTilesAt(const int x, const int y,
+				   dtMeshTile** tiles, const int maxTiles) const;
+
+	/// Returns neighbour tile based on side.
+	int getNeighbourTilesAt(const int x, const int y, const int side,
+							dtMeshTile** tiles, const int maxTiles) const;
+	
+	/// Returns all polygons in neighbour tile based on portal defined by the segment.
+	int findConnectingPolys(const float* va, const float* vb,
+							const dtMeshTile* tile, int side,
+							dtPolyRef* con, float* conarea, int maxcon) const;
+	
+	/// Builds internal polygons links for a tile.
+	void connectIntLinks(dtMeshTile* tile);
+	/// Builds internal polygons links for a tile.
+	void baseOffMeshLinks(dtMeshTile* tile);
+
+	/// Builds external polygon links for a tile.
+	void connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side);
+	/// Builds external polygon links for a tile.
+	void connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side);
+	
+	/// Removes external links at specified side.
+	void unconnectLinks(dtMeshTile* tile, dtMeshTile* target);
+	
+
+	// TODO: These methods are duplicates from dtNavMeshQuery, but are needed for off-mesh connection finding.
+	
+	/// Queries polygons within a tile.
+	int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
+							dtPolyRef* polys, const int maxPolys) const;
+	/// Find nearest polygon within a tile.
+	dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center,
+									const float* halfExtents, float* nearestPt) const;
+	/// Returns whether position is over the poly and the height at the position if so.
+	bool getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const;
+	/// Returns closest point on polygon.
+	void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
+	
+	dtNavMeshParams m_params;			///< Current initialization params. TODO: do not store this info twice.
+	float m_orig[3];					///< Origin of the tile (0,0)
+	float m_tileWidth, m_tileHeight;	///< Dimensions of each tile.
+	int m_maxTiles;						///< Max number of tiles.
+	int m_tileLutSize;					///< Tile hash lookup size (must be pot).
+	int m_tileLutMask;					///< Tile hash lookup mask.
+
+	dtMeshTile** m_posLookup;			///< Tile hash lookup.
+	dtMeshTile* m_nextFree;				///< Freelist of tiles.
+	dtMeshTile* m_tiles;				///< List of tiles.
+		
+#ifndef DT_POLYREF64
+	unsigned int m_saltBits;			///< Number of salt bits in the tile ID.
+	unsigned int m_tileBits;			///< Number of tile bits in the tile ID.
+	unsigned int m_polyBits;			///< Number of poly bits in the tile ID.
+#endif
+
+	friend class dtNavMeshQuery;
+};
+
+/// Allocates a navigation mesh object using the Detour allocator.
+/// @return A navigation mesh that is ready for initialization, or null on failure.
+///  @ingroup detour
+dtNavMesh* dtAllocNavMesh();
+
+/// Frees the specified navigation mesh object using the Detour allocator.
+///  @param[in]	navmesh		A navigation mesh allocated using #dtAllocNavMesh
+///  @ingroup detour
+void dtFreeNavMesh(dtNavMesh* navmesh);
+
+#endif // DETOURNAVMESH_H
+
+///////////////////////////////////////////////////////////////////////////
+
+// This section contains detailed documentation for members that don't have
+// a source file. It reduces clutter in the main section of the header.
+
+/**
+
+@typedef dtPolyRef
+@par
+
+Polygon references are subject to the same invalidate/preserve/restore 
+rules that apply to #dtTileRef's.  If the #dtTileRef for the polygon's
+tile changes, the polygon reference becomes invalid.
+
+Changing a polygon's flags, area id, etc. does not impact its polygon
+reference.
+
+@typedef dtTileRef
+@par
+
+The following changes will invalidate a tile reference:
+
+- The referenced tile has been removed from the navigation mesh.
+- The navigation mesh has been initialized using a different set
+  of #dtNavMeshParams.
+
+A tile reference is preserved/restored if the tile is added to a navigation 
+mesh initialized with the original #dtNavMeshParams and is added at the
+original reference location. (E.g. The lastRef parameter is used with
+dtNavMesh::addTile.)
+
+Basically, if the storage structure of a tile changes, its associated
+tile reference changes.
+
+
+@var unsigned short dtPoly::neis[DT_VERTS_PER_POLYGON]
+@par
+
+Each entry represents data for the edge starting at the vertex of the same index. 
+E.g. The entry at index n represents the edge data for vertex[n] to vertex[n+1].
+
+A value of zero indicates the edge has no polygon connection. (It makes up the 
+border of the navigation mesh.)
+
+The information can be extracted as follows: 
+@code 
+neighborRef = neis[n] & 0xff; // Get the neighbor polygon reference.
+
+if (neis[n] & #DT_EX_LINK)
+{
+    // The edge is an external (portal) edge.
+}
+@endcode
+
+@var float dtMeshHeader::bvQuantFactor
+@par
+
+This value is used for converting between world and bounding volume coordinates.
+For example:
+@code
+const float cs = 1.0f / tile->header->bvQuantFactor;
+const dtBVNode* n = &tile->bvTree[i];
+if (n->i >= 0)
+{
+    // This is a leaf node.
+    float worldMinX = tile->header->bmin[0] + n->bmin[0]*cs;
+    float worldMinY = tile->header->bmin[0] + n->bmin[1]*cs;
+    // Etc...
+}
+@endcode
+
+@struct dtMeshTile
+@par
+
+Tiles generally only exist within the context of a dtNavMesh object.
+
+Some tile content is optional.  For example, a tile may not contain any
+off-mesh connections.  In this case the associated pointer will be null.
+
+If a detail mesh exists it will share vertices with the base polygon mesh.  
+Only the vertices unique to the detail mesh will be stored in #detailVerts.
+
+@warning Tiles returned by a dtNavMesh object are not guarenteed to be populated.
+For example: The tile at a location might not have been loaded yet, or may have been removed.
+In this case, pointers will be null.  So if in doubt, check the polygon count in the 
+tile's header to determine if a tile has polygons defined.
+
+@var float dtOffMeshConnection::pos[6]
+@par
+
+For a properly built navigation mesh, vertex A will always be within the bounds of the mesh. 
+Vertex B is not required to be within the bounds of the mesh.
+
+*/

panda/src/recastdetour/DetourNavMeshBuilder.cpp

@@ -0,0 +1,802 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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;
+}

panda/src/recastdetour/DetourNavMeshBuilder.h

@@ -0,0 +1,149 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURNAVMESHBUILDER_H
+#define DETOURNAVMESHBUILDER_H
+
+#include "DetourAlloc.h"
+
+/// Represents the source data used to build an navigation mesh tile.
+/// @ingroup detour
+struct dtNavMeshCreateParams
+{
+
+	/// @name Polygon Mesh Attributes
+	/// Used to create the base navigation graph.
+	/// See #rcPolyMesh for details related to these attributes.
+	/// @{
+
+	const unsigned short* verts;			///< The polygon mesh vertices. [(x, y, z) * #vertCount] [Unit: vx]
+	int vertCount;							///< The number vertices in the polygon mesh. [Limit: >= 3]
+	const unsigned short* polys;			///< The polygon data. [Size: #polyCount * 2 * #nvp]
+	const unsigned short* polyFlags;		///< The user defined flags assigned to each polygon. [Size: #polyCount]
+	const unsigned char* polyAreas;			///< The user defined area ids assigned to each polygon. [Size: #polyCount]
+	int polyCount;							///< Number of polygons in the mesh. [Limit: >= 1]
+	int nvp;								///< Number maximum number of vertices per polygon. [Limit: >= 3]
+
+	/// @}
+	/// @name Height Detail Attributes (Optional)
+	/// See #rcPolyMeshDetail for details related to these attributes.
+	/// @{
+
+	const unsigned int* detailMeshes;		///< The height detail sub-mesh data. [Size: 4 * #polyCount]
+	const float* detailVerts;				///< The detail mesh vertices. [Size: 3 * #detailVertsCount] [Unit: wu]
+	int detailVertsCount;					///< The number of vertices in the detail mesh.
+	const unsigned char* detailTris;		///< The detail mesh triangles. [Size: 4 * #detailTriCount]
+	int detailTriCount;						///< The number of triangles in the detail mesh.
+
+	/// @}
+	/// @name Off-Mesh Connections Attributes (Optional)
+	/// Used to define a custom point-to-point edge within the navigation graph, an 
+	/// off-mesh connection is a user defined traversable connection made up to two vertices, 
+	/// at least one of which resides within a navigation mesh polygon.
+	/// @{
+
+	/// Off-mesh connection vertices. [(ax, ay, az, bx, by, bz) * #offMeshConCount] [Unit: wu]
+	const float* offMeshConVerts;
+	/// Off-mesh connection radii. [Size: #offMeshConCount] [Unit: wu]
+	const float* offMeshConRad;
+	/// User defined flags assigned to the off-mesh connections. [Size: #offMeshConCount]
+	const unsigned short* offMeshConFlags;
+	/// User defined area ids assigned to the off-mesh connections. [Size: #offMeshConCount]
+	const unsigned char* offMeshConAreas;
+	/// The permitted travel direction of the off-mesh connections. [Size: #offMeshConCount]
+	///
+	/// 0 = Travel only from endpoint A to endpoint B.<br/>
+	/// #DT_OFFMESH_CON_BIDIR = Bidirectional travel.
+	const unsigned char* offMeshConDir;	
+	/// The user defined ids of the off-mesh connection. [Size: #offMeshConCount]
+	const unsigned int* offMeshConUserID;
+	/// The number of off-mesh connections. [Limit: >= 0]
+	int offMeshConCount;
+
+	/// @}
+	/// @name Tile Attributes
+	/// @note The tile grid/layer data can be left at zero if the destination is a single tile mesh.
+	/// @{
+
+	unsigned int userId;	///< The user defined id of the tile.
+	int tileX;				///< The tile's x-grid location within the multi-tile destination mesh. (Along the x-axis.)
+	int tileY;				///< The tile's y-grid location within the multi-tile desitation mesh. (Along the z-axis.)
+	int tileLayer;			///< The tile's layer within the layered destination mesh. [Limit: >= 0] (Along the y-axis.)
+	float bmin[3];			///< The minimum bounds of the tile. [(x, y, z)] [Unit: wu]
+	float bmax[3];			///< The maximum bounds of the tile. [(x, y, z)] [Unit: wu]
+
+	/// @}
+	/// @name General Configuration Attributes
+	/// @{
+
+	float walkableHeight;	///< The agent height. [Unit: wu]
+	float walkableRadius;	///< The agent radius. [Unit: wu]
+	float walkableClimb;	///< The agent maximum traversable ledge. (Up/Down) [Unit: wu]
+	float cs;				///< The xz-plane cell size of the polygon mesh. [Limit: > 0] [Unit: wu]
+	float ch;				///< The y-axis cell height of the polygon mesh. [Limit: > 0] [Unit: wu]
+
+	/// True if a bounding volume tree should be built for the tile.
+	/// @note The BVTree is not normally needed for layered navigation meshes.
+	bool buildBvTree;
+
+	/// @}
+};
+
+/// Builds navigation mesh tile data from the provided tile creation data.
+/// @ingroup detour
+///  @param[in]		params		Tile creation data.
+///  @param[out]	outData		The resulting tile data.
+///  @param[out]	outDataSize	The size of the tile data array.
+/// @return True if the tile data was successfully created.
+bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize);
+
+/// Swaps the endianess of the tile data's header (#dtMeshHeader).
+///  @param[in,out]	data		The tile data array.
+///  @param[in]		dataSize	The size of the data array.
+bool dtNavMeshHeaderSwapEndian(unsigned char* data, const int dataSize);
+
+/// Swaps endianess of the tile data.
+///  @param[in,out]	data		The tile data array.
+///  @param[in]		dataSize	The size of the data array.
+bool dtNavMeshDataSwapEndian(unsigned char* data, const int dataSize);
+
+#endif // DETOURNAVMESHBUILDER_H
+
+// This section contains detailed documentation for members that don't have
+// a source file. It reduces clutter in the main section of the header.
+
+/**
+
+@struct dtNavMeshCreateParams
+@par
+
+This structure is used to marshal data between the Recast mesh generation pipeline and Detour navigation components.
+
+See the rcPolyMesh and rcPolyMeshDetail documentation for detailed information related to mesh structure.
+
+Units are usually in voxels (vx) or world units (wu). The units for voxels, grid size, and cell size 
+are all based on the values of #cs and #ch.
+
+The standard navigation mesh build process is to create tile data using dtCreateNavMeshData, then add the tile 
+to a navigation mesh using either the dtNavMesh single tile <tt>init()</tt> function or the dtNavMesh::addTile()
+function.
+
+@see dtCreateNavMeshData
+
+*/
+

panda/src/recastdetour/DetourNavMeshQuery.cpp

@@ -0,0 +1,3663 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#include <string.h>
+#include "DetourNavMeshQuery.h"
+#include "DetourNavMesh.h"
+#include "DetourNode.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+#include <new>
+
+/// @class dtQueryFilter
+///
+/// <b>The Default Implementation</b>
+/// 
+/// At construction: All area costs default to 1.0.  All flags are included
+/// and none are excluded.
+/// 
+/// If a polygon has both an include and an exclude flag, it will be excluded.
+/// 
+/// The way filtering works, a navigation mesh polygon must have at least one flag 
+/// set to ever be considered by a query. So a polygon with no flags will never
+/// be considered.
+///
+/// Setting the include flags to 0 will result in all polygons being excluded.
+///
+/// <b>Custom Implementations</b>
+/// 
+/// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class.
+/// 
+/// Implement a custom query filter by overriding the virtual passFilter() 
+/// and getCost() functions. If this is done, both functions should be as 
+/// fast as possible. Use cached local copies of data rather than accessing 
+/// your own objects where possible.
+/// 
+/// Custom implementations do not need to adhere to the flags or cost logic 
+/// used by the default implementation.  
+/// 
+/// In order for A* searches to work properly, the cost should be proportional to
+/// the travel distance. Implementing a cost modifier less than 1.0 is likely 
+/// to lead to problems during pathfinding.
+///
+/// @see dtNavMeshQuery
+
+dtQueryFilter::dtQueryFilter() :
+	m_includeFlags(0xffff),
+	m_excludeFlags(0)
+{
+	for (int i = 0; i < DT_MAX_AREAS; ++i)
+		m_areaCost[i] = 1.0f;
+}
+
+#ifdef DT_VIRTUAL_QUERYFILTER
+bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
+							   const dtMeshTile* /*tile*/,
+							   const dtPoly* poly) const
+{
+	return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
+}
+
+float dtQueryFilter::getCost(const float* pa, const float* pb,
+							 const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
+							 const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
+							 const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
+{
+	return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
+}
+#else
+inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
+									  const dtMeshTile* /*tile*/,
+									  const dtPoly* poly) const
+{
+	return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
+}
+
+inline float dtQueryFilter::getCost(const float* pa, const float* pb,
+									const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
+									const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
+									const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
+{
+	return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
+}
+#endif	
+	
+static const float H_SCALE = 0.999f; // Search heuristic scale.
+
+
+dtNavMeshQuery* dtAllocNavMeshQuery()
+{
+	void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtNavMeshQuery;
+}
+
+void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh)
+{
+	if (!navmesh) return;
+	navmesh->~dtNavMeshQuery();
+	dtFree(navmesh);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+
+/// @class dtNavMeshQuery
+///
+/// For methods that support undersized buffers, if the buffer is too small 
+/// to hold the entire result set the return status of the method will include 
+/// the #DT_BUFFER_TOO_SMALL flag.
+///
+/// Constant member functions can be used by multiple clients without side
+/// effects. (E.g. No change to the closed list. No impact on an in-progress
+/// sliced path query. Etc.)
+/// 
+/// Walls and portals: A @e wall is a polygon segment that is 
+/// considered impassable. A @e portal is a passable segment between polygons.
+/// A portal may be treated as a wall based on the dtQueryFilter used for a query.
+///
+/// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery()
+
+dtNavMeshQuery::dtNavMeshQuery() :
+	m_nav(0),
+	m_tinyNodePool(0),
+	m_nodePool(0),
+	m_openList(0)
+{
+	memset(&m_query, 0, sizeof(dtQueryData));
+}
+
+dtNavMeshQuery::~dtNavMeshQuery()
+{
+	if (m_tinyNodePool)
+		m_tinyNodePool->~dtNodePool();
+	if (m_nodePool)
+		m_nodePool->~dtNodePool();
+	if (m_openList)
+		m_openList->~dtNodeQueue();
+	dtFree(m_tinyNodePool);
+	dtFree(m_nodePool);
+	dtFree(m_openList);
+}
+
+/// @par 
+///
+/// Must be the first function called after construction, before other
+/// functions are used.
+///
+/// This function can be used multiple times.
+dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
+{
+	if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	m_nav = nav;
+	
+	if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
+	{
+		if (m_nodePool)
+		{
+			m_nodePool->~dtNodePool();
+			dtFree(m_nodePool);
+			m_nodePool = 0;
+		}
+		m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4));
+		if (!m_nodePool)
+			return DT_FAILURE | DT_OUT_OF_MEMORY;
+	}
+	else
+	{
+		m_nodePool->clear();
+	}
+	
+	if (!m_tinyNodePool)
+	{
+		m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32);
+		if (!m_tinyNodePool)
+			return DT_FAILURE | DT_OUT_OF_MEMORY;
+	}
+	else
+	{
+		m_tinyNodePool->clear();
+	}
+	
+	if (!m_openList || m_openList->getCapacity() < maxNodes)
+	{
+		if (m_openList)
+		{
+			m_openList->~dtNodeQueue();
+			dtFree(m_openList);
+			m_openList = 0;
+		}
+		m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes);
+		if (!m_openList)
+			return DT_FAILURE | DT_OUT_OF_MEMORY;
+	}
+	else
+	{
+		m_openList->clear();
+	}
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
+										 dtPolyRef* randomRef, float* randomPt) const
+{
+	dtAssert(m_nav);
+
+	if (!filter || !frand || !randomRef || !randomPt)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	// Randomly pick one tile. Assume that all tiles cover roughly the same area.
+	const dtMeshTile* tile = 0;
+	float tsum = 0.0f;
+	for (int i = 0; i < m_nav->getMaxTiles(); i++)
+	{
+		const dtMeshTile* t = m_nav->getTile(i);
+		if (!t || !t->header) continue;
+		
+		// Choose random tile using reservoi sampling.
+		const float area = 1.0f; // Could be tile area too.
+		tsum += area;
+		const float u = frand();
+		if (u*tsum <= area)
+			tile = t;
+	}
+	if (!tile)
+		return DT_FAILURE;
+
+	// Randomly pick one polygon weighted by polygon area.
+	const dtPoly* poly = 0;
+	dtPolyRef polyRef = 0;
+	const dtPolyRef base = m_nav->getPolyRefBase(tile);
+
+	float areaSum = 0.0f;
+	for (int i = 0; i < tile->header->polyCount; ++i)
+	{
+		const dtPoly* p = &tile->polys[i];
+		// Do not return off-mesh connection polygons.
+		if (p->getType() != DT_POLYTYPE_GROUND)
+			continue;
+		// Must pass filter
+		const dtPolyRef ref = base | (dtPolyRef)i;
+		if (!filter->passFilter(ref, tile, p))
+			continue;
+
+		// Calc area of the polygon.
+		float polyArea = 0.0f;
+		for (int j = 2; j < p->vertCount; ++j)
+		{
+			const float* va = &tile->verts[p->verts[0]*3];
+			const float* vb = &tile->verts[p->verts[j-1]*3];
+			const float* vc = &tile->verts[p->verts[j]*3];
+			polyArea += dtTriArea2D(va,vb,vc);
+		}
+
+		// Choose random polygon weighted by area, using reservoi sampling.
+		areaSum += polyArea;
+		const float u = frand();
+		if (u*areaSum <= polyArea)
+		{
+			poly = p;
+			polyRef = ref;
+		}
+	}
+	
+	if (!poly)
+		return DT_FAILURE;
+
+	// Randomly pick point on polygon.
+	const float* v = &tile->verts[poly->verts[0]*3];
+	float verts[3*DT_VERTS_PER_POLYGON];
+	float areas[DT_VERTS_PER_POLYGON];
+	dtVcopy(&verts[0*3],v);
+	for (int j = 1; j < poly->vertCount; ++j)
+	{
+		v = &tile->verts[poly->verts[j]*3];
+		dtVcopy(&verts[j*3],v);
+	}
+	
+	const float s = frand();
+	const float t = frand();
+	
+	float pt[3];
+	dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
+	
+	float h = 0.0f;
+	dtStatus status = getPolyHeight(polyRef, pt, &h);
+	if (dtStatusFailed(status))
+		return status;
+	pt[1] = h;
+	
+	dtVcopy(randomPt, pt);
+	*randomRef = polyRef;
+
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius,
+													 const dtQueryFilter* filter, float (*frand)(),
+													 dtPolyRef* randomRef, float* randomPt) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+	
+	// Validate input
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!centerPos || !dtVisfinite(centerPos) ||
+		maxRadius < 0 || !dtMathIsfinite(maxRadius) ||
+		!filter || !frand || !randomRef || !randomPt)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	const dtMeshTile* startTile = 0;
+	const dtPoly* startPoly = 0;
+	m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
+	if (!filter->passFilter(startRef, startTile, startPoly))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, centerPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	dtStatus status = DT_SUCCESS;
+	
+	const float radiusSqr = dtSqr(maxRadius);
+	float areaSum = 0.0f;
+
+	const dtMeshTile* randomTile = 0;
+	const dtPoly* randomPoly = 0;
+	dtPolyRef randomPolyRef = 0;
+
+	while (!m_openList->empty())
+	{
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+
+		// Place random locations on on ground.
+		if (bestPoly->getType() == DT_POLYTYPE_GROUND)
+		{
+			// Calc area of the polygon.
+			float polyArea = 0.0f;
+			for (int j = 2; j < bestPoly->vertCount; ++j)
+			{
+				const float* va = &bestTile->verts[bestPoly->verts[0]*3];
+				const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3];
+				const float* vc = &bestTile->verts[bestPoly->verts[j]*3];
+				polyArea += dtTriArea2D(va,vb,vc);
+			}
+			// Choose random polygon weighted by area, using reservoi sampling.
+			areaSum += polyArea;
+			const float u = frand();
+			if (u*areaSum <= polyArea)
+			{
+				randomTile = bestTile;
+				randomPoly = bestPoly;
+				randomPolyRef = bestRef;
+			}
+		}
+		
+		
+		// Get parent poly and tile.
+		dtPolyRef parentRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		if (bestNode->pidx)
+			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		if (parentRef)
+			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			const dtLink* link = &bestTile->links[i];
+			dtPolyRef neighbourRef = link->ref;
+			// Skip invalid neighbours and do not follow back to parent.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Expand to neighbour
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+			
+			// Do not advance if the polygon is excluded by the filter.
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+			
+			// Find edge and calc distance to the edge.
+			float va[3], vb[3];
+			if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
+				continue;
+			
+			// If the circle is not touching the next polygon, skip it.
+			float tseg;
+			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
+			if (distSqr > radiusSqr)
+				continue;
+			
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
+			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
+				continue;
+			}
+			
+			if (neighbourNode->flags & DT_NODE_CLOSED)
+				continue;
+			
+			// Cost
+			if (neighbourNode->flags == 0)
+				dtVlerp(neighbourNode->pos, va, vb, 0.5f);
+			
+			const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			
+			neighbourNode->id = neighbourRef;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->total = total;
+			
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				neighbourNode->flags = DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+		}
+	}
+	
+	if (!randomPoly)
+		return DT_FAILURE;
+	
+	// Randomly pick point on polygon.
+	const float* v = &randomTile->verts[randomPoly->verts[0]*3];
+	float verts[3*DT_VERTS_PER_POLYGON];
+	float areas[DT_VERTS_PER_POLYGON];
+	dtVcopy(&verts[0*3],v);
+	for (int j = 1; j < randomPoly->vertCount; ++j)
+	{
+		v = &randomTile->verts[randomPoly->verts[j]*3];
+		dtVcopy(&verts[j*3],v);
+	}
+	
+	const float s = frand();
+	const float t = frand();
+	
+	float pt[3];
+	dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
+	
+	float h = 0.0f;
+	dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
+	if (dtStatusFailed(status))
+		return stat;
+	pt[1] = h;
+	
+	dtVcopy(randomPt, pt);
+	*randomRef = randomPolyRef;
+	
+	return DT_SUCCESS;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+
+/// @par
+///
+/// Uses the detail polygons to find the surface height. (Most accurate.)
+///
+/// @p pos does not have to be within the bounds of the polygon or navigation mesh.
+///
+/// See closestPointOnPolyBoundary() for a limited but faster option.
+///
+dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
+{
+	dtAssert(m_nav);
+	if (!m_nav->isValidPolyRef(ref) ||
+		!pos || !dtVisfinite(pos) ||
+		!closest)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+
+	m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly);
+	return DT_SUCCESS;
+}
+
+/// @par
+///
+/// Much faster than closestPointOnPoly().
+///
+/// If the provided position lies within the polygon's xz-bounds (above or below), 
+/// then @p pos and @p closest will be equal.
+///
+/// The height of @p closest will be the polygon boundary.  The height detail is not used.
+/// 
+/// @p pos does not have to be within the bounds of the polybon or the navigation mesh.
+/// 
+dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const
+{
+	dtAssert(m_nav);
+	
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	if (!pos || !dtVisfinite(pos) || !closest)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Collect vertices.
+	float verts[DT_VERTS_PER_POLYGON*3];	
+	float edged[DT_VERTS_PER_POLYGON];
+	float edget[DT_VERTS_PER_POLYGON];
+	int nv = 0;
+	for (int i = 0; i < (int)poly->vertCount; ++i)
+	{
+		dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
+		nv++;
+	}		
+	
+	bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
+	if (inside)
+	{
+		// Point is inside the polygon, return the point.
+		dtVcopy(closest, pos);
+	}
+	else
+	{
+		// Point is outside the polygon, dtClamp to nearest edge.
+		float dmin = edged[0];
+		int imin = 0;
+		for (int i = 1; i < nv; ++i)
+		{
+			if (edged[i] < dmin)
+			{
+				dmin = edged[i];
+				imin = i;
+			}
+		}
+		const float* va = &verts[imin*3];
+		const float* vb = &verts[((imin+1)%nv)*3];
+		dtVlerp(closest, va, vb, edget[imin]);
+	}
+	
+	return DT_SUCCESS;
+}
+
+/// @par
+///
+/// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds 
+/// of the polygon.
+/// 
+dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
+{
+	dtAssert(m_nav);
+
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	if (!pos || !dtVisfinite2D(pos))
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	// We used to return success for offmesh connections, but the
+	// getPolyHeight in DetourNavMesh does not do this, so special
+	// case it here.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		const float* v0 = &tile->verts[poly->verts[0]*3];
+		const float* v1 = &tile->verts[poly->verts[1]*3];
+		float t;
+		dtDistancePtSegSqr2D(pos, v0, v1, t);
+		if (height)
+			*height = v0[1] + (v1[1] - v0[1])*t;
+
+		return DT_SUCCESS;
+	}
+
+	return m_nav->getPolyHeight(tile, poly, pos, height)
+		? DT_SUCCESS
+		: DT_FAILURE | DT_INVALID_PARAM;
+}
+
+class dtFindNearestPolyQuery : public dtPolyQuery
+{
+	const dtNavMeshQuery* m_query;
+	const float* m_center;
+	float m_nearestDistanceSqr;
+	dtPolyRef m_nearestRef;
+	float m_nearestPoint[3];
+
+public:
+	dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center)
+		: m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint()
+	{
+	}
+
+	dtPolyRef nearestRef() const { return m_nearestRef; }
+	const float* nearestPoint() const { return m_nearestPoint; }
+
+	void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count)
+	{
+		dtIgnoreUnused(polys);
+
+		for (int i = 0; i < count; ++i)
+		{
+			dtPolyRef ref = refs[i];
+			float closestPtPoly[3];
+			float diff[3];
+			bool posOverPoly = false;
+			float d;
+			m_query->closestPointOnPoly(ref, m_center, closestPtPoly, &posOverPoly);
+
+			// If a point is directly over a polygon and closer than
+			// climb height, favor that instead of straight line nearest point.
+			dtVsub(diff, m_center, closestPtPoly);
+			if (posOverPoly)
+			{
+				d = dtAbs(diff[1]) - tile->header->walkableClimb;
+				d = d > 0 ? d*d : 0;			
+			}
+			else
+			{
+				d = dtVlenSqr(diff);
+			}
+			
+			if (d < m_nearestDistanceSqr)
+			{
+				dtVcopy(m_nearestPoint, closestPtPoly);
+
+				m_nearestDistanceSqr = d;
+				m_nearestRef = ref;
+			}
+		}
+	}
+};
+
+/// @par 
+///
+/// @note If the search box does not intersect any polygons the search will 
+/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check 
+/// @p nearestRef before using @p nearestPt.
+///
+dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents,
+										 const dtQueryFilter* filter,
+										 dtPolyRef* nearestRef, float* nearestPt) const
+{
+	dtAssert(m_nav);
+
+	if (!nearestRef)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	// queryPolygons below will check rest of params
+	
+	dtFindNearestPolyQuery query(this, center);
+
+	dtStatus status = queryPolygons(center, halfExtents, filter, &query);
+	if (dtStatusFailed(status))
+		return status;
+
+	*nearestRef = query.nearestRef();
+	// Only override nearestPt if we actually found a poly so the nearest point
+	// is valid.
+	if (nearestPt && *nearestRef)
+		dtVcopy(nearestPt, query.nearestPoint());
+	
+	return DT_SUCCESS;
+}
+
+void dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
+										 const dtQueryFilter* filter, dtPolyQuery* query) const
+{
+	dtAssert(m_nav);
+	static const int batchSize = 32;
+	dtPolyRef polyRefs[batchSize];
+	dtPoly* polys[batchSize];
+	int n = 0;
+
+	if (tile->bvTree)
+	{
+		const dtBVNode* node = &tile->bvTree[0];
+		const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
+		const float* tbmin = tile->header->bmin;
+		const float* tbmax = tile->header->bmax;
+		const float qfac = tile->header->bvQuantFactor;
+
+		// Calculate quantized box
+		unsigned short bmin[3], bmax[3];
+		// dtClamp query box to world box.
+		float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
+		float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
+		float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
+		float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
+		float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
+		float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
+		// Quantize
+		bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
+		bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
+		bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
+		bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
+		bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
+		bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
+
+		// Traverse tree
+		const dtPolyRef base = m_nav->getPolyRefBase(tile);
+		while (node < end)
+		{
+			const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
+			const bool isLeafNode = node->i >= 0;
+
+			if (isLeafNode && overlap)
+			{
+				dtPolyRef ref = base | (dtPolyRef)node->i;
+				if (filter->passFilter(ref, tile, &tile->polys[node->i]))
+				{
+					polyRefs[n] = ref;
+					polys[n] = &tile->polys[node->i];
+
+					if (n == batchSize - 1)
+					{
+						query->process(tile, polys, polyRefs, batchSize);
+						n = 0;
+					}
+					else
+					{
+						n++;
+					}
+				}
+			}
+
+			if (overlap || isLeafNode)
+				node++;
+			else
+			{
+				const int escapeIndex = -node->i;
+				node += escapeIndex;
+			}
+		}
+	}
+	else
+	{
+		float bmin[3], bmax[3];
+		const dtPolyRef base = m_nav->getPolyRefBase(tile);
+		for (int i = 0; i < tile->header->polyCount; ++i)
+		{
+			dtPoly* p = &tile->polys[i];
+			// Do not return off-mesh connection polygons.
+			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			// Must pass filter
+			const dtPolyRef ref = base | (dtPolyRef)i;
+			if (!filter->passFilter(ref, tile, p))
+				continue;
+			// Calc polygon bounds.
+			const float* v = &tile->verts[p->verts[0]*3];
+			dtVcopy(bmin, v);
+			dtVcopy(bmax, v);
+			for (int j = 1; j < p->vertCount; ++j)
+			{
+				v = &tile->verts[p->verts[j]*3];
+				dtVmin(bmin, v);
+				dtVmax(bmax, v);
+			}
+			if (dtOverlapBounds(qmin, qmax, bmin, bmax))
+			{
+				polyRefs[n] = ref;
+				polys[n] = p;
+
+				if (n == batchSize - 1)
+				{
+					query->process(tile, polys, polyRefs, batchSize);
+					n = 0;
+				}
+				else
+				{
+					n++;
+				}
+			}
+		}
+	}
+
+	// Process the last polygons that didn't make a full batch.
+	if (n > 0)
+		query->process(tile, polys, polyRefs, n);
+}
+
+class dtCollectPolysQuery : public dtPolyQuery
+{
+	dtPolyRef* m_polys;
+	const int m_maxPolys;
+	int m_numCollected;
+	bool m_overflow;
+
+public:
+	dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys)
+		: m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false)
+	{
+	}
+
+	int numCollected() const { return m_numCollected; }
+	bool overflowed() const { return m_overflow; }
+
+	void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count)
+	{
+		dtIgnoreUnused(tile);
+		dtIgnoreUnused(polys);
+
+		int numLeft = m_maxPolys - m_numCollected;
+		int toCopy = count;
+		if (toCopy > numLeft)
+		{
+			m_overflow = true;
+			toCopy = numLeft;
+		}
+
+		memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef));
+		m_numCollected += toCopy;
+	}
+};
+
+/// @par 
+///
+/// If no polygons are found, the function will return #DT_SUCCESS with a
+/// @p polyCount of zero.
+///
+/// If @p polys is too small to hold the entire result set, then the array will 
+/// be filled to capacity. The method of choosing which polygons from the 
+/// full set are included in the partial result set is undefined.
+///
+dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
+									   const dtQueryFilter* filter,
+									   dtPolyRef* polys, int* polyCount, const int maxPolys) const
+{
+	if (!polys || !polyCount || maxPolys < 0)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	dtCollectPolysQuery collector(polys, maxPolys);
+
+	dtStatus status = queryPolygons(center, halfExtents, filter, &collector);
+	if (dtStatusFailed(status))
+		return status;
+
+	*polyCount = collector.numCollected();
+	return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS;
+}
+
+/// @par 
+///
+/// The query will be invoked with batches of polygons. Polygons passed
+/// to the query have bounding boxes that overlap with the center and halfExtents
+/// passed to this function. The dtPolyQuery::process function is invoked multiple
+/// times until all overlapping polygons have been processed.
+///
+dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
+									   const dtQueryFilter* filter, dtPolyQuery* query) const
+{
+	dtAssert(m_nav);
+
+	if (!center || !dtVisfinite(center) ||
+		!halfExtents || !dtVisfinite(halfExtents) ||
+		!filter || !query)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+
+	float bmin[3], bmax[3];
+	dtVsub(bmin, center, halfExtents);
+	dtVadd(bmax, center, halfExtents);
+	
+	// Find tiles the query touches.
+	int minx, miny, maxx, maxy;
+	m_nav->calcTileLoc(bmin, &minx, &miny);
+	m_nav->calcTileLoc(bmax, &maxx, &maxy);
+
+	static const int MAX_NEIS = 32;
+	const dtMeshTile* neis[MAX_NEIS];
+	
+	for (int y = miny; y <= maxy; ++y)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS);
+			for (int j = 0; j < nneis; ++j)
+			{
+				queryPolygonsInTile(neis[j], bmin, bmax, filter, query);
+			}
+		}
+	}
+	
+	return DT_SUCCESS;
+}
+
+/// @par
+///
+/// If the end polygon cannot be reached through the navigation graph,
+/// the last polygon in the path will be the nearest the end polygon.
+///
+/// If the path array is to small to hold the full result, it will be filled as 
+/// far as possible from the start polygon toward the end polygon.
+///
+/// The start and end positions are used to calculate traversal costs. 
+/// (The y-values impact the result.)
+///
+dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
+								  const float* startPos, const float* endPos,
+								  const dtQueryFilter* filter,
+								  dtPolyRef* path, int* pathCount, const int maxPath) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+
+	if (!pathCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*pathCount = 0;
+	
+	// Validate input
+	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
+		!startPos || !dtVisfinite(startPos) ||
+		!endPos || !dtVisfinite(endPos) ||
+		!filter || !path || maxPath <= 0)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+
+	if (startRef == endRef)
+	{
+		path[0] = startRef;
+		*pathCount = 1;
+		return DT_SUCCESS;
+	}
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, startPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = dtVdist(startPos, endPos) * H_SCALE;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	dtNode* lastBestNode = startNode;
+	float lastBestNodeCost = startNode->total;
+	
+	bool outOfNodes = false;
+	
+	while (!m_openList->empty())
+	{
+		// Remove node from open list and put it in closed list.
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Reached the goal, stop searching.
+		if (bestNode->id == endRef)
+		{
+			lastBestNode = bestNode;
+			break;
+		}
+		
+		// Get current poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+		
+		// Get parent poly and tile.
+		dtPolyRef parentRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		if (bestNode->pidx)
+			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		if (parentRef)
+			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			dtPolyRef neighbourRef = bestTile->links[i].ref;
+			
+			// Skip invalid ids and do not expand back to where we came from.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Get neighbour poly and tile.
+			// The API input has been cheked already, skip checking internal data.
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);			
+			
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+
+			// deal explicitly with crossing tile boundaries
+			unsigned char crossSide = 0;
+			if (bestTile->links[i].side != 0xff)
+				crossSide = bestTile->links[i].side >> 1;
+
+			// get the node
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
+			if (!neighbourNode)
+			{
+				outOfNodes = true;
+				continue;
+			}
+			
+			// If the node is visited the first time, calculate node position.
+			if (neighbourNode->flags == 0)
+			{
+				getEdgeMidPoint(bestRef, bestPoly, bestTile,
+								neighbourRef, neighbourPoly, neighbourTile,
+								neighbourNode->pos);
+			}
+
+			// Calculate cost and heuristic.
+			float cost = 0;
+			float heuristic = 0;
+			
+			// Special case for last node.
+			if (neighbourRef == endRef)
+			{
+				// Cost
+				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
+													  parentRef, parentTile, parentPoly,
+													  bestRef, bestTile, bestPoly,
+													  neighbourRef, neighbourTile, neighbourPoly);
+				const float endCost = filter->getCost(neighbourNode->pos, endPos,
+													  bestRef, bestTile, bestPoly,
+													  neighbourRef, neighbourTile, neighbourPoly,
+													  0, 0, 0);
+				
+				cost = bestNode->cost + curCost + endCost;
+				heuristic = 0;
+			}
+			else
+			{
+				// Cost
+				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
+													  parentRef, parentTile, parentPoly,
+													  bestRef, bestTile, bestPoly,
+													  neighbourRef, neighbourTile, neighbourPoly);
+				cost = bestNode->cost + curCost;
+				heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
+			}
+
+			const float total = cost + heuristic;
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			// The node is already visited and process, and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
+				continue;
+			
+			// Add or update the node.
+			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->id = neighbourRef;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->cost = cost;
+			neighbourNode->total = total;
+			
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				// Already in open, update node location.
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				// Put the node in open list.
+				neighbourNode->flags |= DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+			
+			// Update nearest node to target so far.
+			if (heuristic < lastBestNodeCost)
+			{
+				lastBestNodeCost = heuristic;
+				lastBestNode = neighbourNode;
+			}
+		}
+	}
+
+	dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath);
+
+	if (lastBestNode->id != endRef)
+		status |= DT_PARTIAL_RESULT;
+
+	if (outOfNodes)
+		status |= DT_OUT_OF_NODES;
+	
+	return status;
+}
+
+dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const
+{
+	// Find the length of the entire path.
+	dtNode* curNode = endNode;
+	int length = 0;
+	do
+	{
+		length++;
+		curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
+	} while (curNode);
+
+	// If the path cannot be fully stored then advance to the last node we will be able to store.
+	curNode = endNode;
+	int writeCount;
+	for (writeCount = length; writeCount > maxPath; writeCount--)
+	{
+		dtAssert(curNode);
+
+		curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
+	}
+
+	// Write path
+	for (int i = writeCount - 1; i >= 0; i--)
+	{
+		dtAssert(curNode);
+
+		path[i] = curNode->id;
+		curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
+	}
+
+	dtAssert(!curNode);
+
+	*pathCount = dtMin(length, maxPath);
+
+	if (length > maxPath)
+		return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
+
+	return DT_SUCCESS;
+}
+
+
+/// @par
+///
+/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath() 
+/// or finalizeSlicedFindPathPartial() may result in corrupted data!
+///
+/// The @p filter pointer is stored and used for the duration of the sliced
+/// path query.
+///
+dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
+											const float* startPos, const float* endPos,
+											const dtQueryFilter* filter, const unsigned int options)
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+
+	// Init path state.
+	memset(&m_query, 0, sizeof(dtQueryData));
+	m_query.status = DT_FAILURE;
+	m_query.startRef = startRef;
+	m_query.endRef = endRef;
+	if (startPos)
+		dtVcopy(m_query.startPos, startPos);
+	if (endPos)
+		dtVcopy(m_query.endPos, endPos);
+	m_query.filter = filter;
+	m_query.options = options;
+	m_query.raycastLimitSqr = FLT_MAX;
+	
+	// Validate input
+	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
+		!startPos || !dtVisfinite(startPos) ||
+		!endPos || !dtVisfinite(endPos) || !filter)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+
+	// trade quality with performance?
+	if (options & DT_FINDPATH_ANY_ANGLE)
+	{
+		// limiting to several times the character radius yields nice results. It is not sensitive 
+		// so it is enough to compute it from the first tile.
+		const dtMeshTile* tile = m_nav->getTileByRef(startRef);
+		float agentRadius = tile->header->walkableRadius;
+		m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS);
+	}
+
+	if (startRef == endRef)
+	{
+		m_query.status = DT_SUCCESS;
+		return DT_SUCCESS;
+	}
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, startPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = dtVdist(startPos, endPos) * H_SCALE;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	m_query.status = DT_IN_PROGRESS;
+	m_query.lastBestNode = startNode;
+	m_query.lastBestNodeCost = startNode->total;
+	
+	return m_query.status;
+}
+	
+dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
+{
+	if (!dtStatusInProgress(m_query.status))
+		return m_query.status;
+
+	// Make sure the request is still valid.
+	if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef))
+	{
+		m_query.status = DT_FAILURE;
+		return DT_FAILURE;
+	}
+
+	dtRaycastHit rayHit;
+	rayHit.maxPath = 0;
+		
+	int iter = 0;
+	while (iter < maxIter && !m_openList->empty())
+	{
+		iter++;
+		
+		// Remove node from open list and put it in closed list.
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Reached the goal, stop searching.
+		if (bestNode->id == m_query.endRef)
+		{
+			m_query.lastBestNode = bestNode;
+			const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+			m_query.status = DT_SUCCESS | details;
+			if (doneIters)
+				*doneIters = iter;
+			return m_query.status;
+		}
+		
+		// Get current poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		if (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly)))
+		{
+			// The polygon has disappeared during the sliced query, fail.
+			m_query.status = DT_FAILURE;
+			if (doneIters)
+				*doneIters = iter;
+			return m_query.status;
+		}
+		
+		// Get parent and grand parent poly and tile.
+		dtPolyRef parentRef = 0, grandpaRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		dtNode* parentNode = 0;
+		if (bestNode->pidx)
+		{
+			parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
+			parentRef = parentNode->id;
+			if (parentNode->pidx)
+				grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
+		}
+		if (parentRef)
+		{
+			bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly));
+			if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) )
+			{
+				// The polygon has disappeared during the sliced query, fail.
+				m_query.status = DT_FAILURE;
+				if (doneIters)
+					*doneIters = iter;
+				return m_query.status;
+			}
+		}
+
+		// decide whether to test raycast to previous nodes
+		bool tryLOS = false;
+		if (m_query.options & DT_FINDPATH_ANY_ANGLE)
+		{
+			if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr))
+				tryLOS = true;
+		}
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			dtPolyRef neighbourRef = bestTile->links[i].ref;
+			
+			// Skip invalid ids and do not expand back to where we came from.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Get neighbour poly and tile.
+			// The API input has been cheked already, skip checking internal data.
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);			
+			
+			if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+			
+			// get the neighbor node
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0);
+			if (!neighbourNode)
+			{
+				m_query.status |= DT_OUT_OF_NODES;
+				continue;
+			}
+			
+			// do not expand to nodes that were already visited from the same parent
+			if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx)
+				continue;
+
+			// If the node is visited the first time, calculate node position.
+			if (neighbourNode->flags == 0)
+			{
+				getEdgeMidPoint(bestRef, bestPoly, bestTile,
+								neighbourRef, neighbourPoly, neighbourTile,
+								neighbourNode->pos);
+			}
+			
+			// Calculate cost and heuristic.
+			float cost = 0;
+			float heuristic = 0;
+			
+			// raycast parent
+			bool foundShortCut = false;
+			rayHit.pathCost = rayHit.t = 0;
+			if (tryLOS)
+			{
+				raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
+				foundShortCut = rayHit.t >= 1.0f;
+			}
+
+			// update move cost
+			if (foundShortCut)
+			{
+				// shortcut found using raycast. Using shorter cost instead
+				cost = parentNode->cost + rayHit.pathCost;
+			}
+			else
+			{
+				// No shortcut found.
+				const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
+															  parentRef, parentTile, parentPoly,
+															bestRef, bestTile, bestPoly,
+															neighbourRef, neighbourTile, neighbourPoly);
+				cost = bestNode->cost + curCost;
+			}
+
+			// Special case for last node.
+			if (neighbourRef == m_query.endRef)
+			{
+				const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
+															  bestRef, bestTile, bestPoly,
+															  neighbourRef, neighbourTile, neighbourPoly,
+															  0, 0, 0);
+				
+				cost = cost + endCost;
+				heuristic = 0;
+			}
+			else
+			{
+				heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
+			}
+			
+			const float total = cost + heuristic;
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			// The node is already visited and process, and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
+				continue;
+			
+			// Add or update the node.
+			neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->id = neighbourRef;
+			neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED));
+			neighbourNode->cost = cost;
+			neighbourNode->total = total;
+			if (foundShortCut)
+				neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED);
+			
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				// Already in open, update node location.
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				// Put the node in open list.
+				neighbourNode->flags |= DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+			
+			// Update nearest node to target so far.
+			if (heuristic < m_query.lastBestNodeCost)
+			{
+				m_query.lastBestNodeCost = heuristic;
+				m_query.lastBestNode = neighbourNode;
+			}
+		}
+	}
+	
+	// Exhausted all nodes, but could not find path.
+	if (m_openList->empty())
+	{
+		const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+		m_query.status = DT_SUCCESS | details;
+	}
+
+	if (doneIters)
+		*doneIters = iter;
+
+	return m_query.status;
+}
+
+dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath)
+{
+	if (!pathCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*pathCount = 0;
+
+	if (!path || maxPath <= 0)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	if (dtStatusFailed(m_query.status))
+	{
+		// Reset query.
+		memset(&m_query, 0, sizeof(dtQueryData));
+		return DT_FAILURE;
+	}
+
+	int n = 0;
+
+	if (m_query.startRef == m_query.endRef)
+	{
+		// Special case: the search starts and ends at same poly.
+		path[n++] = m_query.startRef;
+	}
+	else
+	{
+		// Reverse the path.
+		dtAssert(m_query.lastBestNode);
+		
+		if (m_query.lastBestNode->id != m_query.endRef)
+			m_query.status |= DT_PARTIAL_RESULT;
+		
+		dtNode* prev = 0;
+		dtNode* node = m_query.lastBestNode;
+		int prevRay = 0;
+		do
+		{
+			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
+			node->pidx = m_nodePool->getNodeIdx(prev);
+			prev = node;
+			int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
+			node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
+			prevRay = nextRay;
+			node = next;
+		}
+		while (node);
+		
+		// Store path
+		node = prev;
+		do
+		{
+			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
+			dtStatus status = 0;
+			if (node->flags & DT_NODE_PARENT_DETACHED)
+			{
+				float t, normal[3];
+				int m;
+				status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n);
+				n += m;
+				// raycast ends on poly boundary and the path might include the next poly boundary.
+				if (path[n-1] == next->id)
+					n--; // remove to avoid duplicates
+			}
+			else
+			{
+				path[n++] = node->id;
+				if (n >= maxPath)
+					status = DT_BUFFER_TOO_SMALL;
+			}
+
+			if (status & DT_STATUS_DETAIL_MASK)
+			{
+				m_query.status |= status & DT_STATUS_DETAIL_MASK;
+				break;
+			}
+			node = next;
+		}
+		while (node);
+	}
+	
+	const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+
+	// Reset query.
+	memset(&m_query, 0, sizeof(dtQueryData));
+	
+	*pathCount = n;
+	
+	return DT_SUCCESS | details;
+}
+
+dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
+													   dtPolyRef* path, int* pathCount, const int maxPath)
+{
+	if (!pathCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*pathCount = 0;
+
+	if (!existing || existingSize <= 0 || !path || !pathCount || maxPath <= 0)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	if (dtStatusFailed(m_query.status))
+	{
+		// Reset query.
+		memset(&m_query, 0, sizeof(dtQueryData));
+		return DT_FAILURE;
+	}
+	
+	int n = 0;
+	
+	if (m_query.startRef == m_query.endRef)
+	{
+		// Special case: the search starts and ends at same poly.
+		path[n++] = m_query.startRef;
+	}
+	else
+	{
+		// Find furthest existing node that was visited.
+		dtNode* prev = 0;
+		dtNode* node = 0;
+		for (int i = existingSize-1; i >= 0; --i)
+		{
+			m_nodePool->findNodes(existing[i], &node, 1);
+			if (node)
+				break;
+		}
+		
+		if (!node)
+		{
+			m_query.status |= DT_PARTIAL_RESULT;
+			dtAssert(m_query.lastBestNode);
+			node = m_query.lastBestNode;
+		}
+		
+		// Reverse the path.
+		int prevRay = 0;
+		do
+		{
+			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
+			node->pidx = m_nodePool->getNodeIdx(prev);
+			prev = node;
+			int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
+			node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
+			prevRay = nextRay;
+			node = next;
+		}
+		while (node);
+		
+		// Store path
+		node = prev;
+		do
+		{
+			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
+			dtStatus status = 0;
+			if (node->flags & DT_NODE_PARENT_DETACHED)
+			{
+				float t, normal[3];
+				int m;
+				status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n);
+				n += m;
+				// raycast ends on poly boundary and the path might include the next poly boundary.
+				if (path[n-1] == next->id)
+					n--; // remove to avoid duplicates
+			}
+			else
+			{
+				path[n++] = node->id;
+				if (n >= maxPath)
+					status = DT_BUFFER_TOO_SMALL;
+			}
+
+			if (status & DT_STATUS_DETAIL_MASK)
+			{
+				m_query.status |= status & DT_STATUS_DETAIL_MASK;
+				break;
+			}
+			node = next;
+		}
+		while (node);
+	}
+	
+	const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+
+	// Reset query.
+	memset(&m_query, 0, sizeof(dtQueryData));
+	
+	*pathCount = n;
+	
+	return DT_SUCCESS | details;
+}
+
+
+dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
+									  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+									  int* straightPathCount, const int maxStraightPath) const
+{
+	if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
+	{
+		// The vertices are equal, update flags and poly.
+		if (straightPathFlags)
+			straightPathFlags[(*straightPathCount)-1] = flags;
+		if (straightPathRefs)
+			straightPathRefs[(*straightPathCount)-1] = ref;
+	}
+	else
+	{
+		// Append new vertex.
+		dtVcopy(&straightPath[(*straightPathCount)*3], pos);
+		if (straightPathFlags)
+			straightPathFlags[(*straightPathCount)] = flags;
+		if (straightPathRefs)
+			straightPathRefs[(*straightPathCount)] = ref;
+		(*straightPathCount)++;
+
+		// If there is no space to append more vertices, return.
+		if ((*straightPathCount) >= maxStraightPath)
+		{
+			return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
+		}
+
+		// If reached end of path, return.
+		if (flags == DT_STRAIGHTPATH_END)
+		{
+			return DT_SUCCESS;
+		}
+	}
+	return DT_IN_PROGRESS;
+}
+
+dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
+									  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+									  int* straightPathCount, const int maxStraightPath, const int options) const
+{
+	const float* startPos = &straightPath[(*straightPathCount-1)*3];
+	// Append or update last vertex
+	dtStatus stat = 0;
+	for (int i = startIdx; i < endIdx; i++)
+	{
+		// Calculate portal
+		const dtPolyRef from = path[i];
+		const dtMeshTile* fromTile = 0;
+		const dtPoly* fromPoly = 0;
+		if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
+			return DT_FAILURE | DT_INVALID_PARAM;
+		
+		const dtPolyRef to = path[i+1];
+		const dtMeshTile* toTile = 0;
+		const dtPoly* toPoly = 0;
+		if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
+			return DT_FAILURE | DT_INVALID_PARAM;
+		
+		float left[3], right[3];
+		if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
+			break;
+	
+		if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
+		{
+			// Skip intersection if only area crossings are requested.
+			if (fromPoly->getArea() == toPoly->getArea())
+				continue;
+		}
+		
+		// Append intersection
+		float s,t;
+		if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
+		{
+			float pt[3];
+			dtVlerp(pt, left,right, t);
+
+			stat = appendVertex(pt, 0, path[i+1],
+								straightPath, straightPathFlags, straightPathRefs,
+								straightPathCount, maxStraightPath);
+			if (stat != DT_IN_PROGRESS)
+				return stat;
+		}
+	}
+	return DT_IN_PROGRESS;
+}
+
+/// @par
+/// 
+/// This method peforms what is often called 'string pulling'.
+///
+/// The start position is clamped to the first polygon in the path, and the 
+/// end position is clamped to the last. So the start and end positions should 
+/// normally be within or very near the first and last polygons respectively.
+///
+/// The returned polygon references represent the reference id of the polygon 
+/// that is entered at the associated path position. The reference id associated 
+/// with the end point will always be zero.  This allows, for example, matching 
+/// off-mesh link points to their representative polygons.
+///
+/// If the provided result buffers are too small for the entire result set, 
+/// they will be filled as far as possible from the start toward the end 
+/// position.
+///
+dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
+										  const dtPolyRef* path, const int pathSize,
+										  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+										  int* straightPathCount, const int maxStraightPath, const int options) const
+{
+	dtAssert(m_nav);
+
+	if (!straightPathCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*straightPathCount = 0;
+
+	if (!startPos || !dtVisfinite(startPos) ||
+		!endPos || !dtVisfinite(endPos) ||
+		!path || pathSize <= 0 || !path[0] ||
+		maxStraightPath <= 0)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	dtStatus stat = 0;
+	
+	// TODO: Should this be callers responsibility?
+	float closestStartPos[3];
+	if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	float closestEndPos[3];
+	if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Add start point.
+	stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
+						straightPath, straightPathFlags, straightPathRefs,
+						straightPathCount, maxStraightPath);
+	if (stat != DT_IN_PROGRESS)
+		return stat;
+	
+	if (pathSize > 1)
+	{
+		float portalApex[3], portalLeft[3], portalRight[3];
+		dtVcopy(portalApex, closestStartPos);
+		dtVcopy(portalLeft, portalApex);
+		dtVcopy(portalRight, portalApex);
+		int apexIndex = 0;
+		int leftIndex = 0;
+		int rightIndex = 0;
+		
+		unsigned char leftPolyType = 0;
+		unsigned char rightPolyType = 0;
+		
+		dtPolyRef leftPolyRef = path[0];
+		dtPolyRef rightPolyRef = path[0];
+		
+		for (int i = 0; i < pathSize; ++i)
+		{
+			float left[3], right[3];
+			unsigned char toType;
+			
+			if (i+1 < pathSize)
+			{
+				unsigned char fromType; // fromType is ignored.
+
+				// Next portal.
+				if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
+				{
+					// Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
+					// Clamp the end point to path[i], and return the path so far.
+					
+					if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
+					{
+						// This should only happen when the first polygon is invalid.
+						return DT_FAILURE | DT_INVALID_PARAM;
+					}
+
+					// Apeend portals along the current straight path segment.
+					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+					{
+						// Ignore status return value as we're just about to return anyway.
+						appendPortals(apexIndex, i, closestEndPos, path,
+											 straightPath, straightPathFlags, straightPathRefs,
+											 straightPathCount, maxStraightPath, options);
+					}
+
+					// Ignore status return value as we're just about to return anyway.
+					appendVertex(closestEndPos, 0, path[i],
+										straightPath, straightPathFlags, straightPathRefs,
+										straightPathCount, maxStraightPath);
+					
+					return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
+				}
+				
+				// If starting really close the portal, advance.
+				if (i == 0)
+				{
+					float t;
+					if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f))
+						continue;
+				}
+			}
+			else
+			{
+				// End of the path.
+				dtVcopy(left, closestEndPos);
+				dtVcopy(right, closestEndPos);
+				
+				toType = DT_POLYTYPE_GROUND;
+			}
+			
+			// Right vertex.
+			if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
+			{
+				if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
+				{
+					dtVcopy(portalRight, right);
+					rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
+					rightPolyType = toType;
+					rightIndex = i;
+				}
+				else
+				{
+					// Append portals along the current straight path segment.
+					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+					{
+						stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
+											 straightPath, straightPathFlags, straightPathRefs,
+											 straightPathCount, maxStraightPath, options);
+						if (stat != DT_IN_PROGRESS)
+							return stat;					
+					}
+				
+					dtVcopy(portalApex, portalLeft);
+					apexIndex = leftIndex;
+					
+					unsigned char flags = 0;
+					if (!leftPolyRef)
+						flags = DT_STRAIGHTPATH_END;
+					else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
+						flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
+					dtPolyRef ref = leftPolyRef;
+					
+					// Append or update vertex
+					stat = appendVertex(portalApex, flags, ref,
+										straightPath, straightPathFlags, straightPathRefs,
+										straightPathCount, maxStraightPath);
+					if (stat != DT_IN_PROGRESS)
+						return stat;
+					
+					dtVcopy(portalLeft, portalApex);
+					dtVcopy(portalRight, portalApex);
+					leftIndex = apexIndex;
+					rightIndex = apexIndex;
+					
+					// Restart
+					i = apexIndex;
+					
+					continue;
+				}
+			}
+			
+			// Left vertex.
+			if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
+			{
+				if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
+				{
+					dtVcopy(portalLeft, left);
+					leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
+					leftPolyType = toType;
+					leftIndex = i;
+				}
+				else
+				{
+					// Append portals along the current straight path segment.
+					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+					{
+						stat = appendPortals(apexIndex, rightIndex, portalRight, path,
+											 straightPath, straightPathFlags, straightPathRefs,
+											 straightPathCount, maxStraightPath, options);
+						if (stat != DT_IN_PROGRESS)
+							return stat;
+					}
+
+					dtVcopy(portalApex, portalRight);
+					apexIndex = rightIndex;
+					
+					unsigned char flags = 0;
+					if (!rightPolyRef)
+						flags = DT_STRAIGHTPATH_END;
+					else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
+						flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
+					dtPolyRef ref = rightPolyRef;
+
+					// Append or update vertex
+					stat = appendVertex(portalApex, flags, ref,
+										straightPath, straightPathFlags, straightPathRefs,
+										straightPathCount, maxStraightPath);
+					if (stat != DT_IN_PROGRESS)
+						return stat;
+					
+					dtVcopy(portalLeft, portalApex);
+					dtVcopy(portalRight, portalApex);
+					leftIndex = apexIndex;
+					rightIndex = apexIndex;
+					
+					// Restart
+					i = apexIndex;
+					
+					continue;
+				}
+			}
+		}
+
+		// Append portals along the current straight path segment.
+		if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+		{
+			stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
+								 straightPath, straightPathFlags, straightPathRefs,
+								 straightPathCount, maxStraightPath, options);
+			if (stat != DT_IN_PROGRESS)
+				return stat;
+		}
+	}
+
+	// Ignore status return value as we're just about to return anyway.
+	appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
+						straightPath, straightPathFlags, straightPathRefs,
+						straightPathCount, maxStraightPath);
+	
+	return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
+}
+
+/// @par
+///
+/// This method is optimized for small delta movement and a small number of 
+/// polygons. If used for too great a distance, the result set will form an 
+/// incomplete path.
+///
+/// @p resultPos will equal the @p endPos if the end is reached. 
+/// Otherwise the closest reachable position will be returned.
+/// 
+/// @p resultPos is not projected onto the surface of the navigation 
+/// mesh. Use #getPolyHeight if this is needed.
+///
+/// This method treats the end position in the same manner as 
+/// the #raycast method. (As a 2D point.) See that method's documentation 
+/// for details.
+/// 
+/// If the @p visited array is too small to hold the entire result set, it will 
+/// be filled as far as possible from the start position toward the end 
+/// position.
+///
+dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
+										  const dtQueryFilter* filter,
+										  float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_tinyNodePool);
+
+	if (!visitedCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*visitedCount = 0;
+
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!startPos || !dtVisfinite(startPos) ||
+		!endPos || !dtVisfinite(endPos) ||
+		!filter || !resultPos || !visited ||
+		maxVisitedSize <= 0)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	dtStatus status = DT_SUCCESS;
+	
+	static const int MAX_STACK = 48;
+	dtNode* stack[MAX_STACK];
+	int nstack = 0;
+	
+	m_tinyNodePool->clear();
+	
+	dtNode* startNode = m_tinyNodePool->getNode(startRef);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_CLOSED;
+	stack[nstack++] = startNode;
+	
+	float bestPos[3];
+	float bestDist = FLT_MAX;
+	dtNode* bestNode = 0;
+	dtVcopy(bestPos, startPos);
+	
+	// Search constraints
+	float searchPos[3], searchRadSqr;
+	dtVlerp(searchPos, startPos, endPos, 0.5f);
+	searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);
+	
+	float verts[DT_VERTS_PER_POLYGON*3];
+	
+	while (nstack)
+	{
+		// Pop front.
+		dtNode* curNode = stack[0];
+		for (int i = 0; i < nstack-1; ++i)
+			stack[i] = stack[i+1];
+		nstack--;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef curRef = curNode->id;
+		const dtMeshTile* curTile = 0;
+		const dtPoly* curPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);			
+		
+		// Collect vertices.
+		const int nverts = curPoly->vertCount;
+		for (int i = 0; i < nverts; ++i)
+			dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]);
+		
+		// If target is inside the poly, stop search.
+		if (dtPointInPolygon(endPos, verts, nverts))
+		{
+			bestNode = curNode;
+			dtVcopy(bestPos, endPos);
+			break;
+		}
+		
+		// Find wall edges and find nearest point inside the walls.
+		for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++)
+		{
+			// Find links to neighbours.
+			static const int MAX_NEIS = 8;
+			int nneis = 0;
+			dtPolyRef neis[MAX_NEIS];
+			
+			if (curPoly->neis[j] & DT_EXT_LINK)
+			{
+				// Tile border.
+				for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
+				{
+					const dtLink* link = &curTile->links[k];
+					if (link->edge == j)
+					{
+						if (link->ref != 0)
+						{
+							const dtMeshTile* neiTile = 0;
+							const dtPoly* neiPoly = 0;
+							m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
+							if (filter->passFilter(link->ref, neiTile, neiPoly))
+							{
+								if (nneis < MAX_NEIS)
+									neis[nneis++] = link->ref;
+							}
+						}
+					}
+				}
+			}
+			else if (curPoly->neis[j])
+			{
+				const unsigned int idx = (unsigned int)(curPoly->neis[j]-1);
+				const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx;
+				if (filter->passFilter(ref, curTile, &curTile->polys[idx]))
+				{
+					// Internal edge, encode id.
+					neis[nneis++] = ref;
+				}
+			}
+			
+			if (!nneis)
+			{
+				// Wall edge, calc distance.
+				const float* vj = &verts[j*3];
+				const float* vi = &verts[i*3];
+				float tseg;
+				const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg);
+				if (distSqr < bestDist)
+				{
+                    // Update nearest distance.
+					dtVlerp(bestPos, vj,vi, tseg);
+					bestDist = distSqr;
+					bestNode = curNode;
+				}
+			}
+			else
+			{
+				for (int k = 0; k < nneis; ++k)
+				{
+					// Skip if no node can be allocated.
+					dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]);
+					if (!neighbourNode)
+						continue;
+					// Skip if already visited.
+					if (neighbourNode->flags & DT_NODE_CLOSED)
+						continue;
+					
+					// Skip the link if it is too far from search constraint.
+					// TODO: Maybe should use getPortalPoints(), but this one is way faster.
+					const float* vj = &verts[j*3];
+					const float* vi = &verts[i*3];
+					float tseg;
+					float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg);
+					if (distSqr > searchRadSqr)
+						continue;
+					
+					// Mark as the node as visited and push to queue.
+					if (nstack < MAX_STACK)
+					{
+						neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
+						neighbourNode->flags |= DT_NODE_CLOSED;
+						stack[nstack++] = neighbourNode;
+					}
+				}
+			}
+		}
+	}
+	
+	int n = 0;
+	if (bestNode)
+	{
+		// Reverse the path.
+		dtNode* prev = 0;
+		dtNode* node = bestNode;
+		do
+		{
+			dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx);
+			node->pidx = m_tinyNodePool->getNodeIdx(prev);
+			prev = node;
+			node = next;
+		}
+		while (node);
+		
+		// Store result
+		node = prev;
+		do
+		{
+			visited[n++] = node->id;
+			if (n >= maxVisitedSize)
+			{
+				status |= DT_BUFFER_TOO_SMALL;
+				break;
+			}
+			node = m_tinyNodePool->getNodeAtIdx(node->pidx);
+		}
+		while (node);
+	}
+	
+	dtVcopy(resultPos, bestPos);
+	
+	*visitedCount = n;
+	
+	return status;
+}
+
+
+dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
+										 unsigned char& fromType, unsigned char& toType) const
+{
+	dtAssert(m_nav);
+	
+	const dtMeshTile* fromTile = 0;
+	const dtPoly* fromPoly = 0;
+	if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	fromType = fromPoly->getType();
+
+	const dtMeshTile* toTile = 0;
+	const dtPoly* toPoly = 0;
+	if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	toType = toPoly->getType();
+		
+	return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
+}
+
+// Returns portal points between two polygons.
+dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
+										 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
+										 float* left, float* right) const
+{
+	// Find the link that points to the 'to' polygon.
+	const dtLink* link = 0;
+	for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
+	{
+		if (fromTile->links[i].ref == to)
+		{
+			link = &fromTile->links[i];
+			break;
+		}
+	}
+	if (!link)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Handle off-mesh connections.
+	if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		// Find link that points to first vertex.
+		for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
+		{
+			if (fromTile->links[i].ref == to)
+			{
+				const int v = fromTile->links[i].edge;
+				dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]);
+				dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]);
+				return DT_SUCCESS;
+			}
+		}
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
+		{
+			if (toTile->links[i].ref == from)
+			{
+				const int v = toTile->links[i].edge;
+				dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]);
+				dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]);
+				return DT_SUCCESS;
+			}
+		}
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	// Find portal vertices.
+	const int v0 = fromPoly->verts[link->edge];
+	const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
+	dtVcopy(left, &fromTile->verts[v0*3]);
+	dtVcopy(right, &fromTile->verts[v1*3]);
+	
+	// If the link is at tile boundary, dtClamp the vertices to
+	// the link width.
+	if (link->side != 0xff)
+	{
+		// Unpack portal limits.
+		if (link->bmin != 0 || link->bmax != 255)
+		{
+			const float s = 1.0f/255.0f;
+			const float tmin = link->bmin*s;
+			const float tmax = link->bmax*s;
+			dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin);
+			dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax);
+		}
+	}
+	
+	return DT_SUCCESS;
+}
+
+// Returns edge mid point between two polygons.
+dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const
+{
+	float left[3], right[3];
+	unsigned char fromType, toType;
+	if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	mid[0] = (left[0]+right[0])*0.5f;
+	mid[1] = (left[1]+right[1])*0.5f;
+	mid[2] = (left[2]+right[2])*0.5f;
+	return DT_SUCCESS;
+}
+
+dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
+										 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
+										 float* mid) const
+{
+	float left[3], right[3];
+	if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	mid[0] = (left[0]+right[0])*0.5f;
+	mid[1] = (left[1]+right[1])*0.5f;
+	mid[2] = (left[2]+right[2])*0.5f;
+	return DT_SUCCESS;
+}
+
+
+
+/// @par
+///
+/// This method is meant to be used for quick, short distance checks.
+///
+/// If the path array is too small to hold the result, it will be filled as 
+/// far as possible from the start postion toward the end position.
+///
+/// <b>Using the Hit Parameter (t)</b>
+/// 
+/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit 
+/// the end position. In this case the path represents a valid corridor to the 
+/// end position and the value of @p hitNormal is undefined.
+///
+/// If the hit parameter is zero, then the start position is on the wall that 
+/// was hit and the value of @p hitNormal is undefined.
+///
+/// If 0 < t < 1.0 then the following applies:
+///
+/// @code
+/// distanceToHitBorder = distanceToEndPosition * t
+/// hitPoint = startPos + (endPos - startPos) * t
+/// @endcode
+///
+/// <b>Use Case Restriction</b>
+///
+/// The raycast ignores the y-value of the end position. (2D check.) This 
+/// places significant limits on how it can be used. For example:
+///
+/// Consider a scene where there is a main floor with a second floor balcony 
+/// that hangs over the main floor. So the first floor mesh extends below the 
+/// balcony mesh. The start position is somewhere on the first floor. The end 
+/// position is on the balcony.
+///
+/// The raycast will search toward the end position along the first floor mesh. 
+/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
+/// (no wall hit), meaning it reached the end position. This is one example of why
+/// this method is meant for short distance checks.
+///
+dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+								 const dtQueryFilter* filter,
+								 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const
+{
+	dtRaycastHit hit;
+	hit.path = path;
+	hit.maxPath = maxPath;
+
+	dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit);
+	
+	*t = hit.t;
+	if (hitNormal)
+		dtVcopy(hitNormal, hit.hitNormal);
+	if (pathCount)
+		*pathCount = hit.pathCount;
+
+	return status;
+}
+
+
+/// @par
+///
+/// This method is meant to be used for quick, short distance checks.
+///
+/// If the path array is too small to hold the result, it will be filled as 
+/// far as possible from the start postion toward the end position.
+///
+/// <b>Using the Hit Parameter t of RaycastHit</b>
+/// 
+/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit 
+/// the end position. In this case the path represents a valid corridor to the 
+/// end position and the value of @p hitNormal is undefined.
+///
+/// If the hit parameter is zero, then the start position is on the wall that 
+/// was hit and the value of @p hitNormal is undefined.
+///
+/// If 0 < t < 1.0 then the following applies:
+///
+/// @code
+/// distanceToHitBorder = distanceToEndPosition * t
+/// hitPoint = startPos + (endPos - startPos) * t
+/// @endcode
+///
+/// <b>Use Case Restriction</b>
+///
+/// The raycast ignores the y-value of the end position. (2D check.) This 
+/// places significant limits on how it can be used. For example:
+///
+/// Consider a scene where there is a main floor with a second floor balcony 
+/// that hangs over the main floor. So the first floor mesh extends below the 
+/// balcony mesh. The start position is somewhere on the first floor. The end 
+/// position is on the balcony.
+///
+/// The raycast will search toward the end position along the first floor mesh. 
+/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
+/// (no wall hit), meaning it reached the end position. This is one example of why
+/// this method is meant for short distance checks.
+///
+dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+								 const dtQueryFilter* filter, const unsigned int options,
+								 dtRaycastHit* hit, dtPolyRef prevRef) const
+{
+	dtAssert(m_nav);
+
+	if (!hit)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	hit->t = 0;
+	hit->pathCount = 0;
+	hit->pathCost = 0;
+
+	// Validate input
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!startPos || !dtVisfinite(startPos) ||
+		!endPos || !dtVisfinite(endPos) ||
+		!filter ||
+		(prevRef && !m_nav->isValidPolyRef(prevRef)))
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	float dir[3], curPos[3], lastPos[3];
+	float verts[DT_VERTS_PER_POLYGON*3+3];	
+	int n = 0;
+
+	dtVcopy(curPos, startPos);
+	dtVsub(dir, endPos, startPos);
+	dtVset(hit->hitNormal, 0, 0, 0);
+
+	dtStatus status = DT_SUCCESS;
+
+	const dtMeshTile* prevTile, *tile, *nextTile;
+	const dtPoly* prevPoly, *poly, *nextPoly;
+	dtPolyRef curRef;
+
+	// The API input has been checked already, skip checking internal data.
+	curRef = startRef;
+	tile = 0;
+	poly = 0;
+	m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
+	nextTile = prevTile = tile;
+	nextPoly = prevPoly = poly;
+	if (prevRef)
+		m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly);
+
+	while (curRef)
+	{
+		// Cast ray against current polygon.
+		
+		// Collect vertices.
+		int nv = 0;
+		for (int i = 0; i < (int)poly->vertCount; ++i)
+		{
+			dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
+			nv++;
+		}
+		
+		float tmin, tmax;
+		int segMin, segMax;
+		if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
+		{
+			// Could not hit the polygon, keep the old t and report hit.
+			hit->pathCount = n;
+			return status;
+		}
+
+		hit->hitEdgeIndex = segMax;
+
+		// Keep track of furthest t so far.
+		if (tmax > hit->t)
+			hit->t = tmax;
+		
+		// Store visited polygons.
+		if (n < hit->maxPath)
+			hit->path[n++] = curRef;
+		else
+			status |= DT_BUFFER_TOO_SMALL;
+
+		// Ray end is completely inside the polygon.
+		if (segMax == -1)
+		{
+			hit->t = FLT_MAX;
+			hit->pathCount = n;
+			
+			// add the cost
+			if (options & DT_RAYCAST_USE_COSTS)
+				hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
+			return status;
+		}
+
+		// Follow neighbours.
+		dtPolyRef nextRef = 0;
+		
+		for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
+		{
+			const dtLink* link = &tile->links[i];
+			
+			// Find link which contains this edge.
+			if ((int)link->edge != segMax)
+				continue;
+			
+			// Get pointer to the next polygon.
+			nextTile = 0;
+			nextPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
+			
+			// Skip off-mesh connections.
+			if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			
+			// Skip links based on filter.
+			if (!filter->passFilter(link->ref, nextTile, nextPoly))
+				continue;
+			
+			// If the link is internal, just return the ref.
+			if (link->side == 0xff)
+			{
+				nextRef = link->ref;
+				break;
+			}
+			
+			// If the link is at tile boundary,
+			
+			// Check if the link spans the whole edge, and accept.
+			if (link->bmin == 0 && link->bmax == 255)
+			{
+				nextRef = link->ref;
+				break;
+			}
+			
+			// Check for partial edge links.
+			const int v0 = poly->verts[link->edge];
+			const int v1 = poly->verts[(link->edge+1) % poly->vertCount];
+			const float* left = &tile->verts[v0*3];
+			const float* right = &tile->verts[v1*3];
+			
+			// Check that the intersection lies inside the link portal.
+			if (link->side == 0 || link->side == 4)
+			{
+				// Calculate link size.
+				const float s = 1.0f/255.0f;
+				float lmin = left[2] + (right[2] - left[2])*(link->bmin*s);
+				float lmax = left[2] + (right[2] - left[2])*(link->bmax*s);
+				if (lmin > lmax) dtSwap(lmin, lmax);
+				
+				// Find Z intersection.
+				float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
+				if (z >= lmin && z <= lmax)
+				{
+					nextRef = link->ref;
+					break;
+				}
+			}
+			else if (link->side == 2 || link->side == 6)
+			{
+				// Calculate link size.
+				const float s = 1.0f/255.0f;
+				float lmin = left[0] + (right[0] - left[0])*(link->bmin*s);
+				float lmax = left[0] + (right[0] - left[0])*(link->bmax*s);
+				if (lmin > lmax) dtSwap(lmin, lmax);
+				
+				// Find X intersection.
+				float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
+				if (x >= lmin && x <= lmax)
+				{
+					nextRef = link->ref;
+					break;
+				}
+			}
+		}
+		
+		// add the cost
+		if (options & DT_RAYCAST_USE_COSTS)
+		{
+			// compute the intersection point at the furthest end of the polygon
+			// and correct the height (since the raycast moves in 2d)
+			dtVcopy(lastPos, curPos);
+			dtVmad(curPos, startPos, dir, hit->t);
+			float* e1 = &verts[segMax*3];
+			float* e2 = &verts[((segMax+1)%nv)*3];
+			float eDir[3], diff[3];
+			dtVsub(eDir, e2, e1);
+			dtVsub(diff, curPos, e1);
+			float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
+			curPos[1] = e1[1] + eDir[1] * s;
+
+			hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
+		}
+
+		if (!nextRef)
+		{
+			// No neighbour, we hit a wall.
+			
+			// Calculate hit normal.
+			const int a = segMax;
+			const int b = segMax+1 < nv ? segMax+1 : 0;
+			const float* va = &verts[a*3];
+			const float* vb = &verts[b*3];
+			const float dx = vb[0] - va[0];
+			const float dz = vb[2] - va[2];
+			hit->hitNormal[0] = dz;
+			hit->hitNormal[1] = 0;
+			hit->hitNormal[2] = -dx;
+			dtVnormalize(hit->hitNormal);
+			
+			hit->pathCount = n;
+			return status;
+		}
+
+		// No hit, advance to neighbour polygon.
+		prevRef = curRef;
+		curRef = nextRef;
+		prevTile = tile;
+		tile = nextTile;
+		prevPoly = poly;
+		poly = nextPoly;
+	}
+	
+	hit->pathCount = n;
+	
+	return status;
+}
+
+/// @par
+///
+/// At least one result array must be provided.
+///
+/// The order of the result set is from least to highest cost to reach the polygon.
+///
+/// A common use case for this method is to perform Dijkstra searches. 
+/// Candidate polygons are found by searching the graph beginning at the start polygon.
+///
+/// If a polygon is not found via the graph search, even if it intersects the 
+/// search circle, it will not be included in the result set. For example:
+///
+/// polyA is the start polygon.
+/// polyB shares an edge with polyA. (Is adjacent.)
+/// polyC shares an edge with polyB, but not with polyA
+/// Even if the search circle overlaps polyC, it will not be included in the 
+/// result set unless polyB is also in the set.
+/// 
+/// The value of the center point is used as the start position for cost 
+/// calculations. It is not projected onto the surface of the mesh, so its 
+/// y-value will effect the costs.
+///
+/// Intersection tests occur in 2D. All polygons and the search circle are 
+/// projected onto the xz-plane. So the y-value of the center point does not 
+/// effect intersection tests.
+///
+/// If the result arrays are to small to hold the entire result set, they will be 
+/// filled to capacity.
+/// 
+dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
+											   const dtQueryFilter* filter,
+											   dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
+											   int* resultCount, const int maxResult) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+
+	if (!resultCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*resultCount = 0;
+
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!centerPos || !dtVisfinite(centerPos) ||
+		radius < 0 || !dtMathIsfinite(radius) ||
+		!filter || maxResult < 0)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, centerPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	dtStatus status = DT_SUCCESS;
+	
+	int n = 0;
+	
+	const float radiusSqr = dtSqr(radius);
+	
+	while (!m_openList->empty())
+	{
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+		
+		// Get parent poly and tile.
+		dtPolyRef parentRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		if (bestNode->pidx)
+			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		if (parentRef)
+			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+
+		if (n < maxResult)
+		{
+			if (resultRef)
+				resultRef[n] = bestRef;
+			if (resultParent)
+				resultParent[n] = parentRef;
+			if (resultCost)
+				resultCost[n] = bestNode->total;
+			++n;
+		}
+		else
+		{
+			status |= DT_BUFFER_TOO_SMALL;
+		}
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			const dtLink* link = &bestTile->links[i];
+			dtPolyRef neighbourRef = link->ref;
+			// Skip invalid neighbours and do not follow back to parent.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Expand to neighbour
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+		
+			// Do not advance if the polygon is excluded by the filter.
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+			
+			// Find edge and calc distance to the edge.
+			float va[3], vb[3];
+			if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
+				continue;
+			
+			// If the circle is not touching the next polygon, skip it.
+			float tseg;
+			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
+			if (distSqr > radiusSqr)
+				continue;
+			
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
+			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
+				continue;
+			}
+				
+			if (neighbourNode->flags & DT_NODE_CLOSED)
+				continue;
+			
+			// Cost
+			if (neighbourNode->flags == 0)
+				dtVlerp(neighbourNode->pos, va, vb, 0.5f);
+			
+			float cost = filter->getCost(
+				bestNode->pos, neighbourNode->pos,
+				parentRef, parentTile, parentPoly,
+				bestRef, bestTile, bestPoly,
+				neighbourRef, neighbourTile, neighbourPoly);
+
+			const float total = bestNode->total + cost;
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			
+			neighbourNode->id = neighbourRef;
+			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->total = total;
+			
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				neighbourNode->flags = DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+		}
+	}
+	
+	*resultCount = n;
+	
+	return status;
+}
+
+/// @par
+///
+/// The order of the result set is from least to highest cost.
+/// 
+/// At least one result array must be provided.
+///
+/// A common use case for this method is to perform Dijkstra searches. 
+/// Candidate polygons are found by searching the graph beginning at the start 
+/// polygon.
+/// 
+/// The same intersection test restrictions that apply to findPolysAroundCircle()
+/// method apply to this method.
+/// 
+/// The 3D centroid of the search polygon is used as the start position for cost 
+/// calculations.
+/// 
+/// Intersection tests occur in 2D. All polygons are projected onto the 
+/// xz-plane. So the y-values of the vertices do not effect intersection tests.
+/// 
+/// If the result arrays are is too small to hold the entire result set, they will 
+/// be filled to capacity.
+///
+dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
+											  const dtQueryFilter* filter,
+											  dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
+											  int* resultCount, const int maxResult) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+
+	if (!resultCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*resultCount = 0;
+
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!verts || nverts < 3 ||
+		!filter || maxResult < 0)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	// Validate input
+	if (!startRef || !m_nav->isValidPolyRef(startRef))
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	float centerPos[3] = {0,0,0};
+	for (int i = 0; i < nverts; ++i)
+		dtVadd(centerPos,centerPos,&verts[i*3]);
+	dtVscale(centerPos,centerPos,1.0f/nverts);
+
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, centerPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	dtStatus status = DT_SUCCESS;
+
+	int n = 0;
+	
+	while (!m_openList->empty())
+	{
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+		
+		// Get parent poly and tile.
+		dtPolyRef parentRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		if (bestNode->pidx)
+			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		if (parentRef)
+			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+
+		if (n < maxResult)
+		{
+			if (resultRef)
+				resultRef[n] = bestRef;
+			if (resultParent)
+				resultParent[n] = parentRef;
+			if (resultCost)
+				resultCost[n] = bestNode->total;
+
+			++n;
+		}
+		else
+		{
+			status |= DT_BUFFER_TOO_SMALL;
+		}
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			const dtLink* link = &bestTile->links[i];
+			dtPolyRef neighbourRef = link->ref;
+			// Skip invalid neighbours and do not follow back to parent.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Expand to neighbour
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+			
+			// Do not advance if the polygon is excluded by the filter.
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+			
+			// Find edge and calc distance to the edge.
+			float va[3], vb[3];
+			if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
+				continue;
+			
+			// If the poly is not touching the edge to the next polygon, skip the connection it.
+			float tmin, tmax;
+			int segMin, segMax;
+			if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax))
+				continue;
+			if (tmin > 1.0f || tmax < 0.0f)
+				continue;
+			
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
+			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
+				continue;
+			}
+			
+			if (neighbourNode->flags & DT_NODE_CLOSED)
+				continue;
+			
+			// Cost
+			if (neighbourNode->flags == 0)
+				dtVlerp(neighbourNode->pos, va, vb, 0.5f);
+			
+			float cost = filter->getCost(
+				bestNode->pos, neighbourNode->pos,
+				parentRef, parentTile, parentPoly,
+				bestRef, bestTile, bestPoly,
+				neighbourRef, neighbourTile, neighbourPoly);
+
+			const float total = bestNode->total + cost;
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			
+			neighbourNode->id = neighbourRef;
+			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->total = total;
+			
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				neighbourNode->flags = DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+		}
+	}
+	
+	*resultCount = n;
+	
+	return status;
+}
+
+dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const
+{
+	if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*pathCount = 0;
+
+	dtNode* endNode;
+	if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 ||
+		(endNode->flags & DT_NODE_CLOSED) == 0)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	return getPathToNode(endNode, path, pathCount, maxPath);
+}
+
+/// @par
+///
+/// This method is optimized for a small search radius and small number of result 
+/// polygons.
+///
+/// Candidate polygons are found by searching the navigation graph beginning at 
+/// the start polygon.
+///
+/// The same intersection test restrictions that apply to the findPolysAroundCircle 
+/// mehtod applies to this method.
+///
+/// The value of the center point is used as the start point for cost calculations. 
+/// It is not projected onto the surface of the mesh, so its y-value will effect 
+/// the costs.
+/// 
+/// Intersection tests occur in 2D. All polygons and the search circle are 
+/// projected onto the xz-plane. So the y-value of the center point does not 
+/// effect intersection tests.
+/// 
+/// If the result arrays are is too small to hold the entire result set, they will 
+/// be filled to capacity.
+/// 
+dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
+												const dtQueryFilter* filter,
+												dtPolyRef* resultRef, dtPolyRef* resultParent,
+												int* resultCount, const int maxResult) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_tinyNodePool);
+
+	if (!resultCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*resultCount = 0;
+
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!centerPos || !dtVisfinite(centerPos) ||
+		radius < 0 || !dtMathIsfinite(radius) ||
+		!filter || maxResult < 0)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+
+	static const int MAX_STACK = 48;
+	dtNode* stack[MAX_STACK];
+	int nstack = 0;
+	
+	m_tinyNodePool->clear();
+	
+	dtNode* startNode = m_tinyNodePool->getNode(startRef);
+	startNode->pidx = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_CLOSED;
+	stack[nstack++] = startNode;
+	
+	const float radiusSqr = dtSqr(radius);
+	
+	float pa[DT_VERTS_PER_POLYGON*3];
+	float pb[DT_VERTS_PER_POLYGON*3];
+	
+	dtStatus status = DT_SUCCESS;
+	
+	int n = 0;
+	if (n < maxResult)
+	{
+		resultRef[n] = startNode->id;
+		if (resultParent)
+			resultParent[n] = 0;
+		++n;
+	}
+	else
+	{
+		status |= DT_BUFFER_TOO_SMALL;
+	}
+	
+	while (nstack)
+	{
+		// Pop front.
+		dtNode* curNode = stack[0];
+		for (int i = 0; i < nstack-1; ++i)
+			stack[i] = stack[i+1];
+		nstack--;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef curRef = curNode->id;
+		const dtMeshTile* curTile = 0;
+		const dtPoly* curPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
+		
+		for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
+		{
+			const dtLink* link = &curTile->links[i];
+			dtPolyRef neighbourRef = link->ref;
+			// Skip invalid neighbours.
+			if (!neighbourRef)
+				continue;
+			
+			// Skip if cannot alloca more nodes.
+			dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
+			if (!neighbourNode)
+				continue;
+			// Skip visited.
+			if (neighbourNode->flags & DT_NODE_CLOSED)
+				continue;
+			
+			// Expand to neighbour
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+			
+			// Skip off-mesh connections.
+			if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			
+			// Do not advance if the polygon is excluded by the filter.
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+			
+			// Find edge and calc distance to the edge.
+			float va[3], vb[3];
+			if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
+				continue;
+			
+			// If the circle is not touching the next polygon, skip it.
+			float tseg;
+			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
+			if (distSqr > radiusSqr)
+				continue;
+			
+			// Mark node visited, this is done before the overlap test so that
+			// we will not visit the poly again if the test fails.
+			neighbourNode->flags |= DT_NODE_CLOSED;
+			neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
+			
+			// Check that the polygon does not collide with existing polygons.
+			
+			// Collect vertices of the neighbour poly.
+			const int npa = neighbourPoly->vertCount;
+			for (int k = 0; k < npa; ++k)
+				dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]);
+			
+			bool overlap = false;
+			for (int j = 0; j < n; ++j)
+			{
+				dtPolyRef pastRef = resultRef[j];
+				
+				// Connected polys do not overlap.
+				bool connected = false;
+				for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
+				{
+					if (curTile->links[k].ref == pastRef)
+					{
+						connected = true;
+						break;
+					}
+				}
+				if (connected)
+					continue;
+				
+				// Potentially overlapping.
+				const dtMeshTile* pastTile = 0;
+				const dtPoly* pastPoly = 0;
+				m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);
+				
+				// Get vertices and test overlap
+				const int npb = pastPoly->vertCount;
+				for (int k = 0; k < npb; ++k)
+					dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]);
+				
+				if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
+				{
+					overlap = true;
+					break;
+				}
+			}
+			if (overlap)
+				continue;
+			
+			// This poly is fine, store and advance to the poly.
+			if (n < maxResult)
+			{
+				resultRef[n] = neighbourRef;
+				if (resultParent)
+					resultParent[n] = curRef;
+				++n;
+			}
+			else
+			{
+				status |= DT_BUFFER_TOO_SMALL;
+			}
+			
+			if (nstack < MAX_STACK)
+			{
+				stack[nstack++] = neighbourNode;
+			}
+		}
+	}
+	
+	*resultCount = n;
+	
+	return status;
+}
+
+
+struct dtSegInterval
+{
+	dtPolyRef ref;
+	short tmin, tmax;
+};
+
+static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
+						   const short tmin, const short tmax, const dtPolyRef ref)
+{
+	if (nints+1 > maxInts) return;
+	// Find insertion point.
+	int idx = 0;
+	while (idx < nints)
+	{
+		if (tmax <= ints[idx].tmin)
+			break;
+		idx++;
+	}
+	// Move current results.
+	if (nints-idx)
+		memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
+	// Store
+	ints[idx].ref = ref;
+	ints[idx].tmin = tmin;
+	ints[idx].tmax = tmax;
+	nints++;
+}
+
+/// @par
+///
+/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned. 
+/// Otherwise only the wall segments are returned.
+/// 
+/// A segment that is normally a portal will be included in the result set as a 
+/// wall if the @p filter results in the neighbor polygon becoomming impassable.
+/// 
+/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the 
+/// maximum segments per polygon of the source navigation mesh.
+/// 
+dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
+											 float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
+											 const int maxSegments) const
+{
+	dtAssert(m_nav);
+
+	if (!segmentCount)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	*segmentCount = 0;
+
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	if (!filter || !segmentVerts || maxSegments < 0)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	int n = 0;
+	static const int MAX_INTERVAL = 16;
+	dtSegInterval ints[MAX_INTERVAL];
+	int nints;
+	
+	const bool storePortals = segmentRefs != 0;
+	
+	dtStatus status = DT_SUCCESS;
+	
+	for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
+	{
+		// Skip non-solid edges.
+		nints = 0;
+		if (poly->neis[j] & DT_EXT_LINK)
+		{
+			// Tile border.
+			for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
+			{
+				const dtLink* link = &tile->links[k];
+				if (link->edge == j)
+				{
+					if (link->ref != 0)
+					{
+						const dtMeshTile* neiTile = 0;
+						const dtPoly* neiPoly = 0;
+						m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
+						if (filter->passFilter(link->ref, neiTile, neiPoly))
+						{
+							insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref);
+						}
+					}
+				}
+			}
+		}
+		else
+		{
+			// Internal edge
+			dtPolyRef neiRef = 0;
+			if (poly->neis[j])
+			{
+				const unsigned int idx = (unsigned int)(poly->neis[j]-1);
+				neiRef = m_nav->getPolyRefBase(tile) | idx;
+				if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
+					neiRef = 0;
+			}
+
+			// If the edge leads to another polygon and portals are not stored, skip.
+			if (neiRef != 0 && !storePortals)
+				continue;
+			
+			if (n < maxSegments)
+			{
+				const float* vj = &tile->verts[poly->verts[j]*3];
+				const float* vi = &tile->verts[poly->verts[i]*3];
+				float* seg = &segmentVerts[n*6];
+				dtVcopy(seg+0, vj);
+				dtVcopy(seg+3, vi);
+				if (segmentRefs)
+					segmentRefs[n] = neiRef;
+				n++;
+			}
+			else
+			{
+				status |= DT_BUFFER_TOO_SMALL;
+			}
+			
+			continue;
+		}
+		
+		// Add sentinels
+		insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
+		insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
+		
+		// Store segments.
+		const float* vj = &tile->verts[poly->verts[j]*3];
+		const float* vi = &tile->verts[poly->verts[i]*3];
+		for (int k = 1; k < nints; ++k)
+		{
+			// Portal segment.
+			if (storePortals && ints[k].ref)
+			{
+				const float tmin = ints[k].tmin/255.0f; 
+				const float tmax = ints[k].tmax/255.0f; 
+				if (n < maxSegments)
+				{
+					float* seg = &segmentVerts[n*6];
+					dtVlerp(seg+0, vj,vi, tmin);
+					dtVlerp(seg+3, vj,vi, tmax);
+					if (segmentRefs)
+						segmentRefs[n] = ints[k].ref;
+					n++;
+				}
+				else
+				{
+					status |= DT_BUFFER_TOO_SMALL;
+				}
+			}
+
+			// Wall segment.
+			const int imin = ints[k-1].tmax;
+			const int imax = ints[k].tmin;
+			if (imin != imax)
+			{
+				const float tmin = imin/255.0f; 
+				const float tmax = imax/255.0f; 
+				if (n < maxSegments)
+				{
+					float* seg = &segmentVerts[n*6];
+					dtVlerp(seg+0, vj,vi, tmin);
+					dtVlerp(seg+3, vj,vi, tmax);
+					if (segmentRefs)
+						segmentRefs[n] = 0;
+					n++;
+				}
+				else
+				{
+					status |= DT_BUFFER_TOO_SMALL;
+				}
+			}
+		}
+	}
+	
+	*segmentCount = n;
+	
+	return status;
+}
+
+/// @par
+///
+/// @p hitPos is not adjusted using the height detail data.
+///
+/// @p hitDist will equal the search radius if there is no wall within the 
+/// radius. In this case the values of @p hitPos and @p hitNormal are
+/// undefined.
+///
+/// The normal will become unpredicable if @p hitDist is a very small number.
+///
+dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
+											const dtQueryFilter* filter,
+											float* hitDist, float* hitPos, float* hitNormal) const
+{
+	dtAssert(m_nav);
+	dtAssert(m_nodePool);
+	dtAssert(m_openList);
+	
+	// Validate input
+	if (!m_nav->isValidPolyRef(startRef) ||
+		!centerPos || !dtVisfinite(centerPos) ||
+		maxRadius < 0 || !dtMathIsfinite(maxRadius) ||
+		!filter || !hitDist || !hitPos || !hitNormal)
+	{
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+	
+	m_nodePool->clear();
+	m_openList->clear();
+	
+	dtNode* startNode = m_nodePool->getNode(startRef);
+	dtVcopy(startNode->pos, centerPos);
+	startNode->pidx = 0;
+	startNode->cost = 0;
+	startNode->total = 0;
+	startNode->id = startRef;
+	startNode->flags = DT_NODE_OPEN;
+	m_openList->push(startNode);
+	
+	float radiusSqr = dtSqr(maxRadius);
+	
+	dtStatus status = DT_SUCCESS;
+	
+	while (!m_openList->empty())
+	{
+		dtNode* bestNode = m_openList->pop();
+		bestNode->flags &= ~DT_NODE_OPEN;
+		bestNode->flags |= DT_NODE_CLOSED;
+		
+		// Get poly and tile.
+		// The API input has been cheked already, skip checking internal data.
+		const dtPolyRef bestRef = bestNode->id;
+		const dtMeshTile* bestTile = 0;
+		const dtPoly* bestPoly = 0;
+		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+		
+		// Get parent poly and tile.
+		dtPolyRef parentRef = 0;
+		const dtMeshTile* parentTile = 0;
+		const dtPoly* parentPoly = 0;
+		if (bestNode->pidx)
+			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		if (parentRef)
+			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+		
+		// Hit test walls.
+		for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++)
+		{
+			// Skip non-solid edges.
+			if (bestPoly->neis[j] & DT_EXT_LINK)
+			{
+				// Tile border.
+				bool solid = true;
+				for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next)
+				{
+					const dtLink* link = &bestTile->links[k];
+					if (link->edge == j)
+					{
+						if (link->ref != 0)
+						{
+							const dtMeshTile* neiTile = 0;
+							const dtPoly* neiPoly = 0;
+							m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
+							if (filter->passFilter(link->ref, neiTile, neiPoly))
+								solid = false;
+						}
+						break;
+					}
+				}
+				if (!solid) continue;
+			}
+			else if (bestPoly->neis[j])
+			{
+				// Internal edge
+				const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1);
+				const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx;
+				if (filter->passFilter(ref, bestTile, &bestTile->polys[idx]))
+					continue;
+			}
+			
+			// Calc distance to the edge.
+			const float* vj = &bestTile->verts[bestPoly->verts[j]*3];
+			const float* vi = &bestTile->verts[bestPoly->verts[i]*3];
+			float tseg;
+			float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);
+			
+			// Edge is too far, skip.
+			if (distSqr > radiusSqr)
+				continue;
+			
+			// Hit wall, update radius.
+			radiusSqr = distSqr;
+			// Calculate hit pos.
+			hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
+			hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
+			hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
+		}
+		
+		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+		{
+			const dtLink* link = &bestTile->links[i];
+			dtPolyRef neighbourRef = link->ref;
+			// Skip invalid neighbours and do not follow back to parent.
+			if (!neighbourRef || neighbourRef == parentRef)
+				continue;
+			
+			// Expand to neighbour.
+			const dtMeshTile* neighbourTile = 0;
+			const dtPoly* neighbourPoly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+			
+			// Skip off-mesh connections.
+			if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+				continue;
+			
+			// Calc distance to the edge.
+			const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3];
+			const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3];
+			float tseg;
+			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
+			
+			// If the circle is not touching the next polygon, skip it.
+			if (distSqr > radiusSqr)
+				continue;
+			
+			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+				continue;
+
+			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
+			if (!neighbourNode)
+			{
+				status |= DT_OUT_OF_NODES;
+				continue;
+			}
+			
+			if (neighbourNode->flags & DT_NODE_CLOSED)
+				continue;
+			
+			// Cost
+			if (neighbourNode->flags == 0)
+			{
+				getEdgeMidPoint(bestRef, bestPoly, bestTile,
+								neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
+			}
+			
+			const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
+			
+			// The node is already in open list and the new result is worse, skip.
+			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+				continue;
+			
+			neighbourNode->id = neighbourRef;
+			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->total = total;
+				
+			if (neighbourNode->flags & DT_NODE_OPEN)
+			{
+				m_openList->modify(neighbourNode);
+			}
+			else
+			{
+				neighbourNode->flags |= DT_NODE_OPEN;
+				m_openList->push(neighbourNode);
+			}
+		}
+	}
+	
+	// Calc hit normal.
+	dtVsub(hitNormal, centerPos, hitPos);
+	dtVnormalize(hitNormal);
+	
+	*hitDist = dtMathSqrtf(radiusSqr);
+	
+	return status;
+}
+
+bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const
+{
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly);
+	// If cannot get polygon, assume it does not exists and boundary is invalid.
+	if (dtStatusFailed(status))
+		return false;
+	// If cannot pass filter, assume flags has changed and boundary is invalid.
+	if (!filter->passFilter(ref, tile, poly))
+		return false;
+	return true;
+}
+
+/// @par
+///
+/// The closed list is the list of polygons that were fully evaluated during 
+/// the last navigation graph search. (A* or Dijkstra)
+/// 
+bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
+{
+	if (!m_nodePool) return false;
+	
+	dtNode* nodes[DT_MAX_STATES_PER_NODE];
+	int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE);
+
+	for (int i=0; i<n; i++)
+	{
+		if (nodes[i]->flags & DT_NODE_CLOSED)
+			return true;
+	}		
+
+	return false;
+}

panda/src/recastdetour/DetourNavMeshQuery.h

@@ -0,0 +1,573 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURNAVMESHQUERY_H
+#define DETOURNAVMESHQUERY_H
+
+#include "DetourNavMesh.h"
+#include "DetourStatus.h"
+
+
+// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter.
+// On certain platforms indirect or virtual function call is expensive. The default
+// setting is to use non-virtual functions, the actual implementations of the functions
+// are declared as inline for maximum speed. 
+
+//#define DT_VIRTUAL_QUERYFILTER 1
+
+/// Defines polygon filtering and traversal costs for navigation mesh query operations.
+/// @ingroup detour
+class dtQueryFilter
+{
+	float m_areaCost[DT_MAX_AREAS];		///< Cost per area type. (Used by default implementation.)
+	unsigned short m_includeFlags;		///< Flags for polygons that can be visited. (Used by default implementation.)
+	unsigned short m_excludeFlags;		///< Flags for polygons that should not be visted. (Used by default implementation.)
+	
+public:
+	dtQueryFilter();
+	
+#ifdef DT_VIRTUAL_QUERYFILTER
+	virtual ~dtQueryFilter() { }
+#endif
+	
+	/// Returns true if the polygon can be visited.  (I.e. Is traversable.)
+	///  @param[in]		ref		The reference id of the polygon test.
+	///  @param[in]		tile	The tile containing the polygon.
+	///  @param[in]		poly  The polygon to test.
+#ifdef DT_VIRTUAL_QUERYFILTER
+	virtual bool passFilter(const dtPolyRef ref,
+							const dtMeshTile* tile,
+							const dtPoly* poly) const;
+#else
+	bool passFilter(const dtPolyRef ref,
+					const dtMeshTile* tile,
+					const dtPoly* poly) const;
+#endif
+
+	/// Returns cost to move from the beginning to the end of a line segment
+	/// that is fully contained within a polygon.
+	///  @param[in]		pa			The start position on the edge of the previous and current polygon. [(x, y, z)]
+	///  @param[in]		pb			The end position on the edge of the current and next polygon. [(x, y, z)]
+	///  @param[in]		prevRef		The reference id of the previous polygon. [opt]
+	///  @param[in]		prevTile	The tile containing the previous polygon. [opt]
+	///  @param[in]		prevPoly	The previous polygon. [opt]
+	///  @param[in]		curRef		The reference id of the current polygon.
+	///  @param[in]		curTile		The tile containing the current polygon.
+	///  @param[in]		curPoly		The current polygon.
+	///  @param[in]		nextRef		The refernece id of the next polygon. [opt]
+	///  @param[in]		nextTile	The tile containing the next polygon. [opt]
+	///  @param[in]		nextPoly	The next polygon. [opt]
+#ifdef DT_VIRTUAL_QUERYFILTER
+	virtual float getCost(const float* pa, const float* pb,
+						  const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
+						  const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
+						  const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const;
+#else
+	float getCost(const float* pa, const float* pb,
+				  const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
+				  const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
+				  const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const;
+#endif
+
+	/// @name Getters and setters for the default implementation data.
+	///@{
+
+	/// Returns the traversal cost of the area.
+	///  @param[in]		i		The id of the area.
+	/// @returns The traversal cost of the area.
+	inline float getAreaCost(const int i) const { return m_areaCost[i]; }
+
+	/// Sets the traversal cost of the area.
+	///  @param[in]		i		The id of the area.
+	///  @param[in]		cost	The new cost of traversing the area.
+	inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; } 
+
+	/// Returns the include flags for the filter.
+	/// Any polygons that include one or more of these flags will be
+	/// included in the operation.
+	inline unsigned short getIncludeFlags() const { return m_includeFlags; }
+
+	/// Sets the include flags for the filter.
+	/// @param[in]		flags	The new flags.
+	inline void setIncludeFlags(const unsigned short flags) { m_includeFlags = flags; }
+
+	/// Returns the exclude flags for the filter.
+	/// Any polygons that include one ore more of these flags will be
+	/// excluded from the operation.
+	inline unsigned short getExcludeFlags() const { return m_excludeFlags; }
+
+	/// Sets the exclude flags for the filter.
+	/// @param[in]		flags		The new flags.
+	inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; }	
+
+	///@}
+
+};
+
+/// Provides information about raycast hit
+/// filled by dtNavMeshQuery::raycast
+/// @ingroup detour
+struct dtRaycastHit
+{
+	/// The hit parameter. (FLT_MAX if no wall hit.)
+	float t; 
+	
+	/// hitNormal	The normal of the nearest wall hit. [(x, y, z)]
+	float hitNormal[3];
+
+	/// The index of the edge on the final polygon where the wall was hit.
+	int hitEdgeIndex;
+	
+	/// Pointer to an array of reference ids of the visited polygons. [opt]
+	dtPolyRef* path;
+	
+	/// The number of visited polygons. [opt]
+	int pathCount;
+
+	/// The maximum number of polygons the @p path array can hold.
+	int maxPath;
+
+	///  The cost of the path until hit.
+	float pathCost;
+};
+
+/// Provides custom polygon query behavior.
+/// Used by dtNavMeshQuery::queryPolygons.
+/// @ingroup detour
+class dtPolyQuery
+{
+public:
+	virtual ~dtPolyQuery() { }
+
+	/// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons.
+	/// This can be called multiple times for a single query.
+	virtual void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) = 0;
+};
+
+/// Provides the ability to perform pathfinding related queries against
+/// a navigation mesh.
+/// @ingroup detour
+class dtNavMeshQuery
+{
+public:
+	dtNavMeshQuery();
+	~dtNavMeshQuery();
+	
+	/// Initializes the query object.
+	///  @param[in]		nav			Pointer to the dtNavMesh object to use for all queries.
+	///  @param[in]		maxNodes	Maximum number of search nodes. [Limits: 0 < value <= 65535]
+	/// @returns The status flags for the query.
+	dtStatus init(const dtNavMesh* nav, const int maxNodes);
+	
+	/// @name Standard Pathfinding Functions
+	// /@{
+
+	/// Finds a path from the start polygon to the end polygon.
+	///  @param[in]		startRef	The refrence id of the start polygon.
+	///  @param[in]		endRef		The reference id of the end polygon.
+	///  @param[in]		startPos	A position within the start polygon. [(x, y, z)]
+	///  @param[in]		endPos		A position within the end polygon. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	path		An ordered list of polygon references representing the path. (Start to end.) 
+	///  							[(polyRef) * @p pathCount]
+	///  @param[out]	pathCount	The number of polygons returned in the @p path array.
+	///  @param[in]		maxPath		The maximum number of polygons the @p path array can hold. [Limit: >= 1]
+	dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef,
+					  const float* startPos, const float* endPos,
+					  const dtQueryFilter* filter,
+					  dtPolyRef* path, int* pathCount, const int maxPath) const;
+
+	/// Finds the straight path from the start to the end position within the polygon corridor.
+	///  @param[in]		startPos			Path start position. [(x, y, z)]
+	///  @param[in]		endPos				Path end position. [(x, y, z)]
+	///  @param[in]		path				An array of polygon references that represent the path corridor.
+	///  @param[in]		pathSize			The number of polygons in the @p path array.
+	///  @param[out]	straightPath		Points describing the straight path. [(x, y, z) * @p straightPathCount].
+	///  @param[out]	straightPathFlags	Flags describing each point. (See: #dtStraightPathFlags) [opt]
+	///  @param[out]	straightPathRefs	The reference id of the polygon that is being entered at each point. [opt]
+	///  @param[out]	straightPathCount	The number of points in the straight path.
+	///  @param[in]		maxStraightPath		The maximum number of points the straight path arrays can hold.  [Limit: > 0]
+	///  @param[in]		options				Query options. (see: #dtStraightPathOptions)
+	/// @returns The status flags for the query.
+	dtStatus findStraightPath(const float* startPos, const float* endPos,
+							  const dtPolyRef* path, const int pathSize,
+							  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+							  int* straightPathCount, const int maxStraightPath, const int options = 0) const;
+
+	///@}
+	/// @name Sliced Pathfinding Functions
+	/// Common use case:
+	///	-# Call initSlicedFindPath() to initialize the sliced path query.
+	///	-# Call updateSlicedFindPath() until it returns complete.
+	///	-# Call finalizeSlicedFindPath() to get the path.
+	///@{ 
+
+	/// Intializes a sliced path query.
+	///  @param[in]		startRef	The refrence id of the start polygon.
+	///  @param[in]		endRef		The reference id of the end polygon.
+	///  @param[in]		startPos	A position within the start polygon. [(x, y, z)]
+	///  @param[in]		endPos		A position within the end polygon. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[in]		options		query options (see: #dtFindPathOptions)
+	/// @returns The status flags for the query.
+	dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
+								const float* startPos, const float* endPos,
+								const dtQueryFilter* filter, const unsigned int options = 0);
+
+	/// Updates an in-progress sliced path query.
+	///  @param[in]		maxIter		The maximum number of iterations to perform.
+	///  @param[out]	doneIters	The actual number of iterations completed. [opt]
+	/// @returns The status flags for the query.
+	dtStatus updateSlicedFindPath(const int maxIter, int* doneIters);
+
+	/// Finalizes and returns the results of a sliced path query.
+	///  @param[out]	path		An ordered list of polygon references representing the path. (Start to end.) 
+	///  							[(polyRef) * @p pathCount]
+	///  @param[out]	pathCount	The number of polygons returned in the @p path array.
+	///  @param[in]		maxPath		The max number of polygons the path array can hold. [Limit: >= 1]
+	/// @returns The status flags for the query.
+	dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath);
+	
+	/// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest
+	/// polygon on the existing path that was visited during the search.
+	///  @param[in]		existing		An array of polygon references for the existing path.
+	///  @param[in]		existingSize	The number of polygon in the @p existing array.
+	///  @param[out]	path			An ordered list of polygon references representing the path. (Start to end.) 
+	///  								[(polyRef) * @p pathCount]
+	///  @param[out]	pathCount		The number of polygons returned in the @p path array.
+	///  @param[in]		maxPath			The max number of polygons the @p path array can hold. [Limit: >= 1]
+	/// @returns The status flags for the query.
+	dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
+										   dtPolyRef* path, int* pathCount, const int maxPath);
+
+	///@}
+	/// @name Dijkstra Search Functions
+	/// @{ 
+
+	/// Finds the polygons along the navigation graph that touch the specified circle.
+	///  @param[in]		startRef		The reference id of the polygon where the search starts.
+	///  @param[in]		centerPos		The center of the search circle. [(x, y, z)]
+	///  @param[in]		radius			The radius of the search circle.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	resultRef		The reference ids of the polygons touched by the circle. [opt]
+	///  @param[out]	resultParent	The reference ids of the parent polygons for each result. 
+	///  								Zero if a result polygon has no parent. [opt]
+	///  @param[out]	resultCost		The search cost from @p centerPos to the polygon. [opt]
+	///  @param[out]	resultCount		The number of polygons found. [opt]
+	///  @param[in]		maxResult		The maximum number of polygons the result arrays can hold.
+	/// @returns The status flags for the query.
+	dtStatus findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
+								   const dtQueryFilter* filter,
+								   dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
+								   int* resultCount, const int maxResult) const;
+	
+	/// Finds the polygons along the naviation graph that touch the specified convex polygon.
+	///  @param[in]		startRef		The reference id of the polygon where the search starts.
+	///  @param[in]		verts			The vertices describing the convex polygon. (CCW) 
+	///  								[(x, y, z) * @p nverts]
+	///  @param[in]		nverts			The number of vertices in the polygon.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	resultRef		The reference ids of the polygons touched by the search polygon. [opt]
+	///  @param[out]	resultParent	The reference ids of the parent polygons for each result. Zero if a 
+	///  								result polygon has no parent. [opt]
+	///  @param[out]	resultCost		The search cost from the centroid point to the polygon. [opt]
+	///  @param[out]	resultCount		The number of polygons found.
+	///  @param[in]		maxResult		The maximum number of polygons the result arrays can hold.
+	/// @returns The status flags for the query.
+	dtStatus findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
+								  const dtQueryFilter* filter,
+								  dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
+								  int* resultCount, const int maxResult) const;
+	
+	/// Gets a path from the explored nodes in the previous search.
+	///  @param[in]		endRef		The reference id of the end polygon.
+	///  @param[out]	path		An ordered list of polygon references representing the path. (Start to end.)
+	///  							[(polyRef) * @p pathCount]
+	///  @param[out]	pathCount	The number of polygons returned in the @p path array.
+	///  @param[in]		maxPath		The maximum number of polygons the @p path array can hold. [Limit: >= 0]
+	///  @returns		The status flags. Returns DT_FAILURE | DT_INVALID_PARAM if any parameter is wrong, or if
+	///  				@p endRef was not explored in the previous search. Returns DT_SUCCESS | DT_BUFFER_TOO_SMALL
+	///  				if @p path cannot contain the entire path. In this case it is filled to capacity with a partial path.
+	///  				Otherwise returns DT_SUCCESS.
+	///  @remarks		The result of this function depends on the state of the query object. For that reason it should only
+	///  				be used immediately after one of the two Dijkstra searches, findPolysAroundCircle or findPolysAroundShape.
+	dtStatus getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const;
+
+	/// @}
+	/// @name Local Query Functions
+	///@{
+
+	/// Finds the polygon nearest to the specified center point.
+	///  @param[in]		center		The center of the search box. [(x, y, z)]
+	///  @param[in]		halfExtents		The search distance along each axis. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	nearestRef	The reference id of the nearest polygon.
+	///  @param[out]	nearestPt	The nearest point on the polygon. [opt] [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus findNearestPoly(const float* center, const float* halfExtents,
+							 const dtQueryFilter* filter,
+							 dtPolyRef* nearestRef, float* nearestPt) const;
+	
+	/// Finds polygons that overlap the search box.
+	///  @param[in]		center		The center of the search box. [(x, y, z)]
+	///  @param[in]		halfExtents		The search distance along each axis. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	polys		The reference ids of the polygons that overlap the query box.
+	///  @param[out]	polyCount	The number of polygons in the search result.
+	///  @param[in]		maxPolys	The maximum number of polygons the search result can hold.
+	/// @returns The status flags for the query.
+	dtStatus queryPolygons(const float* center, const float* halfExtents,
+						   const dtQueryFilter* filter,
+						   dtPolyRef* polys, int* polyCount, const int maxPolys) const;
+
+	/// Finds polygons that overlap the search box.
+	///  @param[in]		center		The center of the search box. [(x, y, z)]
+	///  @param[in]		halfExtents		The search distance along each axis. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[in]		query		The query. Polygons found will be batched together and passed to this query.
+	dtStatus queryPolygons(const float* center, const float* halfExtents,
+						   const dtQueryFilter* filter, dtPolyQuery* query) const;
+
+	/// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position.
+	///  @param[in]		startRef		The reference id of the polygon where the search starts.
+	///  @param[in]		centerPos		The center of the query circle. [(x, y, z)]
+	///  @param[in]		radius			The radius of the query circle.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	resultRef		The reference ids of the polygons touched by the circle.
+	///  @param[out]	resultParent	The reference ids of the parent polygons for each result. 
+	///  								Zero if a result polygon has no parent. [opt]
+	///  @param[out]	resultCount		The number of polygons found.
+	///  @param[in]		maxResult		The maximum number of polygons the result arrays can hold.
+	/// @returns The status flags for the query.
+	dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
+									const dtQueryFilter* filter,
+									dtPolyRef* resultRef, dtPolyRef* resultParent,
+									int* resultCount, const int maxResult) const;
+
+	/// Moves from the start to the end position constrained to the navigation mesh.
+	///  @param[in]		startRef		The reference id of the start polygon.
+	///  @param[in]		startPos		A position of the mover within the start polygon. [(x, y, x)]
+	///  @param[in]		endPos			The desired end position of the mover. [(x, y, z)]
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	resultPos		The result position of the mover. [(x, y, z)]
+	///  @param[out]	visited			The reference ids of the polygons visited during the move.
+	///  @param[out]	visitedCount	The number of polygons visited during the move.
+	///  @param[in]		maxVisitedSize	The maximum number of polygons the @p visited array can hold.
+	/// @returns The status flags for the query.
+	dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
+							  const dtQueryFilter* filter,
+							  float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const;
+	
+	/// Casts a 'walkability' ray along the surface of the navigation mesh from 
+	/// the start position toward the end position.
+	/// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility.
+	///  @param[in]		startRef	The reference id of the start polygon.
+	///  @param[in]		startPos	A position within the start polygon representing 
+	///  							the start of the ray. [(x, y, z)]
+	///  @param[in]		endPos		The position to cast the ray toward. [(x, y, z)]
+	///  @param[out]	t			The hit parameter. (FLT_MAX if no wall hit.)
+	///  @param[out]	hitNormal	The normal of the nearest wall hit. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[out]	path		The reference ids of the visited polygons. [opt]
+	///  @param[out]	pathCount	The number of visited polygons. [opt]
+	///  @param[in]		maxPath		The maximum number of polygons the @p path array can hold.
+	/// @returns The status flags for the query.
+	dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+					 const dtQueryFilter* filter,
+					 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const;
+	
+	/// Casts a 'walkability' ray along the surface of the navigation mesh from 
+	/// the start position toward the end position.
+	///  @param[in]		startRef	The reference id of the start polygon.
+	///  @param[in]		startPos	A position within the start polygon representing 
+	///  							the start of the ray. [(x, y, z)]
+	///  @param[in]		endPos		The position to cast the ray toward. [(x, y, z)]
+	///  @param[in]		filter		The polygon filter to apply to the query.
+	///  @param[in]		flags		govern how the raycast behaves. See dtRaycastOptions
+	///  @param[out]	hit			Pointer to a raycast hit structure which will be filled by the results.
+	///  @param[in]		prevRef		parent of start ref. Used during for cost calculation [opt]
+	/// @returns The status flags for the query.
+	dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+					 const dtQueryFilter* filter, const unsigned int options,
+					 dtRaycastHit* hit, dtPolyRef prevRef = 0) const;
+
+
+	/// Finds the distance from the specified position to the nearest polygon wall.
+	///  @param[in]		startRef		The reference id of the polygon containing @p centerPos.
+	///  @param[in]		centerPos		The center of the search circle. [(x, y, z)]
+	///  @param[in]		maxRadius		The radius of the search circle.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	hitDist			The distance to the nearest wall from @p centerPos.
+	///  @param[out]	hitPos			The nearest position on the wall that was hit. [(x, y, z)]
+	///  @param[out]	hitNormal		The normalized ray formed from the wall point to the 
+	///  								source point. [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
+								const dtQueryFilter* filter,
+								float* hitDist, float* hitPos, float* hitNormal) const;
+	
+	/// Returns the segments for the specified polygon, optionally including portals.
+	///  @param[in]		ref				The reference id of the polygon.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[out]	segmentVerts	The segments. [(ax, ay, az, bx, by, bz) * segmentCount]
+	///  @param[out]	segmentRefs		The reference ids of each segment's neighbor polygon. 
+	///  								Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount] 
+	///  @param[out]	segmentCount	The number of segments returned.
+	///  @param[in]		maxSegments		The maximum number of segments the result arrays can hold.
+	/// @returns The status flags for the query.
+	dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
+								 float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
+								 const int maxSegments) const;
+
+	/// Returns random location on navmesh.
+	/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[in]		frand			Function returning a random number [0..1).
+	///  @param[out]	randomRef		The reference id of the random location.
+	///  @param[out]	randomPt		The random location. 
+	/// @returns The status flags for the query.
+	dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
+							 dtPolyRef* randomRef, float* randomPt) const;
+
+	/// Returns random location on navmesh within the reach of specified location.
+	/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
+	/// The location is not exactly constrained by the circle, but it limits the visited polygons.
+	///  @param[in]		startRef		The reference id of the polygon where the search starts.
+	///  @param[in]		centerPos		The center of the search circle. [(x, y, z)]
+	///  @param[in]		filter			The polygon filter to apply to the query.
+	///  @param[in]		frand			Function returning a random number [0..1).
+	///  @param[out]	randomRef		The reference id of the random location.
+	///  @param[out]	randomPt		The random location. [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius,
+										 const dtQueryFilter* filter, float (*frand)(),
+										 dtPolyRef* randomRef, float* randomPt) const;
+	
+	/// Finds the closest point on the specified polygon.
+	///  @param[in]		ref			The reference id of the polygon.
+	///  @param[in]		pos			The position to check. [(x, y, z)]
+	///  @param[out]	closest		The closest point on the polygon. [(x, y, z)]
+	///  @param[out]	posOverPoly	True of the position is over the polygon.
+	/// @returns The status flags for the query.
+	dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
+	
+	/// Returns a point on the boundary closest to the source point if the source point is outside the 
+	/// polygon's xz-bounds.
+	///  @param[in]		ref			The reference id to the polygon.
+	///  @param[in]		pos			The position to check. [(x, y, z)]
+	///  @param[out]	closest		The closest point. [(x, y, z)]
+	/// @returns The status flags for the query.
+	dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const;
+	
+	/// Gets the height of the polygon at the provided position using the height detail. (Most accurate.)
+	///  @param[in]		ref			The reference id of the polygon.
+	///  @param[in]		pos			A position within the xz-bounds of the polygon. [(x, y, z)]
+	///  @param[out]	height		The height at the surface of the polygon.
+	/// @returns The status flags for the query.
+	dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const;
+
+	/// @}
+	/// @name Miscellaneous Functions
+	/// @{
+
+	/// Returns true if the polygon reference is valid and passes the filter restrictions.
+	///  @param[in]		ref			The polygon reference to check.
+	///  @param[in]		filter		The filter to apply.
+	bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const;
+
+	/// Returns true if the polygon reference is in the closed list. 
+	///  @param[in]		ref		The reference id of the polygon to check.
+	/// @returns True if the polygon is in closed list.
+	bool isInClosedList(dtPolyRef ref) const;
+	
+	/// Gets the node pool.
+	/// @returns The node pool.
+	class dtNodePool* getNodePool() const { return m_nodePool; }
+	
+	/// Gets the navigation mesh the query object is using.
+	/// @return The navigation mesh the query object is using.
+	const dtNavMesh* getAttachedNavMesh() const { return m_nav; }
+
+	/// @}
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator
+	dtNavMeshQuery(const dtNavMeshQuery&);
+	dtNavMeshQuery& operator=(const dtNavMeshQuery&);
+	
+	/// Queries polygons within a tile.
+	void queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
+							 const dtQueryFilter* filter, dtPolyQuery* query) const;
+
+	/// Returns portal points between two polygons.
+	dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
+							 unsigned char& fromType, unsigned char& toType) const;
+	dtStatus getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
+							 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
+							 float* left, float* right) const;
+	
+	/// Returns edge mid point between two polygons.
+	dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const;
+	dtStatus getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
+							 dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
+							 float* mid) const;
+	
+	// Appends vertex to a straight path
+	dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
+						  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+						  int* straightPathCount, const int maxStraightPath) const;
+
+	// Appends intermediate portal points to a straight path.
+	dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
+						   float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+						   int* straightPathCount, const int maxStraightPath, const int options) const;
+
+	// Gets the path leading to the specified end node.
+	dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const;
+	
+	const dtNavMesh* m_nav;				///< Pointer to navmesh data.
+
+	struct dtQueryData
+	{
+		dtStatus status;
+		struct dtNode* lastBestNode;
+		float lastBestNodeCost;
+		dtPolyRef startRef, endRef;
+		float startPos[3], endPos[3];
+		const dtQueryFilter* filter;
+		unsigned int options;
+		float raycastLimitSqr;
+	};
+	dtQueryData m_query;				///< Sliced query state.
+
+	class dtNodePool* m_tinyNodePool;	///< Pointer to small node pool.
+	class dtNodePool* m_nodePool;		///< Pointer to node pool.
+	class dtNodeQueue* m_openList;		///< Pointer to open list queue.
+};
+
+/// Allocates a query object using the Detour allocator.
+/// @return An allocated query object, or null on failure.
+/// @ingroup detour
+dtNavMeshQuery* dtAllocNavMeshQuery();
+
+/// Frees the specified query object using the Detour allocator.
+///  @param[in]		query		A query object allocated using #dtAllocNavMeshQuery
+/// @ingroup detour
+void dtFreeNavMeshQuery(dtNavMeshQuery* query);
+
+#endif // DETOURNAVMESHQUERY_H

panda/src/recastdetour/DetourNode.cpp

@@ -0,0 +1,200 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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);
+}

panda/src/recastdetour/DetourNode.h

@@ -0,0 +1,168 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURNODE_H
+#define DETOURNODE_H
+
+#include "DetourNavMesh.h"
+
+enum dtNodeFlags
+{
+	DT_NODE_OPEN = 0x01,
+	DT_NODE_CLOSED = 0x02,
+	DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast.
+};
+
+typedef unsigned short dtNodeIndex;
+static const dtNodeIndex DT_NULL_IDX = (dtNodeIndex)~0;
+
+static const int DT_NODE_PARENT_BITS = 24;
+static const int DT_NODE_STATE_BITS = 2;
+struct dtNode
+{
+	float pos[3];								///< Position of the node.
+	float cost;									///< Cost from previous node to current node.
+	float total;								///< Cost up to the node.
+	unsigned int pidx : DT_NODE_PARENT_BITS;	///< Index to parent node.
+	unsigned int state : DT_NODE_STATE_BITS;	///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE
+	unsigned int flags : 3;						///< Node flags. A combination of dtNodeFlags.
+	dtPolyRef id;								///< Polygon ref the node corresponds to.
+};
+
+static const int DT_MAX_STATES_PER_NODE = 1 << DT_NODE_STATE_BITS;	// number of extra states per node. See dtNode::state
+
+class dtNodePool
+{
+public:
+	dtNodePool(int maxNodes, int hashSize);
+	~dtNodePool();
+	void clear();
+
+	// Get a dtNode by ref and extra state information. If there is none then - allocate
+	// There can be more than one node for the same polyRef but with different extra state information
+	dtNode* getNode(dtPolyRef id, unsigned char state=0);	
+	dtNode* findNode(dtPolyRef id, unsigned char state);
+	unsigned int findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes);
+
+	inline unsigned int getNodeIdx(const dtNode* node) const
+	{
+		if (!node) return 0;
+		return (unsigned int)(node - m_nodes) + 1;
+	}
+
+	inline dtNode* getNodeAtIdx(unsigned int idx)
+	{
+		if (!idx) return 0;
+		return &m_nodes[idx - 1];
+	}
+
+	inline const dtNode* getNodeAtIdx(unsigned int idx) const
+	{
+		if (!idx) return 0;
+		return &m_nodes[idx - 1];
+	}
+	
+	inline int getMemUsed() const
+	{
+		return sizeof(*this) +
+			sizeof(dtNode)*m_maxNodes +
+			sizeof(dtNodeIndex)*m_maxNodes +
+			sizeof(dtNodeIndex)*m_hashSize;
+	}
+	
+	inline int getMaxNodes() const { return m_maxNodes; }
+	
+	inline int getHashSize() const { return m_hashSize; }
+	inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; }
+	inline dtNodeIndex getNext(int i) const { return m_next[i]; }
+	inline int getNodeCount() const { return m_nodeCount; }
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtNodePool(const dtNodePool&);
+	dtNodePool& operator=(const dtNodePool&);
+	
+	dtNode* m_nodes;
+	dtNodeIndex* m_first;
+	dtNodeIndex* m_next;
+	const int m_maxNodes;
+	const int m_hashSize;
+	int m_nodeCount;
+};
+
+class dtNodeQueue
+{
+public:
+	dtNodeQueue(int n);
+	~dtNodeQueue();
+	
+	inline void clear() { m_size = 0; }
+	
+	inline dtNode* top() { return m_heap[0]; }
+	
+	inline dtNode* pop()
+	{
+		dtNode* result = m_heap[0];
+		m_size--;
+		trickleDown(0, m_heap[m_size]);
+		return result;
+	}
+	
+	inline void push(dtNode* node)
+	{
+		m_size++;
+		bubbleUp(m_size-1, node);
+	}
+	
+	inline void modify(dtNode* node)
+	{
+		for (int i = 0; i < m_size; ++i)
+		{
+			if (m_heap[i] == node)
+			{
+				bubbleUp(i, node);
+				return;
+			}
+		}
+	}
+	
+	inline bool empty() const { return m_size == 0; }
+	
+	inline int getMemUsed() const
+	{
+		return sizeof(*this) +
+		sizeof(dtNode*) * (m_capacity + 1);
+	}
+	
+	inline int getCapacity() const { return m_capacity; }
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtNodeQueue(const dtNodeQueue&);
+	dtNodeQueue& operator=(const dtNodeQueue&);
+
+	void bubbleUp(int i, dtNode* node);
+	void trickleDown(int i, dtNode* node);
+	
+	dtNode** m_heap;
+	const int m_capacity;
+	int m_size;
+};		
+
+
+#endif // DETOURNODE_H

panda/src/recastdetour/DetourObstacleAvoidance.cpp

@@ -0,0 +1,619 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 "DetourObstacleAvoidance.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+#include <string.h>
+#include <float.h>
+#include <new>
+
+static const float DT_PI = 3.14159265f;
+
+static int sweepCircleCircle(const float* c0, const float r0, const float* v,
+							 const float* c1, const float r1,
+							 float& tmin, float& tmax)
+{
+	static const float EPS = 0.0001f;
+	float s[3];
+	dtVsub(s,c1,c0);
+	float r = r0+r1;
+	float c = dtVdot2D(s,s) - r*r;
+	float a = dtVdot2D(v,v);
+	if (a < EPS) return 0;	// not moving
+	
+	// Overlap, calc time to exit.
+	float b = dtVdot2D(v,s);
+	float d = b*b - a*c;
+	if (d < 0.0f) return 0; // no intersection.
+	a = 1.0f / a;
+	const float rd = dtMathSqrtf(d);
+	tmin = (b - rd) * a;
+	tmax = (b + rd) * a;
+	return 1;
+}
+
+static int isectRaySeg(const float* ap, const float* u,
+					   const float* bp, const float* bq,
+					   float& t)
+{
+	float v[3], w[3];
+	dtVsub(v,bq,bp);
+	dtVsub(w,ap,bp);
+	float d = dtVperp2D(u,v);
+	if (dtMathFabsf(d) < 1e-6f) return 0;
+	d = 1.0f/d;
+	t = dtVperp2D(v,w) * d;
+	if (t < 0 || t > 1) return 0;
+	float s = dtVperp2D(u,w) * d;
+	if (s < 0 || s > 1) return 0;
+	return 1;
+}
+
+
+
+dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData()
+{
+	void* mem = dtAlloc(sizeof(dtObstacleAvoidanceDebugData), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtObstacleAvoidanceDebugData;
+}
+
+void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr)
+{
+	if (!ptr) return;
+	ptr->~dtObstacleAvoidanceDebugData();
+	dtFree(ptr);
+}
+
+
+dtObstacleAvoidanceDebugData::dtObstacleAvoidanceDebugData() :
+	m_nsamples(0),
+	m_maxSamples(0),
+	m_vel(0),
+	m_ssize(0),
+	m_pen(0),
+	m_vpen(0),
+	m_vcpen(0),
+	m_spen(0),
+	m_tpen(0)
+{
+}
+
+dtObstacleAvoidanceDebugData::~dtObstacleAvoidanceDebugData()
+{
+	dtFree(m_vel);
+	dtFree(m_ssize);
+	dtFree(m_pen);
+	dtFree(m_vpen);
+	dtFree(m_vcpen);
+	dtFree(m_spen);
+	dtFree(m_tpen);
+}
+		
+bool dtObstacleAvoidanceDebugData::init(const int maxSamples)
+{
+	dtAssert(maxSamples);
+	m_maxSamples = maxSamples;
+
+	m_vel = (float*)dtAlloc(sizeof(float)*3*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_vel)
+		return false;
+	m_pen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_pen)
+		return false;
+	m_ssize = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_ssize)
+		return false;
+	m_vpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_vpen)
+		return false;
+	m_vcpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_vcpen)
+		return false;
+	m_spen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_spen)
+		return false;
+	m_tpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
+	if (!m_tpen)
+		return false;
+	
+	return true;
+}
+
+void dtObstacleAvoidanceDebugData::reset()
+{
+	m_nsamples = 0;
+}
+
+void dtObstacleAvoidanceDebugData::addSample(const float* vel, const float ssize, const float pen,
+											 const float vpen, const float vcpen, const float spen, const float tpen)
+{
+	if (m_nsamples >= m_maxSamples)
+		return;
+	dtAssert(m_vel);
+	dtAssert(m_ssize);
+	dtAssert(m_pen);
+	dtAssert(m_vpen);
+	dtAssert(m_vcpen);
+	dtAssert(m_spen);
+	dtAssert(m_tpen);
+	dtVcopy(&m_vel[m_nsamples*3], vel);
+	m_ssize[m_nsamples] = ssize;
+	m_pen[m_nsamples] = pen;
+	m_vpen[m_nsamples] = vpen;
+	m_vcpen[m_nsamples] = vcpen;
+	m_spen[m_nsamples] = spen;
+	m_tpen[m_nsamples] = tpen;
+	m_nsamples++;
+}
+
+static void normalizeArray(float* arr, const int n)
+{
+	// Normalize penaly range.
+	float minPen = FLT_MAX;
+	float maxPen = -FLT_MAX;
+	for (int i = 0; i < n; ++i)
+	{
+		minPen = dtMin(minPen, arr[i]);
+		maxPen = dtMax(maxPen, arr[i]);
+	}
+	const float penRange = maxPen-minPen;
+	const float s = penRange > 0.001f ? (1.0f / penRange) : 1;
+	for (int i = 0; i < n; ++i)
+		arr[i] = dtClamp((arr[i]-minPen)*s, 0.0f, 1.0f);
+}
+
+void dtObstacleAvoidanceDebugData::normalizeSamples()
+{
+	normalizeArray(m_pen, m_nsamples);
+	normalizeArray(m_vpen, m_nsamples);
+	normalizeArray(m_vcpen, m_nsamples);
+	normalizeArray(m_spen, m_nsamples);
+	normalizeArray(m_tpen, m_nsamples);
+}
+
+
+dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery()
+{
+	void* mem = dtAlloc(sizeof(dtObstacleAvoidanceQuery), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtObstacleAvoidanceQuery;
+}
+
+void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr)
+{
+	if (!ptr) return;
+	ptr->~dtObstacleAvoidanceQuery();
+	dtFree(ptr);
+}
+
+
+dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() :
+	m_invHorizTime(0),
+	m_vmax(0),
+	m_invVmax(0),
+	m_maxCircles(0),
+	m_circles(0),
+	m_ncircles(0),
+	m_maxSegments(0),
+	m_segments(0),
+	m_nsegments(0)
+{
+}
+
+dtObstacleAvoidanceQuery::~dtObstacleAvoidanceQuery()
+{
+	dtFree(m_circles);
+	dtFree(m_segments);
+}
+
+bool dtObstacleAvoidanceQuery::init(const int maxCircles, const int maxSegments)
+{
+	m_maxCircles = maxCircles;
+	m_ncircles = 0;
+	m_circles = (dtObstacleCircle*)dtAlloc(sizeof(dtObstacleCircle)*m_maxCircles, DT_ALLOC_PERM);
+	if (!m_circles)
+		return false;
+	memset(m_circles, 0, sizeof(dtObstacleCircle)*m_maxCircles);
+
+	m_maxSegments = maxSegments;
+	m_nsegments = 0;
+	m_segments = (dtObstacleSegment*)dtAlloc(sizeof(dtObstacleSegment)*m_maxSegments, DT_ALLOC_PERM);
+	if (!m_segments)
+		return false;
+	memset(m_segments, 0, sizeof(dtObstacleSegment)*m_maxSegments);
+	
+	return true;
+}
+
+void dtObstacleAvoidanceQuery::reset()
+{
+	m_ncircles = 0;
+	m_nsegments = 0;
+}
+
+void dtObstacleAvoidanceQuery::addCircle(const float* pos, const float rad,
+										 const float* vel, const float* dvel)
+{
+	if (m_ncircles >= m_maxCircles)
+		return;
+		
+	dtObstacleCircle* cir = &m_circles[m_ncircles++];
+	dtVcopy(cir->p, pos);
+	cir->rad = rad;
+	dtVcopy(cir->vel, vel);
+	dtVcopy(cir->dvel, dvel);
+}
+
+void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q)
+{
+	if (m_nsegments >= m_maxSegments)
+		return;
+	
+	dtObstacleSegment* seg = &m_segments[m_nsegments++];
+	dtVcopy(seg->p, p);
+	dtVcopy(seg->q, q);
+}
+
+void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
+{
+	// Prepare obstacles
+	for (int i = 0; i < m_ncircles; ++i)
+	{
+		dtObstacleCircle* cir = &m_circles[i];
+		
+		// Side
+		const float* pa = pos;
+		const float* pb = cir->p;
+		
+		const float orig[3] = {0,0,0};
+		float dv[3];
+		dtVsub(cir->dp,pb,pa);
+		dtVnormalize(cir->dp);
+		dtVsub(dv, cir->dvel, dvel);
+		
+		const float a = dtTriArea2D(orig, cir->dp,dv);
+		if (a < 0.01f)
+		{
+			cir->np[0] = -cir->dp[2];
+			cir->np[2] = cir->dp[0];
+		}
+		else
+		{
+			cir->np[0] = cir->dp[2];
+			cir->np[2] = -cir->dp[0];
+		}
+	}	
+
+	for (int i = 0; i < m_nsegments; ++i)
+	{
+		dtObstacleSegment* seg = &m_segments[i];
+		
+		// Precalc if the agent is really close to the segment.
+		const float r = 0.01f;
+		float t;
+		seg->touch = dtDistancePtSegSqr2D(pos, seg->p, seg->q, t) < dtSqr(r);
+	}	
+}
+
+
+/* Calculate the collision penalty for a given velocity vector
+ * 
+ * @param vcand sampled velocity
+ * @param dvel desired velocity
+ * @param minPenalty threshold penalty for early out
+ */
+float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
+											  const float* pos, const float rad,
+											  const float* vel, const float* dvel,
+											  const float minPenalty,
+											  dtObstacleAvoidanceDebugData* debug)
+{
+	// penalty for straying away from the desired and current velocities
+	const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
+	const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
+
+	// find the threshold hit time to bail out based on the early out penalty
+	// (see how the penalty is calculated below to understnad)
+	float minPen = minPenalty - vpen - vcpen;
+	float tThresold = (m_params.weightToi / minPen - 0.1f) * m_params.horizTime;
+	if (tThresold - m_params.horizTime > -FLT_EPSILON)
+		return minPenalty; // already too much
+
+	// Find min time of impact and exit amongst all obstacles.
+	float tmin = m_params.horizTime;
+	float side = 0;
+	int nside = 0;
+	
+	for (int i = 0; i < m_ncircles; ++i)
+	{
+		const dtObstacleCircle* cir = &m_circles[i];
+			
+		// RVO
+		float vab[3];
+		dtVscale(vab, vcand, 2);
+		dtVsub(vab, vab, vel);
+		dtVsub(vab, vab, cir->vel);
+		
+		// Side
+		side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f);
+		nside++;
+		
+		float htmin = 0, htmax = 0;
+		if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax))
+			continue;
+		
+		// Handle overlapping obstacles.
+		if (htmin < 0.0f && htmax > 0.0f)
+		{
+			// Avoid more when overlapped.
+			htmin = -htmin * 0.5f;
+		}
+		
+		if (htmin >= 0.0f)
+		{
+			// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
+			if (htmin < tmin)
+			{
+				tmin = htmin;
+				if (tmin < tThresold)
+					return minPenalty;
+			}
+		}
+	}
+
+	for (int i = 0; i < m_nsegments; ++i)
+	{
+		const dtObstacleSegment* seg = &m_segments[i];
+		float htmin = 0;
+		
+		if (seg->touch)
+		{
+			// Special case when the agent is very close to the segment.
+			float sdir[3], snorm[3];
+			dtVsub(sdir, seg->q, seg->p);
+			snorm[0] = -sdir[2];
+			snorm[2] = sdir[0];
+			// If the velocity is pointing towards the segment, no collision.
+			if (dtVdot2D(snorm, vcand) < 0.0f)
+				continue;
+			// Else immediate collision.
+			htmin = 0.0f;
+		}
+		else
+		{
+			if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin))
+				continue;
+		}
+		
+		// Avoid less when facing walls.
+		htmin *= 2.0f;
+		
+		// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
+		if (htmin < tmin)
+		{
+			tmin = htmin;
+			if (tmin < tThresold)
+				return minPenalty;
+		}
+	}
+	
+	// Normalize side bias, to prevent it dominating too much.
+	if (nside)
+		side /= nside;
+	
+	const float spen = m_params.weightSide * side;
+	const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
+	
+	const float penalty = vpen + vcpen + spen + tpen;
+	
+	// Store different penalties for debug viewing
+	if (debug)
+		debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
+	
+	return penalty;
+}
+
+int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax,
+												 const float* vel, const float* dvel, float* nvel,
+												 const dtObstacleAvoidanceParams* params,
+												 dtObstacleAvoidanceDebugData* debug)
+{
+	prepare(pos, dvel);
+	
+	memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
+	m_invHorizTime = 1.0f / m_params.horizTime;
+	m_vmax = vmax;
+	m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
+	
+	dtVset(nvel, 0,0,0);
+	
+	if (debug)
+		debug->reset();
+
+	const float cvx = dvel[0] * m_params.velBias;
+	const float cvz = dvel[2] * m_params.velBias;
+	const float cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1);
+	const float half = (m_params.gridSize-1)*cs*0.5f;
+		
+	float minPenalty = FLT_MAX;
+	int ns = 0;
+		
+	for (int y = 0; y < m_params.gridSize; ++y)
+	{
+		for (int x = 0; x < m_params.gridSize; ++x)
+		{
+			float vcand[3];
+			vcand[0] = cvx + x*cs - half;
+			vcand[1] = 0;
+			vcand[2] = cvz + y*cs - half;
+			
+			if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
+			
+			const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
+			ns++;
+			if (penalty < minPenalty)
+			{
+				minPenalty = penalty;
+				dtVcopy(nvel, vcand);
+			}
+		}
+	}
+	
+	return ns;
+}
+
+
+// vector normalization that ignores the y-component.
+inline void dtNormalize2D(float* v)
+{
+	float d = dtMathSqrtf(v[0] * v[0] + v[2] * v[2]);
+	if (d==0)
+		return;
+	d = 1.0f / d;
+	v[0] *= d;
+	v[2] *= d;
+}
+
+// vector normalization that ignores the y-component.
+inline void dtRorate2D(float* dest, const float* v, float ang)
+{
+	float c = cosf(ang);
+	float s = sinf(ang);
+	dest[0] = v[0]*c - v[2]*s;
+	dest[2] = v[0]*s + v[2]*c;
+	dest[1] = v[1];
+}
+
+
+int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
+													 const float* vel, const float* dvel, float* nvel,
+													 const dtObstacleAvoidanceParams* params,
+													 dtObstacleAvoidanceDebugData* debug)
+{
+	prepare(pos, dvel);
+	
+	memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
+	m_invHorizTime = 1.0f / m_params.horizTime;
+	m_vmax = vmax;
+	m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
+	
+	dtVset(nvel, 0,0,0);
+	
+	if (debug)
+		debug->reset();
+
+	// Build sampling pattern aligned to desired velocity.
+	float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
+	int npat = 0;
+
+	const int ndivs = (int)m_params.adaptiveDivs;
+	const int nrings= (int)m_params.adaptiveRings;
+	const int depth = (int)m_params.adaptiveDepth;
+	
+	const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
+	const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
+	const float da = (1.0f/nd) * DT_PI*2;
+	const float ca = cosf(da);
+	const float sa = sinf(da);
+
+	// desired direction
+	float ddir[6];
+	dtVcopy(ddir, dvel);
+	dtNormalize2D(ddir);
+	dtRorate2D (ddir+3, ddir, da*0.5f); // rotated by da/2
+
+	// Always add sample at zero
+	pat[npat*2+0] = 0;
+	pat[npat*2+1] = 0;
+	npat++;
+	
+	for (int j = 0; j < nr; ++j)
+	{
+		const float r = (float)(nr-j)/(float)nr;
+		pat[npat*2+0] = ddir[(j%2)*3] * r;
+		pat[npat*2+1] = ddir[(j%2)*3+2] * r;
+		float* last1 = pat + npat*2;
+		float* last2 = last1;
+		npat++;
+
+		for (int i = 1; i < nd-1; i+=2)
+		{
+			// get next point on the "right" (rotate CW)
+			pat[npat*2+0] = last1[0]*ca + last1[1]*sa;
+			pat[npat*2+1] = -last1[0]*sa + last1[1]*ca;
+			// get next point on the "left" (rotate CCW)
+			pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
+			pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
+
+			last1 = pat + npat*2;
+			last2 = last1 + 2;
+			npat += 2;
+		}
+
+		if ((nd&1) == 0)
+		{
+			pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
+			pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
+			npat++;
+		}
+	}
+
+
+	// Start sampling.
+	float cr = vmax * (1.0f - m_params.velBias);
+	float res[3];
+	dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
+	int ns = 0;
+
+	for (int k = 0; k < depth; ++k)
+	{
+		float minPenalty = FLT_MAX;
+		float bvel[3];
+		dtVset(bvel, 0,0,0);
+		
+		for (int i = 0; i < npat; ++i)
+		{
+			float vcand[3];
+			vcand[0] = res[0] + pat[i*2+0]*cr;
+			vcand[1] = 0;
+			vcand[2] = res[2] + pat[i*2+1]*cr;
+			
+			if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
+			
+			const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
+			ns++;
+			if (penalty < minPenalty)
+			{
+				minPenalty = penalty;
+				dtVcopy(bvel, vcand);
+			}
+		}
+
+		dtVcopy(res, bvel);
+
+		cr *= 0.5f;
+	}	
+	
+	dtVcopy(nvel, res);
+	
+	return ns;
+}

panda/src/recastdetour/DetourObstacleAvoidance.h

@@ -0,0 +1,159 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOUROBSTACLEAVOIDANCE_H
+#define DETOUROBSTACLEAVOIDANCE_H
+
+struct dtObstacleCircle
+{
+	float p[3];				///< Position of the obstacle
+	float vel[3];			///< Velocity of the obstacle
+	float dvel[3];			///< Velocity of the obstacle
+	float rad;				///< Radius of the obstacle
+	float dp[3], np[3];		///< Use for side selection during sampling.
+};
+
+struct dtObstacleSegment
+{
+	float p[3], q[3];		///< End points of the obstacle segment
+	bool touch;
+};
+
+
+class dtObstacleAvoidanceDebugData
+{
+public:
+	dtObstacleAvoidanceDebugData();
+	~dtObstacleAvoidanceDebugData();
+	
+	bool init(const int maxSamples);
+	void reset();
+	void addSample(const float* vel, const float ssize, const float pen,
+				   const float vpen, const float vcpen, const float spen, const float tpen);
+	
+	void normalizeSamples();
+	
+	inline int getSampleCount() const { return m_nsamples; }
+	inline const float* getSampleVelocity(const int i) const { return &m_vel[i*3]; }
+	inline float getSampleSize(const int i) const { return m_ssize[i]; }
+	inline float getSamplePenalty(const int i) const { return m_pen[i]; }
+	inline float getSampleDesiredVelocityPenalty(const int i) const { return m_vpen[i]; }
+	inline float getSampleCurrentVelocityPenalty(const int i) const { return m_vcpen[i]; }
+	inline float getSamplePreferredSidePenalty(const int i) const { return m_spen[i]; }
+	inline float getSampleCollisionTimePenalty(const int i) const { return m_tpen[i]; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtObstacleAvoidanceDebugData(const dtObstacleAvoidanceDebugData&);
+	dtObstacleAvoidanceDebugData& operator=(const dtObstacleAvoidanceDebugData&);
+
+	int m_nsamples;
+	int m_maxSamples;
+	float* m_vel;
+	float* m_ssize;
+	float* m_pen;
+	float* m_vpen;
+	float* m_vcpen;
+	float* m_spen;
+	float* m_tpen;
+};
+
+dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData();
+void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr);
+
+
+static const int DT_MAX_PATTERN_DIVS = 32;	///< Max numver of adaptive divs.
+static const int DT_MAX_PATTERN_RINGS = 4;	///< Max number of adaptive rings.
+
+struct dtObstacleAvoidanceParams
+{
+	float velBias;
+	float weightDesVel;
+	float weightCurVel;
+	float weightSide;
+	float weightToi;
+	float horizTime;
+	unsigned char gridSize;	///< grid
+	unsigned char adaptiveDivs;	///< adaptive
+	unsigned char adaptiveRings;	///< adaptive
+	unsigned char adaptiveDepth;	///< adaptive
+};
+
+class dtObstacleAvoidanceQuery
+{
+public:
+	dtObstacleAvoidanceQuery();
+	~dtObstacleAvoidanceQuery();
+	
+	bool init(const int maxCircles, const int maxSegments);
+	
+	void reset();
+
+	void addCircle(const float* pos, const float rad,
+				   const float* vel, const float* dvel);
+				   
+	void addSegment(const float* p, const float* q);
+
+	int sampleVelocityGrid(const float* pos, const float rad, const float vmax,
+						   const float* vel, const float* dvel, float* nvel,
+						   const dtObstacleAvoidanceParams* params,
+						   dtObstacleAvoidanceDebugData* debug = 0);
+
+	int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
+							   const float* vel, const float* dvel, float* nvel,
+							   const dtObstacleAvoidanceParams* params, 
+							   dtObstacleAvoidanceDebugData* debug = 0);
+	
+	inline int getObstacleCircleCount() const { return m_ncircles; }
+	const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; }
+
+	inline int getObstacleSegmentCount() const { return m_nsegments; }
+	const dtObstacleSegment* getObstacleSegment(const int i) { return &m_segments[i]; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtObstacleAvoidanceQuery(const dtObstacleAvoidanceQuery&);
+	dtObstacleAvoidanceQuery& operator=(const dtObstacleAvoidanceQuery&);
+
+	void prepare(const float* pos, const float* dvel);
+
+	float processSample(const float* vcand, const float cs,
+						const float* pos, const float rad,
+						const float* vel, const float* dvel,
+						const float minPenalty,
+						dtObstacleAvoidanceDebugData* debug);
+
+	dtObstacleAvoidanceParams m_params;
+	float m_invHorizTime;
+	float m_vmax;
+	float m_invVmax;
+
+	int m_maxCircles;
+	dtObstacleCircle* m_circles;
+	int m_ncircles;
+
+	int m_maxSegments;
+	dtObstacleSegment* m_segments;
+	int m_nsegments;
+};
+
+dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery();
+void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr);
+
+
+#endif // DETOUROBSTACLEAVOIDANCE_H

panda/src/recastdetour/DetourPathCorridor.cpp

@@ -0,0 +1,597 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <string.h>
+#include "DetourPathCorridor.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourCommon.h"
+#include "DetourAssert.h"
+#include "DetourAlloc.h"
+
+
+int dtMergeCorridorStartMoved(dtPolyRef* path, const int npath, const int maxPath,
+							  const dtPolyRef* visited, const int nvisited)
+{
+	int furthestPath = -1;
+	int furthestVisited = -1;
+	
+	// Find furthest common polygon.
+	for (int i = npath-1; i >= 0; --i)
+	{
+		bool found = false;
+		for (int j = nvisited-1; j >= 0; --j)
+		{
+			if (path[i] == visited[j])
+			{
+				furthestPath = i;
+				furthestVisited = j;
+				found = true;
+			}
+		}
+		if (found)
+			break;
+	}
+	
+	// If no intersection found just return current path. 
+	if (furthestPath == -1 || furthestVisited == -1)
+		return npath;
+	
+	// Concatenate paths.	
+	
+	// Adjust beginning of the buffer to include the visited.
+	const int req = nvisited - furthestVisited;
+	const int orig = dtMin(furthestPath+1, npath);
+	int size = dtMax(0, npath-orig);
+	if (req+size > maxPath)
+		size = maxPath-req;
+	if (size)
+		memmove(path+req, path+orig, size*sizeof(dtPolyRef));
+	
+	// Store visited
+	for (int i = 0; i < req; ++i)
+		path[i] = visited[(nvisited-1)-i];				
+	
+	return req+size;
+}
+
+int dtMergeCorridorEndMoved(dtPolyRef* path, const int npath, const int maxPath,
+							const dtPolyRef* visited, const int nvisited)
+{
+	int furthestPath = -1;
+	int furthestVisited = -1;
+	
+	// Find furthest common polygon.
+	for (int i = 0; i < npath; ++i)
+	{
+		bool found = false;
+		for (int j = nvisited-1; j >= 0; --j)
+		{
+			if (path[i] == visited[j])
+			{
+				furthestPath = i;
+				furthestVisited = j;
+				found = true;
+			}
+		}
+		if (found)
+			break;
+	}
+	
+	// If no intersection found just return current path. 
+	if (furthestPath == -1 || furthestVisited == -1)
+		return npath;
+	
+	// Concatenate paths.
+	const int ppos = furthestPath+1;
+	const int vpos = furthestVisited+1;
+	const int count = dtMin(nvisited-vpos, maxPath-ppos);
+	dtAssert(ppos+count <= maxPath);
+	if (count)
+		memcpy(path+ppos, visited+vpos, sizeof(dtPolyRef)*count);
+	
+	return ppos+count;
+}
+
+int dtMergeCorridorStartShortcut(dtPolyRef* path, const int npath, const int maxPath,
+								 const dtPolyRef* visited, const int nvisited)
+{
+	int furthestPath = -1;
+	int furthestVisited = -1;
+	
+	// Find furthest common polygon.
+	for (int i = npath-1; i >= 0; --i)
+	{
+		bool found = false;
+		for (int j = nvisited-1; j >= 0; --j)
+		{
+			if (path[i] == visited[j])
+			{
+				furthestPath = i;
+				furthestVisited = j;
+				found = true;
+			}
+		}
+		if (found)
+			break;
+	}
+	
+	// If no intersection found just return current path. 
+	if (furthestPath == -1 || furthestVisited == -1)
+		return npath;
+	
+	// Concatenate paths.	
+	
+	// Adjust beginning of the buffer to include the visited.
+	const int req = furthestVisited;
+	if (req <= 0)
+		return npath;
+	
+	const int orig = furthestPath;
+	int size = dtMax(0, npath-orig);
+	if (req+size > maxPath)
+		size = maxPath-req;
+	if (size)
+		memmove(path+req, path+orig, size*sizeof(dtPolyRef));
+	
+	// Store visited
+	for (int i = 0; i < req; ++i)
+		path[i] = visited[i];
+	
+	return req+size;
+}
+
+/**
+@class dtPathCorridor
+@par
+
+The corridor is loaded with a path, usually obtained from a #dtNavMeshQuery::findPath() query. The corridor
+is then used to plan local movement, with the corridor automatically updating as needed to deal with inaccurate 
+agent locomotion.
+
+Example of a common use case:
+
+-# Construct the corridor object and call #init() to allocate its path buffer.
+-# Obtain a path from a #dtNavMeshQuery object.
+-# Use #reset() to set the agent's current position. (At the beginning of the path.)
+-# Use #setCorridor() to load the path and target.
+-# Use #findCorners() to plan movement. (This handles dynamic path straightening.)
+-# Use #movePosition() to feed agent movement back into the corridor. (The corridor will automatically adjust as needed.)
+-# If the target is moving, use #moveTargetPosition() to update the end of the corridor. 
+   (The corridor will automatically adjust as needed.)
+-# Repeat the previous 3 steps to continue to move the agent.
+
+The corridor position and target are always constrained to the navigation mesh.
+
+One of the difficulties in maintaining a path is that floating point errors, locomotion inaccuracies, and/or local 
+steering can result in the agent crossing the boundary of the path corridor, temporarily invalidating the path. 
+This class uses local mesh queries to detect and update the corridor as needed to handle these types of issues. 
+
+The fact that local mesh queries are used to move the position and target locations results in two beahviors that 
+need to be considered:
+
+Every time a move function is used there is a chance that the path will become non-optimial. Basically, the further 
+the target is moved from its original location, and the further the position is moved outside the original corridor, 
+the more likely the path will become non-optimal. This issue can be addressed by periodically running the 
+#optimizePathTopology() and #optimizePathVisibility() methods.
+
+All local mesh queries have distance limitations. (Review the #dtNavMeshQuery methods for details.) So the most accurate 
+use case is to move the position and target in small increments. If a large increment is used, then the corridor 
+may not be able to accurately find the new location.  Because of this limiation, if a position is moved in a large
+increment, then compare the desired and resulting polygon references. If the two do not match, then path replanning 
+may be needed.  E.g. If you move the target, check #getLastPoly() to see if it is the expected polygon.
+
+*/
+
+dtPathCorridor::dtPathCorridor() :
+	m_path(0),
+	m_npath(0),
+	m_maxPath(0)
+{
+}
+
+dtPathCorridor::~dtPathCorridor()
+{
+	dtFree(m_path);
+}
+
+/// @par
+///
+/// @warning Cannot be called more than once.
+bool dtPathCorridor::init(const int maxPath)
+{
+	dtAssert(!m_path);
+	m_path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*maxPath, DT_ALLOC_PERM);
+	if (!m_path)
+		return false;
+	m_npath = 0;
+	m_maxPath = maxPath;
+	return true;
+}
+
+/// @par
+///
+/// Essentially, the corridor is set of one polygon in size with the target
+/// equal to the position.
+void dtPathCorridor::reset(dtPolyRef ref, const float* pos)
+{
+	dtAssert(m_path);
+	dtVcopy(m_pos, pos);
+	dtVcopy(m_target, pos);
+	m_path[0] = ref;
+	m_npath = 1;
+}
+
+/**
+@par
+
+This is the function used to plan local movement within the corridor. One or more corners can be 
+detected in order to plan movement. It performs essentially the same function as #dtNavMeshQuery::findStraightPath.
+
+Due to internal optimizations, the maximum number of corners returned will be (@p maxCorners - 1) 
+For example: If the buffers are sized to hold 10 corners, the function will never return more than 9 corners. 
+So if 10 corners are needed, the buffers should be sized for 11 corners.
+
+If the target is within range, it will be the last corner and have a polygon reference id of zero.
+*/
+int dtPathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags,
+							  dtPolyRef* cornerPolys, const int maxCorners,
+							  dtNavMeshQuery* navquery, const dtQueryFilter* /*filter*/)
+{
+	dtAssert(m_path);
+	dtAssert(m_npath);
+	
+	static const float MIN_TARGET_DIST = 0.01f;
+	
+	int ncorners = 0;
+	navquery->findStraightPath(m_pos, m_target, m_path, m_npath,
+							   cornerVerts, cornerFlags, cornerPolys, &ncorners, maxCorners);
+	
+	// Prune points in the beginning of the path which are too close.
+	while (ncorners)
+	{
+		if ((cornerFlags[0] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
+			dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST))
+			break;
+		ncorners--;
+		if (ncorners)
+		{
+			memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners);
+			memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners);
+			memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners);
+		}
+	}
+	
+	// Prune points after an off-mesh connection.
+	for (int i = 0; i < ncorners; ++i)
+	{
+		if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
+		{
+			ncorners = i+1;
+			break;
+		}
+	}
+	
+	return ncorners;
+}
+
+/** 
+@par
+
+Inaccurate locomotion or dynamic obstacle avoidance can force the argent position significantly outside the 
+original corridor. Over time this can result in the formation of a non-optimal corridor. Non-optimal paths can 
+also form near the corners of tiles.
+
+This function uses an efficient local visibility search to try to optimize the corridor 
+between the current position and @p next.
+
+The corridor will change only if @p next is visible from the current position and moving directly toward the point 
+is better than following the existing path.
+
+The more inaccurate the agent movement, the more beneficial this function becomes. Simply adjust the frequency 
+of the call to match the needs to the agent.
+
+This function is not suitable for long distance searches.
+*/
+void dtPathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange,
+										  dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	dtAssert(m_path);
+	
+	// Clamp the ray to max distance.
+	float goal[3];
+	dtVcopy(goal, next);
+	float dist = dtVdist2D(m_pos, goal);
+	
+	// If too close to the goal, do not try to optimize.
+	if (dist < 0.01f)
+		return;
+	
+	// Overshoot a little. This helps to optimize open fields in tiled meshes.
+	dist = dtMin(dist+0.01f, pathOptimizationRange);
+	
+	// Adjust ray length.
+	float delta[3];
+	dtVsub(delta, goal, m_pos);
+	dtVmad(goal, m_pos, delta, pathOptimizationRange/dist);
+	
+	static const int MAX_RES = 32;
+	dtPolyRef res[MAX_RES];
+	float t, norm[3];
+	int nres = 0;
+	navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES);
+	if (nres > 1 && t > 0.99f)
+	{
+		m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
+	}
+}
+
+/**
+@par
+
+Inaccurate locomotion or dynamic obstacle avoidance can force the agent position significantly outside the 
+original corridor. Over time this can result in the formation of a non-optimal corridor. This function will use a 
+local area path search to try to re-optimize the corridor.
+
+The more inaccurate the agent movement, the more beneficial this function becomes. Simply adjust the frequency of 
+the call to match the needs to the agent.
+*/
+bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	dtAssert(navquery);
+	dtAssert(filter);
+	dtAssert(m_path);
+	
+	if (m_npath < 3)
+		return false;
+	
+	static const int MAX_ITER = 32;
+	static const int MAX_RES = 32;
+	
+	dtPolyRef res[MAX_RES];
+	int nres = 0;
+	navquery->initSlicedFindPath(m_path[0], m_path[m_npath-1], m_pos, m_target, filter);
+	navquery->updateSlicedFindPath(MAX_ITER, 0);
+	dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES);
+	
+	if (dtStatusSucceed(status) && nres > 0)
+	{
+		m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
+		return true;
+	}
+	
+	return false;
+}
+
+bool dtPathCorridor::moveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs,
+											   float* startPos, float* endPos,
+											   dtNavMeshQuery* navquery)
+{
+	dtAssert(navquery);
+	dtAssert(m_path);
+	dtAssert(m_npath);
+
+	// Advance the path up to and over the off-mesh connection.
+	dtPolyRef prevRef = 0, polyRef = m_path[0];
+	int npos = 0;
+	while (npos < m_npath && polyRef != offMeshConRef)
+	{
+		prevRef = polyRef;
+		polyRef = m_path[npos];
+		npos++;
+	}
+	if (npos == m_npath)
+	{
+		// Could not find offMeshConRef
+		return false;
+	}
+	
+	// Prune path
+	for (int i = npos; i < m_npath; ++i)
+		m_path[i-npos] = m_path[i];
+	m_npath -= npos;
+
+	refs[0] = prevRef;
+	refs[1] = polyRef;
+	
+	const dtNavMesh* nav = navquery->getAttachedNavMesh();
+	dtAssert(nav);
+
+	dtStatus status = nav->getOffMeshConnectionPolyEndPoints(refs[0], refs[1], startPos, endPos);
+	if (dtStatusSucceed(status))
+	{
+		dtVcopy(m_pos, endPos);
+		return true;
+	}
+
+	return false;
+}
+
+/**
+@par
+
+Behavior:
+
+- The movement is constrained to the surface of the navigation mesh. 
+- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid. 
+- The new position will be located in the adjusted corridor's first polygon.
+
+The expected use case is that the desired position will be 'near' the current corridor. What is considered 'near' 
+depends on local polygon density, query search half extents, etc.
+
+The resulting position will differ from the desired position if the desired position is not on the navigation mesh, 
+or it can't be reached using a local search.
+*/
+bool dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	dtAssert(m_path);
+	dtAssert(m_npath);
+	
+	// Move along navmesh and update new position.
+	float result[3];
+	static const int MAX_VISITED = 16;
+	dtPolyRef visited[MAX_VISITED];
+	int nvisited = 0;
+	dtStatus status = navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
+												 result, visited, &nvisited, MAX_VISITED);
+	if (dtStatusSucceed(status)) {
+		m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
+		
+		// Adjust the position to stay on top of the navmesh.
+		float h = m_pos[1];
+		navquery->getPolyHeight(m_path[0], result, &h);
+		result[1] = h;
+		dtVcopy(m_pos, result);
+		return true;
+	}
+	return false;
+}
+
+/**
+@par
+
+Behavior:
+
+- The movement is constrained to the surface of the navigation mesh. 
+- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid. 
+- The new target will be located in the adjusted corridor's last polygon.
+
+The expected use case is that the desired target will be 'near' the current corridor. What is considered 'near' depends on local polygon density, query search half extents, etc.
+
+The resulting target will differ from the desired target if the desired target is not on the navigation mesh, or it can't be reached using a local search.
+*/
+bool dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	dtAssert(m_path);
+	dtAssert(m_npath);
+	
+	// Move along navmesh and update new position.
+	float result[3];
+	static const int MAX_VISITED = 16;
+	dtPolyRef visited[MAX_VISITED];
+	int nvisited = 0;
+	dtStatus status = navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter,
+												 result, visited, &nvisited, MAX_VISITED);
+	if (dtStatusSucceed(status))
+	{
+		m_npath = dtMergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited);
+		// TODO: should we do that?
+		// Adjust the position to stay on top of the navmesh.
+		/*	float h = m_target[1];
+		 navquery->getPolyHeight(m_path[m_npath-1], result, &h);
+		 result[1] = h;*/
+		
+		dtVcopy(m_target, result);
+		
+		return true;
+	}
+	return false;
+}
+
+/// @par
+///
+/// The current corridor position is expected to be within the first polygon in the path. The target 
+/// is expected to be in the last polygon. 
+/// 
+/// @warning The size of the path must not exceed the size of corridor's path buffer set during #init().
+void dtPathCorridor::setCorridor(const float* target, const dtPolyRef* path, const int npath)
+{
+	dtAssert(m_path);
+	dtAssert(npath > 0);
+	dtAssert(npath < m_maxPath);
+	
+	dtVcopy(m_target, target);
+	memcpy(m_path, path, sizeof(dtPolyRef)*npath);
+	m_npath = npath;
+}
+
+bool dtPathCorridor::fixPathStart(dtPolyRef safeRef, const float* safePos)
+{
+	dtAssert(m_path);
+
+	dtVcopy(m_pos, safePos);
+	if (m_npath < 3 && m_npath > 0)
+	{
+		m_path[2] = m_path[m_npath-1];
+		m_path[0] = safeRef;
+		m_path[1] = 0;
+		m_npath = 3;
+	}
+	else
+	{
+		m_path[0] = safeRef;
+		m_path[1] = 0;
+	}
+	
+	return true;
+}
+
+bool dtPathCorridor::trimInvalidPath(dtPolyRef safeRef, const float* safePos,
+									 dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	dtAssert(navquery);
+	dtAssert(filter);
+	dtAssert(m_path);
+	
+	// Keep valid path as far as possible.
+	int n = 0;
+	while (n < m_npath && navquery->isValidPolyRef(m_path[n], filter)) {
+		n++;
+	}
+	
+	if (n == m_npath)
+	{
+		// All valid, no need to fix.
+		return true;
+	}
+	else if (n == 0)
+	{
+		// The first polyref is bad, use current safe values.
+		dtVcopy(m_pos, safePos);
+		m_path[0] = safeRef;
+		m_npath = 1;
+	}
+	else
+	{
+		// The path is partially usable.
+		m_npath = n;
+	}
+	
+	// Clamp target pos to last poly
+	float tgt[3];
+	dtVcopy(tgt, m_target);
+	navquery->closestPointOnPolyBoundary(m_path[m_npath-1], tgt, m_target);
+	
+	return true;
+}
+
+/// @par
+///
+/// The path can be invalidated if there are structural changes to the underlying navigation mesh, or the state of 
+/// a polygon within the path changes resulting in it being filtered out. (E.g. An exclusion or inclusion flag changes.)
+bool dtPathCorridor::isValid(const int maxLookAhead, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
+{
+	// Check that all polygons still pass query filter.
+	const int n = dtMin(m_npath, maxLookAhead);
+	for (int i = 0; i < n; ++i)
+	{
+		if (!navquery->isValidPolyRef(m_path[i], filter))
+			return false;
+	}
+
+	return true;
+}

panda/src/recastdetour/DetourPathCorridor.h

@@ -0,0 +1,151 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOUTPATHCORRIDOR_H
+#define DETOUTPATHCORRIDOR_H
+
+#include "DetourNavMeshQuery.h"
+
+/// Represents a dynamic polygon corridor used to plan agent movement.
+/// @ingroup crowd, detour
+class dtPathCorridor
+{
+	float m_pos[3];
+	float m_target[3];
+	
+	dtPolyRef* m_path;
+	int m_npath;
+	int m_maxPath;
+	
+public:
+	dtPathCorridor();
+	~dtPathCorridor();
+	
+	/// Allocates the corridor's path buffer. 
+	///  @param[in]		maxPath		The maximum path size the corridor can handle.
+	/// @return True if the initialization succeeded.
+	bool init(const int maxPath);
+	
+	/// Resets the path corridor to the specified position.
+	///  @param[in]		ref		The polygon reference containing the position.
+	///  @param[in]		pos		The new position in the corridor. [(x, y, z)]
+	void reset(dtPolyRef ref, const float* pos);
+	
+	/// Finds the corners in the corridor from the position toward the target. (The straightened path.)
+	///  @param[out]	cornerVerts		The corner vertices. [(x, y, z) * cornerCount] [Size: <= maxCorners]
+	///  @param[out]	cornerFlags		The flag for each corner. [(flag) * cornerCount] [Size: <= maxCorners]
+	///  @param[out]	cornerPolys		The polygon reference for each corner. [(polyRef) * cornerCount] 
+	///  								[Size: <= @p maxCorners]
+	///  @param[in]		maxCorners		The maximum number of corners the buffers can hold.
+	///  @param[in]		navquery		The query object used to build the corridor.
+	///  @param[in]		filter			The filter to apply to the operation.
+	/// @return The number of corners returned in the corner buffers. [0 <= value <= @p maxCorners]
+	int findCorners(float* cornerVerts, unsigned char* cornerFlags,
+					dtPolyRef* cornerPolys, const int maxCorners,
+					dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	/// Attempts to optimize the path if the specified point is visible from the current position.
+	///  @param[in]		next					The point to search toward. [(x, y, z])
+	///  @param[in]		pathOptimizationRange	The maximum range to search. [Limit: > 0]
+	///  @param[in]		navquery				The query object used to build the corridor.
+	///  @param[in]		filter					The filter to apply to the operation.			
+	void optimizePathVisibility(const float* next, const float pathOptimizationRange,
+								dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	/// Attempts to optimize the path using a local area search. (Partial replanning.) 
+	///  @param[in]		navquery	The query object used to build the corridor.
+	///  @param[in]		filter		The filter to apply to the operation.	
+	bool optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	bool moveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs,
+								   float* startPos, float* endPos,
+								   dtNavMeshQuery* navquery);
+
+	bool fixPathStart(dtPolyRef safeRef, const float* safePos);
+
+	bool trimInvalidPath(dtPolyRef safeRef, const float* safePos,
+						 dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	/// Checks the current corridor path to see if its polygon references remain valid. 
+	///  @param[in]		maxLookAhead	The number of polygons from the beginning of the corridor to search.
+	///  @param[in]		navquery		The query object used to build the corridor.
+	///  @param[in]		filter			The filter to apply to the operation.	
+	bool isValid(const int maxLookAhead, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	/// Moves the position from the current location to the desired location, adjusting the corridor 
+	/// as needed to reflect the change.
+	///  @param[in]		npos		The desired new position. [(x, y, z)]
+	///  @param[in]		navquery	The query object used to build the corridor.
+	///  @param[in]		filter		The filter to apply to the operation.
+	/// @return Returns true if move succeeded.
+	bool movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+
+	/// Moves the target from the curent location to the desired location, adjusting the corridor
+	/// as needed to reflect the change. 
+	///  @param[in]		npos		The desired new target position. [(x, y, z)]
+	///  @param[in]		navquery	The query object used to build the corridor.
+	///  @param[in]		filter		The filter to apply to the operation.
+	/// @return Returns true if move succeeded.
+	bool moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
+	
+	/// Loads a new path and target into the corridor.
+	///  @param[in]		target		The target location within the last polygon of the path. [(x, y, z)]
+	///  @param[in]		path		The path corridor. [(polyRef) * @p npolys]
+	///  @param[in]		npath		The number of polygons in the path.
+	void setCorridor(const float* target, const dtPolyRef* polys, const int npath);
+	
+	/// Gets the current position within the corridor. (In the first polygon.)
+	/// @return The current position within the corridor.
+	inline const float* getPos() const { return m_pos; }
+
+	/// Gets the current target within the corridor. (In the last polygon.)
+	/// @return The current target within the corridor.
+	inline const float* getTarget() const { return m_target; }
+	
+	/// The polygon reference id of the first polygon in the corridor, the polygon containing the position.
+	/// @return The polygon reference id of the first polygon in the corridor. (Or zero if there is no path.)
+	inline dtPolyRef getFirstPoly() const { return m_npath ? m_path[0] : 0; }
+
+	/// The polygon reference id of the last polygon in the corridor, the polygon containing the target.
+	/// @return The polygon reference id of the last polygon in the corridor. (Or zero if there is no path.)
+	inline dtPolyRef getLastPoly() const { return m_npath ? m_path[m_npath-1] : 0; }
+	
+	/// The corridor's path.
+	/// @return The corridor's path. [(polyRef) * #getPathCount()]
+	inline const dtPolyRef* getPath() const { return m_path; }
+
+	/// The number of polygons in the current corridor path.
+	/// @return The number of polygons in the current corridor path.
+	inline int getPathCount() const { return m_npath; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtPathCorridor(const dtPathCorridor&);
+	dtPathCorridor& operator=(const dtPathCorridor&);
+};
+
+int dtMergeCorridorStartMoved(dtPolyRef* path, const int npath, const int maxPath,
+							  const dtPolyRef* visited, const int nvisited);
+
+int dtMergeCorridorEndMoved(dtPolyRef* path, const int npath, const int maxPath,
+							const dtPolyRef* visited, const int nvisited);
+
+int dtMergeCorridorStartShortcut(dtPolyRef* path, const int npath, const int maxPath,
+								 const dtPolyRef* visited, const int nvisited);
+
+#endif // DETOUTPATHCORRIDOR_H

panda/src/recastdetour/DetourPathQueue.cpp

@@ -0,0 +1,200 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <string.h>
+#include "DetourPathQueue.h"
+#include "DetourNavMesh.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourAlloc.h"
+#include "DetourCommon.h"
+
+
+dtPathQueue::dtPathQueue() :
+	m_nextHandle(1),
+	m_maxPathSize(0),
+	m_queueHead(0),
+	m_navquery(0)
+{
+	for (int i = 0; i < MAX_QUEUE; ++i)
+		m_queue[i].path = 0;
+}
+
+dtPathQueue::~dtPathQueue()
+{
+	purge();
+}
+
+void dtPathQueue::purge()
+{
+	dtFreeNavMeshQuery(m_navquery);
+	m_navquery = 0;
+	for (int i = 0; i < MAX_QUEUE; ++i)
+	{
+		dtFree(m_queue[i].path);
+		m_queue[i].path = 0;
+	}
+}
+
+bool dtPathQueue::init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav)
+{
+	purge();
+
+	m_navquery = dtAllocNavMeshQuery();
+	if (!m_navquery)
+		return false;
+	if (dtStatusFailed(m_navquery->init(nav, maxSearchNodeCount)))
+		return false;
+	
+	m_maxPathSize = maxPathSize;
+	for (int i = 0; i < MAX_QUEUE; ++i)
+	{
+		m_queue[i].ref = DT_PATHQ_INVALID;
+		m_queue[i].path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathSize, DT_ALLOC_PERM);
+		if (!m_queue[i].path)
+			return false;
+	}
+	
+	m_queueHead = 0;
+	
+	return true;
+}
+
+void dtPathQueue::update(const int maxIters)
+{
+	static const int MAX_KEEP_ALIVE = 2; // in update ticks.
+
+	// Update path request until there is nothing to update
+	// or upto maxIters pathfinder iterations has been consumed.
+	int iterCount = maxIters;
+	
+	for (int i = 0; i < MAX_QUEUE; ++i)
+	{
+		PathQuery& q = m_queue[m_queueHead % MAX_QUEUE];
+		
+		// Skip inactive requests.
+		if (q.ref == DT_PATHQ_INVALID)
+		{
+			m_queueHead++;
+			continue;
+		}
+		
+		// Handle completed request.
+		if (dtStatusSucceed(q.status) || dtStatusFailed(q.status))
+		{
+			// If the path result has not been read in few frames, free the slot.
+			q.keepAlive++;
+			if (q.keepAlive > MAX_KEEP_ALIVE)
+			{
+				q.ref = DT_PATHQ_INVALID;
+				q.status = 0;
+			}
+			
+			m_queueHead++;
+			continue;
+		}
+		
+		// Handle query start.
+		if (q.status == 0)
+		{
+			q.status = m_navquery->initSlicedFindPath(q.startRef, q.endRef, q.startPos, q.endPos, q.filter);
+		}		
+		// Handle query in progress.
+		if (dtStatusInProgress(q.status))
+		{
+			int iters = 0;
+			q.status = m_navquery->updateSlicedFindPath(iterCount, &iters);
+			iterCount -= iters;
+		}
+		if (dtStatusSucceed(q.status))
+		{
+			q.status = m_navquery->finalizeSlicedFindPath(q.path, &q.npath, m_maxPathSize);
+		}
+
+		if (iterCount <= 0)
+			break;
+
+		m_queueHead++;
+	}
+}
+
+dtPathQueueRef dtPathQueue::request(dtPolyRef startRef, dtPolyRef endRef,
+									const float* startPos, const float* endPos,
+									const dtQueryFilter* filter)
+{
+	// Find empty slot
+	int slot = -1;
+	for (int i = 0; i < MAX_QUEUE; ++i)
+	{
+		if (m_queue[i].ref == DT_PATHQ_INVALID)
+		{
+			slot = i;
+			break;
+		}
+	}
+	// Could not find slot.
+	if (slot == -1)
+		return DT_PATHQ_INVALID;
+	
+	dtPathQueueRef ref = m_nextHandle++;
+	if (m_nextHandle == DT_PATHQ_INVALID) m_nextHandle++;
+	
+	PathQuery& q = m_queue[slot];
+	q.ref = ref;
+	dtVcopy(q.startPos, startPos);
+	q.startRef = startRef;
+	dtVcopy(q.endPos, endPos);
+	q.endRef = endRef;
+	
+	q.status = 0;
+	q.npath = 0;
+	q.filter = filter;
+	q.keepAlive = 0;
+	
+	return ref;
+}
+
+dtStatus dtPathQueue::getRequestStatus(dtPathQueueRef ref) const
+{
+	for (int i = 0; i < MAX_QUEUE; ++i)
+	{
+		if (m_queue[i].ref == ref)
+			return m_queue[i].status;
+	}
+	return DT_FAILURE;
+}
+
+dtStatus dtPathQueue::getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pathSize, const int maxPath)
+{
+	for (int i = 0; i < MAX_QUEUE; ++i)
+	{
+		if (m_queue[i].ref == ref)
+		{
+			PathQuery& q = m_queue[i];
+			dtStatus details = q.status & DT_STATUS_DETAIL_MASK;
+			// Free request for reuse.
+			q.ref = DT_PATHQ_INVALID;
+			q.status = 0;
+			// Copy path
+			int n = dtMin(q.npath, maxPath);
+			memcpy(path, q.path, sizeof(dtPolyRef)*n);
+			*pathSize = n;
+			return details | DT_SUCCESS;
+		}
+	}
+	return DT_FAILURE;
+}

panda/src/recastdetour/DetourPathQueue.h

@@ -0,0 +1,79 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURPATHQUEUE_H
+#define DETOURPATHQUEUE_H
+
+#include "DetourNavMesh.h"
+#include "DetourNavMeshQuery.h"
+
+static const unsigned int DT_PATHQ_INVALID = 0;
+
+typedef unsigned int dtPathQueueRef;
+
+class dtPathQueue
+{
+	struct PathQuery
+	{
+		dtPathQueueRef ref;
+		/// Path find start and end location.
+		float startPos[3], endPos[3];
+		dtPolyRef startRef, endRef;
+		/// Result.
+		dtPolyRef* path;
+		int npath;
+		/// State.
+		dtStatus status;
+		int keepAlive;
+		const dtQueryFilter* filter; ///< TODO: This is potentially dangerous!
+	};
+	
+	static const int MAX_QUEUE = 8;
+	PathQuery m_queue[MAX_QUEUE];
+	dtPathQueueRef m_nextHandle;
+	int m_maxPathSize;
+	int m_queueHead;
+	dtNavMeshQuery* m_navquery;
+	
+	void purge();
+	
+public:
+	dtPathQueue();
+	~dtPathQueue();
+	
+	bool init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav);
+	
+	void update(const int maxIters);
+	
+	dtPathQueueRef request(dtPolyRef startRef, dtPolyRef endRef,
+						   const float* startPos, const float* endPos, 
+						   const dtQueryFilter* filter);
+	
+	dtStatus getRequestStatus(dtPathQueueRef ref) const;
+	
+	dtStatus getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pathSize, const int maxPath);
+	
+	inline const dtNavMeshQuery* getNavQuery() const { return m_navquery; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtPathQueue(const dtPathQueue&);
+	dtPathQueue& operator=(const dtPathQueue&);
+};
+
+#endif // DETOURPATHQUEUE_H

panda/src/recastdetour/DetourProximityGrid.cpp

@@ -0,0 +1,194 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <string.h>
+#include <new>
+#include "DetourProximityGrid.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+
+
+dtProximityGrid* dtAllocProximityGrid()
+{
+	void* mem = dtAlloc(sizeof(dtProximityGrid), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtProximityGrid;
+}
+
+void dtFreeProximityGrid(dtProximityGrid* ptr)
+{
+	if (!ptr) return;
+	ptr->~dtProximityGrid();
+	dtFree(ptr);
+}
+
+
+inline int hashPos2(int x, int y, int n)
+{
+	return ((x*73856093) ^ (y*19349663)) & (n-1);
+}
+
+
+dtProximityGrid::dtProximityGrid() :
+	m_cellSize(0),
+	m_invCellSize(0),
+	m_pool(0),
+	m_poolHead(0),
+	m_poolSize(0),
+	m_buckets(0),
+	m_bucketsSize(0)
+{
+}
+
+dtProximityGrid::~dtProximityGrid()
+{
+	dtFree(m_buckets);
+	dtFree(m_pool);
+}
+
+bool dtProximityGrid::init(const int poolSize, const float cellSize)
+{
+	dtAssert(poolSize > 0);
+	dtAssert(cellSize > 0.0f);
+	
+	m_cellSize = cellSize;
+	m_invCellSize = 1.0f / m_cellSize;
+	
+	// Allocate hashs buckets
+	m_bucketsSize = dtNextPow2(poolSize);
+	m_buckets = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_bucketsSize, DT_ALLOC_PERM);
+	if (!m_buckets)
+		return false;
+	
+	// Allocate pool of items.
+	m_poolSize = poolSize;
+	m_poolHead = 0;
+	m_pool = (Item*)dtAlloc(sizeof(Item)*m_poolSize, DT_ALLOC_PERM);
+	if (!m_pool)
+		return false;
+	
+	clear();
+	
+	return true;
+}
+
+void dtProximityGrid::clear()
+{
+	memset(m_buckets, 0xff, sizeof(unsigned short)*m_bucketsSize);
+	m_poolHead = 0;
+	m_bounds[0] = 0xffff;
+	m_bounds[1] = 0xffff;
+	m_bounds[2] = -0xffff;
+	m_bounds[3] = -0xffff;
+}
+
+void dtProximityGrid::addItem(const unsigned short id,
+							  const float minx, const float miny,
+							  const float maxx, const float maxy)
+{
+	const int iminx = (int)dtMathFloorf(minx * m_invCellSize);
+	const int iminy = (int)dtMathFloorf(miny * m_invCellSize);
+	const int imaxx = (int)dtMathFloorf(maxx * m_invCellSize);
+	const int imaxy = (int)dtMathFloorf(maxy * m_invCellSize);
+	
+	m_bounds[0] = dtMin(m_bounds[0], iminx);
+	m_bounds[1] = dtMin(m_bounds[1], iminy);
+	m_bounds[2] = dtMax(m_bounds[2], imaxx);
+	m_bounds[3] = dtMax(m_bounds[3], imaxy);
+	
+	for (int y = iminy; y <= imaxy; ++y)
+	{
+		for (int x = iminx; x <= imaxx; ++x)
+		{
+			if (m_poolHead < m_poolSize)
+			{
+				const int h = hashPos2(x, y, m_bucketsSize);
+				const unsigned short idx = (unsigned short)m_poolHead;
+				m_poolHead++;
+				Item& item = m_pool[idx];
+				item.x = (short)x;
+				item.y = (short)y;
+				item.id = id;
+				item.next = m_buckets[h];
+				m_buckets[h] = idx;
+			}
+		}
+	}
+}
+
+int dtProximityGrid::queryItems(const float minx, const float miny,
+								const float maxx, const float maxy,
+								unsigned short* ids, const int maxIds) const
+{
+	const int iminx = (int)dtMathFloorf(minx * m_invCellSize);
+	const int iminy = (int)dtMathFloorf(miny * m_invCellSize);
+	const int imaxx = (int)dtMathFloorf(maxx * m_invCellSize);
+	const int imaxy = (int)dtMathFloorf(maxy * m_invCellSize);
+	
+	int n = 0;
+	
+	for (int y = iminy; y <= imaxy; ++y)
+	{
+		for (int x = iminx; x <= imaxx; ++x)
+		{
+			const int h = hashPos2(x, y, m_bucketsSize);
+			unsigned short idx = m_buckets[h];
+			while (idx != 0xffff)
+			{
+				Item& item = m_pool[idx];
+				if ((int)item.x == x && (int)item.y == y)
+				{
+					// Check if the id exists already.
+					const unsigned short* end = ids + n;
+					unsigned short* i = ids;
+					while (i != end && *i != item.id)
+						++i;
+					// Item not found, add it.
+					if (i == end)
+					{
+						if (n >= maxIds)
+							return n;
+						ids[n++] = item.id;
+					}
+				}
+				idx = item.next;
+			}
+		}
+	}
+	
+	return n;
+}
+
+int dtProximityGrid::getItemCountAt(const int x, const int y) const
+{
+	int n = 0;
+	
+	const int h = hashPos2(x, y, m_bucketsSize);
+	unsigned short idx = m_buckets[h];
+	while (idx != 0xffff)
+	{
+		Item& item = m_pool[idx];
+		if ((int)item.x == x && (int)item.y == y)
+			n++;
+		idx = item.next;
+	}
+	
+	return n;
+}

panda/src/recastdetour/DetourProximityGrid.h

@@ -0,0 +1,74 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURPROXIMITYGRID_H
+#define DETOURPROXIMITYGRID_H
+
+class dtProximityGrid
+{
+	float m_cellSize;
+	float m_invCellSize;
+	
+	struct Item
+	{
+		unsigned short id;
+		short x,y;
+		unsigned short next;
+	};
+	Item* m_pool;
+	int m_poolHead;
+	int m_poolSize;
+	
+	unsigned short* m_buckets;
+	int m_bucketsSize;
+	
+	int m_bounds[4];
+	
+public:
+	dtProximityGrid();
+	~dtProximityGrid();
+	
+	bool init(const int poolSize, const float cellSize);
+	
+	void clear();
+	
+	void addItem(const unsigned short id,
+				 const float minx, const float miny,
+				 const float maxx, const float maxy);
+	
+	int queryItems(const float minx, const float miny,
+				   const float maxx, const float maxy,
+				   unsigned short* ids, const int maxIds) const;
+	
+	int getItemCountAt(const int x, const int y) const;
+	
+	inline const int* getBounds() const { return m_bounds; }
+	inline float getCellSize() const { return m_cellSize; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtProximityGrid(const dtProximityGrid&);
+	dtProximityGrid& operator=(const dtProximityGrid&);
+};
+
+dtProximityGrid* dtAllocProximityGrid();
+void dtFreeProximityGrid(dtProximityGrid* ptr);
+
+
+#endif // DETOURPROXIMITYGRID_H
+

panda/src/recastdetour/DetourStatus.h

@@ -0,0 +1,65 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURSTATUS_H
+#define DETOURSTATUS_H
+
+typedef unsigned int dtStatus;
+
+// High level status.
+static const unsigned int DT_FAILURE = 1u << 31;			// Operation failed.
+static const unsigned int DT_SUCCESS = 1u << 30;			// Operation succeed.
+static const unsigned int DT_IN_PROGRESS = 1u << 29;		// Operation still in progress.
+
+// Detail information for status.
+static const unsigned int DT_STATUS_DETAIL_MASK = 0x0ffffff;
+static const unsigned int DT_WRONG_MAGIC = 1 << 0;		// Input data is not recognized.
+static const unsigned int DT_WRONG_VERSION = 1 << 1;	// Input data is in wrong version.
+static const unsigned int DT_OUT_OF_MEMORY = 1 << 2;	// Operation ran out of memory.
+static const unsigned int DT_INVALID_PARAM = 1 << 3;	// An input parameter was invalid.
+static const unsigned int DT_BUFFER_TOO_SMALL = 1 << 4;	// Result buffer for the query was too small to store all results.
+static const unsigned int DT_OUT_OF_NODES = 1 << 5;		// Query ran out of nodes during search.
+static const unsigned int DT_PARTIAL_RESULT = 1 << 6;	// Query did not reach the end location, returning best guess. 
+static const unsigned int DT_ALREADY_OCCUPIED = 1 << 7;	// A tile has already been assigned to the given x,y coordinate
+
+
+// Returns true of status is success.
+inline bool dtStatusSucceed(dtStatus status)
+{
+	return (status & DT_SUCCESS) != 0;
+}
+
+// Returns true of status is failure.
+inline bool dtStatusFailed(dtStatus status)
+{
+	return (status & DT_FAILURE) != 0;
+}
+
+// Returns true of status is in progress.
+inline bool dtStatusInProgress(dtStatus status)
+{
+	return (status & DT_IN_PROGRESS) != 0;
+}
+
+// Returns true if specific detail is set.
+inline bool dtStatusDetail(dtStatus status, unsigned int detail)
+{
+	return (status & detail) != 0;
+}
+
+#endif // DETOURSTATUS_H

panda/src/recastdetour/DetourTileCache.cpp

@@ -0,0 +1,820 @@
+#include "DetourTileCache.h"
+#include "DetourTileCacheBuilder.h"
+#include "DetourNavMeshBuilder.h"
+#include "DetourNavMesh.h"
+#include "DetourCommon.h"
+#include "DetourMath.h"
+#include "DetourAlloc.h"
+#include "DetourAssert.h"
+#include <string.h>
+#include <new>
+
+dtTileCache* dtAllocTileCache()
+{
+	void* mem = dtAlloc(sizeof(dtTileCache), DT_ALLOC_PERM);
+	if (!mem) return 0;
+	return new(mem) dtTileCache;
+}
+
+void dtFreeTileCache(dtTileCache* tc)
+{
+	if (!tc) return;
+	tc->~dtTileCache();
+	dtFree(tc);
+}
+
+static bool contains(const dtCompressedTileRef* a, const int n, const dtCompressedTileRef v)
+{
+	for (int i = 0; i < n; ++i)
+		if (a[i] == v)
+			return true;
+	return false;
+}
+
+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);
+}
+
+
+struct NavMeshTileBuildContext
+{
+	inline NavMeshTileBuildContext(struct dtTileCacheAlloc* a) : layer(0), lcset(0), lmesh(0), alloc(a) {}
+	inline ~NavMeshTileBuildContext() { purge(); }
+	void purge()
+	{
+		dtFreeTileCacheLayer(alloc, layer);
+		layer = 0;
+		dtFreeTileCacheContourSet(alloc, lcset);
+		lcset = 0;
+		dtFreeTileCachePolyMesh(alloc, lmesh);
+		lmesh = 0;
+	}
+	struct dtTileCacheLayer* layer;
+	struct dtTileCacheContourSet* lcset;
+	struct dtTileCachePolyMesh* lmesh;
+	struct dtTileCacheAlloc* alloc;
+};
+
+
+dtTileCache::dtTileCache() :
+	m_tileLutSize(0),
+	m_tileLutMask(0),
+	m_posLookup(0),
+	m_nextFreeTile(0),	
+	m_tiles(0),	
+	m_saltBits(0),
+	m_tileBits(0),
+	m_talloc(0),
+	m_tcomp(0),
+	m_tmproc(0),
+	m_obstacles(0),
+	m_nextFreeObstacle(0),
+	m_nreqs(0),
+	m_nupdate(0)
+{
+	memset(&m_params, 0, sizeof(m_params));
+	memset(m_reqs, 0, sizeof(ObstacleRequest) * MAX_REQUESTS);
+}
+	
+dtTileCache::~dtTileCache()
+{
+	for (int i = 0; i < m_params.maxTiles; ++i)
+	{
+		if (m_tiles[i].flags & DT_COMPRESSEDTILE_FREE_DATA)
+		{
+			dtFree(m_tiles[i].data);
+			m_tiles[i].data = 0;
+		}
+	}
+	dtFree(m_obstacles);
+	m_obstacles = 0;
+	dtFree(m_posLookup);
+	m_posLookup = 0;
+	dtFree(m_tiles);
+	m_tiles = 0;
+	m_nreqs = 0;
+	m_nupdate = 0;
+}
+
+const dtCompressedTile* dtTileCache::getTileByRef(dtCompressedTileRef ref) const
+{
+	if (!ref)
+		return 0;
+	unsigned int tileIndex = decodeTileIdTile(ref);
+	unsigned int tileSalt = decodeTileIdSalt(ref);
+	if ((int)tileIndex >= m_params.maxTiles)
+		return 0;
+	const dtCompressedTile* tile = &m_tiles[tileIndex];
+	if (tile->salt != tileSalt)
+		return 0;
+	return tile;
+}
+
+
+dtStatus dtTileCache::init(const dtTileCacheParams* params,
+						   dtTileCacheAlloc* talloc,
+						   dtTileCacheCompressor* tcomp,
+						   dtTileCacheMeshProcess* tmproc)
+{
+	m_talloc = talloc;
+	m_tcomp = tcomp;
+	m_tmproc = tmproc;
+	m_nreqs = 0;
+	memcpy(&m_params, params, sizeof(m_params));
+	
+	// Alloc space for obstacles.
+	m_obstacles = (dtTileCacheObstacle*)dtAlloc(sizeof(dtTileCacheObstacle)*m_params.maxObstacles, DT_ALLOC_PERM);
+	if (!m_obstacles)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(m_obstacles, 0, sizeof(dtTileCacheObstacle)*m_params.maxObstacles);
+	m_nextFreeObstacle = 0;
+	for (int i = m_params.maxObstacles-1; i >= 0; --i)
+	{
+		m_obstacles[i].salt = 1;
+		m_obstacles[i].next = m_nextFreeObstacle;
+		m_nextFreeObstacle = &m_obstacles[i];
+	}
+	
+	// Init tiles
+	m_tileLutSize = dtNextPow2(m_params.maxTiles/4);
+	if (!m_tileLutSize) m_tileLutSize = 1;
+	m_tileLutMask = m_tileLutSize-1;
+	
+	m_tiles = (dtCompressedTile*)dtAlloc(sizeof(dtCompressedTile)*m_params.maxTiles, DT_ALLOC_PERM);
+	if (!m_tiles)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	m_posLookup = (dtCompressedTile**)dtAlloc(sizeof(dtCompressedTile*)*m_tileLutSize, DT_ALLOC_PERM);
+	if (!m_posLookup)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(m_tiles, 0, sizeof(dtCompressedTile)*m_params.maxTiles);
+	memset(m_posLookup, 0, sizeof(dtCompressedTile*)*m_tileLutSize);
+	m_nextFreeTile = 0;
+	for (int i = m_params.maxTiles-1; i >= 0; --i)
+	{
+		m_tiles[i].salt = 1;
+		m_tiles[i].next = m_nextFreeTile;
+		m_nextFreeTile = &m_tiles[i];
+	}
+	
+	// Init ID generator values.
+	m_tileBits = dtIlog2(dtNextPow2((unsigned int)m_params.maxTiles));
+	// Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow.
+	m_saltBits = dtMin((unsigned int)31, 32 - m_tileBits);
+	if (m_saltBits < 10)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	return DT_SUCCESS;
+}
+
+int dtTileCache::getTilesAt(const int tx, const int ty, dtCompressedTileRef* tiles, const int maxTiles) const 
+{
+	int n = 0;
+	
+	// Find tile based on hash.
+	int h = computeTileHash(tx,ty,m_tileLutMask);
+	dtCompressedTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->tx == tx &&
+			tile->header->ty == ty)
+		{
+			if (n < maxTiles)
+				tiles[n++] = getTileRef(tile);
+		}
+		tile = tile->next;
+	}
+	
+	return n;
+}
+
+dtCompressedTile* dtTileCache::getTileAt(const int tx, const int ty, const int tlayer)
+{
+	// Find tile based on hash.
+	int h = computeTileHash(tx,ty,m_tileLutMask);
+	dtCompressedTile* tile = m_posLookup[h];
+	while (tile)
+	{
+		if (tile->header &&
+			tile->header->tx == tx &&
+			tile->header->ty == ty &&
+			tile->header->tlayer == tlayer)
+		{
+			return tile;
+		}
+		tile = tile->next;
+	}
+	return 0;
+}
+
+dtCompressedTileRef dtTileCache::getTileRef(const dtCompressedTile* tile) const
+{
+	if (!tile) return 0;
+	const unsigned int it = (unsigned int)(tile - m_tiles);
+	return (dtCompressedTileRef)encodeTileId(tile->salt, it);
+}
+
+dtObstacleRef dtTileCache::getObstacleRef(const dtTileCacheObstacle* ob) const
+{
+	if (!ob) return 0;
+	const unsigned int idx = (unsigned int)(ob - m_obstacles);
+	return encodeObstacleId(ob->salt, idx);
+}
+
+const dtTileCacheObstacle* dtTileCache::getObstacleByRef(dtObstacleRef ref)
+{
+	if (!ref)
+		return 0;
+	unsigned int idx = decodeObstacleIdObstacle(ref);
+	if ((int)idx >= m_params.maxObstacles)
+		return 0;
+	const dtTileCacheObstacle* ob = &m_obstacles[idx];
+	unsigned int salt = decodeObstacleIdSalt(ref);
+	if (ob->salt != salt)
+		return 0;
+	return ob;
+}
+
+dtStatus dtTileCache::addTile(unsigned char* data, const int dataSize, unsigned char flags, dtCompressedTileRef* result)
+{
+	// Make sure the data is in right format.
+	dtTileCacheLayerHeader* header = (dtTileCacheLayerHeader*)data;
+	if (header->magic != DT_TILECACHE_MAGIC)
+		return DT_FAILURE | DT_WRONG_MAGIC;
+	if (header->version != DT_TILECACHE_VERSION)
+		return DT_FAILURE | DT_WRONG_VERSION;
+	
+	// Make sure the location is free.
+	if (getTileAt(header->tx, header->ty, header->tlayer))
+		return DT_FAILURE;
+	
+	// Allocate a tile.
+	dtCompressedTile* tile = 0;
+	if (m_nextFreeTile)
+	{
+		tile = m_nextFreeTile;
+		m_nextFreeTile = tile->next;
+		tile->next = 0;
+	}
+	
+	// Make sure we could allocate a tile.
+	if (!tile)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	// Insert tile into the position lut.
+	int h = computeTileHash(header->tx, header->ty, m_tileLutMask);
+	tile->next = m_posLookup[h];
+	m_posLookup[h] = tile;
+	
+	// Init tile.
+	const int headerSize = dtAlign4(sizeof(dtTileCacheLayerHeader));
+	tile->header = (dtTileCacheLayerHeader*)data;
+	tile->data = data;
+	tile->dataSize = dataSize;
+	tile->compressed = tile->data + headerSize;
+	tile->compressedSize = tile->dataSize - headerSize;
+	tile->flags = flags;
+	
+	if (result)
+		*result = getTileRef(tile);
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::removeTile(dtCompressedTileRef ref, unsigned char** data, int* dataSize)
+{
+	if (!ref)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	unsigned int tileIndex = decodeTileIdTile(ref);
+	unsigned int tileSalt = decodeTileIdSalt(ref);
+	if ((int)tileIndex >= m_params.maxTiles)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	dtCompressedTile* tile = &m_tiles[tileIndex];
+	if (tile->salt != tileSalt)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	// Remove tile from hash lookup.
+	const int h = computeTileHash(tile->header->tx,tile->header->ty,m_tileLutMask);
+	dtCompressedTile* prev = 0;
+	dtCompressedTile* cur = m_posLookup[h];
+	while (cur)
+	{
+		if (cur == tile)
+		{
+			if (prev)
+				prev->next = cur->next;
+			else
+				m_posLookup[h] = cur->next;
+			break;
+		}
+		prev = cur;
+		cur = cur->next;
+	}
+	
+	// Reset tile.
+	if (tile->flags & DT_COMPRESSEDTILE_FREE_DATA)
+	{
+		// Owns data
+		dtFree(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->data = 0;
+	tile->dataSize = 0;
+	tile->compressed = 0;
+	tile->compressedSize = 0;
+	tile->flags = 0;
+	
+	// Update salt, salt should never be zero.
+	tile->salt = (tile->salt+1) & ((1<<m_saltBits)-1);
+	if (tile->salt == 0)
+		tile->salt++;
+	
+	// Add to free list.
+	tile->next = m_nextFreeTile;
+	m_nextFreeTile = tile;
+	
+	return DT_SUCCESS;
+}
+
+
+dtStatus dtTileCache::addObstacle(const float* pos, const float radius, const float height, dtObstacleRef* result)
+{
+	if (m_nreqs >= MAX_REQUESTS)
+		return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+	
+	dtTileCacheObstacle* ob = 0;
+	if (m_nextFreeObstacle)
+	{
+		ob = m_nextFreeObstacle;
+		m_nextFreeObstacle = ob->next;
+		ob->next = 0;
+	}
+	if (!ob)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	unsigned short salt = ob->salt;
+	memset(ob, 0, sizeof(dtTileCacheObstacle));
+	ob->salt = salt;
+	ob->state = DT_OBSTACLE_PROCESSING;
+	ob->type = DT_OBSTACLE_CYLINDER;
+	dtVcopy(ob->cylinder.pos, pos);
+	ob->cylinder.radius = radius;
+	ob->cylinder.height = height;
+	
+	ObstacleRequest* req = &m_reqs[m_nreqs++];
+	memset(req, 0, sizeof(ObstacleRequest));
+	req->action = REQUEST_ADD;
+	req->ref = getObstacleRef(ob);
+	
+	if (result)
+		*result = req->ref;
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::addBoxObstacle(const float* bmin, const float* bmax, dtObstacleRef* result)
+{
+	if (m_nreqs >= MAX_REQUESTS)
+		return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+	
+	dtTileCacheObstacle* ob = 0;
+	if (m_nextFreeObstacle)
+	{
+		ob = m_nextFreeObstacle;
+		m_nextFreeObstacle = ob->next;
+		ob->next = 0;
+	}
+	if (!ob)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	unsigned short salt = ob->salt;
+	memset(ob, 0, sizeof(dtTileCacheObstacle));
+	ob->salt = salt;
+	ob->state = DT_OBSTACLE_PROCESSING;
+	ob->type = DT_OBSTACLE_BOX;
+	dtVcopy(ob->box.bmin, bmin);
+	dtVcopy(ob->box.bmax, bmax);
+	
+	ObstacleRequest* req = &m_reqs[m_nreqs++];
+	memset(req, 0, sizeof(ObstacleRequest));
+	req->action = REQUEST_ADD;
+	req->ref = getObstacleRef(ob);
+	
+	if (result)
+		*result = req->ref;
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::addBoxObstacle(const float* center, const float* halfExtents, const float yRadians, dtObstacleRef* result)
+{
+	if (m_nreqs >= MAX_REQUESTS)
+		return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+
+	dtTileCacheObstacle* ob = 0;
+	if (m_nextFreeObstacle)
+	{
+		ob = m_nextFreeObstacle;
+		m_nextFreeObstacle = ob->next;
+		ob->next = 0;
+	}
+	if (!ob)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	unsigned short salt = ob->salt;
+	memset(ob, 0, sizeof(dtTileCacheObstacle));
+	ob->salt = salt;
+	ob->state = DT_OBSTACLE_PROCESSING;
+	ob->type = DT_OBSTACLE_ORIENTED_BOX;
+	dtVcopy(ob->orientedBox.center, center);
+	dtVcopy(ob->orientedBox.halfExtents, halfExtents);
+
+	float coshalf= cosf(0.5f*yRadians);
+	float sinhalf = sinf(-0.5f*yRadians);
+	ob->orientedBox.rotAux[0] = coshalf*sinhalf;
+	ob->orientedBox.rotAux[1] = coshalf*coshalf - 0.5f;
+
+	ObstacleRequest* req = &m_reqs[m_nreqs++];
+	memset(req, 0, sizeof(ObstacleRequest));
+	req->action = REQUEST_ADD;
+	req->ref = getObstacleRef(ob);
+
+	if (result)
+		*result = req->ref;
+
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::removeObstacle(const dtObstacleRef ref)
+{
+	if (!ref)
+		return DT_SUCCESS;
+	if (m_nreqs >= MAX_REQUESTS)
+		return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+	
+	ObstacleRequest* req = &m_reqs[m_nreqs++];
+	memset(req, 0, sizeof(ObstacleRequest));
+	req->action = REQUEST_REMOVE;
+	req->ref = ref;
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::queryTiles(const float* bmin, const float* bmax,
+								 dtCompressedTileRef* results, int* resultCount, const int maxResults) const 
+{
+	const int MAX_TILES = 32;
+	dtCompressedTileRef tiles[MAX_TILES];
+	
+	int n = 0;
+	
+	const float tw = m_params.width * m_params.cs;
+	const float th = m_params.height * m_params.cs;
+	const int tx0 = (int)dtMathFloorf((bmin[0]-m_params.orig[0]) / tw);
+	const int tx1 = (int)dtMathFloorf((bmax[0]-m_params.orig[0]) / tw);
+	const int ty0 = (int)dtMathFloorf((bmin[2]-m_params.orig[2]) / th);
+	const int ty1 = (int)dtMathFloorf((bmax[2]-m_params.orig[2]) / th);
+	
+	for (int ty = ty0; ty <= ty1; ++ty)
+	{
+		for (int tx = tx0; tx <= tx1; ++tx)
+		{
+			const int ntiles = getTilesAt(tx,ty,tiles,MAX_TILES);
+			
+			for (int i = 0; i < ntiles; ++i)
+			{
+				const dtCompressedTile* tile = &m_tiles[decodeTileIdTile(tiles[i])];
+				float tbmin[3], tbmax[3];
+				calcTightTileBounds(tile->header, tbmin, tbmax);
+				
+				if (dtOverlapBounds(bmin,bmax, tbmin,tbmax))
+				{
+					if (n < maxResults)
+						results[n++] = tiles[i];
+				}
+			}
+		}
+	}
+	
+	*resultCount = n;
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::update(const float /*dt*/, dtNavMesh* navmesh,
+							 bool* upToDate)
+{
+	if (m_nupdate == 0)
+	{
+		// Process requests.
+		for (int i = 0; i < m_nreqs; ++i)
+		{
+			ObstacleRequest* req = &m_reqs[i];
+			
+			unsigned int idx = decodeObstacleIdObstacle(req->ref);
+			if ((int)idx >= m_params.maxObstacles)
+				continue;
+			dtTileCacheObstacle* ob = &m_obstacles[idx];
+			unsigned int salt = decodeObstacleIdSalt(req->ref);
+			if (ob->salt != salt)
+				continue;
+			
+			if (req->action == REQUEST_ADD)
+			{
+				// Find touched tiles.
+				float bmin[3], bmax[3];
+				getObstacleBounds(ob, bmin, bmax);
+
+				int ntouched = 0;
+				queryTiles(bmin, bmax, ob->touched, &ntouched, DT_MAX_TOUCHED_TILES);
+				ob->ntouched = (unsigned char)ntouched;
+				// Add tiles to update list.
+				ob->npending = 0;
+				for (int j = 0; j < ob->ntouched; ++j)
+				{
+					if (m_nupdate < MAX_UPDATE)
+					{
+						if (!contains(m_update, m_nupdate, ob->touched[j]))
+							m_update[m_nupdate++] = ob->touched[j];
+						ob->pending[ob->npending++] = ob->touched[j];
+					}
+				}
+			}
+			else if (req->action == REQUEST_REMOVE)
+			{
+				// Prepare to remove obstacle.
+				ob->state = DT_OBSTACLE_REMOVING;
+				// Add tiles to update list.
+				ob->npending = 0;
+				for (int j = 0; j < ob->ntouched; ++j)
+				{
+					if (m_nupdate < MAX_UPDATE)
+					{
+						if (!contains(m_update, m_nupdate, ob->touched[j]))
+							m_update[m_nupdate++] = ob->touched[j];
+						ob->pending[ob->npending++] = ob->touched[j];
+					}
+				}
+			}
+		}
+		
+		m_nreqs = 0;
+	}
+	
+	dtStatus status = DT_SUCCESS;
+	// Process updates
+	if (m_nupdate)
+	{
+		// Build mesh
+		const dtCompressedTileRef ref = m_update[0];
+		status = buildNavMeshTile(ref, navmesh);
+		m_nupdate--;
+		if (m_nupdate > 0)
+			memmove(m_update, m_update+1, m_nupdate*sizeof(dtCompressedTileRef));
+
+		// Update obstacle states.
+		for (int i = 0; i < m_params.maxObstacles; ++i)
+		{
+			dtTileCacheObstacle* ob = &m_obstacles[i];
+			if (ob->state == DT_OBSTACLE_PROCESSING || ob->state == DT_OBSTACLE_REMOVING)
+			{
+				// Remove handled tile from pending list.
+				for (int j = 0; j < (int)ob->npending; j++)
+				{
+					if (ob->pending[j] == ref)
+					{
+						ob->pending[j] = ob->pending[(int)ob->npending-1];
+						ob->npending--;
+						break;
+					}
+				}
+				
+				// If all pending tiles processed, change state.
+				if (ob->npending == 0)
+				{
+					if (ob->state == DT_OBSTACLE_PROCESSING)
+					{
+						ob->state = DT_OBSTACLE_PROCESSED;
+					}
+					else if (ob->state == DT_OBSTACLE_REMOVING)
+					{
+						ob->state = DT_OBSTACLE_EMPTY;
+						// Update salt, salt should never be zero.
+						ob->salt = (ob->salt+1) & ((1<<16)-1);
+						if (ob->salt == 0)
+							ob->salt++;
+						// Return obstacle to free list.
+						ob->next = m_nextFreeObstacle;
+						m_nextFreeObstacle = ob;
+					}
+				}
+			}
+		}
+	}
+	
+	if (upToDate)
+		*upToDate = m_nupdate == 0 && m_nreqs == 0;
+
+	return status;
+}
+
+
+dtStatus dtTileCache::buildNavMeshTilesAt(const int tx, const int ty, dtNavMesh* navmesh)
+{
+	const int MAX_TILES = 32;
+	dtCompressedTileRef tiles[MAX_TILES];
+	const int ntiles = getTilesAt(tx,ty,tiles,MAX_TILES);
+	
+	for (int i = 0; i < ntiles; ++i)
+	{
+		dtStatus status = buildNavMeshTile(tiles[i], navmesh);
+		if (dtStatusFailed(status))
+			return status;
+	}
+	
+	return DT_SUCCESS;
+}
+
+dtStatus dtTileCache::buildNavMeshTile(const dtCompressedTileRef ref, dtNavMesh* navmesh)
+{	
+	dtAssert(m_talloc);
+	dtAssert(m_tcomp);
+	
+	unsigned int idx = decodeTileIdTile(ref);
+	if (idx > (unsigned int)m_params.maxTiles)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	const dtCompressedTile* tile = &m_tiles[idx];
+	unsigned int salt = decodeTileIdSalt(ref);
+	if (tile->salt != salt)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	
+	m_talloc->reset();
+	
+	NavMeshTileBuildContext bc(m_talloc);
+	const int walkableClimbVx = (int)(m_params.walkableClimb / m_params.ch);
+	dtStatus status;
+	
+	// Decompress tile layer data. 
+	status = dtDecompressTileCacheLayer(m_talloc, m_tcomp, tile->data, tile->dataSize, &bc.layer);
+	if (dtStatusFailed(status))
+		return status;
+	
+	// Rasterize obstacles.
+	for (int i = 0; i < m_params.maxObstacles; ++i)
+	{
+		const dtTileCacheObstacle* ob = &m_obstacles[i];
+		if (ob->state == DT_OBSTACLE_EMPTY || ob->state == DT_OBSTACLE_REMOVING)
+			continue;
+		if (contains(ob->touched, ob->ntouched, ref))
+		{
+			if (ob->type == DT_OBSTACLE_CYLINDER)
+			{
+				dtMarkCylinderArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch,
+							    ob->cylinder.pos, ob->cylinder.radius, ob->cylinder.height, 0);
+			}
+			else if (ob->type == DT_OBSTACLE_BOX)
+			{
+				dtMarkBoxArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch,
+					ob->box.bmin, ob->box.bmax, 0);
+			}
+			else if (ob->type == DT_OBSTACLE_ORIENTED_BOX)
+			{
+				dtMarkBoxArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch,
+					ob->orientedBox.center, ob->orientedBox.halfExtents, ob->orientedBox.rotAux, 0);
+			}
+		}
+	}
+	
+	// Build navmesh
+	status = dtBuildTileCacheRegions(m_talloc, *bc.layer, walkableClimbVx);
+	if (dtStatusFailed(status))
+		return status;
+	
+	bc.lcset = dtAllocTileCacheContourSet(m_talloc);
+	if (!bc.lcset)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	status = dtBuildTileCacheContours(m_talloc, *bc.layer, walkableClimbVx,
+									  m_params.maxSimplificationError, *bc.lcset);
+	if (dtStatusFailed(status))
+		return status;
+	
+	bc.lmesh = dtAllocTileCachePolyMesh(m_talloc);
+	if (!bc.lmesh)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	status = dtBuildTileCachePolyMesh(m_talloc, *bc.lcset, *bc.lmesh);
+	if (dtStatusFailed(status))
+		return status;
+	
+	// Early out if the mesh tile is empty.
+	if (!bc.lmesh->npolys)
+	{
+		// Remove existing tile.
+		navmesh->removeTile(navmesh->getTileRefAt(tile->header->tx,tile->header->ty,tile->header->tlayer),0,0);
+		return DT_SUCCESS;
+	}
+	
+	dtNavMeshCreateParams params;
+	memset(&params, 0, sizeof(params));
+	params.verts = bc.lmesh->verts;
+	params.vertCount = bc.lmesh->nverts;
+	params.polys = bc.lmesh->polys;
+	params.polyAreas = bc.lmesh->areas;
+	params.polyFlags = bc.lmesh->flags;
+	params.polyCount = bc.lmesh->npolys;
+	params.nvp = DT_VERTS_PER_POLYGON;
+	params.walkableHeight = m_params.walkableHeight;
+	params.walkableRadius = m_params.walkableRadius;
+	params.walkableClimb = m_params.walkableClimb;
+	params.tileX = tile->header->tx;
+	params.tileY = tile->header->ty;
+	params.tileLayer = tile->header->tlayer;
+	params.cs = m_params.cs;
+	params.ch = m_params.ch;
+	params.buildBvTree = false;
+	dtVcopy(params.bmin, tile->header->bmin);
+	dtVcopy(params.bmax, tile->header->bmax);
+	
+	if (m_tmproc)
+	{
+		m_tmproc->process(&params, bc.lmesh->areas, bc.lmesh->flags);
+	}
+	
+	unsigned char* navData = 0;
+	int navDataSize = 0;
+	if (!dtCreateNavMeshData(&params, &navData, &navDataSize))
+		return DT_FAILURE;
+
+	// Remove existing tile.
+	navmesh->removeTile(navmesh->getTileRefAt(tile->header->tx,tile->header->ty,tile->header->tlayer),0,0);
+
+	// Add new tile, or leave the location empty.
+	if (navData)
+	{
+		// Let the navmesh own the data.
+		status = navmesh->addTile(navData,navDataSize,DT_TILE_FREE_DATA,0,0);
+		if (dtStatusFailed(status))
+		{
+			dtFree(navData);
+			return status;
+		}
+	}
+	
+	return DT_SUCCESS;
+}
+
+void dtTileCache::calcTightTileBounds(const dtTileCacheLayerHeader* header, float* bmin, float* bmax) const
+{
+	const float cs = m_params.cs;
+	bmin[0] = header->bmin[0] + header->minx*cs;
+	bmin[1] = header->bmin[1];
+	bmin[2] = header->bmin[2] + header->miny*cs;
+	bmax[0] = header->bmin[0] + (header->maxx+1)*cs;
+	bmax[1] = header->bmax[1];
+	bmax[2] = header->bmin[2] + (header->maxy+1)*cs;
+}
+
+void dtTileCache::getObstacleBounds(const struct dtTileCacheObstacle* ob, float* bmin, float* bmax) const
+{
+	if (ob->type == DT_OBSTACLE_CYLINDER)
+	{
+		const dtObstacleCylinder &cl = ob->cylinder;
+
+		bmin[0] = cl.pos[0] - cl.radius;
+		bmin[1] = cl.pos[1];
+		bmin[2] = cl.pos[2] - cl.radius;
+		bmax[0] = cl.pos[0] + cl.radius;
+		bmax[1] = cl.pos[1] + cl.height;
+		bmax[2] = cl.pos[2] + cl.radius;
+	}
+	else if (ob->type == DT_OBSTACLE_BOX)
+	{
+		dtVcopy(bmin, ob->box.bmin);
+		dtVcopy(bmax, ob->box.bmax);
+	}
+	else if (ob->type == DT_OBSTACLE_ORIENTED_BOX)
+	{
+		const dtObstacleOrientedBox &orientedBox = ob->orientedBox;
+
+		float maxr = 1.41f*dtMax(orientedBox.halfExtents[0], orientedBox.halfExtents[2]);
+		bmin[0] = orientedBox.center[0] - maxr;
+		bmax[0] = orientedBox.center[0] + maxr;
+		bmin[1] = orientedBox.center[1] - orientedBox.halfExtents[1];
+		bmax[1] = orientedBox.center[1] + orientedBox.halfExtents[1];
+		bmin[2] = orientedBox.center[2] - maxr;
+		bmax[2] = orientedBox.center[2] + maxr;
+	}
+}

panda/src/recastdetour/DetourTileCache.h

@@ -0,0 +1,262 @@
+#ifndef DETOURTILECACHE_H
+#define DETOURTILECACHE_H
+
+#include "DetourStatus.h"
+
+
+
+typedef unsigned int dtObstacleRef;
+
+typedef unsigned int dtCompressedTileRef;
+
+/// Flags for addTile
+enum dtCompressedTileFlags
+{
+	DT_COMPRESSEDTILE_FREE_DATA = 0x01,					///< Navmesh owns the tile memory and should free it.
+};
+
+struct dtCompressedTile
+{
+	unsigned int salt;						///< Counter describing modifications to the tile.
+	struct dtTileCacheLayerHeader* header;
+	unsigned char* compressed;
+	int compressedSize;
+	unsigned char* data;
+	int dataSize;
+	unsigned int flags;
+	dtCompressedTile* next;
+};
+
+enum ObstacleState
+{
+	DT_OBSTACLE_EMPTY,
+	DT_OBSTACLE_PROCESSING,
+	DT_OBSTACLE_PROCESSED,
+	DT_OBSTACLE_REMOVING,
+};
+
+enum ObstacleType
+{
+	DT_OBSTACLE_CYLINDER,
+	DT_OBSTACLE_BOX, // AABB
+	DT_OBSTACLE_ORIENTED_BOX, // OBB
+};
+
+struct dtObstacleCylinder
+{
+	float pos[ 3 ];
+	float radius;
+	float height;
+};
+
+struct dtObstacleBox
+{
+	float bmin[ 3 ];
+	float bmax[ 3 ];
+};
+
+struct dtObstacleOrientedBox
+{
+	float center[ 3 ];
+	float halfExtents[ 3 ];
+	float rotAux[ 2 ]; //{ cos(0.5f*angle)*sin(-0.5f*angle); cos(0.5f*angle)*cos(0.5f*angle) - 0.5 }
+};
+
+static const int DT_MAX_TOUCHED_TILES = 8;
+struct dtTileCacheObstacle
+{
+	union
+	{
+		dtObstacleCylinder cylinder;
+		dtObstacleBox box;
+		dtObstacleOrientedBox orientedBox;
+	};
+
+	dtCompressedTileRef touched[DT_MAX_TOUCHED_TILES];
+	dtCompressedTileRef pending[DT_MAX_TOUCHED_TILES];
+	unsigned short salt;
+	unsigned char type;
+	unsigned char state;
+	unsigned char ntouched;
+	unsigned char npending;
+	dtTileCacheObstacle* next;
+};
+
+struct dtTileCacheParams
+{
+	float orig[3];
+	float cs, ch;
+	int width, height;
+	float walkableHeight;
+	float walkableRadius;
+	float walkableClimb;
+	float maxSimplificationError;
+	int maxTiles;
+	int maxObstacles;
+};
+
+struct dtTileCacheMeshProcess
+{
+	virtual ~dtTileCacheMeshProcess() { }
+
+	virtual void process(struct dtNavMeshCreateParams* params,
+						 unsigned char* polyAreas, unsigned short* polyFlags) = 0;
+};
+
+
+class dtTileCache
+{
+public:
+	dtTileCache();
+	~dtTileCache();
+	
+	struct dtTileCacheAlloc* getAlloc() { return m_talloc; }
+	struct dtTileCacheCompressor* getCompressor() { return m_tcomp; }
+	const dtTileCacheParams* getParams() const { return &m_params; }
+	
+	inline int getTileCount() const { return m_params.maxTiles; }
+	inline const dtCompressedTile* getTile(const int i) const { return &m_tiles[i]; }
+	
+	inline int getObstacleCount() const { return m_params.maxObstacles; }
+	inline const dtTileCacheObstacle* getObstacle(const int i) const { return &m_obstacles[i]; }
+	
+	const dtTileCacheObstacle* getObstacleByRef(dtObstacleRef ref);
+	
+	dtObstacleRef getObstacleRef(const dtTileCacheObstacle* obmin) const;
+	
+	dtStatus init(const dtTileCacheParams* params,
+				  struct dtTileCacheAlloc* talloc,
+				  struct dtTileCacheCompressor* tcomp,
+				  struct dtTileCacheMeshProcess* tmproc);
+	
+	int getTilesAt(const int tx, const int ty, dtCompressedTileRef* tiles, const int maxTiles) const ;
+	
+	dtCompressedTile* getTileAt(const int tx, const int ty, const int tlayer);
+	dtCompressedTileRef getTileRef(const dtCompressedTile* tile) const;
+	const dtCompressedTile* getTileByRef(dtCompressedTileRef ref) const;
+	
+	dtStatus addTile(unsigned char* data, const int dataSize, unsigned char flags, dtCompressedTileRef* result);
+	
+	dtStatus removeTile(dtCompressedTileRef ref, unsigned char** data, int* dataSize);
+	
+	// Cylinder obstacle.
+	dtStatus addObstacle(const float* pos, const float radius, const float height, dtObstacleRef* result);
+
+	// Aabb obstacle.
+	dtStatus addBoxObstacle(const float* bmin, const float* bmax, dtObstacleRef* result);
+
+	// Box obstacle: can be rotated in Y.
+	dtStatus addBoxObstacle(const float* center, const float* halfExtents, const float yRadians, dtObstacleRef* result);
+	
+	dtStatus removeObstacle(const dtObstacleRef ref);
+	
+	dtStatus queryTiles(const float* bmin, const float* bmax,
+						dtCompressedTileRef* results, int* resultCount, const int maxResults) const;
+	
+	/// Updates the tile cache by rebuilding tiles touched by unfinished obstacle requests.
+	///  @param[in]		dt			The time step size. Currently not used.
+	///  @param[in]		navmesh		The mesh to affect when rebuilding tiles.
+	///  @param[out]	upToDate	Whether the tile cache is fully up to date with obstacle requests and tile rebuilds.
+	///  							If the tile cache is up to date another (immediate) call to update will have no effect;
+	///  							otherwise another call will continue processing obstacle requests and tile rebuilds.
+	dtStatus update(const float dt, class dtNavMesh* navmesh, bool* upToDate = 0);
+	
+	dtStatus buildNavMeshTilesAt(const int tx, const int ty, class dtNavMesh* navmesh);
+	
+	dtStatus buildNavMeshTile(const dtCompressedTileRef ref, class dtNavMesh* navmesh);
+	
+	void calcTightTileBounds(const struct dtTileCacheLayerHeader* header, float* bmin, float* bmax) const;
+	
+	void getObstacleBounds(const struct dtTileCacheObstacle* ob, float* bmin, float* bmax) const;
+	
+
+	/// Encodes a tile id.
+	inline dtCompressedTileRef encodeTileId(unsigned int salt, unsigned int it) const
+	{
+		return ((dtCompressedTileRef)salt << m_tileBits) | (dtCompressedTileRef)it;
+	}
+	
+	/// Decodes a tile salt.
+	inline unsigned int decodeTileIdSalt(dtCompressedTileRef ref) const
+	{
+		const dtCompressedTileRef saltMask = ((dtCompressedTileRef)1<<m_saltBits)-1;
+		return (unsigned int)((ref >> m_tileBits) & saltMask);
+	}
+	
+	/// Decodes a tile id.
+	inline unsigned int decodeTileIdTile(dtCompressedTileRef ref) const
+	{
+		const dtCompressedTileRef tileMask = ((dtCompressedTileRef)1<<m_tileBits)-1;
+		return (unsigned int)(ref & tileMask);
+	}
+
+	/// Encodes an obstacle id.
+	inline dtObstacleRef encodeObstacleId(unsigned int salt, unsigned int it) const
+	{
+		return ((dtObstacleRef)salt << 16) | (dtObstacleRef)it;
+	}
+	
+	/// Decodes an obstacle salt.
+	inline unsigned int decodeObstacleIdSalt(dtObstacleRef ref) const
+	{
+		const dtObstacleRef saltMask = ((dtObstacleRef)1<<16)-1;
+		return (unsigned int)((ref >> 16) & saltMask);
+	}
+	
+	/// Decodes an obstacle id.
+	inline unsigned int decodeObstacleIdObstacle(dtObstacleRef ref) const
+	{
+		const dtObstacleRef tileMask = ((dtObstacleRef)1<<16)-1;
+		return (unsigned int)(ref & tileMask);
+	}
+	
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	dtTileCache(const dtTileCache&);
+	dtTileCache& operator=(const dtTileCache&);
+
+	enum ObstacleRequestAction
+	{
+		REQUEST_ADD,
+		REQUEST_REMOVE,
+	};
+	
+	struct ObstacleRequest
+	{
+		int action;
+		dtObstacleRef ref;
+	};
+	
+	int m_tileLutSize;						///< Tile hash lookup size (must be pot).
+	int m_tileLutMask;						///< Tile hash lookup mask.
+	
+	dtCompressedTile** m_posLookup;			///< Tile hash lookup.
+	dtCompressedTile* m_nextFreeTile;		///< Freelist of tiles.
+	dtCompressedTile* m_tiles;				///< List of tiles.
+	
+	unsigned int m_saltBits;				///< Number of salt bits in the tile ID.
+	unsigned int m_tileBits;				///< Number of tile bits in the tile ID.
+	
+	dtTileCacheParams m_params;
+	
+	dtTileCacheAlloc* m_talloc;
+	dtTileCacheCompressor* m_tcomp;
+	dtTileCacheMeshProcess* m_tmproc;
+	
+	dtTileCacheObstacle* m_obstacles;
+	dtTileCacheObstacle* m_nextFreeObstacle;
+	
+	static const int MAX_REQUESTS = 64;
+	ObstacleRequest m_reqs[MAX_REQUESTS];
+	int m_nreqs;
+	
+	static const int MAX_UPDATE = 64;
+	dtCompressedTileRef m_update[MAX_UPDATE];
+	int m_nupdate;
+};
+
+dtTileCache* dtAllocTileCache();
+void dtFreeTileCache(dtTileCache* tc);
+
+#endif

panda/src/recastdetour/DetourTileCacheBuilder.cpp

@@ -0,0 +1,2250 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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"
+#include "DetourStatus.h"
+#include "DetourAssert.h"
+#include "DetourTileCacheBuilder.h"
+#include <string.h>
+
+
+template<class T> class dtFixedArray
+{
+	dtTileCacheAlloc* m_alloc;
+	T* m_ptr;
+	const int m_size;
+	inline void operator=(dtFixedArray<T>& p);
+public:
+	inline dtFixedArray(dtTileCacheAlloc* a, const int s) : m_alloc(a), m_ptr((T*)a->alloc(sizeof(T)*s)), m_size(s) {}
+	inline ~dtFixedArray() { if (m_alloc) m_alloc->free(m_ptr); }
+	inline operator T*() { return m_ptr; }
+	inline int size() const { return m_size; }
+};
+
+inline int getDirOffsetX(int dir)
+{
+	const int offset[4] = { -1, 0, 1, 0, };
+	return offset[dir&0x03];
+}
+
+inline int getDirOffsetY(int dir)
+{
+	const int offset[4] = { 0, 1, 0, -1 };
+	return offset[dir&0x03];
+}
+
+static const int MAX_VERTS_PER_POLY = 6;	// TODO: use the DT_VERTS_PER_POLYGON
+static const int MAX_REM_EDGES = 48;		// TODO: make this an expression.
+
+
+
+dtTileCacheContourSet* dtAllocTileCacheContourSet(dtTileCacheAlloc* alloc)
+{
+	dtAssert(alloc);
+
+	dtTileCacheContourSet* cset = (dtTileCacheContourSet*)alloc->alloc(sizeof(dtTileCacheContourSet));
+	memset(cset, 0, sizeof(dtTileCacheContourSet));
+	return cset;
+}
+
+void dtFreeTileCacheContourSet(dtTileCacheAlloc* alloc, dtTileCacheContourSet* cset)
+{
+	dtAssert(alloc);
+
+	if (!cset) return;
+	for (int i = 0; i < cset->nconts; ++i)
+		alloc->free(cset->conts[i].verts);
+	alloc->free(cset->conts);
+	alloc->free(cset);
+}
+
+dtTileCachePolyMesh* dtAllocTileCachePolyMesh(dtTileCacheAlloc* alloc)
+{
+	dtAssert(alloc);
+
+	dtTileCachePolyMesh* lmesh = (dtTileCachePolyMesh*)alloc->alloc(sizeof(dtTileCachePolyMesh));
+	memset(lmesh, 0, sizeof(dtTileCachePolyMesh));
+	return lmesh;
+}
+
+void dtFreeTileCachePolyMesh(dtTileCacheAlloc* alloc, dtTileCachePolyMesh* lmesh)
+{
+	dtAssert(alloc);
+	
+	if (!lmesh) return;
+	alloc->free(lmesh->verts);
+	alloc->free(lmesh->polys);
+	alloc->free(lmesh->flags);
+	alloc->free(lmesh->areas);
+	alloc->free(lmesh);
+}
+
+
+
+struct dtLayerSweepSpan
+{
+	unsigned short ns;	// number samples
+	unsigned char id;	// region id
+	unsigned char nei;	// neighbour id
+};
+
+static const int DT_LAYER_MAX_NEIS = 16;
+
+struct dtLayerMonotoneRegion
+{
+	int area;
+	unsigned char neis[DT_LAYER_MAX_NEIS];
+	unsigned char nneis;
+	unsigned char regId;
+	unsigned char areaId;
+};
+
+struct dtTempContour
+{
+	inline dtTempContour(unsigned char* vbuf, const int nvbuf,
+						 unsigned short* pbuf, const int npbuf) :
+		verts(vbuf), nverts(0), cverts(nvbuf),
+		poly(pbuf), npoly(0), cpoly(npbuf) 
+	{
+	}
+	unsigned char* verts;
+	int nverts;
+	int cverts;
+	unsigned short* poly;
+	int npoly;
+	int cpoly;
+};
+
+
+
+
+inline bool overlapRangeExl(const unsigned short amin, const unsigned short amax,
+							const unsigned short bmin, const unsigned short bmax)
+{
+	return (amin >= bmax || amax <= bmin) ? false : true;
+}
+
+static void addUniqueLast(unsigned char* a, unsigned char& an, unsigned char v)
+{
+	const int n = (int)an;
+	if (n > 0 && a[n-1] == v) return;
+	a[an] = v;
+	an++;
+}
+
+inline bool isConnected(const dtTileCacheLayer& layer,
+						const int ia, const int ib, const int walkableClimb)
+{
+	if (layer.areas[ia] != layer.areas[ib]) return false;
+	if (dtAbs((int)layer.heights[ia] - (int)layer.heights[ib]) > walkableClimb) return false;
+	return true;
+}
+
+static bool canMerge(unsigned char oldRegId, unsigned char newRegId, const dtLayerMonotoneRegion* regs, const int nregs)
+{
+	int count = 0;
+	for (int i = 0; i < nregs; ++i)
+	{
+		const dtLayerMonotoneRegion& reg = regs[i];
+		if (reg.regId != oldRegId) continue;
+		const int nnei = (int)reg.nneis;
+		for (int j = 0; j < nnei; ++j)
+		{
+			if (regs[reg.neis[j]].regId == newRegId)
+				count++;
+		}
+	}
+	return count == 1;
+}
+
+
+dtStatus dtBuildTileCacheRegions(dtTileCacheAlloc* alloc,
+								 dtTileCacheLayer& layer,
+								 const int walkableClimb)
+{
+	dtAssert(alloc);
+	
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	
+	memset(layer.regs,0xff,sizeof(unsigned char)*w*h);
+	
+	const int nsweeps = w;
+	dtFixedArray<dtLayerSweepSpan> sweeps(alloc, nsweeps);
+	if (!sweeps)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(sweeps,0,sizeof(dtLayerSweepSpan)*nsweeps);
+	
+	// Partition walkable area into monotone regions.
+	unsigned char prevCount[256];
+	unsigned char regId = 0;
+	
+	for (int y = 0; y < h; ++y)
+	{
+		if (regId > 0)
+			memset(prevCount,0,sizeof(unsigned char)*regId);
+		unsigned char sweepId = 0;
+		
+		for (int x = 0; x < w; ++x)
+		{
+			const int idx = x + y*w;
+			if (layer.areas[idx] == DT_TILECACHE_NULL_AREA) continue;
+			
+			unsigned char sid = 0xff;
+			
+			// -x
+			const int xidx = (x-1)+y*w;
+			if (x > 0 && isConnected(layer, idx, xidx, walkableClimb))
+			{
+				if (layer.regs[xidx] != 0xff)
+					sid = layer.regs[xidx];
+			}
+			
+			if (sid == 0xff)
+			{
+				sid = sweepId++;
+				sweeps[sid].nei = 0xff;
+				sweeps[sid].ns = 0;
+			}
+			
+			// -y
+			const int yidx = x+(y-1)*w;
+			if (y > 0 && isConnected(layer, idx, yidx, walkableClimb))
+			{
+				const unsigned char nr = layer.regs[yidx];
+				if (nr != 0xff)
+				{
+					// Set neighbour when first valid neighbour is encoutered.
+					if (sweeps[sid].ns == 0)
+						sweeps[sid].nei = nr;
+					
+					if (sweeps[sid].nei == nr)
+					{
+						// Update existing neighbour
+						sweeps[sid].ns++;
+						prevCount[nr]++;
+					}
+					else
+					{
+						// This is hit if there is nore than one neighbour.
+						// Invalidate the neighbour.
+						sweeps[sid].nei = 0xff;
+					}
+				}
+			}
+			
+			layer.regs[idx] = sid;
+		}
+		
+		// Create unique ID.
+		for (int i = 0; i < sweepId; ++i)
+		{
+			// If the neighbour is set and there is only one continuous connection to it,
+			// the sweep will be merged with the previous one, else new region is created.
+			if (sweeps[i].nei != 0xff && (unsigned short)prevCount[sweeps[i].nei] == sweeps[i].ns)
+			{
+				sweeps[i].id = sweeps[i].nei;
+			}
+			else
+			{
+				if (regId == 255)
+				{
+					// Region ID's overflow.
+					return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+				}
+				sweeps[i].id = regId++;
+			}
+		}
+		
+		// Remap local sweep ids to region ids.
+		for (int x = 0; x < w; ++x)
+		{
+			const int idx = x+y*w;
+			if (layer.regs[idx] != 0xff)
+				layer.regs[idx] = sweeps[layer.regs[idx]].id;
+		}
+	}
+	
+	// Allocate and init layer regions.
+	const int nregs = (int)regId;
+	dtFixedArray<dtLayerMonotoneRegion> regs(alloc, nregs);
+	if (!regs)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	memset(regs, 0, sizeof(dtLayerMonotoneRegion)*nregs);
+	for (int i = 0; i < nregs; ++i)
+		regs[i].regId = 0xff;
+	
+	// Find region neighbours.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int idx = x+y*w;
+			const unsigned char ri = layer.regs[idx];
+			if (ri == 0xff)
+				continue;
+			
+			// Update area.
+			regs[ri].area++;
+			regs[ri].areaId = layer.areas[idx];
+			
+			// Update neighbours
+			const int ymi = x+(y-1)*w;
+			if (y > 0 && isConnected(layer, idx, ymi, walkableClimb))
+			{
+				const unsigned char rai = layer.regs[ymi];
+				if (rai != 0xff && rai != ri)
+				{
+					addUniqueLast(regs[ri].neis, regs[ri].nneis, rai);
+					addUniqueLast(regs[rai].neis, regs[rai].nneis, ri);
+				}
+			}
+		}
+	}
+	
+	for (int i = 0; i < nregs; ++i)
+		regs[i].regId = (unsigned char)i;
+	
+	for (int i = 0; i < nregs; ++i)
+	{
+		dtLayerMonotoneRegion& reg = regs[i];
+		
+		int merge = -1;
+		int mergea = 0;
+		for (int j = 0; j < (int)reg.nneis; ++j)
+		{
+			const unsigned char nei = reg.neis[j];
+			dtLayerMonotoneRegion& regn = regs[nei];
+			if (reg.regId == regn.regId)
+				continue;
+			if (reg.areaId != regn.areaId)
+				continue;
+			if (regn.area > mergea)
+			{
+				if (canMerge(reg.regId, regn.regId, regs, nregs))
+				{
+					mergea = regn.area;
+					merge = (int)nei;
+				}
+			}
+		}
+		if (merge != -1)
+		{
+			const unsigned char oldId = reg.regId;
+			const unsigned char newId = regs[merge].regId;
+			for (int j = 0; j < nregs; ++j)
+				if (regs[j].regId == oldId)
+					regs[j].regId = newId;
+		}
+	}
+	
+	// Compact ids.
+	unsigned char remap[256];
+	memset(remap, 0, 256);
+	// Find number of unique regions.
+	regId = 0;
+	for (int i = 0; i < nregs; ++i)
+		remap[regs[i].regId] = 1;
+	for (int i = 0; i < 256; ++i)
+		if (remap[i])
+			remap[i] = regId++;
+	// Remap ids.
+	for (int i = 0; i < nregs; ++i)
+		regs[i].regId = remap[regs[i].regId];
+	
+	layer.regCount = regId;
+	
+	for (int i = 0; i < w*h; ++i)
+	{
+		if (layer.regs[i] != 0xff)
+			layer.regs[i] = regs[layer.regs[i]].regId;
+	}
+	
+	return DT_SUCCESS;
+}
+
+
+
+static bool appendVertex(dtTempContour& cont, const int x, const int y, const int z, const int r)
+{
+	// Try to merge with existing segments.
+	if (cont.nverts > 1)
+	{
+		unsigned char* pa = &cont.verts[(cont.nverts-2)*4];
+		unsigned char* pb = &cont.verts[(cont.nverts-1)*4];
+		if ((int)pb[3] == r)
+		{
+			if (pa[0] == pb[0] && (int)pb[0] == x)
+			{
+				// The verts are aligned aling x-axis, update z.
+				pb[1] = (unsigned char)y;
+				pb[2] = (unsigned char)z;
+				return true;
+			}
+			else if (pa[2] == pb[2] && (int)pb[2] == z)
+			{
+				// The verts are aligned aling z-axis, update x.
+				pb[0] = (unsigned char)x;
+				pb[1] = (unsigned char)y;
+				return true;
+			}
+		}
+	}
+	
+	// Add new point.
+	if (cont.nverts+1 > cont.cverts)
+		return false;
+	
+	unsigned char* v = &cont.verts[cont.nverts*4];
+	v[0] = (unsigned char)x;
+	v[1] = (unsigned char)y;
+	v[2] = (unsigned char)z;
+	v[3] = (unsigned char)r;
+	cont.nverts++;
+	
+	return true;
+}
+
+
+static unsigned char getNeighbourReg(dtTileCacheLayer& layer,
+									 const int ax, const int ay, const int dir)
+{
+	const int w = (int)layer.header->width;
+	const int ia = ax + ay*w;
+	
+	const unsigned char con = layer.cons[ia] & 0xf;
+	const unsigned char portal = layer.cons[ia] >> 4;
+	const unsigned char mask = (unsigned char)(1<<dir);
+	
+	if ((con & mask) == 0)
+	{
+		// No connection, return portal or hard edge.
+		if (portal & mask)
+			return 0xf8 + (unsigned char)dir;
+		return 0xff;
+	}
+	
+	const int bx = ax + getDirOffsetX(dir);
+	const int by = ay + getDirOffsetY(dir);
+	const int ib = bx + by*w;
+	
+	return layer.regs[ib];
+}
+
+static bool walkContour(dtTileCacheLayer& layer, int x, int y, dtTempContour& cont)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	
+	cont.nverts = 0;
+	
+	int startX = x;
+	int startY = y;
+	int startDir = -1;
+	
+	for (int i = 0; i < 4; ++i)
+	{
+		const int dir = (i+3)&3;
+		unsigned char rn = getNeighbourReg(layer, x, y, dir);
+		if (rn != layer.regs[x+y*w])
+		{
+			startDir = dir;
+			break;
+		}
+	}
+	if (startDir == -1)
+		return true;
+	
+	int dir = startDir;
+	const int maxIter = w*h;
+	
+	int iter = 0;
+	while (iter < maxIter)
+	{
+		unsigned char rn = getNeighbourReg(layer, x, y, dir);
+		
+		int nx = x;
+		int ny = y;
+		int ndir = dir;
+		
+		if (rn != layer.regs[x+y*w])
+		{
+			// Solid edge.
+			int px = x;
+			int pz = y;
+			switch(dir)
+			{
+				case 0: pz++; break;
+				case 1: px++; pz++; break;
+				case 2: px++; break;
+			}
+			
+			// Try to merge with previous vertex.
+			if (!appendVertex(cont, px, (int)layer.heights[x+y*w], pz,rn))
+				return false;
+			
+			ndir = (dir+1) & 0x3;  // Rotate CW
+		}
+		else
+		{
+			// Move to next.
+			nx = x + getDirOffsetX(dir);
+			ny = y + getDirOffsetY(dir);
+			ndir = (dir+3) & 0x3;	// Rotate CCW
+		}
+		
+		if (iter > 0 && x == startX && y == startY && dir == startDir)
+			break;
+		
+		x = nx;
+		y = ny;
+		dir = ndir;
+		
+		iter++;
+	}
+	
+	// Remove last vertex if it is duplicate of the first one.
+	unsigned char* pa = &cont.verts[(cont.nverts-1)*4];
+	unsigned char* pb = &cont.verts[0];
+	if (pa[0] == pb[0] && pa[2] == pb[2])
+		cont.nverts--;
+	
+	return true;
+}	
+
+
+static float distancePtSeg(const int x, const int z,
+						   const int px, const int pz,
+						   const int qx, const int qz)
+{
+	float pqx = (float)(qx - px);
+	float pqz = (float)(qz - pz);
+	float dx = (float)(x - px);
+	float dz = (float)(z - pz);
+	float d = pqx*pqx + pqz*pqz;
+	float t = pqx*dx + pqz*dz;
+	if (d > 0)
+		t /= d;
+	if (t < 0)
+		t = 0;
+	else if (t > 1)
+		t = 1;
+	
+	dx = px + t*pqx - x;
+	dz = pz + t*pqz - z;
+	
+	return dx*dx + dz*dz;
+}
+
+static void simplifyContour(dtTempContour& cont, const float maxError)
+{
+	cont.npoly = 0;
+	
+	for (int i = 0; i < cont.nverts; ++i)
+	{
+		int j = (i+1) % cont.nverts;
+		// Check for start of a wall segment.
+		unsigned char ra = cont.verts[j*4+3];
+		unsigned char rb = cont.verts[i*4+3];
+		if (ra != rb)
+			cont.poly[cont.npoly++] = (unsigned short)i;
+	}
+	if (cont.npoly < 2)
+	{
+		// If there is no transitions at all,
+		// create some initial points for the simplification process. 
+		// Find lower-left and upper-right vertices of the contour.
+		int llx = cont.verts[0];
+		int llz = cont.verts[2];
+		int lli = 0;
+		int urx = cont.verts[0];
+		int urz = cont.verts[2];
+		int uri = 0;
+		for (int i = 1; i < cont.nverts; ++i)
+		{
+			int x = cont.verts[i*4+0];
+			int z = cont.verts[i*4+2];
+			if (x < llx || (x == llx && z < llz))
+			{
+				llx = x;
+				llz = z;
+				lli = i;
+			}
+			if (x > urx || (x == urx && z > urz))
+			{
+				urx = x;
+				urz = z;
+				uri = i;
+			}
+		}
+		cont.npoly = 0;
+		cont.poly[cont.npoly++] = (unsigned short)lli;
+		cont.poly[cont.npoly++] = (unsigned short)uri;
+	}
+	
+	// Add points until all raw points are within
+	// error tolerance to the simplified shape.
+	for (int i = 0; i < cont.npoly; )
+	{
+		int ii = (i+1) % cont.npoly;
+		
+		const int ai = (int)cont.poly[i];
+		const int ax = (int)cont.verts[ai*4+0];
+		const int az = (int)cont.verts[ai*4+2];
+		
+		const int bi = (int)cont.poly[ii];
+		const int bx = (int)cont.verts[bi*4+0];
+		const int bz = (int)cont.verts[bi*4+2];
+		
+		// Find maximum deviation from the segment.
+		float maxd = 0;
+		int maxi = -1;
+		int ci, cinc, endi;
+		
+		// Traverse the segment in lexilogical order so that the
+		// max deviation is calculated similarly when traversing
+		// opposite segments.
+		if (bx > ax || (bx == ax && bz > az))
+		{
+			cinc = 1;
+			ci = (ai+cinc) % cont.nverts;
+			endi = bi;
+		}
+		else
+		{
+			cinc = cont.nverts-1;
+			ci = (bi+cinc) % cont.nverts;
+			endi = ai;
+		}
+		
+		// Tessellate only outer edges or edges between areas.
+		while (ci != endi)
+		{
+			float d = distancePtSeg(cont.verts[ci*4+0], cont.verts[ci*4+2], ax, az, bx, bz);
+			if (d > maxd)
+			{
+				maxd = d;
+				maxi = ci;
+			}
+			ci = (ci+cinc) % cont.nverts;
+		}
+		
+		
+		// If the max deviation is larger than accepted error,
+		// add new point, else continue to next segment.
+		if (maxi != -1 && maxd > (maxError*maxError))
+		{
+			cont.npoly++;
+			for (int j = cont.npoly-1; j > i; --j)
+				cont.poly[j] = cont.poly[j-1];
+			cont.poly[i+1] = (unsigned short)maxi;
+		}
+		else
+		{
+			++i;
+		}
+	}
+	
+	// Remap vertices
+	int start = 0;
+	for (int i = 1; i < cont.npoly; ++i)
+		if (cont.poly[i] < cont.poly[start])
+			start = i;
+	
+	cont.nverts = 0;
+	for (int i = 0; i < cont.npoly; ++i)
+	{
+		const int j = (start+i) % cont.npoly;
+		unsigned char* src = &cont.verts[cont.poly[j]*4];
+		unsigned char* dst = &cont.verts[cont.nverts*4];
+		dst[0] = src[0];
+		dst[1] = src[1];
+		dst[2] = src[2];
+		dst[3] = src[3];
+		cont.nverts++;
+	}
+}
+
+static unsigned char getCornerHeight(dtTileCacheLayer& layer,
+									 const int x, const int y, const int z,
+									 const int walkableClimb,
+									 bool& shouldRemove)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	
+	int n = 0;
+	
+	unsigned char portal = 0xf;
+	unsigned char height = 0;
+	unsigned char preg = 0xff;
+	bool allSameReg = true;
+	
+	for (int dz = -1; dz <= 0; ++dz)
+	{
+		for (int dx = -1; dx <= 0; ++dx)
+		{
+			const int px = x+dx;
+			const int pz = z+dz;
+			if (px >= 0 && pz >= 0 && px < w && pz < h)
+			{
+				const int idx  = px + pz*w;
+				const int lh = (int)layer.heights[idx];
+				if (dtAbs(lh-y) <= walkableClimb && layer.areas[idx] != DT_TILECACHE_NULL_AREA)
+				{
+					height = dtMax(height, (unsigned char)lh);
+					portal &= (layer.cons[idx] >> 4);
+					if (preg != 0xff && preg != layer.regs[idx])
+						allSameReg = false;
+					preg = layer.regs[idx]; 
+					n++;
+				}
+			}
+		}
+	}
+	
+	int portalCount = 0;
+	for (int dir = 0; dir < 4; ++dir)
+		if (portal & (1<<dir))
+			portalCount++;
+	
+	shouldRemove = false;
+	if (n > 1 && portalCount == 1 && allSameReg)
+	{
+		shouldRemove = true;
+	}
+	
+	return height;
+}
+
+
+// TODO: move this somewhere else, once the layer meshing is done.
+dtStatus dtBuildTileCacheContours(dtTileCacheAlloc* alloc,
+								  dtTileCacheLayer& layer,
+								  const int walkableClimb, 	const float maxError,
+								  dtTileCacheContourSet& lcset)
+{
+	dtAssert(alloc);
+
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	
+	lcset.nconts = layer.regCount;
+	lcset.conts = (dtTileCacheContour*)alloc->alloc(sizeof(dtTileCacheContour)*lcset.nconts);
+	if (!lcset.conts)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(lcset.conts, 0, sizeof(dtTileCacheContour)*lcset.nconts);
+	
+	// Allocate temp buffer for contour tracing.
+	const int maxTempVerts = (w+h)*2 * 2; // Twice around the layer.
+	
+	dtFixedArray<unsigned char> tempVerts(alloc, maxTempVerts*4);
+	if (!tempVerts)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	dtFixedArray<unsigned short> tempPoly(alloc, maxTempVerts);
+	if (!tempPoly)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	dtTempContour temp(tempVerts, maxTempVerts, tempPoly, maxTempVerts);
+	
+	// Find contours.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int idx = x+y*w;
+			const unsigned char ri = layer.regs[idx];
+			if (ri == 0xff)
+				continue;
+			
+			dtTileCacheContour& cont = lcset.conts[ri];
+			
+			if (cont.nverts > 0)
+				continue;
+			
+			cont.reg = ri;
+			cont.area = layer.areas[idx];
+			
+			if (!walkContour(layer, x, y, temp))
+			{
+				// Too complex contour.
+				// Note: If you hit here ofte, try increasing 'maxTempVerts'.
+				return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+			}
+			
+			simplifyContour(temp, maxError);
+			
+			// Store contour.
+			cont.nverts = temp.nverts;
+			if (cont.nverts > 0)
+			{
+				cont.verts = (unsigned char*)alloc->alloc(sizeof(unsigned char)*4*temp.nverts);
+				if (!cont.verts)
+					return DT_FAILURE | DT_OUT_OF_MEMORY;
+				
+				for (int i = 0, j = temp.nverts-1; i < temp.nverts; j=i++)
+				{
+					unsigned char* dst = &cont.verts[j*4];
+					unsigned char* v = &temp.verts[j*4];
+					unsigned char* vn = &temp.verts[i*4];
+					unsigned char nei = vn[3]; // The neighbour reg is stored at segment vertex of a segment. 
+					bool shouldRemove = false;
+					unsigned char lh = getCornerHeight(layer, (int)v[0], (int)v[1], (int)v[2],
+													   walkableClimb, shouldRemove);
+					
+					dst[0] = v[0];
+					dst[1] = lh;
+					dst[2] = v[2];
+					
+					// Store portal direction and remove status to the fourth component.
+					dst[3] = 0x0f;
+					if (nei != 0xff && nei >= 0xf8)
+						dst[3] = nei - 0xf8;
+					if (shouldRemove)
+						dst[3] |= 0x80;
+				}
+			}
+		}
+	}
+	
+	return DT_SUCCESS;
+}	
+
+
+
+static const int VERTEX_BUCKET_COUNT2 = (1<<8);
+
+inline int computeVertexHash2(int x, int y, int z)
+{
+	const unsigned int h1 = 0x8da6b343; // Large multiplicative constants;
+	const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes
+	const unsigned int h3 = 0xcb1ab31f;
+	unsigned int n = h1 * x + h2 * y + h3 * z;
+	return (int)(n & (VERTEX_BUCKET_COUNT2-1));
+}
+
+static unsigned short addVertex(unsigned short x, unsigned short y, unsigned short z,
+								unsigned short* verts, unsigned short* firstVert, unsigned short* nextVert, int& nv)
+{
+	int bucket = computeVertexHash2(x, 0, z);
+	unsigned short i = firstVert[bucket];
+	
+	while (i != DT_TILECACHE_NULL_IDX)
+	{
+		const unsigned short* v = &verts[i*3];
+		if (v[0] == x && v[2] == z && (dtAbs(v[1] - y) <= 2))
+			return i;
+		i = nextVert[i]; // next
+	}
+	
+	// Could not find, create new.
+	i = (unsigned short)nv; nv++;
+	unsigned short* v = &verts[i*3];
+	v[0] = x;
+	v[1] = y;
+	v[2] = z;
+	nextVert[i] = firstVert[bucket];
+	firstVert[bucket] = i;
+	
+	return (unsigned short)i;
+}
+
+
+struct rcEdge
+{
+	unsigned short vert[2];
+	unsigned short polyEdge[2];
+	unsigned short poly[2];
+};
+
+static bool buildMeshAdjacency(dtTileCacheAlloc* alloc,
+							   unsigned short* polys, const int npolys,
+							   const unsigned short* verts, const int nverts,
+							   const dtTileCacheContourSet& lcset)
+{
+	// Based on code by Eric Lengyel from:
+	// http://www.terathon.com/code/edges.php
+	
+	const int maxEdgeCount = npolys*MAX_VERTS_PER_POLY;
+	dtFixedArray<unsigned short> firstEdge(alloc, nverts + maxEdgeCount);
+	if (!firstEdge)
+		return false;
+	unsigned short* nextEdge = firstEdge + nverts;
+	int edgeCount = 0;
+	
+	dtFixedArray<rcEdge> edges(alloc, maxEdgeCount);
+	if (!edges)
+		return false;
+	
+	for (int i = 0; i < nverts; i++)
+		firstEdge[i] = DT_TILECACHE_NULL_IDX;
+	
+	for (int i = 0; i < npolys; ++i)
+	{
+		unsigned short* t = &polys[i*MAX_VERTS_PER_POLY*2];
+		for (int j = 0; j < MAX_VERTS_PER_POLY; ++j)
+		{
+			if (t[j] == DT_TILECACHE_NULL_IDX) break;
+			unsigned short v0 = t[j];
+			unsigned short v1 = (j+1 >= MAX_VERTS_PER_POLY || t[j+1] == DT_TILECACHE_NULL_IDX) ? t[0] : t[j+1];
+			if (v0 < v1)
+			{
+				rcEdge& edge = edges[edgeCount];
+				edge.vert[0] = v0;
+				edge.vert[1] = v1;
+				edge.poly[0] = (unsigned short)i;
+				edge.polyEdge[0] = (unsigned short)j;
+				edge.poly[1] = (unsigned short)i;
+				edge.polyEdge[1] = 0xff;
+				// Insert edge
+				nextEdge[edgeCount] = firstEdge[v0];
+				firstEdge[v0] = (unsigned short)edgeCount;
+				edgeCount++;
+			}
+		}
+	}
+	
+	for (int i = 0; i < npolys; ++i)
+	{
+		unsigned short* t = &polys[i*MAX_VERTS_PER_POLY*2];
+		for (int j = 0; j < MAX_VERTS_PER_POLY; ++j)
+		{
+			if (t[j] == DT_TILECACHE_NULL_IDX) break;
+			unsigned short v0 = t[j];
+			unsigned short v1 = (j+1 >= MAX_VERTS_PER_POLY || t[j+1] == DT_TILECACHE_NULL_IDX) ? t[0] : t[j+1];
+			if (v0 > v1)
+			{
+				bool found = false;
+				for (unsigned short e = firstEdge[v1]; e != DT_TILECACHE_NULL_IDX; e = nextEdge[e])
+				{
+					rcEdge& edge = edges[e];
+					if (edge.vert[1] == v0 && edge.poly[0] == edge.poly[1])
+					{
+						edge.poly[1] = (unsigned short)i;
+						edge.polyEdge[1] = (unsigned short)j;
+						found = true;
+						break;
+					}
+				}
+				if (!found)
+				{
+					// Matching edge not found, it is an open edge, add it.
+					rcEdge& edge = edges[edgeCount];
+					edge.vert[0] = v1;
+					edge.vert[1] = v0;
+					edge.poly[0] = (unsigned short)i;
+					edge.polyEdge[0] = (unsigned short)j;
+					edge.poly[1] = (unsigned short)i;
+					edge.polyEdge[1] = 0xff;
+					// Insert edge
+					nextEdge[edgeCount] = firstEdge[v1];
+					firstEdge[v1] = (unsigned short)edgeCount;
+					edgeCount++;
+				}
+			}
+		}
+	}
+	
+	// Mark portal edges.
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		dtTileCacheContour& cont = lcset.conts[i];
+		if (cont.nverts < 3)
+			continue;
+		
+		for (int j = 0, k = cont.nverts-1; j < cont.nverts; k=j++)
+		{
+			const unsigned char* va = &cont.verts[k*4];
+			const unsigned char* vb = &cont.verts[j*4];
+			const unsigned char dir = va[3] & 0xf;
+			if (dir == 0xf)
+				continue;
+			
+			if (dir == 0 || dir == 2)
+			{
+				// Find matching vertical edge
+				const unsigned short x = (unsigned short)va[0];
+				unsigned short zmin = (unsigned short)va[2];
+				unsigned short zmax = (unsigned short)vb[2];
+				if (zmin > zmax)
+					dtSwap(zmin, zmax);
+				
+				for (int m = 0; m < edgeCount; ++m)
+				{
+					rcEdge& e = edges[m];
+					// Skip connected edges.
+					if (e.poly[0] != e.poly[1])
+						continue;
+					const unsigned short* eva = &verts[e.vert[0]*3];
+					const unsigned short* evb = &verts[e.vert[1]*3];
+					if (eva[0] == x && evb[0] == x)
+					{
+						unsigned short ezmin = eva[2];
+						unsigned short ezmax = evb[2];
+						if (ezmin > ezmax)
+							dtSwap(ezmin, ezmax);
+						if (overlapRangeExl(zmin,zmax, ezmin, ezmax))
+						{
+							// Reuse the other polyedge to store dir.
+							e.polyEdge[1] = dir;
+						}
+					}
+				}
+			}
+			else
+			{
+				// Find matching vertical edge
+				const unsigned short z = (unsigned short)va[2];
+				unsigned short xmin = (unsigned short)va[0];
+				unsigned short xmax = (unsigned short)vb[0];
+				if (xmin > xmax)
+					dtSwap(xmin, xmax);
+				for (int m = 0; m < edgeCount; ++m)
+				{
+					rcEdge& e = edges[m];
+					// Skip connected edges.
+					if (e.poly[0] != e.poly[1])
+						continue;
+					const unsigned short* eva = &verts[e.vert[0]*3];
+					const unsigned short* evb = &verts[e.vert[1]*3];
+					if (eva[2] == z && evb[2] == z)
+					{
+						unsigned short exmin = eva[0];
+						unsigned short exmax = evb[0];
+						if (exmin > exmax)
+							dtSwap(exmin, exmax);
+						if (overlapRangeExl(xmin,xmax, exmin, exmax))
+						{
+							// Reuse the other polyedge to store dir.
+							e.polyEdge[1] = dir;
+						}
+					}
+				}
+			}
+		}
+	}
+	
+	
+	// Store adjacency
+	for (int i = 0; i < edgeCount; ++i)
+	{
+		const rcEdge& e = edges[i];
+		if (e.poly[0] != e.poly[1])
+		{
+			unsigned short* p0 = &polys[e.poly[0]*MAX_VERTS_PER_POLY*2];
+			unsigned short* p1 = &polys[e.poly[1]*MAX_VERTS_PER_POLY*2];
+			p0[MAX_VERTS_PER_POLY + e.polyEdge[0]] = e.poly[1];
+			p1[MAX_VERTS_PER_POLY + e.polyEdge[1]] = e.poly[0];
+		}
+		else if (e.polyEdge[1] != 0xff)
+		{
+			unsigned short* p0 = &polys[e.poly[0]*MAX_VERTS_PER_POLY*2];
+			p0[MAX_VERTS_PER_POLY + e.polyEdge[0]] = 0x8000 | (unsigned short)e.polyEdge[1];
+		}
+		
+	}
+	
+	return true;
+}
+
+
+// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv).
+inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; }
+inline int next(int i, int n) { return i+1 < n ? i+1 : 0; }
+
+inline int area2(const unsigned char* a, const unsigned char* b, const unsigned char* c)
+{
+	return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) - ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]);
+}
+
+//	Exclusive or: true iff exactly one argument is true.
+//	The arguments are negated to ensure that they are 0/1
+//	values.  Then the bitwise Xor operator may apply.
+//	(This idea is due to Michael Baldwin.)
+inline bool xorb(bool x, bool y)
+{
+	return !x ^ !y;
+}
+
+// Returns true iff c is strictly to the left of the directed
+// line through a to b.
+inline bool left(const unsigned char* a, const unsigned char* b, const unsigned char* c)
+{
+	return area2(a, b, c) < 0;
+}
+
+inline bool leftOn(const unsigned char* a, const unsigned char* b, const unsigned char* c)
+{
+	return area2(a, b, c) <= 0;
+}
+
+inline bool collinear(const unsigned char* a, const unsigned char* b, const unsigned char* c)
+{
+	return area2(a, b, c) == 0;
+}
+
+//	Returns true iff ab properly intersects cd: they share
+//	a point interior to both segments.  The properness of the
+//	intersection is ensured by using strict leftness.
+static bool intersectProp(const unsigned char* a, const unsigned char* b,
+						  const unsigned char* c, const unsigned char* d)
+{
+	// Eliminate improper cases.
+	if (collinear(a,b,c) || collinear(a,b,d) ||
+		collinear(c,d,a) || collinear(c,d,b))
+		return false;
+	
+	return xorb(left(a,b,c), left(a,b,d)) && xorb(left(c,d,a), left(c,d,b));
+}
+
+// Returns T iff (a,b,c) are collinear and point c lies 
+// on the closed segement ab.
+static bool between(const unsigned char* a, const unsigned char* b, const unsigned char* c)
+{
+	if (!collinear(a, b, c))
+		return false;
+	// If ab not vertical, check betweenness on x; else on y.
+	if (a[0] != b[0])
+		return ((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0]));
+	else
+		return ((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2]));
+}
+
+// Returns true iff segments ab and cd intersect, properly or improperly.
+static bool intersect(const unsigned char* a, const unsigned char* b,
+					  const unsigned char* c, const unsigned char* d)
+{
+	if (intersectProp(a, b, c, d))
+		return true;
+	else if (between(a, b, c) || between(a, b, d) ||
+			 between(c, d, a) || between(c, d, b))
+		return true;
+	else
+		return false;
+}
+
+static bool vequal(const unsigned char* a, const unsigned char* b)
+{
+	return a[0] == b[0] && a[2] == b[2];
+}
+
+// Returns T iff (v_i, v_j) is a proper internal *or* external
+// diagonal of P, *ignoring edges incident to v_i and v_j*.
+static bool diagonalie(int i, int j, int n, const unsigned char* verts, const unsigned short* indices)
+{
+	const unsigned char* d0 = &verts[(indices[i] & 0x7fff) * 4];
+	const unsigned char* d1 = &verts[(indices[j] & 0x7fff) * 4];
+	
+	// For each edge (k,k+1) of P
+	for (int k = 0; k < n; k++)
+	{
+		int k1 = next(k, n);
+		// Skip edges incident to i or j
+		if (!((k == i) || (k1 == i) || (k == j) || (k1 == j)))
+		{
+			const unsigned char* p0 = &verts[(indices[k] & 0x7fff) * 4];
+			const unsigned char* p1 = &verts[(indices[k1] & 0x7fff) * 4];
+			
+			if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1))
+				continue;
+			
+			if (intersect(d0, d1, p0, p1))
+				return false;
+		}
+	}
+	return true;
+}
+
+// Returns true iff the diagonal (i,j) is strictly internal to the 
+// polygon P in the neighborhood of the i endpoint.
+static bool	inCone(int i, int j, int n, const unsigned char* verts, const unsigned short* indices)
+{
+	const unsigned char* pi = &verts[(indices[i] & 0x7fff) * 4];
+	const unsigned char* pj = &verts[(indices[j] & 0x7fff) * 4];
+	const unsigned char* pi1 = &verts[(indices[next(i, n)] & 0x7fff) * 4];
+	const unsigned char* pin1 = &verts[(indices[prev(i, n)] & 0x7fff) * 4];
+	
+	// If P[i] is a convex vertex [ i+1 left or on (i-1,i) ].
+	if (leftOn(pin1, pi, pi1))
+		return left(pi, pj, pin1) && left(pj, pi, pi1);
+	// Assume (i-1,i,i+1) not collinear.
+	// else P[i] is reflex.
+	return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1));
+}
+
+// Returns T iff (v_i, v_j) is a proper internal
+// diagonal of P.
+static bool diagonal(int i, int j, int n, const unsigned char* verts, const unsigned short* indices)
+{
+	return inCone(i, j, n, verts, indices) && diagonalie(i, j, n, verts, indices);
+}
+
+static int triangulate(int n, const unsigned char* verts, unsigned short* indices, unsigned short* tris)
+{
+	int ntris = 0;
+	unsigned short* dst = tris;
+	
+	// The last bit of the index is used to indicate if the vertex can be removed.
+	for (int i = 0; i < n; i++)
+	{
+		int i1 = next(i, n);
+		int i2 = next(i1, n);
+		if (diagonal(i, i2, n, verts, indices))
+			indices[i1] |= 0x8000;
+	}
+	
+	while (n > 3)
+	{
+		int minLen = -1;
+		int mini = -1;
+		for (int i = 0; i < n; i++)
+		{
+			int i1 = next(i, n);
+			if (indices[i1] & 0x8000)
+			{
+				const unsigned char* p0 = &verts[(indices[i] & 0x7fff) * 4];
+				const unsigned char* p2 = &verts[(indices[next(i1, n)] & 0x7fff) * 4];
+				
+				const int dx = (int)p2[0] - (int)p0[0];
+				const int dz = (int)p2[2] - (int)p0[2];
+				const int len = dx*dx + dz*dz;
+				if (minLen < 0 || len < minLen)
+				{
+					minLen = len;
+					mini = i;
+				}
+			}
+		}
+		
+		if (mini == -1)
+		{
+			// Should not happen.
+			/*			printf("mini == -1 ntris=%d n=%d\n", ntris, n);
+			 for (int i = 0; i < n; i++)
+			 {
+			 printf("%d ", indices[i] & 0x0fffffff);
+			 }
+			 printf("\n");*/
+			return -ntris;
+		}
+		
+		int i = mini;
+		int i1 = next(i, n);
+		int i2 = next(i1, n);
+		
+		*dst++ = indices[i] & 0x7fff;
+		*dst++ = indices[i1] & 0x7fff;
+		*dst++ = indices[i2] & 0x7fff;
+		ntris++;
+		
+		// Removes P[i1] by copying P[i+1]...P[n-1] left one index.
+		n--;
+		for (int k = i1; k < n; k++)
+			indices[k] = indices[k+1];
+		
+		if (i1 >= n) i1 = 0;
+		i = prev(i1,n);
+		// Update diagonal flags.
+		if (diagonal(prev(i, n), i1, n, verts, indices))
+			indices[i] |= 0x8000;
+		else
+			indices[i] &= 0x7fff;
+		
+		if (diagonal(i, next(i1, n), n, verts, indices))
+			indices[i1] |= 0x8000;
+		else
+			indices[i1] &= 0x7fff;
+	}
+	
+	// Append the remaining triangle.
+	*dst++ = indices[0] & 0x7fff;
+	*dst++ = indices[1] & 0x7fff;
+	*dst++ = indices[2] & 0x7fff;
+	ntris++;
+	
+	return ntris;
+}
+
+
+static int countPolyVerts(const unsigned short* p)
+{
+	for (int i = 0; i < MAX_VERTS_PER_POLY; ++i)
+		if (p[i] == DT_TILECACHE_NULL_IDX)
+			return i;
+	return MAX_VERTS_PER_POLY;
+}
+
+inline bool uleft(const unsigned short* a, const unsigned short* b, const unsigned short* c)
+{
+	return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) -
+	((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) < 0;
+}
+
+static int getPolyMergeValue(unsigned short* pa, unsigned short* pb,
+							 const unsigned short* verts, int& ea, int& eb)
+{
+	const int na = countPolyVerts(pa);
+	const int nb = countPolyVerts(pb);
+	
+	// If the merged polygon would be too big, do not merge.
+	if (na+nb-2 > MAX_VERTS_PER_POLY)
+		return -1;
+	
+	// Check if the polygons share an edge.
+	ea = -1;
+	eb = -1;
+	
+	for (int i = 0; i < na; ++i)
+	{
+		unsigned short va0 = pa[i];
+		unsigned short va1 = pa[(i+1) % na];
+		if (va0 > va1)
+			dtSwap(va0, va1);
+		for (int j = 0; j < nb; ++j)
+		{
+			unsigned short vb0 = pb[j];
+			unsigned short vb1 = pb[(j+1) % nb];
+			if (vb0 > vb1)
+				dtSwap(vb0, vb1);
+			if (va0 == vb0 && va1 == vb1)
+			{
+				ea = i;
+				eb = j;
+				break;
+			}
+		}
+	}
+	
+	// No common edge, cannot merge.
+	if (ea == -1 || eb == -1)
+		return -1;
+	
+	// Check to see if the merged polygon would be convex.
+	unsigned short va, vb, vc;
+	
+	va = pa[(ea+na-1) % na];
+	vb = pa[ea];
+	vc = pb[(eb+2) % nb];
+	if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3]))
+		return -1;
+	
+	va = pb[(eb+nb-1) % nb];
+	vb = pb[eb];
+	vc = pa[(ea+2) % na];
+	if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3]))
+		return -1;
+	
+	va = pa[ea];
+	vb = pa[(ea+1)%na];
+	
+	int dx = (int)verts[va*3+0] - (int)verts[vb*3+0];
+	int dy = (int)verts[va*3+2] - (int)verts[vb*3+2];
+	
+	return dx*dx + dy*dy;
+}
+
+static void mergePolys(unsigned short* pa, unsigned short* pb, int ea, int eb)
+{
+	unsigned short tmp[MAX_VERTS_PER_POLY*2];
+	
+	const int na = countPolyVerts(pa);
+	const int nb = countPolyVerts(pb);
+	
+	// Merge polygons.
+	memset(tmp, 0xff, sizeof(unsigned short)*MAX_VERTS_PER_POLY*2);
+	int n = 0;
+	// Add pa
+	for (int i = 0; i < na-1; ++i)
+		tmp[n++] = pa[(ea+1+i) % na];
+	// Add pb
+	for (int i = 0; i < nb-1; ++i)
+		tmp[n++] = pb[(eb+1+i) % nb];
+	
+	memcpy(pa, tmp, sizeof(unsigned short)*MAX_VERTS_PER_POLY);
+}
+
+
+static void pushFront(unsigned short v, unsigned short* arr, int& an)
+{
+	an++;
+	for (int i = an-1; i > 0; --i)
+		arr[i] = arr[i-1];
+	arr[0] = v;
+}
+
+static void pushBack(unsigned short v, unsigned short* arr, int& an)
+{
+	arr[an] = v;
+	an++;
+}
+
+static bool canRemoveVertex(dtTileCachePolyMesh& mesh, const unsigned short rem)
+{
+	// Count number of polygons to remove.
+	int numRemovedVerts = 0;
+	int numTouchedVerts = 0;
+	int numRemainingEdges = 0;
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*MAX_VERTS_PER_POLY*2];
+		const int nv = countPolyVerts(p);
+		int numRemoved = 0;
+		int numVerts = 0;
+		for (int j = 0; j < nv; ++j)
+		{
+			if (p[j] == rem)
+			{
+				numTouchedVerts++;
+				numRemoved++;
+			}
+			numVerts++;
+		}
+		if (numRemoved)
+		{
+			numRemovedVerts += numRemoved;
+			numRemainingEdges += numVerts-(numRemoved+1);
+		}
+	}
+	
+	// There would be too few edges remaining to create a polygon.
+	// This can happen for example when a tip of a triangle is marked
+	// as deletion, but there are no other polys that share the vertex.
+	// In this case, the vertex should not be removed.
+	if (numRemainingEdges <= 2)
+		return false;
+	
+	// Check that there is enough memory for the test.
+	const int maxEdges = numTouchedVerts*2;
+	if (maxEdges > MAX_REM_EDGES)
+		return false;
+	
+	// Find edges which share the removed vertex.
+	unsigned short edges[MAX_REM_EDGES];
+	int nedges = 0;
+	
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*MAX_VERTS_PER_POLY*2];
+		const int nv = countPolyVerts(p);
+		
+		// Collect edges which touches the removed vertex.
+		for (int j = 0, k = nv-1; j < nv; k = j++)
+		{
+			if (p[j] == rem || p[k] == rem)
+			{
+				// Arrange edge so that a=rem.
+				int a = p[j], b = p[k];
+				if (b == rem)
+					dtSwap(a,b);
+				
+				// Check if the edge exists
+				bool exists = false;
+				for (int m = 0; m < nedges; ++m)
+				{
+					unsigned short* e = &edges[m*3];
+					if (e[1] == b)
+					{
+						// Exists, increment vertex share count.
+						e[2]++;
+						exists = true;
+					}
+				}
+				// Add new edge.
+				if (!exists)
+				{
+					unsigned short* e = &edges[nedges*3];
+					e[0] = (unsigned short)a;
+					e[1] = (unsigned short)b;
+					e[2] = 1;
+					nedges++;
+				}
+			}
+		}
+	}
+	
+	// There should be no more than 2 open edges.
+	// This catches the case that two non-adjacent polygons
+	// share the removed vertex. In that case, do not remove the vertex.
+	int numOpenEdges = 0;
+	for (int i = 0; i < nedges; ++i)
+	{
+		if (edges[i*3+2] < 2)
+			numOpenEdges++;
+	}
+	if (numOpenEdges > 2)
+		return false;
+	
+	return true;
+}
+
+static dtStatus removeVertex(dtTileCachePolyMesh& mesh, const unsigned short rem, const int maxTris)
+{
+	// Count number of polygons to remove.
+	int numRemovedVerts = 0;
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*MAX_VERTS_PER_POLY*2];
+		const int nv = countPolyVerts(p);
+		for (int j = 0; j < nv; ++j)
+		{
+			if (p[j] == rem)
+				numRemovedVerts++;
+		}
+	}
+	
+	int nedges = 0;
+	unsigned short edges[MAX_REM_EDGES*3];
+	int nhole = 0;
+	unsigned short hole[MAX_REM_EDGES];
+	int nharea = 0;
+	unsigned short harea[MAX_REM_EDGES];
+	
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*MAX_VERTS_PER_POLY*2];
+		const int nv = countPolyVerts(p);
+		bool hasRem = false;
+		for (int j = 0; j < nv; ++j)
+			if (p[j] == rem) hasRem = true;
+		if (hasRem)
+		{
+			// Collect edges which does not touch the removed vertex.
+			for (int j = 0, k = nv-1; j < nv; k = j++)
+			{
+				if (p[j] != rem && p[k] != rem)
+				{
+					if (nedges >= MAX_REM_EDGES)
+						return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+					unsigned short* e = &edges[nedges*3];
+					e[0] = p[k];
+					e[1] = p[j];
+					e[2] = mesh.areas[i];
+					nedges++;
+				}
+			}
+			// Remove the polygon.
+			unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*MAX_VERTS_PER_POLY*2];
+			memcpy(p,p2,sizeof(unsigned short)*MAX_VERTS_PER_POLY);
+			memset(p+MAX_VERTS_PER_POLY,0xff,sizeof(unsigned short)*MAX_VERTS_PER_POLY);
+			mesh.areas[i] = mesh.areas[mesh.npolys-1];
+			mesh.npolys--;
+			--i;
+		}
+	}
+	
+	// Remove vertex.
+	for (int i = (int)rem; i < mesh.nverts; ++i)
+	{
+		mesh.verts[i*3+0] = mesh.verts[(i+1)*3+0];
+		mesh.verts[i*3+1] = mesh.verts[(i+1)*3+1];
+		mesh.verts[i*3+2] = mesh.verts[(i+1)*3+2];
+	}
+	mesh.nverts--;
+	
+	// Adjust indices to match the removed vertex layout.
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*MAX_VERTS_PER_POLY*2];
+		const int nv = countPolyVerts(p);
+		for (int j = 0; j < nv; ++j)
+			if (p[j] > rem) p[j]--;
+	}
+	for (int i = 0; i < nedges; ++i)
+	{
+		if (edges[i*3+0] > rem) edges[i*3+0]--;
+		if (edges[i*3+1] > rem) edges[i*3+1]--;
+	}
+	
+	if (nedges == 0)
+		return DT_SUCCESS;
+	
+	// Start with one vertex, keep appending connected
+	// segments to the start and end of the hole.
+	pushBack(edges[0], hole, nhole);
+	pushBack(edges[2], harea, nharea);
+	
+	while (nedges)
+	{
+		bool match = false;
+		
+		for (int i = 0; i < nedges; ++i)
+		{
+			const unsigned short ea = edges[i*3+0];
+			const unsigned short eb = edges[i*3+1];
+			const unsigned short a = edges[i*3+2];
+			bool add = false;
+			if (hole[0] == eb)
+			{
+				// The segment matches the beginning of the hole boundary.
+				if (nhole >= MAX_REM_EDGES)
+					return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+				pushFront(ea, hole, nhole);
+				pushFront(a, harea, nharea);
+				add = true;
+			}
+			else if (hole[nhole-1] == ea)
+			{
+				// The segment matches the end of the hole boundary.
+				if (nhole >= MAX_REM_EDGES)
+					return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+				pushBack(eb, hole, nhole);
+				pushBack(a, harea, nharea);
+				add = true;
+			}
+			if (add)
+			{
+				// The edge segment was added, remove it.
+				edges[i*3+0] = edges[(nedges-1)*3+0];
+				edges[i*3+1] = edges[(nedges-1)*3+1];
+				edges[i*3+2] = edges[(nedges-1)*3+2];
+				--nedges;
+				match = true;
+				--i;
+			}
+		}
+		
+		if (!match)
+			break;
+	}
+	
+	
+	unsigned short tris[MAX_REM_EDGES*3];
+	unsigned char tverts[MAX_REM_EDGES*3];
+	unsigned short tpoly[MAX_REM_EDGES*3];
+	
+	// Generate temp vertex array for triangulation.
+	for (int i = 0; i < nhole; ++i)
+	{
+		const unsigned short pi = hole[i];
+		tverts[i*4+0] = (unsigned char)mesh.verts[pi*3+0];
+		tverts[i*4+1] = (unsigned char)mesh.verts[pi*3+1];
+		tverts[i*4+2] = (unsigned char)mesh.verts[pi*3+2];
+		tverts[i*4+3] = 0;
+		tpoly[i] = (unsigned short)i;
+	}
+	
+	// Triangulate the hole.
+	int ntris = triangulate(nhole, tverts, tpoly, tris);
+	if (ntris < 0)
+	{
+		// TODO: issue warning!
+		ntris = -ntris;
+	}
+	
+	if (ntris > MAX_REM_EDGES)
+		return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+	
+	unsigned short polys[MAX_REM_EDGES*MAX_VERTS_PER_POLY];
+	unsigned char pareas[MAX_REM_EDGES];
+	
+	// Build initial polygons.
+	int npolys = 0;
+	memset(polys, 0xff, ntris*MAX_VERTS_PER_POLY*sizeof(unsigned short));
+	for (int j = 0; j < ntris; ++j)
+	{
+		unsigned short* t = &tris[j*3];
+		if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2])
+		{
+			polys[npolys*MAX_VERTS_PER_POLY+0] = hole[t[0]];
+			polys[npolys*MAX_VERTS_PER_POLY+1] = hole[t[1]];
+			polys[npolys*MAX_VERTS_PER_POLY+2] = hole[t[2]];
+			pareas[npolys] = (unsigned char)harea[t[0]];
+			npolys++;
+		}
+	}
+	if (!npolys)
+		return DT_SUCCESS;
+	
+	// Merge polygons.
+	int maxVertsPerPoly = MAX_VERTS_PER_POLY;
+	if (maxVertsPerPoly > 3)
+	{
+		for (;;)
+		{
+			// Find best polygons to merge.
+			int bestMergeVal = 0;
+			int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0;
+			
+			for (int j = 0; j < npolys-1; ++j)
+			{
+				unsigned short* pj = &polys[j*MAX_VERTS_PER_POLY];
+				for (int k = j+1; k < npolys; ++k)
+				{
+					unsigned short* pk = &polys[k*MAX_VERTS_PER_POLY];
+					int ea, eb;
+					int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb);
+					if (v > bestMergeVal)
+					{
+						bestMergeVal = v;
+						bestPa = j;
+						bestPb = k;
+						bestEa = ea;
+						bestEb = eb;
+					}
+				}
+			}
+			
+			if (bestMergeVal > 0)
+			{
+				// Found best, merge.
+				unsigned short* pa = &polys[bestPa*MAX_VERTS_PER_POLY];
+				unsigned short* pb = &polys[bestPb*MAX_VERTS_PER_POLY];
+				mergePolys(pa, pb, bestEa, bestEb);
+				memcpy(pb, &polys[(npolys-1)*MAX_VERTS_PER_POLY], sizeof(unsigned short)*MAX_VERTS_PER_POLY);
+				pareas[bestPb] = pareas[npolys-1];
+				npolys--;
+			}
+			else
+			{
+				// Could not merge any polygons, stop.
+				break;
+			}
+		}
+	}
+	
+	// Store polygons.
+	for (int i = 0; i < npolys; ++i)
+	{
+		if (mesh.npolys >= maxTris) break;
+		unsigned short* p = &mesh.polys[mesh.npolys*MAX_VERTS_PER_POLY*2];
+		memset(p,0xff,sizeof(unsigned short)*MAX_VERTS_PER_POLY*2);
+		for (int j = 0; j < MAX_VERTS_PER_POLY; ++j)
+			p[j] = polys[i*MAX_VERTS_PER_POLY+j];
+		mesh.areas[mesh.npolys] = pareas[i];
+		mesh.npolys++;
+		if (mesh.npolys > maxTris)
+			return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+	}
+	
+	return DT_SUCCESS;
+}
+
+
+dtStatus dtBuildTileCachePolyMesh(dtTileCacheAlloc* alloc,
+								  dtTileCacheContourSet& lcset,
+								  dtTileCachePolyMesh& mesh)
+{
+	dtAssert(alloc);
+	
+	int maxVertices = 0;
+	int maxTris = 0;
+	int maxVertsPerCont = 0;
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		// Skip null contours.
+		if (lcset.conts[i].nverts < 3) continue;
+		maxVertices += lcset.conts[i].nverts;
+		maxTris += lcset.conts[i].nverts - 2;
+		maxVertsPerCont = dtMax(maxVertsPerCont, lcset.conts[i].nverts);
+	}
+
+	// TODO: warn about too many vertices?
+	
+	mesh.nvp = MAX_VERTS_PER_POLY;
+	
+	dtFixedArray<unsigned char> vflags(alloc, maxVertices);
+	if (!vflags)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(vflags, 0, maxVertices);
+	
+	mesh.verts = (unsigned short*)alloc->alloc(sizeof(unsigned short)*maxVertices*3);
+	if (!mesh.verts)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	mesh.polys = (unsigned short*)alloc->alloc(sizeof(unsigned short)*maxTris*MAX_VERTS_PER_POLY*2);
+	if (!mesh.polys)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	mesh.areas = (unsigned char*)alloc->alloc(sizeof(unsigned char)*maxTris);
+	if (!mesh.areas)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	mesh.flags = (unsigned short*)alloc->alloc(sizeof(unsigned short)*maxTris);
+	if (!mesh.flags)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	// Just allocate and clean the mesh flags array. The user is resposible for filling it.
+	memset(mesh.flags, 0, sizeof(unsigned short) * maxTris);
+		
+	mesh.nverts = 0;
+	mesh.npolys = 0;
+	
+	memset(mesh.verts, 0, sizeof(unsigned short)*maxVertices*3);
+	memset(mesh.polys, 0xff, sizeof(unsigned short)*maxTris*MAX_VERTS_PER_POLY*2);
+	memset(mesh.areas, 0, sizeof(unsigned char)*maxTris);
+	
+	unsigned short firstVert[VERTEX_BUCKET_COUNT2];
+	for (int i = 0; i < VERTEX_BUCKET_COUNT2; ++i)
+		firstVert[i] = DT_TILECACHE_NULL_IDX;
+	
+	dtFixedArray<unsigned short> nextVert(alloc, maxVertices);
+	if (!nextVert)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(nextVert, 0, sizeof(unsigned short)*maxVertices);
+	
+	dtFixedArray<unsigned short> indices(alloc, maxVertsPerCont);
+	if (!indices)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	dtFixedArray<unsigned short> tris(alloc, maxVertsPerCont*3);
+	if (!tris)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+
+	dtFixedArray<unsigned short> polys(alloc, maxVertsPerCont*MAX_VERTS_PER_POLY);
+	if (!polys)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		dtTileCacheContour& cont = lcset.conts[i];
+		
+		// Skip null contours.
+		if (cont.nverts < 3)
+			continue;
+		
+		// Triangulate contour
+		for (int j = 0; j < cont.nverts; ++j)
+			indices[j] = (unsigned short)j;
+		
+		int ntris = triangulate(cont.nverts, cont.verts, &indices[0], &tris[0]);
+		if (ntris <= 0)
+		{
+			// TODO: issue warning!
+			ntris = -ntris;
+		}
+		
+		// Add and merge vertices.
+		for (int j = 0; j < cont.nverts; ++j)
+		{
+			const unsigned char* v = &cont.verts[j*4];
+			indices[j] = addVertex((unsigned short)v[0], (unsigned short)v[1], (unsigned short)v[2],
+								   mesh.verts, firstVert, nextVert, mesh.nverts);
+			if (v[3] & 0x80)
+			{
+				// This vertex should be removed.
+				vflags[indices[j]] = 1;
+			}
+		}
+		
+		// Build initial polygons.
+		int npolys = 0;
+		memset(polys, 0xff, sizeof(unsigned short) * maxVertsPerCont * MAX_VERTS_PER_POLY);
+		for (int j = 0; j < ntris; ++j)
+		{
+			const unsigned short* t = &tris[j*3];
+			if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2])
+			{
+				polys[npolys*MAX_VERTS_PER_POLY+0] = indices[t[0]];
+				polys[npolys*MAX_VERTS_PER_POLY+1] = indices[t[1]];
+				polys[npolys*MAX_VERTS_PER_POLY+2] = indices[t[2]];
+				npolys++;
+			}
+		}
+		if (!npolys)
+			continue;
+		
+		// Merge polygons.
+		int maxVertsPerPoly =MAX_VERTS_PER_POLY ;
+		if (maxVertsPerPoly > 3)
+		{
+			for(;;)
+			{
+				// Find best polygons to merge.
+				int bestMergeVal = 0;
+				int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0;
+				
+				for (int j = 0; j < npolys-1; ++j)
+				{
+					unsigned short* pj = &polys[j*MAX_VERTS_PER_POLY];
+					for (int k = j+1; k < npolys; ++k)
+					{
+						unsigned short* pk = &polys[k*MAX_VERTS_PER_POLY];
+						int ea, eb;
+						int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb);
+						if (v > bestMergeVal)
+						{
+							bestMergeVal = v;
+							bestPa = j;
+							bestPb = k;
+							bestEa = ea;
+							bestEb = eb;
+						}
+					}
+				}
+				
+				if (bestMergeVal > 0)
+				{
+					// Found best, merge.
+					unsigned short* pa = &polys[bestPa*MAX_VERTS_PER_POLY];
+					unsigned short* pb = &polys[bestPb*MAX_VERTS_PER_POLY];
+					mergePolys(pa, pb, bestEa, bestEb);
+					memcpy(pb, &polys[(npolys-1)*MAX_VERTS_PER_POLY], sizeof(unsigned short)*MAX_VERTS_PER_POLY);
+					npolys--;
+				}
+				else
+				{
+					// Could not merge any polygons, stop.
+					break;
+				}
+			}
+		}
+		
+		// Store polygons.
+		for (int j = 0; j < npolys; ++j)
+		{
+			unsigned short* p = &mesh.polys[mesh.npolys*MAX_VERTS_PER_POLY*2];
+			unsigned short* q = &polys[j*MAX_VERTS_PER_POLY];
+			for (int k = 0; k < MAX_VERTS_PER_POLY; ++k)
+				p[k] = q[k];
+			mesh.areas[mesh.npolys] = cont.area;
+			mesh.npolys++;
+			if (mesh.npolys > maxTris)
+				return DT_FAILURE | DT_BUFFER_TOO_SMALL;
+		}
+	}
+	
+	
+	// Remove edge vertices.
+	for (int i = 0; i < mesh.nverts; ++i)
+	{
+		if (vflags[i])
+		{
+			if (!canRemoveVertex(mesh, (unsigned short)i))
+				continue;
+			dtStatus status = removeVertex(mesh, (unsigned short)i, maxTris);
+			if (dtStatusFailed(status))
+				return status;
+			// Remove vertex
+			// Note: mesh.nverts is already decremented inside removeVertex()!
+			for (int j = i; j < mesh.nverts; ++j)
+				vflags[j] = vflags[j+1];
+			--i;
+		}
+	}
+	
+	// Calculate adjacency.
+	if (!buildMeshAdjacency(alloc, mesh.polys, mesh.npolys, mesh.verts, mesh.nverts, lcset))
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+		
+	return DT_SUCCESS;
+}
+
+dtStatus dtMarkCylinderArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch,
+							const float* pos, const float radius, const float height, const unsigned char areaId)
+{
+	float bmin[3], bmax[3];
+	bmin[0] = pos[0] - radius;
+	bmin[1] = pos[1];
+	bmin[2] = pos[2] - radius;
+	bmax[0] = pos[0] + radius;
+	bmax[1] = pos[1] + height;
+	bmax[2] = pos[2] + radius;
+	const float r2 = dtSqr(radius/cs + 0.5f);
+
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	const float ics = 1.0f/cs;
+	const float ich = 1.0f/ch;
+	
+	const float px = (pos[0]-orig[0])*ics;
+	const float pz = (pos[2]-orig[2])*ics;
+	
+	int minx = (int)dtMathFloorf((bmin[0]-orig[0])*ics);
+	int miny = (int)dtMathFloorf((bmin[1]-orig[1])*ich);
+	int minz = (int)dtMathFloorf((bmin[2]-orig[2])*ics);
+	int maxx = (int)dtMathFloorf((bmax[0]-orig[0])*ics);
+	int maxy = (int)dtMathFloorf((bmax[1]-orig[1])*ich);
+	int maxz = (int)dtMathFloorf((bmax[2]-orig[2])*ics);
+
+	if (maxx < 0) return DT_SUCCESS;
+	if (minx >= w) return DT_SUCCESS;
+	if (maxz < 0) return DT_SUCCESS;
+	if (minz >= h) return DT_SUCCESS;
+	
+	if (minx < 0) minx = 0;
+	if (maxx >= w) maxx = w-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= h) maxz = h-1;
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const float dx = (float)(x+0.5f) - px;
+			const float dz = (float)(z+0.5f) - pz;
+			if (dx*dx + dz*dz > r2)
+				continue;
+			const int y = layer.heights[x+z*w];
+			if (y < miny || y > maxy)
+				continue;
+			layer.areas[x+z*w] = areaId;
+		}
+	}
+
+	return DT_SUCCESS;
+}
+
+dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch,
+					   const float* bmin, const float* bmax, const unsigned char areaId)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	const float ics = 1.0f/cs;
+	const float ich = 1.0f/ch;
+
+	int minx = (int)floorf((bmin[0]-orig[0])*ics);
+	int miny = (int)floorf((bmin[1]-orig[1])*ich);
+	int minz = (int)floorf((bmin[2]-orig[2])*ics);
+	int maxx = (int)floorf((bmax[0]-orig[0])*ics);
+	int maxy = (int)floorf((bmax[1]-orig[1])*ich);
+	int maxz = (int)floorf((bmax[2]-orig[2])*ics);
+	
+	if (maxx < 0) return DT_SUCCESS;
+	if (minx >= w) return DT_SUCCESS;
+	if (maxz < 0) return DT_SUCCESS;
+	if (minz >= h) return DT_SUCCESS;
+
+	if (minx < 0) minx = 0;
+	if (maxx >= w) maxx = w-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= h) maxz = h-1;
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const int y = layer.heights[x+z*w];
+			if (y < miny || y > maxy)
+				continue;
+			layer.areas[x+z*w] = areaId;
+		}
+	}
+
+	return DT_SUCCESS;
+}
+
+dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch,
+					   const float* center, const float* halfExtents, const float* rotAux, const unsigned char areaId)
+{
+	const int w = (int)layer.header->width;
+	const int h = (int)layer.header->height;
+	const float ics = 1.0f/cs;
+	const float ich = 1.0f/ch;
+
+	float cx = (center[0] - orig[0])*ics;
+	float cz = (center[2] - orig[2])*ics;
+	
+	float maxr = 1.41f*dtMax(halfExtents[0], halfExtents[2]);
+	int minx = (int)floorf(cx - maxr*ics);
+	int maxx = (int)floorf(cx + maxr*ics);
+	int minz = (int)floorf(cz - maxr*ics);
+	int maxz = (int)floorf(cz + maxr*ics);
+	int miny = (int)floorf((center[1]-halfExtents[1]-orig[1])*ich);
+	int maxy = (int)floorf((center[1]+halfExtents[1]-orig[1])*ich);
+
+	if (maxx < 0) return DT_SUCCESS;
+	if (minx >= w) return DT_SUCCESS;
+	if (maxz < 0) return DT_SUCCESS;
+	if (minz >= h) return DT_SUCCESS;
+
+	if (minx < 0) minx = 0;
+	if (maxx >= w) maxx = w-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= h) maxz = h-1;
+	
+	float xhalf = halfExtents[0]*ics + 0.5f;
+	float zhalf = halfExtents[2]*ics + 0.5f;
+
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{			
+			float x2 = 2.0f*(float(x) - cx);
+			float z2 = 2.0f*(float(z) - cz);
+			float xrot = rotAux[1]*x2 + rotAux[0]*z2;
+			if (xrot > xhalf || xrot < -xhalf)
+				continue;
+			float zrot = rotAux[1]*z2 - rotAux[0]*x2;
+			if (zrot > zhalf || zrot < -zhalf)
+				continue;
+			const int y = layer.heights[x+z*w];
+			if (y < miny || y > maxy)
+				continue;
+			layer.areas[x+z*w] = areaId;
+		}
+	}
+
+	return DT_SUCCESS;
+}
+
+dtStatus dtBuildTileCacheLayer(dtTileCacheCompressor* comp,
+							   dtTileCacheLayerHeader* header,
+							   const unsigned char* heights,
+							   const unsigned char* areas,
+							   const unsigned char* cons,
+							   unsigned char** outData, int* outDataSize)
+{
+	const int headerSize = dtAlign4(sizeof(dtTileCacheLayerHeader));
+	const int gridSize = (int)header->width * (int)header->height;
+	const int maxDataSize = headerSize + comp->maxCompressedSize(gridSize*3);
+	unsigned char* data = (unsigned char*)dtAlloc(maxDataSize, DT_ALLOC_PERM);
+	if (!data)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(data, 0, maxDataSize);
+	
+	// Store header
+	memcpy(data, header, sizeof(dtTileCacheLayerHeader));
+	
+	// Concatenate grid data for compression.
+	const int bufferSize = gridSize*3;
+	unsigned char* buffer = (unsigned char*)dtAlloc(bufferSize, DT_ALLOC_TEMP);
+	if (!buffer)
+	{
+		dtFree(data);
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	}
+
+	memcpy(buffer, heights, gridSize);
+	memcpy(buffer+gridSize, areas, gridSize);
+	memcpy(buffer+gridSize*2, cons, gridSize);
+	
+	// Compress
+	unsigned char* compressed = data + headerSize;
+	const int maxCompressedSize = maxDataSize - headerSize;
+	int compressedSize = 0;
+	dtStatus status = comp->compress(buffer, bufferSize, compressed, maxCompressedSize, &compressedSize);
+	if (dtStatusFailed(status))
+	{
+		dtFree(buffer);
+		dtFree(data);
+		return status;
+	}
+
+	*outData = data;
+	*outDataSize = headerSize + compressedSize;
+	
+	dtFree(buffer);
+	
+	return DT_SUCCESS;
+}
+
+void dtFreeTileCacheLayer(dtTileCacheAlloc* alloc, dtTileCacheLayer* layer)
+{
+	dtAssert(alloc);
+	// The layer is allocated as one conitguous blob of data.
+	alloc->free(layer);
+}
+
+dtStatus dtDecompressTileCacheLayer(dtTileCacheAlloc* alloc, dtTileCacheCompressor* comp,
+									unsigned char* compressed, const int compressedSize,
+									dtTileCacheLayer** layerOut)
+{
+	dtAssert(alloc);
+	dtAssert(comp);
+
+	if (!layerOut)
+		return DT_FAILURE | DT_INVALID_PARAM;
+	if (!compressed)
+		return DT_FAILURE | DT_INVALID_PARAM;
+
+	*layerOut = 0;
+
+	dtTileCacheLayerHeader* compressedHeader = (dtTileCacheLayerHeader*)compressed;
+	if (compressedHeader->magic != DT_TILECACHE_MAGIC)
+		return DT_FAILURE | DT_WRONG_MAGIC;
+	if (compressedHeader->version != DT_TILECACHE_VERSION)
+		return DT_FAILURE | DT_WRONG_VERSION;
+	
+	const int layerSize = dtAlign4(sizeof(dtTileCacheLayer));
+	const int headerSize = dtAlign4(sizeof(dtTileCacheLayerHeader));
+	const int gridSize = (int)compressedHeader->width * (int)compressedHeader->height;
+	const int bufferSize = layerSize + headerSize + gridSize*4;
+	
+	unsigned char* buffer = (unsigned char*)alloc->alloc(bufferSize);
+	if (!buffer)
+		return DT_FAILURE | DT_OUT_OF_MEMORY;
+	memset(buffer, 0, bufferSize);
+
+	dtTileCacheLayer* layer = (dtTileCacheLayer*)buffer;
+	dtTileCacheLayerHeader* header = (dtTileCacheLayerHeader*)(buffer + layerSize);
+	unsigned char* grids = buffer + layerSize + headerSize;
+	const int gridsSize = bufferSize - (layerSize + headerSize); 
+	
+	// Copy header
+	memcpy(header, compressedHeader, headerSize);
+	// Decompress grid.
+	int size = 0;
+	dtStatus status = comp->decompress(compressed+headerSize, compressedSize-headerSize,
+									   grids, gridsSize, &size);
+	if (dtStatusFailed(status))
+	{
+		alloc->free(buffer);
+		return status;
+	}
+	
+	layer->header = header;
+	layer->heights = grids;
+	layer->areas = grids + gridSize;
+	layer->cons = grids + gridSize*2;
+	layer->regs = grids + gridSize*3;
+	
+	*layerOut = layer;
+	
+	return DT_SUCCESS;
+}
+
+
+
+bool dtTileCacheHeaderSwapEndian(unsigned char* data, const int dataSize)
+{
+	dtIgnoreUnused(dataSize);
+	dtTileCacheLayerHeader* header = (dtTileCacheLayerHeader*)data;
+	
+	int swappedMagic = DT_TILECACHE_MAGIC;
+	int swappedVersion = DT_TILECACHE_VERSION;
+	dtSwapEndian(&swappedMagic);
+	dtSwapEndian(&swappedVersion);
+	
+	if ((header->magic != DT_TILECACHE_MAGIC || header->version != DT_TILECACHE_VERSION) &&
+		(header->magic != swappedMagic || header->version != swappedVersion))
+	{
+		return false;
+	}
+	
+	dtSwapEndian(&header->magic);
+	dtSwapEndian(&header->version);
+	dtSwapEndian(&header->tx);
+	dtSwapEndian(&header->ty);
+	dtSwapEndian(&header->tlayer);
+	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->hmin);
+	dtSwapEndian(&header->hmax);
+	
+	// width, height, minx, maxx, miny, maxy are unsigned char, no need to swap.
+	
+	return true;
+}
+

panda/src/recastdetour/DetourTileCacheBuilder.h

@@ -0,0 +1,156 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef DETOURTILECACHEBUILDER_H
+#define DETOURTILECACHEBUILDER_H
+
+#include "DetourAlloc.h"
+#include "DetourStatus.h"
+
+static const int DT_TILECACHE_MAGIC = 'D'<<24 | 'T'<<16 | 'L'<<8 | 'R'; ///< 'DTLR';
+static const int DT_TILECACHE_VERSION = 1;
+
+static const unsigned char DT_TILECACHE_NULL_AREA = 0;
+static const unsigned char DT_TILECACHE_WALKABLE_AREA = 63;
+static const unsigned short DT_TILECACHE_NULL_IDX = 0xffff;
+
+struct dtTileCacheLayerHeader
+{
+	int magic;								///< Data magic
+	int version;							///< Data version
+	int tx,ty,tlayer;
+	float bmin[3], bmax[3];
+	unsigned short hmin, hmax;				///< Height min/max range
+	unsigned char width, height;			///< Dimension of the layer.
+	unsigned char minx, maxx, miny, maxy;	///< Usable sub-region.
+};
+
+struct dtTileCacheLayer
+{
+	dtTileCacheLayerHeader* header;
+	unsigned char regCount;					///< Region count.
+	unsigned char* heights;
+	unsigned char* areas;
+	unsigned char* cons;
+	unsigned char* regs;
+};
+
+struct dtTileCacheContour
+{
+	int nverts;
+	unsigned char* verts;
+	unsigned char reg;
+	unsigned char area;
+};
+
+struct dtTileCacheContourSet
+{
+	int nconts;
+	dtTileCacheContour* conts;
+};
+
+struct dtTileCachePolyMesh
+{
+	int nvp;
+	int nverts;				///< Number of vertices.
+	int npolys;				///< Number of polygons.
+	unsigned short* verts;	///< Vertices of the mesh, 3 elements per vertex.
+	unsigned short* polys;	///< Polygons of the mesh, nvp*2 elements per polygon.
+	unsigned short* flags;	///< Per polygon flags.
+	unsigned char* areas;	///< Area ID of polygons.
+};
+
+
+struct dtTileCacheAlloc
+{
+	virtual ~dtTileCacheAlloc() {}
+
+	virtual void reset() {}
+	
+	virtual void* alloc(const size_t size)
+	{
+		return dtAlloc(size, DT_ALLOC_TEMP);
+	}
+	
+	virtual void free(void* ptr)
+	{
+		dtFree(ptr);
+	}
+};
+
+struct dtTileCacheCompressor
+{
+	virtual ~dtTileCacheCompressor() { }
+
+	virtual int maxCompressedSize(const int bufferSize) = 0;
+	virtual dtStatus compress(const unsigned char* buffer, const int bufferSize,
+							  unsigned char* compressed, const int maxCompressedSize, int* compressedSize) = 0;
+	virtual dtStatus decompress(const unsigned char* compressed, const int compressedSize,
+								unsigned char* buffer, const int maxBufferSize, int* bufferSize) = 0;
+};
+
+
+dtStatus dtBuildTileCacheLayer(dtTileCacheCompressor* comp,
+							   dtTileCacheLayerHeader* header,
+							   const unsigned char* heights,
+							   const unsigned char* areas,
+							   const unsigned char* cons,
+							   unsigned char** outData, int* outDataSize);
+
+void dtFreeTileCacheLayer(dtTileCacheAlloc* alloc, dtTileCacheLayer* layer);
+
+dtStatus dtDecompressTileCacheLayer(dtTileCacheAlloc* alloc, dtTileCacheCompressor* comp,
+									unsigned char* compressed, const int compressedSize,
+									dtTileCacheLayer** layerOut);
+
+dtTileCacheContourSet* dtAllocTileCacheContourSet(dtTileCacheAlloc* alloc);
+void dtFreeTileCacheContourSet(dtTileCacheAlloc* alloc, dtTileCacheContourSet* cset);
+
+dtTileCachePolyMesh* dtAllocTileCachePolyMesh(dtTileCacheAlloc* alloc);
+void dtFreeTileCachePolyMesh(dtTileCacheAlloc* alloc, dtTileCachePolyMesh* lmesh);
+
+dtStatus dtMarkCylinderArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch,
+							const float* pos, const float radius, const float height, const unsigned char areaId);
+
+dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch,
+					   const float* bmin, const float* bmax, const unsigned char areaId);
+
+dtStatus dtMarkBoxArea(dtTileCacheLayer& layer, const float* orig, const float cs, const float ch,
+					   const float* center, const float* halfExtents, const float* rotAux, const unsigned char areaId);
+
+dtStatus dtBuildTileCacheRegions(dtTileCacheAlloc* alloc,
+								 dtTileCacheLayer& layer,
+								 const int walkableClimb);
+
+dtStatus dtBuildTileCacheContours(dtTileCacheAlloc* alloc,
+								  dtTileCacheLayer& layer,
+								  const int walkableClimb, 	const float maxError,
+								  dtTileCacheContourSet& lcset);
+
+dtStatus dtBuildTileCachePolyMesh(dtTileCacheAlloc* alloc,
+								  dtTileCacheContourSet& lcset,
+								  dtTileCachePolyMesh& mesh);
+
+/// Swaps the endianess of the compressed tile data's header (#dtTileCacheLayerHeader).
+/// Tile layer data does not need endian swapping as it consits only of bytes.
+///  @param[in,out]	data		The tile data array.
+///  @param[in]		dataSize	The size of the data array.
+bool dtTileCacheHeaderSwapEndian(unsigned char* data, const int dataSize);
+
+
+#endif // DETOURTILECACHEBUILDER_H

panda/src/recastdetour/InputGeom.cxx

@@ -0,0 +1,596 @@
+
+#define _USE_MATH_DEFINES
+
+#include <math.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <string.h>
+#include <algorithm>
+
+#ifndef CPPPARSER
+#include "Recast.h"
+#include "DebugDraw.h"
+#include "RecastDebugDraw.h"
+#include "DetourNavMesh.h"
+#endif
+#include "MeshLoaderObj.h"
+#include "InputGeom.h"
+#include "NavMeshSample.h"
+#include "nodePath.h"
+
+static bool intersectSegmentTriangle(const float* sp, const float* sq,
+	const float* a, const float* b, const float* c,
+	float &t)
+{
+	float v, w;
+	float ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
+	rcVsub(ab, b, a);
+	rcVsub(ac, c, a);
+	rcVsub(qp, sp, sq);
+
+	// Compute triangle normal. Can be precalculated or cached if
+	// intersecting multiple segments against the same triangle
+	rcVcross(norm, ab, ac);
+
+	// Compute denominator d. If d <= 0, segment is parallel to or points
+	// away from triangle, so exit early
+	float d = rcVdot(qp, norm);
+	if (d <= 0.0f) return false;
+
+	// Compute intersection t value of pq with plane of triangle. A ray
+	// intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
+	// dividing by d until intersection has been found to pierce triangle
+	rcVsub(ap, sp, a);
+	t = rcVdot(ap, norm);
+	if (t < 0.0f) return false;
+	if (t > d) return false; // For segment; exclude this code line for a ray test
+
+	// Compute barycentric coordinate components and test if within bounds
+	rcVcross(e, qp, ap);
+	v = rcVdot(ac, e);
+	if (v < 0.0f || v > d) return false;
+	w = -rcVdot(ab, e);
+	if (w < 0.0f || v + w > d) return false;
+
+	// Segment/ray intersects triangle. Perform delayed division
+	t /= d;
+
+	return true;
+}
+
+static char* parse_Row(char* buf, char* bufEnd, char* row, int len)
+{
+	bool start = true;
+	bool done = false;
+	int n = 0;
+	while (!done && buf < bufEnd)
+	{
+		char c = *buf;
+		buf++;
+		// multirow
+		switch (c)
+		{
+		case '\n':
+			if (start) break;
+			done = true;
+			break;
+		case '\r':
+			break;
+		case '\t':
+		case ' ':
+			if (start) break;
+			// else falls through
+		default:
+			start = false;
+			row[n++] = c;
+			if (n >= len - 1)
+				done = true;
+			break;
+		}
+	}
+	row[n] = '\0';
+	return buf;
+}
+
+
+
+InputGeom::InputGeom() :
+	m_mesh(0),
+	m_hasBuildSettings(false),
+	m_offMeshConCount(0),
+	m_volumeCount(0)
+{
+}
+
+InputGeom::~InputGeom()
+{
+	delete m_mesh;
+}
+
+bool InputGeom::loadMesh(rcContext* ctx, const std::string& filepath)
+{
+	if (m_mesh)
+	{
+		delete m_mesh;
+		m_mesh = 0;
+	}
+	m_offMeshConCount = 0;
+	m_volumeCount = 0;
+
+	m_mesh = new rcMeshLoaderObj;
+	if (!m_mesh)
+	{
+		ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
+		return false;
+	}
+	if (!m_mesh->load(filepath))
+	{
+		ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath.c_str());
+		return false;
+	}
+
+	rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);
+
+	return true;
+}
+
+bool InputGeom::loadMesh(rcContext* ctx, NodePath node)
+{
+	if (m_mesh)
+	{
+		delete m_mesh;
+		m_mesh = 0;
+	}
+	m_offMeshConCount = 0;
+	m_volumeCount = 0;
+
+	m_mesh = new rcMeshLoaderObj;
+	if (!m_mesh)
+	{
+		ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
+		return false;
+	}
+	if (!m_mesh->load(node))
+	{
+		ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load 'NodePath'");
+		return false;
+	}
+
+	rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);
+
+	return true;
+}
+
+bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
+{
+	char* buf = 0;
+	FILE* fp = fopen(filepath.c_str(), "rb");
+	if (!fp)
+	{
+		return false;
+	}
+	if (fseek(fp, 0, SEEK_END) != 0)
+	{
+		fclose(fp);
+		return false;
+	}
+
+	long bufSize = ftell(fp);
+	if (bufSize < 0)
+	{
+		fclose(fp);
+		return false;
+	}
+	if (fseek(fp, 0, SEEK_SET) != 0)
+	{
+		fclose(fp);
+		return false;
+	}
+	buf = new char[bufSize];
+	if (!buf)
+	{
+		fclose(fp);
+		return false;
+	}
+	size_t readLen = fread(buf, bufSize, 1, fp);
+	fclose(fp);
+	if (readLen != 1)
+	{
+		delete[] buf;
+		return false;
+	}
+
+	m_offMeshConCount = 0;
+	m_volumeCount = 0;
+	delete m_mesh;
+	m_mesh = 0;
+
+	char* src = buf;
+	char* srcEnd = buf + bufSize;
+	char row[512];
+	while (src < srcEnd)
+	{
+		// Parse one row
+		row[0] = '\0';
+		src = parse_Row(src, srcEnd, row, sizeof(row) / sizeof(char));
+		if (row[0] == 'f')
+		{
+			// File name.
+			const char* name = row + 1;
+			// Skip white spaces
+			while (*name && isspace(*name))
+				name++;
+			if (*name)
+			{
+				if (!loadMesh(ctx, name))
+				{
+					delete[] buf;
+					return false;
+				}
+			}
+		}
+		else if (row[0] == 'c')
+		{
+			// Off-mesh connection
+			if (m_offMeshConCount < MAX_OFFMESH_CONNECTIONS)
+			{
+				float* v = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
+				int bidir, area = 0, flags = 0;
+				float rad;
+				sscanf(row + 1, "%f %f %f  %f %f %f %f %d %d %d",
+					&v[0], &v[1], &v[2], &v[3], &v[4], &v[5], &rad, &bidir, &area, &flags);
+				m_offMeshConRads[m_offMeshConCount] = rad;
+				m_offMeshConDirs[m_offMeshConCount] = (unsigned char)bidir;
+				m_offMeshConAreas[m_offMeshConCount] = (unsigned char)area;
+				m_offMeshConFlags[m_offMeshConCount] = (unsigned short)flags;
+				m_offMeshConCount++;
+			}
+		}
+		else if (row[0] == 'v')
+		{
+			// Convex volumes
+			if (m_volumeCount < MAX_VOLUMES)
+			{
+				ConvexVolume* vol = &m_volumes[m_volumeCount++];
+				sscanf(row + 1, "%d %d %f %f", &vol->nverts, &vol->area, &vol->hmin, &vol->hmax);
+				for (int i = 0; i < vol->nverts; ++i)
+				{
+					row[0] = '\0';
+					src = parse_Row(src, srcEnd, row, sizeof(row) / sizeof(char));
+					sscanf(row, "%f %f %f", &vol->verts[i * 3 + 0], &vol->verts[i * 3 + 1], &vol->verts[i * 3 + 2]);
+				}
+			}
+		}
+		else if (row[0] == 's')
+		{
+			// Settings
+			m_hasBuildSettings = true;
+			sscanf(row + 1, "%f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f",
+				&m_buildSettings.cellSize,
+				&m_buildSettings.cellHeight,
+				&m_buildSettings.agentHeight,
+				&m_buildSettings.agentRadius,
+				&m_buildSettings.agentMaxClimb,
+				&m_buildSettings.agentMaxSlope,
+				&m_buildSettings.regionMinSize,
+				&m_buildSettings.regionMergeSize,
+				&m_buildSettings.edgeMaxLen,
+				&m_buildSettings.edgeMaxError,
+				&m_buildSettings.vertsPerPoly,
+				&m_buildSettings.detailSampleDist,
+				&m_buildSettings.detailSampleMaxError,
+				&m_buildSettings.partitionType,
+				&m_buildSettings.navMeshBMin[0],
+				&m_buildSettings.navMeshBMin[1],
+				&m_buildSettings.navMeshBMin[2],
+				&m_buildSettings.navMeshBMax[0],
+				&m_buildSettings.navMeshBMax[1],
+				&m_buildSettings.navMeshBMax[2],
+				&m_buildSettings.tileSize);
+		}
+	}
+
+	delete[] buf;
+
+	return true;
+}
+
+bool InputGeom::load(rcContext* ctx, NodePath node) 
+{
+	return loadMesh(ctx, node);
+}
+
+bool InputGeom::load(rcContext* ctx, const std::string& filepath)
+{
+	size_t extensionPos = filepath.find_last_of('.');
+	if (extensionPos == std::string::npos)
+		return false;
+
+	std::string extension = filepath.substr(extensionPos);
+	std::transform(extension.begin(), extension.end(), extension.begin(), tolower);
+
+	if (extension == ".gset")
+		return loadGeomSet(ctx, filepath);
+	if (extension == ".obj")
+		return loadMesh(ctx, filepath);
+
+	return false;
+}
+
+bool InputGeom::saveGeomSet(const BuildSettings* settings)
+{
+	if (!m_mesh) return false;
+
+	// Change extension
+	std::string filepath = m_mesh->getFileName();
+	size_t extPos = filepath.find_last_of('.');
+	if (extPos != std::string::npos)
+		filepath = filepath.substr(0, extPos);
+
+	filepath += ".gset";
+
+	FILE* fp = fopen(filepath.c_str(), "w");
+	if (!fp) return false;
+
+	// Store mesh filename.
+	fprintf(fp, "f %s\n", m_mesh->getFileName().c_str());
+
+	// Store settings if any
+	if (settings)
+	{
+		fprintf(fp,
+			"s %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f\n",
+			settings->cellSize,
+			settings->cellHeight,
+			settings->agentHeight,
+			settings->agentRadius,
+			settings->agentMaxClimb,
+			settings->agentMaxSlope,
+			settings->regionMinSize,
+			settings->regionMergeSize,
+			settings->edgeMaxLen,
+			settings->edgeMaxError,
+			settings->vertsPerPoly,
+			settings->detailSampleDist,
+			settings->detailSampleMaxError,
+			settings->partitionType,
+			settings->navMeshBMin[0],
+			settings->navMeshBMin[1],
+			settings->navMeshBMin[2],
+			settings->navMeshBMax[0],
+			settings->navMeshBMax[1],
+			settings->navMeshBMax[2],
+			settings->tileSize);
+	}
+
+	// Store off-mesh links.
+	for (int i = 0; i < m_offMeshConCount; ++i)
+	{
+		const float* v = &m_offMeshConVerts[i * 3 * 2];
+		const float rad = m_offMeshConRads[i];
+		const int bidir = m_offMeshConDirs[i];
+		const int area = m_offMeshConAreas[i];
+		const int flags = m_offMeshConFlags[i];
+		fprintf(fp, "c %f %f %f  %f %f %f  %f %d %d %d\n",
+			v[0], v[1], v[2], v[3], v[4], v[5], rad, bidir, area, flags);
+	}
+
+	// Convex volumes
+	for (int i = 0; i < m_volumeCount; ++i)
+	{
+		ConvexVolume* vol = &m_volumes[i];
+		fprintf(fp, "v %d %d %f %f\n", vol->nverts, vol->area, vol->hmin, vol->hmax);
+		for (int j = 0; j < vol->nverts; ++j)
+			fprintf(fp, "%f %f %f\n", vol->verts[j * 3 + 0], vol->verts[j * 3 + 1], vol->verts[j * 3 + 2]);
+	}
+
+	fclose(fp);
+
+	return true;
+}
+
+static bool isectSegAABB(const float* sp, const float* sq,
+	const float* amin, const float* amax,
+	float& tmin, float& tmax)
+{
+	static const float EPS = 1e-6f;
+
+	float d[3];
+	d[0] = sq[0] - sp[0];
+	d[1] = sq[1] - sp[1];
+	d[2] = sq[2] - sp[2];
+	tmin = 0.0;
+	tmax = 1.0f;
+
+	for (int i = 0; i < 3; i++)
+	{
+		if (fabsf(d[i]) < EPS)
+		{
+			if (sp[i] < amin[i] || sp[i] > amax[i])
+				return false;
+		}
+		else
+		{
+			const float ood = 1.0f / d[i];
+			float t1 = (amin[i] - sp[i]) * ood;
+			float t2 = (amax[i] - sp[i]) * ood;
+			if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
+			if (t1 > tmin) tmin = t1;
+			if (t2 < tmax) tmax = t2;
+			if (tmin > tmax) return false;
+		}
+	}
+
+	return true;
+}
+
+
+bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
+{
+	float dir[3];
+	rcVsub(dir, dst, src);
+
+	// Prune hit ray.
+	float btmin, btmax;
+	if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax))
+		return false;
+	float p[2], q[2];
+	p[0] = src[0] + (dst[0] - src[0])*btmin;
+	p[1] = src[2] + (dst[2] - src[2])*btmin;
+	q[0] = src[0] + (dst[0] - src[0])*btmax;
+	q[1] = src[2] + (dst[2] - src[2])*btmax;
+
+	int cid[512];
+	
+	tmin = 1.0f;
+	bool hit = false;
+	const float* verts = m_mesh->getVerts();
+
+	return hit;
+}
+
+void InputGeom::addOffMeshConnection(const float* spos, const float* epos, const float rad,
+	unsigned char bidir, unsigned char area, unsigned short flags)
+{
+	if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) return;
+	float* v = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
+	m_offMeshConRads[m_offMeshConCount] = rad;
+	m_offMeshConDirs[m_offMeshConCount] = bidir;
+	m_offMeshConAreas[m_offMeshConCount] = area;
+	m_offMeshConFlags[m_offMeshConCount] = flags;
+	m_offMeshConId[m_offMeshConCount] = 1000 + m_offMeshConCount;
+	rcVcopy(&v[0], spos);
+	rcVcopy(&v[3], epos);
+	m_offMeshConCount++;
+}
+
+void InputGeom::deleteOffMeshConnection(int i)
+{
+	m_offMeshConCount--;
+	float* src = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
+	float* dst = &m_offMeshConVerts[i * 3 * 2];
+	rcVcopy(&dst[0], &src[0]);
+	rcVcopy(&dst[3], &src[3]);
+	m_offMeshConRads[i] = m_offMeshConRads[m_offMeshConCount];
+	m_offMeshConDirs[i] = m_offMeshConDirs[m_offMeshConCount];
+	m_offMeshConAreas[i] = m_offMeshConAreas[m_offMeshConCount];
+	m_offMeshConFlags[i] = m_offMeshConFlags[m_offMeshConCount];
+}
+
+void InputGeom::drawOffMeshConnections(duDebugDraw* dd, bool hilight)
+{
+	unsigned int conColor = duRGBA(192, 0, 128, 192);
+	unsigned int baseColor = duRGBA(0, 0, 0, 64);
+	dd->depthMask(false);
+
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	for (int i = 0; i < m_offMeshConCount; ++i)
+	{
+		float* v = &m_offMeshConVerts[i * 3 * 2];
+
+		dd->vertex(v[0], v[1], v[2], baseColor);
+		dd->vertex(v[0], v[1] + 0.2f, v[2], baseColor);
+
+		dd->vertex(v[3], v[4], v[5], baseColor);
+		dd->vertex(v[3], v[4] + 0.2f, v[5], baseColor);
+
+		duAppendCircle(dd, v[0], v[1] + 0.1f, v[2], m_offMeshConRads[i], baseColor);
+		duAppendCircle(dd, v[3], v[4] + 0.1f, v[5], m_offMeshConRads[i], baseColor);
+
+		if (hilight)
+		{
+			duAppendArc(dd, v[0], v[1], v[2], v[3], v[4], v[5], 0.25f,
+				(m_offMeshConDirs[i] & 1) ? 0.6f : 0.0f, 0.6f, conColor);
+		}
+	}
+	dd->end();
+
+	dd->depthMask(true);
+}
+
+void InputGeom::addConvexVolume(const float* verts, const int nverts,
+	const float minh, const float maxh, unsigned char area)
+{
+	if (m_volumeCount >= MAX_VOLUMES) return;
+	ConvexVolume* vol = &m_volumes[m_volumeCount++];
+	memset(vol, 0, sizeof(ConvexVolume));
+	memcpy(vol->verts, verts, sizeof(float) * 3 * nverts);
+	vol->hmin = minh;
+	vol->hmax = maxh;
+	vol->nverts = nverts;
+	vol->area = area;
+}
+
+void InputGeom::deleteConvexVolume(int i)
+{
+	m_volumeCount--;
+	m_volumes[i] = m_volumes[m_volumeCount];
+}
+
+void InputGeom::drawConvexVolumes(struct duDebugDraw* dd, bool /*hilight*/)
+{
+	dd->depthMask(false);
+
+	dd->begin(DU_DRAW_TRIS);
+
+	for (int i = 0; i < m_volumeCount; ++i)
+	{
+		const ConvexVolume* vol = &m_volumes[i];
+		unsigned int col = duTransCol(dd->areaToCol(vol->area), 32);
+		for (int j = 0, k = vol->nverts - 1; j < vol->nverts; k = j++)
+		{
+			const float* va = &vol->verts[k * 3];
+			const float* vb = &vol->verts[j * 3];
+
+			dd->vertex(vol->verts[0], vol->hmax, vol->verts[2], col);
+			dd->vertex(vb[0], vol->hmax, vb[2], col);
+			dd->vertex(va[0], vol->hmax, va[2], col);
+
+			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
+			dd->vertex(va[0], vol->hmax, va[2], col);
+			dd->vertex(vb[0], vol->hmax, vb[2], col);
+
+			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
+			dd->vertex(vb[0], vol->hmax, vb[2], col);
+			dd->vertex(vb[0], vol->hmin, vb[2], duDarkenCol(col));
+		}
+	}
+
+	dd->end();
+
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	for (int i = 0; i < m_volumeCount; ++i)
+	{
+		const ConvexVolume* vol = &m_volumes[i];
+		unsigned int col = duTransCol(dd->areaToCol(vol->area), 220);
+		for (int j = 0, k = vol->nverts - 1; j < vol->nverts; k = j++)
+		{
+			const float* va = &vol->verts[k * 3];
+			const float* vb = &vol->verts[j * 3];
+			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
+			dd->vertex(vb[0], vol->hmin, vb[2], duDarkenCol(col));
+			dd->vertex(va[0], vol->hmax, va[2], col);
+			dd->vertex(vb[0], vol->hmax, vb[2], col);
+			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
+			dd->vertex(va[0], vol->hmax, va[2], col);
+		}
+	}
+	dd->end();
+
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	for (int i = 0; i < m_volumeCount; ++i)
+	{
+		const ConvexVolume* vol = &m_volumes[i];
+		unsigned int col = duDarkenCol(duTransCol(dd->areaToCol(vol->area), 220));
+		for (int j = 0; j < vol->nverts; ++j)
+		{
+			dd->vertex(vol->verts[j * 3 + 0], vol->verts[j * 3 + 1] + 0.1f, vol->verts[j * 3 + 2], col);
+			dd->vertex(vol->verts[j * 3 + 0], vol->hmin, vol->verts[j * 3 + 2], col);
+			dd->vertex(vol->verts[j * 3 + 0], vol->hmax, vol->verts[j * 3 + 2], col);
+		}
+	}
+	dd->end();
+
+
+	dd->depthMask(true);
+}

panda/src/recastdetour/InputGeom.h

@@ -0,0 +1,135 @@
+
+
+#ifndef INPUTGEOM_H
+#define INPUTGEOM_H
+
+#include "MeshLoaderObj.h"
+
+static const int MAX_CONVEXVOL_PTS = 12;
+struct ConvexVolume
+{
+	float verts[MAX_CONVEXVOL_PTS * 3];
+	float hmin, hmax;
+	int nverts;
+	int area;
+};
+
+struct BuildSettings
+{
+	// Cell size in world units
+	float cellSize;
+	// Cell height in world units
+	float cellHeight;
+	// Agent height in world units
+	float agentHeight;
+	// Agent radius in world units
+	float agentRadius;
+	// Agent max climb in world units
+	float agentMaxClimb;
+	// Agent max slope in degrees
+	float agentMaxSlope;
+	// Region minimum size in voxels.
+	// regionMinSize = sqrt(regionMinArea)
+	float regionMinSize;
+	// Region merge size in voxels.
+	// regionMergeSize = sqrt(regionMergeArea)
+	float regionMergeSize;
+	// Edge max length in world units
+	float edgeMaxLen;
+	// Edge max error in voxels
+	float edgeMaxError;
+	float vertsPerPoly;
+	// Detail sample distance in voxels
+	float detailSampleDist;
+	// Detail sample max error in voxel heights.
+	float detailSampleMaxError;
+	// Partition type, see SamplePartitionType
+	int partitionType;
+	// Bounds of the area to mesh
+	float navMeshBMin[3];
+	float navMeshBMax[3];
+	// Size of the tiles in voxels
+	float tileSize;
+};
+
+class InputGeom
+{
+	rcMeshLoaderObj* m_mesh;
+	float m_meshBMin[3], m_meshBMax[3];
+	BuildSettings m_buildSettings;
+	bool m_hasBuildSettings;
+
+	/// @name Off-Mesh connections.
+	///@{
+	static const int MAX_OFFMESH_CONNECTIONS = 256;
+	float m_offMeshConVerts[MAX_OFFMESH_CONNECTIONS * 3 * 2];
+	float m_offMeshConRads[MAX_OFFMESH_CONNECTIONS];
+	unsigned char m_offMeshConDirs[MAX_OFFMESH_CONNECTIONS];
+	unsigned char m_offMeshConAreas[MAX_OFFMESH_CONNECTIONS];
+	unsigned short m_offMeshConFlags[MAX_OFFMESH_CONNECTIONS];
+	unsigned int m_offMeshConId[MAX_OFFMESH_CONNECTIONS];
+	int m_offMeshConCount;
+	///@}
+
+	/// @name Convex Volumes.
+	///@{
+	static const int MAX_VOLUMES = 256;
+	ConvexVolume m_volumes[MAX_VOLUMES];
+	int m_volumeCount;
+	///@}
+
+	bool loadMesh(class rcContext* ctx, const std::string& filepath);
+	bool loadMesh(class rcContext* ctx, NodePath node);
+	bool loadGeomSet(class rcContext* ctx, const std::string& filepath);
+public:
+	InputGeom();
+	~InputGeom();
+
+
+	bool load(class rcContext* ctx, const std::string& filepath);
+	bool load(class rcContext* ctx, NodePath node);
+	bool saveGeomSet(const BuildSettings* settings);
+
+
+
+	/// Method to return static mesh data.
+	const rcMeshLoaderObj* getMesh() const { return m_mesh; }
+	const float* getMeshBoundsMin() const { return m_meshBMin; }
+	const float* getMeshBoundsMax() const { return m_meshBMax; }
+	const float* getNavMeshBoundsMin() const { return m_hasBuildSettings ? m_buildSettings.navMeshBMin : m_meshBMin; }
+	const float* getNavMeshBoundsMax() const { return m_hasBuildSettings ? m_buildSettings.navMeshBMax : m_meshBMax; }
+	const BuildSettings* getBuildSettings() const { return m_hasBuildSettings ? &m_buildSettings : 0; }
+	bool raycastMesh(float* src, float* dst, float& tmin);
+
+	/// @name Off-Mesh connections.
+	///@{
+	int getOffMeshConnectionCount() const { return m_offMeshConCount; }
+	const float* getOffMeshConnectionVerts() const { return m_offMeshConVerts; }
+	const float* getOffMeshConnectionRads() const { return m_offMeshConRads; }
+	const unsigned char* getOffMeshConnectionDirs() const { return m_offMeshConDirs; }
+	const unsigned char* getOffMeshConnectionAreas() const { return m_offMeshConAreas; }
+	const unsigned short* getOffMeshConnectionFlags() const { return m_offMeshConFlags; }
+	const unsigned int* getOffMeshConnectionId() const { return m_offMeshConId; }
+	void addOffMeshConnection(const float* spos, const float* epos, const float rad,
+		unsigned char bidir, unsigned char area, unsigned short flags);
+	void deleteOffMeshConnection(int i);
+	void drawOffMeshConnections(struct duDebugDraw* dd, bool hilight = false);
+	///@}
+
+	/// @name Box Volumes.
+	///@{
+	int getConvexVolumeCount() const { return m_volumeCount; }
+	const ConvexVolume* getConvexVolumes() const { return m_volumes; }
+	void addConvexVolume(const float* verts, const int nverts,
+		const float minh, const float maxh, unsigned char area);
+	void deleteConvexVolume(int i);
+	void drawConvexVolumes(struct duDebugDraw* dd, bool hilight = false);
+	///@}
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	InputGeom(const InputGeom&);
+	InputGeom& operator=(const InputGeom&);
+};
+
+#endif // INPUTGEOM_H

panda/src/recastdetour/MeshLoaderObj.cxx

@@ -0,0 +1,385 @@
+
+#include "MeshLoaderObj.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <cstring>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include "geom.h"
+#include "nodePath.h"
+#include "geomVertexReader.h"
+#include "geomNode.h"
+#include "pmap.h"
+#include "pvector.h"
+
+
+std::map<LVector3,int> mp;
+int indexTemp = 0;
+
+rcMeshLoaderObj::rcMeshLoaderObj() :
+	m_scale(1.0f),
+	m_verts(0),
+	m_tris(0),
+	m_normals(0),
+	m_vertCount(0),
+	m_triCount(0),
+	temp_vcount(0)
+{
+}
+
+rcMeshLoaderObj::~rcMeshLoaderObj()
+{
+	delete[] m_verts;
+	delete[] m_normals;
+	delete[] m_tris;
+}
+
+void rcMeshLoaderObj::addVertex(float x, float y, float z, int& cap)
+{
+	if (m_vertCount + 1 > cap)
+	{
+		cap = !cap ? 8 : cap * 2;
+		float* nv = new float[cap * 3];
+		if (m_vertCount)
+			memcpy(nv, m_verts, m_vertCount * 3 * sizeof(float));
+		delete[] m_verts;
+		m_verts = nv;
+	}
+	float* dst = &m_verts[m_vertCount * 3];
+	*dst++ = x * m_scale;
+	*dst++ = y * m_scale;
+	*dst++ = z * m_scale;
+	m_vertCount++;
+}
+
+void rcMeshLoaderObj::addTriangle(int a, int b, int c, int& cap)
+{
+	if (m_triCount + 1 > cap)
+	{
+		cap = !cap ? 8 : cap * 2;
+		int* nv = new int[cap * 3];
+		if (m_triCount)
+			memcpy(nv, m_tris, m_triCount * 3 * sizeof(int));
+		delete[] m_tris;
+		m_tris = nv;
+	}
+	int* dst = &m_tris[m_triCount * 3];
+	*dst++ = a;
+	*dst++ = b;
+	*dst++ = c;
+	m_triCount++;
+}
+
+
+static char* parseRow(char* buf, char* bufEnd, char* row, int len)
+{
+	bool start = true;
+	bool done = false;
+	int n = 0;
+	while (!done && buf < bufEnd)
+	{
+		char c = *buf;
+		buf++;
+		// multirow
+		switch (c)
+		{
+		case '\\':
+			break;
+		case '\n':
+			if (start) break;
+			done = true;
+			break;
+		case '\r':
+			break;
+		case '\t':
+		case ' ':
+			if (start) break;
+			// else falls through
+		default:
+			start = false;
+			row[n++] = c;
+			if (n >= len - 1)
+				done = true;
+			break;
+		}
+	}
+	row[n] = '\0';
+	return buf;
+}
+
+static int parseFace(char* row, int* data, int n, int vcnt)
+{
+	int j = 0;
+	while (*row != '\0')
+	{
+		// Skip initial white space
+		while (*row != '\0' && (*row == ' ' || *row == '\t'))
+			row++;
+		char* s = row;
+		// Find vertex delimiter and terminated the string there for conversion.
+		while (*row != '\0' && *row != ' ' && *row != '\t')
+		{
+			if (*row == '/') *row = '\0';
+			row++;
+		}
+		if (*s == '\0')
+			continue;
+		int vi = atoi(s);
+		data[j++] = vi < 0 ? vi + vcnt : vi - 1;
+		if (j >= n) return j;
+	}
+	return j;
+}
+
+bool rcMeshLoaderObj::load(const std::string& filename)
+{
+	char* buf = 0;
+	FILE* fp = fopen(filename.c_str(), "rb");
+	if (!fp)
+		return false;
+	if (fseek(fp, 0, SEEK_END) != 0)
+	{
+		fclose(fp);
+		return false;
+	}
+	long bufSize = ftell(fp);
+	if (bufSize < 0)
+	{
+		fclose(fp);
+		return false;
+	}
+	if (fseek(fp, 0, SEEK_SET) != 0)
+	{
+		fclose(fp);
+		return false;
+	}
+	buf = new char[bufSize];
+	if (!buf)
+	{
+		fclose(fp);
+		return false;
+	}
+	size_t readLen = fread(buf, bufSize, 1, fp);
+	fclose(fp);
+
+	if (readLen != 1)
+	{
+		delete[] buf;
+		return false;
+	}
+
+	char* src = buf;
+	char* srcEnd = buf + bufSize;
+	char row[512];
+	int face[32];
+	float x, y, z;
+	int nv;
+	int vcap = 0;
+	int tcap = 0;
+
+	while (src < srcEnd)
+	{
+		// Parse one row
+		row[0] = '\0';
+		src = parseRow(src, srcEnd, row, sizeof(row) / sizeof(char));
+		// Skip comments
+		if (row[0] == '#') continue;
+		if (row[0] == 'v' && row[1] != 'n' && row[1] != 't')
+		{
+			// Vertex pos
+			sscanf(row + 1, "%f %f %f", &x, &y, &z);
+			addVertex(x, z, -y, vcap);		//exchanged y and z to match with panda3d's coordinate system
+		}
+		if (row[0] == 'f')
+		{
+			// Faces
+			nv = parseFace(row + 1, face, 32, m_vertCount);
+			for (int i = 2; i < nv; ++i)
+			{
+				const int a = face[0];
+				const int b = face[i - 1];
+				const int c = face[i];
+				if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount)
+					continue;
+				addTriangle(a, b, c, tcap);
+			}
+		}
+	}
+
+	delete[] buf;
+
+	// Calculate normals.
+	m_normals = new float[m_triCount * 3];
+	for (int i = 0; i < m_triCount * 3; i += 3)
+	{
+		const float* v0 = &m_verts[m_tris[i] * 3];
+		const float* v1 = &m_verts[m_tris[i + 1] * 3];
+		const float* v2 = &m_verts[m_tris[i + 2] * 3];
+		float e0[3], e1[3];
+		for (int j = 0; j < 3; ++j)
+		{
+			e0[j] = v1[j] - v0[j];
+			e1[j] = v2[j] - v0[j];
+		}
+		float* n = &m_normals[i];
+		n[0] = e0[1] * e1[2] - e0[2] * e1[1];
+		n[1] = e0[2] * e1[0] - e0[0] * e1[2];
+		n[2] = e0[0] * e1[1] - e0[1] * e1[0];
+		float d = sqrtf(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
+		if (d > 0)
+		{
+			d = 1.0f / d;
+			n[0] *= d;
+			n[1] *= d;
+			n[2] *= d;
+		}
+	}
+
+	m_filename = filename;
+	return true;
+}
+std::vector<LVector3> vc,fc;
+bool rcMeshLoaderObj::load(NodePath node)
+{
+	NodePathCollection geomNodeCollection = node.find_all_matches("**/+GeomNode");
+	//std::cout << "Number of geomNode_paths: " << geomNodeCollection.get_num_paths() << std::endl;
+	int vcap = 0;
+	int tcap = 0;
+
+
+	for (size_t i = 0; i < geomNodeCollection.get_num_paths(); ++i)
+	{
+		temp_vcount = m_vertCount;
+		//std::cout << "Vertices count: " << m_vertCount << std::endl;
+		PT(GeomNode) g = DCAST(GeomNode, geomNodeCollection.get_path(i).node());
+		//std::cout << "GeomPath i = " << i << std::endl;
+		processGeomNode(g, vcap, tcap);
+	}
+	for (int i = 0;i < vc.size();i++) {
+		//std::cout << "v " << vc[i][0] << " " << vc[i][1] << " " << vc[i][2] << "\n";
+	}
+	for (int i = 0;i < fc.size();i++) {
+		//std::cout << "f " << fc[i][0] << " " << fc[i][1] << " " << fc[i][2] << "\n";
+	}
+	return true;
+}
+
+void rcMeshLoaderObj::processPrimitive(const GeomPrimitive *orig_prim, const GeomVertexData *vdata, int& tcap) {
+
+	GeomVertexReader vertex(vdata, "vertex");
+
+	CPT(GeomPrimitive) prim = orig_prim->decompose();
+	//std::cout << "prim->get_num_primitives(): " << prim->get_num_primitives() << std::endl;
+	for (size_t k = 0; k < prim->get_num_primitives(); ++k) {
+		int s = prim->get_primitive_start(k);
+		int e = prim->get_primitive_end(k);
+		//std::cout << "(s, e): " << s << "\t" << e << "\n";
+		LVector3 v;
+		if (e - s == 3)
+		{
+			int a = prim->get_vertex(s);
+			vertex.set_row(a);
+			v = vertex.get_data3();
+			a = mp[v];
+
+			int b = prim->get_vertex(s + 1);
+			vertex.set_row(b);
+			v = vertex.get_data3();
+			b = mp[v];
+			
+			int c = prim->get_vertex(s + 2);
+			vertex.set_row(c);
+			v = vertex.get_data3();
+			c = mp[v];
+
+			//if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount)
+				//continue;
+			//std::cout <<"f: " << a << "\t" << b << "\t" << c << "\ntcap: "<<tcap<<"\n";
+			LVector3 xvx = { float(a + 1), float(b + 1), float(c + 1) };
+			fc.push_back(xvx);
+			addTriangle(a, b, c, tcap);
+		}
+		else if(e - s > 3 ) {
+			//std::cout << "More than 3 vertices\n";
+			for (int i = s+2; i < e; ++i)
+			{
+				int a = prim->get_vertex(s);
+				vertex.set_row(a);
+				v = vertex.get_data3();
+				a = mp[v];
+
+				int b = prim->get_vertex(i-1);
+				vertex.set_row(b);
+				v = vertex.get_data3();
+				b = mp[v];
+
+				int c = prim->get_vertex(i);
+				vertex.set_row(c);
+				v = vertex.get_data3();
+				c = mp[v];
+
+				//if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount)
+					//continue;
+				//std::cout << "f: " << a << "\t" << b << "\t" << c << "\ntcap: " << tcap << "\n";
+				LVector3 xvx = { float(a + 1), float(b + 1), float(c + 1) };
+				fc.push_back(xvx);
+				addTriangle(a, b, c, tcap);
+			}
+		}
+		else continue;
+		
+	}
+	return;
+}
+
+void rcMeshLoaderObj::processVertexData(const GeomVertexData *vdata, int& vcap) {
+	GeomVertexReader vertex(vdata, "vertex");
+	float x, y, z;
+	
+	while (!vertex.is_at_end()) {
+		//std::cout << vcap++ << std::endl;
+		LVector3 v = vertex.get_data3();
+		x = v[0];
+		y = v[1];
+		z = v[2];
+		if (mp.find(v) == mp.end())
+		{
+			addVertex(x, z, -y, vcap);
+			mp[v] = indexTemp++;
+			LVector3 xvx = { v[0],v[2],-v[1] };
+			vc.push_back(xvx);
+		}
+		
+		//nout << "vcount: " << m_vertCount << "\tvcap: "<< vcap <<"\n";
+		//nout << "V = " << v << std::endl;
+	}
+	return;
+}
+void rcMeshLoaderObj::processGeom(CPT(Geom) geom, int& vcap, int& tcap) {
+	
+	CPT(GeomVertexData) vdata = geom->get_vertex_data();
+	
+	processVertexData(vdata, vcap);
+	//std::cout << "geom->get_num_primitives(): " << geom->get_num_primitives() << std::endl;
+	for (size_t i = 0; i < geom->get_num_primitives(); ++i) {
+		
+		CPT(GeomPrimitive) prim = geom->get_primitive(i);
+		processPrimitive(prim, vdata, tcap);
+	}
+	return;
+}
+void rcMeshLoaderObj::processGeomNode(GeomNode *geomnode, int& vcap, int& tcap) {
+
+	//std::cout << "Number of geoms: " << geomnode->get_num_geoms() << std::endl;
+	if (geomnode->get_num_geoms() > 1)
+	{
+		std::cout << "Cannot proceed: Make sure number of geoms = 1\n";
+		return;
+	}
+	for (size_t j = 0; j < geomnode->get_num_geoms(); ++j) {
+		
+		CPT(Geom) geom = geomnode->get_geom(j);
+		processGeom(geom, vcap, tcap);
+	}
+	return;
+}

panda/src/recastdetour/MeshLoaderObj.h

@@ -0,0 +1,50 @@
+
+
+#ifndef MESHLOADER_OBJ
+#define MESHLOADER_OBJ
+
+#include <string>
+#include "geom.h"
+
+class rcMeshLoaderObj
+{
+public:
+	rcMeshLoaderObj();
+	~rcMeshLoaderObj();
+
+	bool load(const std::string& fileName);
+	bool load(NodePath node);
+
+	const float* getVerts() const { return m_verts; }
+	const float* getNormals() const { return m_normals; }
+	const int* getTris() const { return m_tris; }
+	int getVertCount() const { return m_vertCount; }
+	int getTriCount() const { return m_triCount; }
+	const std::string& getFileName() const { return m_filename; }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	rcMeshLoaderObj(const rcMeshLoaderObj&);
+	rcMeshLoaderObj& operator=(const rcMeshLoaderObj&);
+
+	void addVertex(float x, float y, float z, int& cap);
+	void addTriangle(int a, int b, int c, int& cap);
+
+
+	void processGeomNode(GeomNode *geomnode, int& vcap, int& tcap);
+	void processGeom(CPT(Geom) geom, int& vcap, int& tcap);
+	void processVertexData(const GeomVertexData *vdata, int& vcap);
+	void processPrimitive(const GeomPrimitive *orig_prim, const GeomVertexData *vdata, int& tcap);
+
+	std::string m_filename;
+	float m_scale;
+	float* m_verts;
+	int* m_tris;
+	float* m_normals;
+	int m_vertCount;
+	int m_triCount;
+
+	int temp_vcount;
+};
+
+#endif // MESHLOADER_OBJ

panda/src/recastdetour/NavMeshSample.cxx

@@ -0,0 +1,482 @@
+#include "NavMeshSample.h"
+
+#ifndef CPPPARSER
+#include "Recast.h"
+#include "DetourNavMesh.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourCrowd.h"
+#endif
+
+#include "InputGeom.h"
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include <string>
+#include <iostream>
+#include "geom.h"
+#include "geomVertexFormat.h"
+#include "geomVertexWriter.h"
+#include "geomTrifans.h"
+
+
+#ifdef WIN32
+#	define snprintf _snprintf
+#endif
+
+NavMeshSample::NavMeshSample():
+	m_geom(0),
+	m_navMesh(0),
+	m_navQuery(0),
+	m_crowd(0),
+	m_filterLowHangingObstacles(true),
+	m_filterLedgeSpans(true),
+	m_filterWalkableLowHeightSpans(true),
+	m_ctx(0),
+	m_triareas(0),
+	m_solid(0),
+	m_chf(0),
+	m_cset(0),
+	m_pmesh(0),
+	m_dmesh(0)
+{
+	m_ctx = new rcContext;
+	resetCommonSettings();
+	m_navQuery = dtAllocNavMeshQuery();
+	m_crowd = dtAllocCrowd();
+}
+
+NavMeshSample::~NavMeshSample()
+{
+	dtFreeNavMeshQuery(m_navQuery);
+	dtFreeNavMesh(m_navMesh);
+	dtFreeCrowd(m_crowd);
+
+	cleanup();
+
+}
+
+void NavMeshSample::cleanup()
+{
+	delete[] m_triareas;
+	m_triareas = 0;
+	rcFreeHeightField(m_solid);
+	m_solid = 0;
+	rcFreeCompactHeightfield(m_chf);
+	m_chf = 0;
+	rcFreeContourSet(m_cset);
+	m_cset = 0;
+	rcFreePolyMesh(m_pmesh);
+	m_pmesh = 0;
+	rcFreePolyMeshDetail(m_dmesh);
+	m_dmesh = 0;
+	dtFreeNavMesh(m_navMesh);
+	m_navMesh = 0;
+}
+
+void NavMeshSample::collectSettings(BuildSettings& settings)
+{
+	settings.cellSize = m_cellSize;
+	settings.cellHeight = m_cellHeight;
+	settings.agentHeight = m_agentHeight;
+	settings.agentRadius = m_agentRadius;
+	settings.agentMaxClimb = m_agentMaxClimb;
+	settings.agentMaxSlope = m_agentMaxSlope;
+	settings.regionMinSize = m_regionMinSize;
+	settings.regionMergeSize = m_regionMergeSize;
+	settings.edgeMaxLen = m_edgeMaxLen;
+	settings.edgeMaxError = m_edgeMaxError;
+	settings.vertsPerPoly = m_vertsPerPoly;
+	settings.detailSampleDist = m_detailSampleDist;
+	settings.detailSampleMaxError = m_detailSampleMaxError;
+	settings.partitionType = m_partitionType;
+}
+
+void NavMeshSample::resetCommonSettings()
+{
+	m_cellSize = 0.3f;
+	m_cellHeight = 0.2f;
+	m_agentHeight = 2.0f;
+	m_agentRadius = 0.6f;
+	m_agentMaxClimb = 0.9f;
+	m_agentMaxSlope = 45.0f;
+	m_regionMinSize = 8;
+	m_regionMergeSize = 20;
+	m_edgeMaxLen = 12.0f;
+	m_edgeMaxError = 1.3f;
+	m_vertsPerPoly = 6.0f;
+	m_detailSampleDist = 6.0f;
+	m_detailSampleMaxError = 1.0f;
+	m_partitionType = SAMPLE_PARTITION_WATERSHED;
+}
+
+static const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET';
+static const int NAVMESHSET_VERSION = 1;
+
+
+bool NavMeshSample::handleBuild() {
+	if (!m_geom || !m_geom->getMesh())
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Input mesh is not specified.");
+		std::cout << "\nbuildNavigation: Input mesh is not specified.\n";
+		return false;
+	}
+
+	cleanup();
+
+	const float* bmin = m_geom->getNavMeshBoundsMin();
+	const float* bmax = m_geom->getNavMeshBoundsMax();
+	const float* verts = m_geom->getMesh()->getVerts();
+	const int nverts = m_geom->getMesh()->getVertCount();
+	const int* tris = m_geom->getMesh()->getTris();
+	const int ntris = m_geom->getMesh()->getTriCount();
+	std::cout << "BMIN: " << bmin[0] << " " << bmin[1] << std::endl;
+	std::cout << "BMAX: " << bmax[0] << " " << bmax[1] << std::endl;
+	//
+	// Step 1. Initialize build config.
+	//
+	// Init build configuration from GUI
+	memset(&m_cfg, 0, sizeof(m_cfg));
+	m_cfg.cs = m_cellSize;
+	m_cfg.ch = m_cellHeight;
+	m_cfg.walkableSlopeAngle = m_agentMaxSlope;
+	m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch);
+	m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch);
+	m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs);
+	m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize);
+	m_cfg.maxSimplificationError = m_edgeMaxError;
+	m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize);		// Note: area = size*size
+	m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize);	// Note: area = size*size
+	m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly;
+	m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist;
+	m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError;
+
+	// Set the area where the navigation will be build.
+	// Here the bounds of the input mesh are used, but the
+	// area could be specified by an user defined box, etc.
+	rcVcopy(m_cfg.bmin, bmin);
+	rcVcopy(m_cfg.bmax, bmax);
+	rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height);
+
+	// Reset build times gathering.
+	m_ctx->resetTimers();
+
+	// Start the build process.	
+	m_ctx->startTimer(RC_TIMER_TOTAL);
+
+	m_ctx->log(RC_LOG_PROGRESS, "Building navigation:");
+	std::cout << "\nBuilding navigation:\n";
+	m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height);
+	std::cout << "\n - " << m_cfg.width << " x " << m_cfg.height << " cells\n";
+	m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts / 1000.0f, ntris / 1000.0f);
+	std::cout << "\n - " << nverts / 1000.0f << "K vetrs, " << ntris / 1000.0f << "K tris\n";
+
+	//
+	// Step 2. Rasterize input polygon soup.
+	//
+
+	// Allocate voxel heightfield where we rasterize our input data to.
+	m_solid = rcAllocHeightfield();
+	if (!m_solid)
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
+		std::cout << "\nbuildNavigation: Out of memory 'solid'.\n";
+		return false;
+	}
+	if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
+		std::cout << "\nbuildNavigation: Could not create solid heightfield.\n";
+		return false;
+	}
+
+	// Allocate array that can hold triangle area types.
+	// If you have multiple meshes you need to process, allocate
+	// and array which can hold the max number of triangles you need to process.
+	m_triareas = new unsigned char[ntris];
+	if (!m_triareas)
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris);
+		std::cout << "\nbuildNavigation: Out of memory 'm_triareas' (" << ntris << ").\n";
+		return false;
+	}
+
+	// Find triangles which are walkable based on their slope and rasterize them.
+	// If your input data is multiple meshes, you can transform them here, calculate
+	// the are type for each of the meshes and rasterize them.
+	memset(m_triareas, 0, ntris * sizeof(unsigned char));
+	rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas);
+	if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles.");
+		std::cout << "\nbuildNavigation: Could not rasterize triangles.\n";
+		return false;
+	}
+
+
+	//
+	// Step 3. Filter walkables surfaces.
+	//
+
+	// Once all geoemtry is rasterized, we do initial pass of filtering to
+	// remove unwanted overhangs caused by the conservative rasterization
+	// as well as filter spans where the character cannot possibly stand.
+	if (m_filterLowHangingObstacles)
+		rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid);
+	if (m_filterLedgeSpans)
+		rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
+	if (m_filterWalkableLowHeightSpans)
+		rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid);
+
+
+	//
+	// Step 4. Partition walkable surface to simple regions.
+	//
+
+	// Compact the heightfield so that it is faster to handle from now on.
+	// This will result more cache coherent data as well as the neighbours
+	// between walkable cells will be calculated.
+	m_chf = rcAllocCompactHeightfield();
+	if (!m_chf)
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
+		std::cout << "\nbuildNavigation: Out of memory 'chf'.\n";
+		return false;
+	}
+	if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
+		std::cout << "\nbuildNavigation: Could not build compact data.\n";
+		return false;
+	}
+
+
+	// Erode the walkable area by agent radius.
+	if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
+		std::cout << "\nbuildNavigation: Could not erode.\n";
+		return false;
+	}
+
+	// (Optional) Mark areas.
+	const ConvexVolume* vols = m_geom->getConvexVolumes();
+	for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i)
+		rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);
+
+
+	// Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas.
+	// There are 3 martitioning methods, each with some pros and cons:
+	// 1) Watershed partitioning
+	//   - the classic Recast partitioning
+	//   - creates the nicest tessellation
+	//   - usually slowest
+	//   - partitions the heightfield into nice regions without holes or overlaps
+	//   - the are some corner cases where this method creates produces holes and overlaps
+	//      - holes may appear when a small obstacles is close to large open area (triangulation can handle this)
+	//      - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail
+	//   * generally the best choice if you precompute the nacmesh, use this if you have large open areas
+	// 2) Monotone partioning
+	//   - fastest
+	//   - partitions the heightfield into regions without holes and overlaps (guaranteed)
+	//   - creates long thin polygons, which sometimes causes paths with detours
+	//   * use this if you want fast navmesh generation
+	// 3) Layer partitoining
+	//   - quite fast
+	//   - partitions the heighfield into non-overlapping regions
+	//   - relies on the triangulation code to cope with holes (thus slower than monotone partitioning)
+	//   - produces better triangles than monotone partitioning
+	//   - does not have the corner cases of watershed partitioning
+	//   - can be slow and create a bit ugly tessellation (still better than monotone)
+	//     if you have large open areas with small obstacles (not a problem if you use tiles)
+	//   * good choice to use for tiled navmesh with medium and small sized tiles
+
+	if (m_partitionType == SAMPLE_PARTITION_WATERSHED)
+	{
+		// Prepare for region partitioning, by calculating distance field along the walkable surface.
+		if (!rcBuildDistanceField(m_ctx, *m_chf))
+		{
+			m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
+			std::cout << "\nbuildNavigation: Could not build distance field.\n";
+			return false;
+		}
+
+		// Partition the walkable surface into simple regions without holes.
+		if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
+		{
+			m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions.");
+			std::cout << "\nbuildNavigation: Could not build watershed regions.\n";
+			return false;
+		}
+	}
+	else if (m_partitionType == SAMPLE_PARTITION_MONOTONE)
+	{
+		// Partition the walkable surface into simple regions without holes.
+		// Monotone partitioning does not need distancefield.
+		if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
+		{
+			m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions.");
+			std::cout << "\nbuildNavigation: Could not build monotone regions.\n";
+			return false;
+		}
+	}
+	else // SAMPLE_PARTITION_LAYERS
+	{
+		// Partition the walkable surface into simple regions without holes.
+		if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea))
+		{
+			m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions.");
+			std::cout << "\nbuildNavigation: Could not build layer regions.\n";
+			return false;
+		}
+	}
+
+	//
+	// Step 5. Trace and simplify region contours.
+	//
+
+	// Create contours.
+	m_cset = rcAllocContourSet();
+	if (!m_cset)
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
+		std::cout << "\nbuildNavigation: Out of memory 'cset'.\n";
+		return false;
+	}
+	if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
+		std::cout << "\nbuildNavigation: Could not create contours.\n";
+		return false;
+	}
+
+	//
+	// Step 6. Build polygons mesh from contours.
+	//
+
+	// Build polygon navmesh from the contours.
+	m_pmesh = rcAllocPolyMesh();
+	if (!m_pmesh)
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
+		std::cout << "\nbuildNavigation: Out of memory 'pmesh'.\n";
+		return false;
+	}
+	if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
+		std::cout << "\nbuildNavigation: Could not triangulate contours.\n";
+		return false;
+	}
+
+	//
+	// Step 7. Create detail mesh which allows to access approximate height on each polygon.
+	//
+
+	m_dmesh = rcAllocPolyMeshDetail();
+	if (!m_dmesh)
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'.");
+		std::cout << "\nbuildNavigation: Out of memory 'pmdt1'.\n";
+		return false;
+	}
+
+	if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh))
+	{
+		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh.");
+		std::cout << "\nbuildNavigation: Could not build detail mesh.\n";
+		return false;
+	}
+	std::cout << "Number of vertices: " << m_pmesh->nverts << std::endl;
+	std::cout << "Number of polygons: " << m_pmesh->npolys << std::endl;
+	std::cout << "Number of allocated polygons: " << m_pmesh->maxpolys << std::endl;
+	return true;
+	// At this point the navigation mesh data is ready, you can access it from m_pmesh.
+	// See duDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
+
+}
+
+
+
+PT(GeomNode) NavMeshSample::getPolyMeshGeom() {
+
+	PT(GeomVertexData) vdata;
+	vdata = new GeomVertexData("vertexInfo", GeomVertexFormat::get_v3(), Geom::UH_static);
+	vdata->set_num_rows(m_pmesh->nverts);
+
+	GeomVertexWriter vertex(vdata, "vertex");
+
+	const int nvp = m_pmesh->nvp;
+	std::cout << "nvp: " << nvp << std::endl;
+	const float cs = m_pmesh->cs;
+	std::cout << "cs: " << cs << std::endl;
+	const float ch = m_pmesh->ch;
+	std::cout << "ch: " << ch << std::endl;
+	const float* orig = m_pmesh->bmin;
+	std::cout << "orig: " << orig[0] << "\t" << orig[1] << "\t" << orig[2] << std::endl;
+
+	std::cout << "m_pmesh->npolys: " << m_pmesh->npolys << std::endl;
+	std::cout << "m_pmesh->nverts: " << m_pmesh->nverts << std::endl;
+
+	for (int i = 0;i < m_pmesh->nverts*3;i += 3) {
+
+		const unsigned short* v = &m_pmesh->verts[i];
+
+		//convert to world space
+		const float x = orig[0] + v[0] * cs;
+		const float y = orig[1] + v[1] * ch;
+		const float z = orig[2] + v[2] * cs;
+		
+		vertex.add_data3(x, -z, y);
+		std::cout << "index: " << i / 3 << "\t" << x << "\t" << y << "\t" << z << "\n";
+
+	}
+
+	PT(GeomNode) node;
+	node = new GeomNode("gnode");
+
+	PT(GeomTrifans) prim;
+	prim = new GeomTrifans(Geom::UH_static);
+
+	for (int i = 0; i < m_pmesh->npolys; ++i)
+	{
+		
+		
+		const unsigned short* p = &m_pmesh->polys[i*nvp * 2];
+
+		// Iterate the vertices.
+		//unsigned short vi[3];  // The vertex indices.
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX)
+			{
+				break;// End of vertices.
+			}
+			if (p[j + nvp] == RC_MESH_NULL_IDX)
+			{
+				prim->add_vertex(p[j]);
+				// The edge beginning with this vertex is a solid border.
+			}
+			else
+			{
+				prim->add_vertex(p[j]);
+				// The edge beginning with this vertex connects to 
+				// polygon p[j + nvp].
+			}
+			std::cout << "p[j]: " << p[j] << std::endl;
+			
+		}
+		prim->close_primitive();
+		
+	}
+	PT(Geom) polymeshgeom;
+	polymeshgeom = new Geom(vdata);
+	polymeshgeom->add_primitive(prim);
+
+	node->add_geom(polymeshgeom);
+	std::cout << "Number of Polygons: " << m_pmesh->npolys << std::endl;
+
+	return node;
+}
+
+
+

panda/src/recastdetour/NavMeshSample.h

@@ -0,0 +1,105 @@
+#ifndef NAVMESHSAMPLE_H
+#define NAVMESHSAMPLE_H
+
+#ifndef CPPPARSER
+#include "Recast.h"
+#else
+class rcContext;
+struct rcHeightfield;
+struct rcCompactHeightfield;
+struct rcContourSet;
+struct rcPolyMesh;
+struct rcConfig;
+struct rcPolyMeshDetail;
+#endif
+
+#include "InputGeom.h"
+#include <string>
+#include "geom.h"
+#include "geomNode.h"
+#include "nodePath.h"
+
+class NavMeshSample
+{
+protected:
+	class InputGeom* m_geom;
+	class dtNavMesh* m_navMesh;
+	class dtNavMeshQuery* m_navQuery;
+	class dtCrowd* m_crowd;
+
+	float m_cellSize;
+	float m_cellHeight;
+	float m_agentHeight;
+	float m_agentRadius;
+	float m_agentMaxClimb;
+	float m_agentMaxSlope;
+	float m_regionMinSize;
+	float m_regionMergeSize;
+	float m_edgeMaxLen;
+	float m_edgeMaxError;
+	float m_vertsPerPoly;
+	float m_detailSampleDist;
+	float m_detailSampleMaxError;
+	int m_partitionType;
+
+	bool m_filterLowHangingObstacles;
+	bool m_filterLedgeSpans;
+	bool m_filterWalkableLowHeightSpans;
+
+
+	rcContext* m_ctx;
+
+	dtNavMesh* loadAll(const std::string& path);
+	void saveAll(const std::string& path, const dtNavMesh* mesh);
+
+	unsigned char* m_triareas;
+	rcHeightfield* m_solid;
+	rcCompactHeightfield* m_chf;
+	rcContourSet* m_cset;
+	rcPolyMesh* m_pmesh;
+	rcConfig m_cfg;
+	rcPolyMeshDetail* m_dmesh;
+
+	void cleanup();
+
+public:
+
+	NavMeshSample();
+	~NavMeshSample();
+	void setContext(rcContext* ctx) { m_ctx = ctx; }
+	
+	virtual void collectSettings(struct BuildSettings& settings);
+
+	void resetCommonSettings();
+	void handleCommonSettings();
+
+	bool handleBuild();
+	void LoadGeom(const std::string& filepath)
+	{ 
+		m_geom = new InputGeom;
+		m_geom->load(m_ctx, filepath);
+	}
+	void LoadGeom(NodePath node)
+	{
+		m_geom = new InputGeom;
+		m_geom->load(m_ctx, node);
+	}
+
+	PT(GeomNode) getPolyMeshGeom();
+
+
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	NavMeshSample(const NavMeshSample&);
+	NavMeshSample& operator=(const NavMeshSample&);
+};
+
+enum SamplePartitionType
+{
+	SAMPLE_PARTITION_WATERSHED,
+	SAMPLE_PARTITION_MONOTONE,
+	SAMPLE_PARTITION_LAYERS,
+};
+
+#endif // NAVMESHSAMPLE_H

panda/src/recastdetour/Recast.cpp

@@ -0,0 +1,575 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+namespace
+{
+/// Allocates and constructs an object of the given type, returning a pointer.
+/// TODO: Support constructor args.
+/// @param[in]		hint	Hint to the allocator.
+template <typename T>
+T* rcNew(rcAllocHint hint) {
+	T* ptr = (T*)rcAlloc(sizeof(T), hint);
+	::new(rcNewTag(), (void*)ptr) T();
+	return ptr;
+}
+
+/// Destroys and frees an object allocated with rcNew.
+/// @param[in]     ptr    The object pointer to delete.
+template <typename T>
+void rcDelete(T* ptr) {
+	if (ptr) {
+		ptr->~T();
+		rcFree((void*)ptr);
+	}
+}
+}  // namespace
+
+
+float rcSqrt(float x)
+{
+	return sqrtf(x);
+}
+
+/// @class rcContext
+/// @par
+///
+/// This class does not provide logging or timer functionality on its 
+/// own.  Both must be provided by a concrete implementation 
+/// by overriding the protected member functions.  Also, this class does not 
+/// provide an interface for extracting log messages. (Only adding them.) 
+/// So concrete implementations must provide one.
+///
+/// If no logging or timers are required, just pass an instance of this 
+/// class through the Recast build process.
+///
+
+/// @par
+///
+/// Example:
+/// @code
+/// // Where ctx is an instance of rcContext and filepath is a char array.
+/// ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath);
+/// @endcode
+void rcContext::log(const rcLogCategory category, const char* format, ...)
+{
+	if (!m_logEnabled)
+		return;
+	static const int MSG_SIZE = 512;
+	char msg[MSG_SIZE];
+	va_list ap;
+	va_start(ap, format);
+	int len = vsnprintf(msg, MSG_SIZE, format, ap);
+	if (len >= MSG_SIZE)
+	{
+		len = MSG_SIZE-1;
+		msg[MSG_SIZE-1] = '\0';
+	}
+	va_end(ap);
+	doLog(category, msg, len);
+}
+
+rcHeightfield* rcAllocHeightfield()
+{
+	return rcNew<rcHeightfield>(RC_ALLOC_PERM);
+}
+rcHeightfield::rcHeightfield()
+	: width()
+	, height()
+	, bmin()
+	, bmax()
+	, cs()
+	, ch()
+	, spans()
+	, pools()
+	, freelist()
+{
+}
+
+rcHeightfield::~rcHeightfield()
+{
+	// Delete span array.
+	rcFree(spans);
+	// Delete span pools.
+	while (pools)
+	{
+		rcSpanPool* next = pools->next;
+		rcFree(pools);
+		pools = next;
+	}
+}
+
+void rcFreeHeightField(rcHeightfield* hf)
+{
+	rcDelete(hf);
+}
+
+rcCompactHeightfield* rcAllocCompactHeightfield()
+{
+	return rcNew<rcCompactHeightfield>(RC_ALLOC_PERM);
+}
+
+void rcFreeCompactHeightfield(rcCompactHeightfield* chf)
+{
+	rcDelete(chf);
+}
+
+rcCompactHeightfield::rcCompactHeightfield()
+	: width(),
+	height(),
+	spanCount(),
+	walkableHeight(),
+	walkableClimb(),
+	borderSize(),
+	maxDistance(),
+	maxRegions(),
+	bmin(),
+	bmax(),
+	cs(),
+	ch(),
+	cells(),
+	spans(),
+	dist(),
+	areas()
+{
+}
+rcCompactHeightfield::~rcCompactHeightfield()
+{
+	rcFree(cells);
+	rcFree(spans);
+	rcFree(dist);
+	rcFree(areas);
+}
+
+rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
+{
+	return rcNew<rcHeightfieldLayerSet>(RC_ALLOC_PERM);
+}
+void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset)
+{
+	rcDelete(lset);
+}
+
+rcHeightfieldLayerSet::rcHeightfieldLayerSet()
+	: layers(),	nlayers() {}
+rcHeightfieldLayerSet::~rcHeightfieldLayerSet()
+{
+	for (int i = 0; i < nlayers; ++i)
+	{
+		rcFree(layers[i].heights);
+		rcFree(layers[i].areas);
+		rcFree(layers[i].cons);
+	}
+	rcFree(layers);
+}
+
+
+rcContourSet* rcAllocContourSet()
+{
+	return rcNew<rcContourSet>(RC_ALLOC_PERM);
+}
+void rcFreeContourSet(rcContourSet* cset)
+{
+	rcDelete(cset);
+}
+
+rcContourSet::rcContourSet()
+	: conts(),
+	nconts(),
+	bmin(),
+	bmax(),
+	cs(),
+	ch(),
+	width(),
+	height(),
+	borderSize(),
+	maxError() {}
+rcContourSet::~rcContourSet()
+{
+	for (int i = 0; i < nconts; ++i)
+	{
+		rcFree(conts[i].verts);
+		rcFree(conts[i].rverts);
+	}
+	rcFree(conts);
+}
+
+
+rcPolyMesh* rcAllocPolyMesh()
+{
+	return rcNew<rcPolyMesh>(RC_ALLOC_PERM);
+}
+void rcFreePolyMesh(rcPolyMesh* pmesh)
+{
+	rcDelete(pmesh);
+}
+
+rcPolyMesh::rcPolyMesh()
+	: verts(),
+	polys(),
+	regs(),
+	flags(),
+	areas(),
+	nverts(),
+	npolys(),
+	maxpolys(),
+	nvp(),
+	bmin(),
+	bmax(),
+	cs(),
+	ch(),
+	borderSize(),
+	maxEdgeError() {}
+
+rcPolyMesh::~rcPolyMesh()
+{
+	rcFree(verts);
+	rcFree(polys);
+	rcFree(regs);
+	rcFree(flags);
+	rcFree(areas);
+}
+
+rcPolyMeshDetail* rcAllocPolyMeshDetail()
+{
+	rcPolyMeshDetail* dmesh = (rcPolyMeshDetail*)rcAlloc(sizeof(rcPolyMeshDetail), RC_ALLOC_PERM);
+	memset(dmesh, 0, sizeof(rcPolyMeshDetail));
+	return dmesh;
+}
+
+void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh)
+{
+	if (!dmesh) return;
+	rcFree(dmesh->meshes);
+	rcFree(dmesh->verts);
+	rcFree(dmesh->tris);
+	rcFree(dmesh);
+}
+
+void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
+{
+	// Calculate bounding box.
+	rcVcopy(bmin, verts);
+	rcVcopy(bmax, verts);
+	for (int i = 1; i < nv; ++i)
+	{
+		const float* v = &verts[i*3];
+		rcVmin(bmin, v);
+		rcVmax(bmax, v);
+	}
+}
+
+void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h)
+{
+	*w = (int)((bmax[0] - bmin[0])/cs+0.5f);
+	*h = (int)((bmax[2] - bmin[2])/cs+0.5f);
+}
+
+/// @par
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// @see rcAllocHeightfield, rcHeightfield 
+bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height,
+						 const float* bmin, const float* bmax,
+						 float cs, float ch)
+{
+	rcIgnoreUnused(ctx);
+	
+	hf.width = width;
+	hf.height = height;
+	rcVcopy(hf.bmin, bmin);
+	rcVcopy(hf.bmax, bmax);
+	hf.cs = cs;
+	hf.ch = ch;
+	hf.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*)*hf.width*hf.height, RC_ALLOC_PERM);
+	if (!hf.spans)
+		return false;
+	memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height);
+	return true;
+}
+
+static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm)
+{
+	float e0[3], e1[3];
+	rcVsub(e0, v1, v0);
+	rcVsub(e1, v2, v0);
+	rcVcross(norm, e0, e1);
+	rcVnormalize(norm);
+}
+
+/// @par
+///
+/// Only sets the area id's for the walkable triangles.  Does not alter the
+/// area id's for unwalkable triangles.
+/// 
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
+void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
+							 const float* verts, int nv,
+							 const int* tris, int nt,
+							 unsigned char* areas)
+{
+	rcIgnoreUnused(ctx);
+	rcIgnoreUnused(nv);
+	
+	const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
+
+	float norm[3];
+	
+	for (int i = 0; i < nt; ++i)
+	{
+		const int* tri = &tris[i*3];
+		calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
+		// Check if the face is walkable.
+		if (norm[1] > walkableThr)
+			areas[i] = RC_WALKABLE_AREA;
+	}
+}
+
+/// @par
+///
+/// Only sets the area id's for the unwalkable triangles.  Does not alter the
+/// area id's for walkable triangles.
+/// 
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
+void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
+								const float* verts, int /*nv*/,
+								const int* tris, int nt,
+								unsigned char* areas)
+{
+	rcIgnoreUnused(ctx);
+	
+	const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
+	
+	float norm[3];
+	
+	for (int i = 0; i < nt; ++i)
+	{
+		const int* tri = &tris[i*3];
+		calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
+		// Check if the face is walkable.
+		if (norm[1] <= walkableThr)
+			areas[i] = RC_NULL_AREA;
+	}
+}
+
+int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf)
+{
+	rcIgnoreUnused(ctx);
+	
+	const int w = hf.width;
+	const int h = hf.height;
+	int spanCount = 0;
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next)
+			{
+				if (s->area != RC_NULL_AREA)
+					spanCount++;
+			}
+		}
+	}
+	return spanCount;
+}
+
+/// @par
+///
+/// This is just the beginning of the process of fully building a compact heightfield.
+/// Various filters may be applied, then the distance field and regions built.
+/// E.g: #rcBuildDistanceField and #rcBuildRegions
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcAllocCompactHeightfield, rcHeightfield, rcCompactHeightfield, rcConfig
+bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb,
+							   rcHeightfield& hf, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
+	
+	const int w = hf.width;
+	const int h = hf.height;
+	const int spanCount = rcGetHeightFieldSpanCount(ctx, hf);
+
+	// Fill in header.
+	chf.width = w;
+	chf.height = h;
+	chf.spanCount = spanCount;
+	chf.walkableHeight = walkableHeight;
+	chf.walkableClimb = walkableClimb;
+	chf.maxRegions = 0;
+	rcVcopy(chf.bmin, hf.bmin);
+	rcVcopy(chf.bmax, hf.bmax);
+	chf.bmax[1] += walkableHeight*hf.ch;
+	chf.cs = hf.cs;
+	chf.ch = hf.ch;
+	chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*w*h, RC_ALLOC_PERM);
+	if (!chf.cells)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h);
+		return false;
+	}
+	memset(chf.cells, 0, sizeof(rcCompactCell)*w*h);
+	chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*spanCount, RC_ALLOC_PERM);
+	if (!chf.spans)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
+		return false;
+	}
+	memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount);
+	chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*spanCount, RC_ALLOC_PERM);
+	if (!chf.areas)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount);
+		return false;
+	}
+	memset(chf.areas, RC_NULL_AREA, sizeof(unsigned char)*spanCount);
+	
+	const int MAX_HEIGHT = 0xffff;
+	
+	// Fill in cells and spans.
+	int idx = 0;
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcSpan* s = hf.spans[x + y*w];
+			// If there are no spans at this cell, just leave the data to index=0, count=0.
+			if (!s) continue;
+			rcCompactCell& c = chf.cells[x+y*w];
+			c.index = idx;
+			c.count = 0;
+			while (s)
+			{
+				if (s->area != RC_NULL_AREA)
+				{
+					const int bot = (int)s->smax;
+					const int top = s->next ? (int)s->next->smin : MAX_HEIGHT;
+					chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff);
+					chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
+					chf.areas[idx] = s->area;
+					idx++;
+					c.count++;
+				}
+				s = s->next;
+			}
+		}
+	}
+
+	// Find neighbour connections.
+	const int MAX_LAYERS = RC_NOT_CONNECTED-1;
+	int tooHighNeighbour = 0;
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					rcSetCon(s, dir, RC_NOT_CONNECTED);
+					const int nx = x + rcGetDirOffsetX(dir);
+					const int ny = y + rcGetDirOffsetY(dir);
+					// First check that the neighbour cell is in bounds.
+					if (nx < 0 || ny < 0 || nx >= w || ny >= h)
+						continue;
+						
+					// Iterate over all neighbour spans and check if any of the is
+					// accessible from current cell.
+					const rcCompactCell& nc = chf.cells[nx+ny*w];
+					for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k)
+					{
+						const rcCompactSpan& ns = chf.spans[k];
+						const int bot = rcMax(s.y, ns.y);
+						const int top = rcMin(s.y+s.h, ns.y+ns.h);
+
+						// Check that the gap between the spans is walkable,
+						// and that the climb height between the gaps is not too high.
+						if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
+						{
+							// Mark direction as walkable.
+							const int lidx = k - (int)nc.index;
+							if (lidx < 0 || lidx > MAX_LAYERS)
+							{
+								tooHighNeighbour = rcMax(tooHighNeighbour, lidx);
+								continue;
+							}
+							rcSetCon(s, dir, lidx);
+							break;
+						}
+					}
+					
+				}
+			}
+		}
+	}
+	
+	if (tooHighNeighbour > MAX_LAYERS)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)",
+				 tooHighNeighbour, MAX_LAYERS);
+	}
+	
+	return true;
+}
+
+/*
+static int getHeightfieldMemoryUsage(const rcHeightfield& hf)
+{
+	int size = 0;
+	size += sizeof(hf);
+	size += hf.width * hf.height * sizeof(rcSpan*);
+	
+	rcSpanPool* pool = hf.pools;
+	while (pool)
+	{
+		size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL;
+		pool = pool->next;
+	}
+	return size;
+}
+
+static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf)
+{
+	int size = 0;
+	size += sizeof(rcCompactHeightfield);
+	size += sizeof(rcCompactSpan) * chf.spanCount;
+	size += sizeof(rcCompactCell) * chf.width * chf.height;
+	return size;
+}
+*/

panda/src/recastdetour/Recast.h

@@ -0,0 +1,1208 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+ 
+#ifndef RECAST_H
+#define RECAST_H
+
+/// The value of PI used by Recast.
+static const float RC_PI = 3.14159265f;
+
+/// Recast log categories.
+/// @see rcContext
+enum rcLogCategory
+{
+	RC_LOG_PROGRESS = 1,	///< A progress log entry.
+	RC_LOG_WARNING,			///< A warning log entry.
+	RC_LOG_ERROR,			///< An error log entry.
+};
+
+/// Recast performance timer categories.
+/// @see rcContext
+enum rcTimerLabel
+{
+	/// The user defined total time of the build.
+	RC_TIMER_TOTAL,
+	/// A user defined build time.
+	RC_TIMER_TEMP,
+	/// The time to rasterize the triangles. (See: #rcRasterizeTriangle)
+	RC_TIMER_RASTERIZE_TRIANGLES,
+	/// The time to build the compact heightfield. (See: #rcBuildCompactHeightfield)
+	RC_TIMER_BUILD_COMPACTHEIGHTFIELD,
+	/// The total time to build the contours. (See: #rcBuildContours)
+	RC_TIMER_BUILD_CONTOURS,
+	/// The time to trace the boundaries of the contours. (See: #rcBuildContours)
+	RC_TIMER_BUILD_CONTOURS_TRACE,
+	/// The time to simplify the contours. (See: #rcBuildContours)
+	RC_TIMER_BUILD_CONTOURS_SIMPLIFY,
+	/// The time to filter ledge spans. (See: #rcFilterLedgeSpans)
+	RC_TIMER_FILTER_BORDER,
+	/// The time to filter low height spans. (See: #rcFilterWalkableLowHeightSpans)
+	RC_TIMER_FILTER_WALKABLE,
+	/// The time to apply the median filter. (See: #rcMedianFilterWalkableArea)
+	RC_TIMER_MEDIAN_AREA,
+	/// The time to filter low obstacles. (See: #rcFilterLowHangingWalkableObstacles)
+	RC_TIMER_FILTER_LOW_OBSTACLES,
+	/// The time to build the polygon mesh. (See: #rcBuildPolyMesh)
+	RC_TIMER_BUILD_POLYMESH,
+	/// The time to merge polygon meshes. (See: #rcMergePolyMeshes)
+	RC_TIMER_MERGE_POLYMESH,
+	/// The time to erode the walkable area. (See: #rcErodeWalkableArea)
+	RC_TIMER_ERODE_AREA,
+	/// The time to mark a box area. (See: #rcMarkBoxArea)
+	RC_TIMER_MARK_BOX_AREA,
+	/// The time to mark a cylinder area. (See: #rcMarkCylinderArea)
+	RC_TIMER_MARK_CYLINDER_AREA,
+	/// The time to mark a convex polygon area. (See: #rcMarkConvexPolyArea)
+	RC_TIMER_MARK_CONVEXPOLY_AREA,
+	/// The total time to build the distance field. (See: #rcBuildDistanceField)
+	RC_TIMER_BUILD_DISTANCEFIELD,
+	/// The time to build the distances of the distance field. (See: #rcBuildDistanceField)
+	RC_TIMER_BUILD_DISTANCEFIELD_DIST,
+	/// The time to blur the distance field. (See: #rcBuildDistanceField)
+	RC_TIMER_BUILD_DISTANCEFIELD_BLUR,
+	/// The total time to build the regions. (See: #rcBuildRegions, #rcBuildRegionsMonotone)
+	RC_TIMER_BUILD_REGIONS,
+	/// The total time to apply the watershed algorithm. (See: #rcBuildRegions)
+	RC_TIMER_BUILD_REGIONS_WATERSHED,
+	/// The time to expand regions while applying the watershed algorithm. (See: #rcBuildRegions)
+	RC_TIMER_BUILD_REGIONS_EXPAND,
+	/// The time to flood regions while applying the watershed algorithm. (See: #rcBuildRegions)
+	RC_TIMER_BUILD_REGIONS_FLOOD,
+	/// The time to filter out small regions. (See: #rcBuildRegions, #rcBuildRegionsMonotone)
+	RC_TIMER_BUILD_REGIONS_FILTER,
+	/// The time to build heightfield layers. (See: #rcBuildHeightfieldLayers)
+	RC_TIMER_BUILD_LAYERS, 
+	/// The time to build the polygon mesh detail. (See: #rcBuildPolyMeshDetail)
+	RC_TIMER_BUILD_POLYMESHDETAIL,
+	/// The time to merge polygon mesh details. (See: #rcMergePolyMeshDetails)
+	RC_TIMER_MERGE_POLYMESHDETAIL,
+	/// The maximum number of timers.  (Used for iterating timers.)
+	RC_MAX_TIMERS
+};
+
+/// Provides an interface for optional logging and performance tracking of the Recast 
+/// build process.
+/// @ingroup recast
+class rcContext
+{
+public:
+
+	/// Contructor.
+	///  @param[in]		state	TRUE if the logging and performance timers should be enabled.  [Default: true]
+	inline rcContext(bool state = true) : m_logEnabled(state), m_timerEnabled(state) {}
+	virtual ~rcContext() {}
+
+	/// Enables or disables logging.
+	///  @param[in]		state	TRUE if logging should be enabled.
+	inline void enableLog(bool state) { m_logEnabled = state; }
+
+	/// Clears all log entries.
+	inline void resetLog() { if (m_logEnabled) doResetLog(); }
+
+	/// Logs a message.
+	///  @param[in]		category	The category of the message.
+	///  @param[in]		format		The message.
+	void log(const rcLogCategory category, const char* format, ...);
+
+	/// Enables or disables the performance timers.
+	///  @param[in]		state	TRUE if timers should be enabled.
+	inline void enableTimer(bool state) { m_timerEnabled = state; }
+
+	/// Clears all peformance timers. (Resets all to unused.)
+	inline void resetTimers() { if (m_timerEnabled) doResetTimers(); }
+
+	/// Starts the specified performance timer.
+	///  @param	label	The category of the timer.
+	inline void startTimer(const rcTimerLabel label) { if (m_timerEnabled) doStartTimer(label); }
+
+	/// Stops the specified performance timer.
+	///  @param	label	The category of the timer.
+	inline void stopTimer(const rcTimerLabel label) { if (m_timerEnabled) doStopTimer(label); }
+
+	/// Returns the total accumulated time of the specified performance timer.
+	///  @param	label	The category of the timer.
+	///  @return The accumulated time of the timer, or -1 if timers are disabled or the timer has never been started.
+	inline int getAccumulatedTime(const rcTimerLabel label) const { return m_timerEnabled ? doGetAccumulatedTime(label) : -1; }
+
+protected:
+
+	/// Clears all log entries.
+	virtual void doResetLog() {}
+
+	/// Logs a message.
+	///  @param[in]		category	The category of the message.
+	///  @param[in]		msg			The formatted message.
+	///  @param[in]		len			The length of the formatted message.
+	virtual void doLog(const rcLogCategory /*category*/, const char* /*msg*/, const int /*len*/) {}
+
+	/// Clears all timers. (Resets all to unused.)
+	virtual void doResetTimers() {}
+
+	/// Starts the specified performance timer.
+	///  @param[in]		label	The category of timer.
+	virtual void doStartTimer(const rcTimerLabel /*label*/) {}
+
+	/// Stops the specified performance timer.
+	///  @param[in]		label	The category of the timer.
+	virtual void doStopTimer(const rcTimerLabel /*label*/) {}
+
+	/// Returns the total accumulated time of the specified performance timer.
+	///  @param[in]		label	The category of the timer.
+	///  @return The accumulated time of the timer, or -1 if timers are disabled or the timer has never been started.
+	virtual int doGetAccumulatedTime(const rcTimerLabel /*label*/) const { return -1; }
+	
+	/// True if logging is enabled.
+	bool m_logEnabled;
+
+	/// True if the performance timers are enabled.
+	bool m_timerEnabled;
+};
+
+/// A helper to first start a timer and then stop it when this helper goes out of scope.
+/// @see rcContext
+class rcScopedTimer
+{
+public:
+	/// Constructs an instance and starts the timer.
+	///  @param[in]		ctx		The context to use.
+	///  @param[in]		label	The category of the timer.
+	inline rcScopedTimer(rcContext* ctx, const rcTimerLabel label) : m_ctx(ctx), m_label(label) { m_ctx->startTimer(m_label); }
+	inline ~rcScopedTimer() { m_ctx->stopTimer(m_label); }
+
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	rcScopedTimer(const rcScopedTimer&);
+	rcScopedTimer& operator=(const rcScopedTimer&);
+	
+	rcContext* const m_ctx;
+	const rcTimerLabel m_label;
+};
+
+/// Specifies a configuration to use when performing Recast builds.
+/// @ingroup recast
+struct rcConfig
+{
+	/// The width of the field along the x-axis. [Limit: >= 0] [Units: vx]
+	int width;
+
+	/// The height of the field along the z-axis. [Limit: >= 0] [Units: vx]
+	int height;
+	
+	/// The width/height size of tile's on the xz-plane. [Limit: >= 0] [Units: vx]
+	int tileSize;
+	
+	/// The size of the non-navigable border around the heightfield. [Limit: >=0] [Units: vx]
+	int borderSize;
+
+	/// The xz-plane cell size to use for fields. [Limit: > 0] [Units: wu] 
+	float cs;
+
+	/// The y-axis cell size to use for fields. [Limit: > 0] [Units: wu]
+	float ch;
+
+	/// The minimum bounds of the field's AABB. [(x, y, z)] [Units: wu]
+	float bmin[3]; 
+
+	/// The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu]
+	float bmax[3];
+
+	/// The maximum slope that is considered walkable. [Limits: 0 <= value < 90] [Units: Degrees] 
+	float walkableSlopeAngle;
+
+	/// Minimum floor to 'ceiling' height that will still allow the floor area to 
+	/// be considered walkable. [Limit: >= 3] [Units: vx] 
+	int walkableHeight;
+	
+	/// Maximum ledge height that is considered to still be traversable. [Limit: >=0] [Units: vx] 
+	int walkableClimb;
+	
+	/// The distance to erode/shrink the walkable area of the heightfield away from 
+	/// obstructions.  [Limit: >=0] [Units: vx] 
+	int walkableRadius;
+	
+	/// The maximum allowed length for contour edges along the border of the mesh. [Limit: >=0] [Units: vx] 
+	int maxEdgeLen;
+	
+	/// The maximum distance a simplfied contour's border edges should deviate 
+	/// the original raw contour. [Limit: >=0] [Units: vx]
+	float maxSimplificationError;
+	
+	/// The minimum number of cells allowed to form isolated island areas. [Limit: >=0] [Units: vx] 
+	int minRegionArea;
+	
+	/// Any regions with a span count smaller than this value will, if possible, 
+	/// be merged with larger regions. [Limit: >=0] [Units: vx] 
+	int mergeRegionArea;
+	
+	/// The maximum number of vertices allowed for polygons generated during the 
+	/// contour to polygon conversion process. [Limit: >= 3] 
+	int maxVertsPerPoly;
+	
+	/// Sets the sampling distance to use when generating the detail mesh.
+	/// (For height detail only.) [Limits: 0 or >= 0.9] [Units: wu] 
+	float detailSampleDist;
+	
+	/// The maximum distance the detail mesh surface should deviate from heightfield
+	/// data. (For height detail only.) [Limit: >=0] [Units: wu] 
+	float detailSampleMaxError;
+};
+
+/// Defines the number of bits allocated to rcSpan::smin and rcSpan::smax.
+static const int RC_SPAN_HEIGHT_BITS = 13;
+/// Defines the maximum value for rcSpan::smin and rcSpan::smax.
+static const int RC_SPAN_MAX_HEIGHT = (1 << RC_SPAN_HEIGHT_BITS) - 1;
+
+/// The number of spans allocated per span spool.
+/// @see rcSpanPool
+static const int RC_SPANS_PER_POOL = 2048;
+
+/// Represents a span in a heightfield.
+/// @see rcHeightfield
+struct rcSpan
+{
+	unsigned int smin : RC_SPAN_HEIGHT_BITS; ///< The lower limit of the span. [Limit: < #smax]
+	unsigned int smax : RC_SPAN_HEIGHT_BITS; ///< The upper limit of the span. [Limit: <= #RC_SPAN_MAX_HEIGHT]
+	unsigned int area : 6;                   ///< The area id assigned to the span.
+	rcSpan* next;                            ///< The next span higher up in column.
+};
+
+/// A memory pool used for quick allocation of spans within a heightfield.
+/// @see rcHeightfield
+struct rcSpanPool
+{
+	rcSpanPool* next;					///< The next span pool.
+	rcSpan items[RC_SPANS_PER_POOL];	///< Array of spans in the pool.
+};
+
+/// A dynamic heightfield representing obstructed space.
+/// @ingroup recast
+struct rcHeightfield
+{
+	rcHeightfield();
+	~rcHeightfield();
+
+	int width;			///< The width of the heightfield. (Along the x-axis in cell units.)
+	int height;			///< The height of the heightfield. (Along the z-axis in cell units.)
+	float bmin[3];  	///< The minimum bounds in world space. [(x, y, z)]
+	float bmax[3];		///< The maximum bounds in world space. [(x, y, z)]
+	float cs;			///< The size of each cell. (On the xz-plane.)
+	float ch;			///< The height of each cell. (The minimum increment along the y-axis.)
+	rcSpan** spans;		///< Heightfield of spans (width*height).
+	rcSpanPool* pools;	///< Linked list of span pools.
+	rcSpan* freelist;	///< The next free span.
+
+private:
+	// Explicitly-disabled copy constructor and copy assignment operator.
+	rcHeightfield(const rcHeightfield&);
+	rcHeightfield& operator=(const rcHeightfield&);
+};
+
+/// Provides information on the content of a cell column in a compact heightfield. 
+struct rcCompactCell
+{
+	unsigned int index : 24;	///< Index to the first span in the column.
+	unsigned int count : 8;		///< Number of spans in the column.
+};
+
+/// Represents a span of unobstructed space within a compact heightfield.
+struct rcCompactSpan
+{
+	unsigned short y;			///< The lower extent of the span. (Measured from the heightfield's base.)
+	unsigned short reg;			///< The id of the region the span belongs to. (Or zero if not in a region.)
+	unsigned int con : 24;		///< Packed neighbor connection data.
+	unsigned int h : 8;			///< The height of the span.  (Measured from #y.)
+};
+
+/// A compact, static heightfield representing unobstructed space.
+/// @ingroup recast
+struct rcCompactHeightfield
+{
+	rcCompactHeightfield();
+	~rcCompactHeightfield();
+	int width;					///< The width of the heightfield. (Along the x-axis in cell units.)
+	int height;					///< The height of the heightfield. (Along the z-axis in cell units.)
+	int spanCount;				///< The number of spans in the heightfield.
+	int walkableHeight;			///< The walkable height used during the build of the field.  (See: rcConfig::walkableHeight)
+	int walkableClimb;			///< The walkable climb used during the build of the field. (See: rcConfig::walkableClimb)
+	int borderSize;				///< The AABB border size used during the build of the field. (See: rcConfig::borderSize)
+	unsigned short maxDistance;	///< The maximum distance value of any span within the field. 
+	unsigned short maxRegions;	///< The maximum region id of any span within the field. 
+	float bmin[3];				///< The minimum bounds in world space. [(x, y, z)]
+	float bmax[3];				///< The maximum bounds in world space. [(x, y, z)]
+	float cs;					///< The size of each cell. (On the xz-plane.)
+	float ch;					///< The height of each cell. (The minimum increment along the y-axis.)
+	rcCompactCell* cells;		///< Array of cells. [Size: #width*#height]
+	rcCompactSpan* spans;		///< Array of spans. [Size: #spanCount]
+	unsigned short* dist;		///< Array containing border distance data. [Size: #spanCount]
+	unsigned char* areas;		///< Array containing area id data. [Size: #spanCount]
+};
+
+/// Represents a heightfield layer within a layer set.
+/// @see rcHeightfieldLayerSet
+struct rcHeightfieldLayer
+{
+	float bmin[3];				///< The minimum bounds in world space. [(x, y, z)]
+	float bmax[3];				///< The maximum bounds in world space. [(x, y, z)]
+	float cs;					///< The size of each cell. (On the xz-plane.)
+	float ch;					///< The height of each cell. (The minimum increment along the y-axis.)
+	int width;					///< The width of the heightfield. (Along the x-axis in cell units.)
+	int height;					///< The height of the heightfield. (Along the z-axis in cell units.)
+	int minx;					///< The minimum x-bounds of usable data.
+	int maxx;					///< The maximum x-bounds of usable data.
+	int miny;					///< The minimum y-bounds of usable data. (Along the z-axis.)
+	int maxy;					///< The maximum y-bounds of usable data. (Along the z-axis.)
+	int hmin;					///< The minimum height bounds of usable data. (Along the y-axis.)
+	int hmax;					///< The maximum height bounds of usable data. (Along the y-axis.)
+	unsigned char* heights;		///< The heightfield. [Size: width * height]
+	unsigned char* areas;		///< Area ids. [Size: Same as #heights]
+	unsigned char* cons;		///< Packed neighbor connection information. [Size: Same as #heights]
+};
+
+/// Represents a set of heightfield layers.
+/// @ingroup recast
+/// @see rcAllocHeightfieldLayerSet, rcFreeHeightfieldLayerSet 
+struct rcHeightfieldLayerSet
+{
+	rcHeightfieldLayerSet();
+	~rcHeightfieldLayerSet();
+	rcHeightfieldLayer* layers;			///< The layers in the set. [Size: #nlayers]
+	int nlayers;						///< The number of layers in the set.
+};
+
+/// Represents a simple, non-overlapping contour in field space.
+struct rcContour
+{
+	int* verts;			///< Simplified contour vertex and connection data. [Size: 4 * #nverts]
+	int nverts;			///< The number of vertices in the simplified contour. 
+	int* rverts;		///< Raw contour vertex and connection data. [Size: 4 * #nrverts]
+	int nrverts;		///< The number of vertices in the raw contour. 
+	unsigned short reg;	///< The region id of the contour.
+	unsigned char area;	///< The area id of the contour.
+};
+
+/// Represents a group of related contours.
+/// @ingroup recast
+struct rcContourSet
+{
+	rcContourSet();
+	~rcContourSet();
+	rcContour* conts;	///< An array of the contours in the set. [Size: #nconts]
+	int nconts;			///< The number of contours in the set.
+	float bmin[3];  	///< The minimum bounds in world space. [(x, y, z)]
+	float bmax[3];		///< The maximum bounds in world space. [(x, y, z)]
+	float cs;			///< The size of each cell. (On the xz-plane.)
+	float ch;			///< The height of each cell. (The minimum increment along the y-axis.)
+	int width;			///< The width of the set. (Along the x-axis in cell units.) 
+	int height;			///< The height of the set. (Along the z-axis in cell units.) 
+	int borderSize;		///< The AABB border size used to generate the source data from which the contours were derived.
+	float maxError;		///< The max edge error that this contour set was simplified with.
+};
+
+/// Represents a polygon mesh suitable for use in building a navigation mesh. 
+/// @ingroup recast
+struct rcPolyMesh
+{
+	rcPolyMesh();
+	~rcPolyMesh();
+	unsigned short* verts;	///< The mesh vertices. [Form: (x, y, z) * #nverts]
+	unsigned short* polys;	///< Polygon and neighbor data. [Length: #maxpolys * 2 * #nvp]
+	unsigned short* regs;	///< The region id assigned to each polygon. [Length: #maxpolys]
+	unsigned short* flags;	///< The user defined flags for each polygon. [Length: #maxpolys]
+	unsigned char* areas;	///< The area id assigned to each polygon. [Length: #maxpolys]
+	int nverts;				///< The number of vertices.
+	int npolys;				///< The number of polygons.
+	int maxpolys;			///< The number of allocated polygons.
+	int nvp;				///< The maximum number of vertices per polygon.
+	float bmin[3];			///< The minimum bounds in world space. [(x, y, z)]
+	float bmax[3];			///< The maximum bounds in world space. [(x, y, z)]
+	float cs;				///< The size of each cell. (On the xz-plane.)
+	float ch;				///< The height of each cell. (The minimum increment along the y-axis.)
+	int borderSize;			///< The AABB border size used to generate the source data from which the mesh was derived.
+	float maxEdgeError;		///< The max error of the polygon edges in the mesh.
+};
+
+/// Contains triangle meshes that represent detailed height data associated 
+/// with the polygons in its associated polygon mesh object.
+/// @ingroup recast
+struct rcPolyMeshDetail
+{
+	unsigned int* meshes;	///< The sub-mesh data. [Size: 4*#nmeshes] 
+	float* verts;			///< The mesh vertices. [Size: 3*#nverts] 
+	unsigned char* tris;	///< The mesh triangles. [Size: 4*#ntris] 
+	int nmeshes;			///< The number of sub-meshes defined by #meshes.
+	int nverts;				///< The number of vertices in #verts.
+	int ntris;				///< The number of triangles in #tris.
+};
+
+/// @name Allocation Functions
+/// Functions used to allocate and de-allocate Recast objects.
+/// @see rcAllocSetCustom
+/// @{
+
+/// Allocates a heightfield object using the Recast allocator.
+///  @return A heightfield that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcCreateHeightfield, rcFreeHeightField
+rcHeightfield* rcAllocHeightfield();
+
+/// Frees the specified heightfield object using the Recast allocator.
+///  @param[in]		hf	A heightfield allocated using #rcAllocHeightfield
+///  @ingroup recast
+///  @see rcAllocHeightfield
+void rcFreeHeightField(rcHeightfield* hf);
+
+/// Allocates a compact heightfield object using the Recast allocator.
+///  @return A compact heightfield that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildCompactHeightfield, rcFreeCompactHeightfield
+rcCompactHeightfield* rcAllocCompactHeightfield();
+
+/// Frees the specified compact heightfield object using the Recast allocator.
+///  @param[in]		chf		A compact heightfield allocated using #rcAllocCompactHeightfield
+///  @ingroup recast
+///  @see rcAllocCompactHeightfield
+void rcFreeCompactHeightfield(rcCompactHeightfield* chf);
+
+/// Allocates a heightfield layer set using the Recast allocator.
+///  @return A heightfield layer set that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildHeightfieldLayers, rcFreeHeightfieldLayerSet
+rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet();
+
+/// Frees the specified heightfield layer set using the Recast allocator.
+///  @param[in]		lset	A heightfield layer set allocated using #rcAllocHeightfieldLayerSet
+///  @ingroup recast
+///  @see rcAllocHeightfieldLayerSet
+void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset);
+
+/// Allocates a contour set object using the Recast allocator.
+///  @return A contour set that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildContours, rcFreeContourSet
+rcContourSet* rcAllocContourSet();
+
+/// Frees the specified contour set using the Recast allocator.
+///  @param[in]		cset	A contour set allocated using #rcAllocContourSet
+///  @ingroup recast
+///  @see rcAllocContourSet
+void rcFreeContourSet(rcContourSet* cset);
+
+/// Allocates a polygon mesh object using the Recast allocator.
+///  @return A polygon mesh that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildPolyMesh, rcFreePolyMesh
+rcPolyMesh* rcAllocPolyMesh();
+
+/// Frees the specified polygon mesh using the Recast allocator.
+///  @param[in]		pmesh	A polygon mesh allocated using #rcAllocPolyMesh
+///  @ingroup recast
+///  @see rcAllocPolyMesh
+void rcFreePolyMesh(rcPolyMesh* pmesh);
+
+/// Allocates a detail mesh object using the Recast allocator.
+///  @return A detail mesh that is ready for initialization, or null on failure.
+///  @ingroup recast
+///  @see rcBuildPolyMeshDetail, rcFreePolyMeshDetail
+rcPolyMeshDetail* rcAllocPolyMeshDetail();
+
+/// Frees the specified detail mesh using the Recast allocator.
+///  @param[in]		dmesh	A detail mesh allocated using #rcAllocPolyMeshDetail
+///  @ingroup recast
+///  @see rcAllocPolyMeshDetail
+void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh);
+
+/// @}
+
+/// Heighfield border flag.
+/// If a heightfield region ID has this bit set, then the region is a border 
+/// region and its spans are considered unwalkable.
+/// (Used during the region and contour build process.)
+/// @see rcCompactSpan::reg
+static const unsigned short RC_BORDER_REG = 0x8000;
+
+/// Polygon touches multiple regions.
+/// If a polygon has this region ID it was merged with or created
+/// from polygons of different regions during the polymesh
+/// build step that removes redundant border vertices. 
+/// (Used during the polymesh and detail polymesh build processes)
+/// @see rcPolyMesh::regs
+static const unsigned short RC_MULTIPLE_REGS = 0;
+
+/// Border vertex flag.
+/// If a region ID has this bit set, then the associated element lies on
+/// a tile border. If a contour vertex's region ID has this bit set, the 
+/// vertex will later be removed in order to match the segments and vertices 
+/// at tile boundaries.
+/// (Used during the build process.)
+/// @see rcCompactSpan::reg, #rcContour::verts, #rcContour::rverts
+static const int RC_BORDER_VERTEX = 0x10000;
+
+/// Area border flag.
+/// If a region ID has this bit set, then the associated element lies on
+/// the border of an area.
+/// (Used during the region and contour build process.)
+/// @see rcCompactSpan::reg, #rcContour::verts, #rcContour::rverts
+static const int RC_AREA_BORDER = 0x20000;
+
+/// Contour build flags.
+/// @see rcBuildContours
+enum rcBuildContoursFlags
+{
+	RC_CONTOUR_TESS_WALL_EDGES = 0x01,	///< Tessellate solid (impassable) edges during contour simplification.
+	RC_CONTOUR_TESS_AREA_EDGES = 0x02,	///< Tessellate edges between areas during contour simplification.
+};
+
+/// Applied to the region id field of contour vertices in order to extract the region id.
+/// The region id field of a vertex may have several flags applied to it.  So the
+/// fields value can't be used directly.
+/// @see rcContour::verts, rcContour::rverts
+static const int RC_CONTOUR_REG_MASK = 0xffff;
+
+/// An value which indicates an invalid index within a mesh.
+/// @note This does not necessarily indicate an error.
+/// @see rcPolyMesh::polys
+static const unsigned short RC_MESH_NULL_IDX = 0xffff;
+
+/// Represents the null area.
+/// When a data element is given this value it is considered to no longer be 
+/// assigned to a usable area.  (E.g. It is unwalkable.)
+static const unsigned char RC_NULL_AREA = 0;
+
+/// The default area id used to indicate a walkable polygon. 
+/// This is also the maximum allowed area id, and the only non-null area id 
+/// recognized by some steps in the build process. 
+static const unsigned char RC_WALKABLE_AREA = 63;
+
+/// The value returned by #rcGetCon if the specified direction is not connected
+/// to another span. (Has no neighbor.)
+static const int RC_NOT_CONNECTED = 0x3f;
+
+/// @name General helper functions
+/// @{
+
+/// Used to ignore a function parameter.  VS complains about unused parameters
+/// and this silences the warning.
+///  @param [in] _ Unused parameter
+template<class T> void rcIgnoreUnused(const T&) { }
+
+/// Swaps the values of the two parameters.
+///  @param[in,out]	a	Value A
+///  @param[in,out]	b	Value B
+template<class T> inline void rcSwap(T& a, T& b) { T t = a; a = b; b = t; }
+
+/// Returns the minimum of two values.
+///  @param[in]		a	Value A
+///  @param[in]		b	Value B
+///  @return The minimum of the two values.
+template<class T> inline T rcMin(T a, T b) { return a < b ? a : b; }
+
+/// Returns the maximum of two values.
+///  @param[in]		a	Value A
+///  @param[in]		b	Value B
+///  @return The maximum of the two values.
+template<class T> inline T rcMax(T a, T b) { return a > b ? a : b; }
+
+/// Returns the absolute value.
+///  @param[in]		a	The value.
+///  @return The absolute value of the specified value.
+template<class T> inline T rcAbs(T a) { return a < 0 ? -a : a; }
+
+/// Returns the square of the value.
+///  @param[in]		a	The value.
+///  @return The square of the value.
+template<class T> inline T rcSqr(T a) { return a*a; }
+
+/// Clamps the value to the specified range.
+///  @param[in]		v	The value to clamp.
+///  @param[in]		mn	The minimum permitted return value.
+///  @param[in]		mx	The maximum permitted return value.
+///  @return The value, clamped to the specified range.
+template<class T> inline T rcClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); }
+
+/// Returns the square root of the value.
+///  @param[in]		x	The value.
+///  @return The square root of the vlaue.
+float rcSqrt(float x);
+
+/// @}
+/// @name Vector helper functions.
+/// @{
+
+/// Derives the cross product of two vectors. (@p v1 x @p v2)
+///  @param[out]	dest	The cross product. [(x, y, z)]
+///  @param[in]		v1		A Vector [(x, y, z)]
+///  @param[in]		v2		A vector [(x, y, z)]
+inline void rcVcross(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[1]*v2[2] - v1[2]*v2[1];
+	dest[1] = v1[2]*v2[0] - v1[0]*v2[2];
+	dest[2] = v1[0]*v2[1] - v1[1]*v2[0];
+}
+
+/// Derives the dot product of two vectors. (@p v1 . @p v2)
+///  @param[in]		v1	A Vector [(x, y, z)]
+///  @param[in]		v2	A vector [(x, y, z)]
+/// @return The dot product.
+inline float rcVdot(const float* v1, const float* v2)
+{
+	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
+}
+
+/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s))
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v1		The base vector. [(x, y, z)]
+///  @param[in]		v2		The vector to scale and add to @p v1. [(x, y, z)]
+///  @param[in]		s		The amount to scale @p v2 by before adding to @p v1.
+inline void rcVmad(float* dest, const float* v1, const float* v2, const float s)
+{
+	dest[0] = v1[0]+v2[0]*s;
+	dest[1] = v1[1]+v2[1]*s;
+	dest[2] = v1[2]+v2[2]*s;
+}
+
+/// Performs a vector addition. (@p v1 + @p v2)
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v1		The base vector. [(x, y, z)]
+///  @param[in]		v2		The vector to add to @p v1. [(x, y, z)]
+inline void rcVadd(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[0]+v2[0];
+	dest[1] = v1[1]+v2[1];
+	dest[2] = v1[2]+v2[2];
+}
+
+/// Performs a vector subtraction. (@p v1 - @p v2)
+///  @param[out]	dest	The result vector. [(x, y, z)]
+///  @param[in]		v1		The base vector. [(x, y, z)]
+///  @param[in]		v2		The vector to subtract from @p v1. [(x, y, z)]
+inline void rcVsub(float* dest, const float* v1, const float* v2)
+{
+	dest[0] = v1[0]-v2[0];
+	dest[1] = v1[1]-v2[1];
+	dest[2] = v1[2]-v2[2];
+}
+
+/// Selects the minimum value of each element from the specified vectors.
+///  @param[in,out]	mn	A vector.  (Will be updated with the result.) [(x, y, z)]
+///  @param[in]		v	A vector. [(x, y, z)]
+inline void rcVmin(float* mn, const float* v)
+{
+	mn[0] = rcMin(mn[0], v[0]);
+	mn[1] = rcMin(mn[1], v[1]);
+	mn[2] = rcMin(mn[2], v[2]);
+}
+
+/// Selects the maximum value of each element from the specified vectors.
+///  @param[in,out]	mx	A vector.  (Will be updated with the result.) [(x, y, z)]
+///  @param[in]		v	A vector. [(x, y, z)]
+inline void rcVmax(float* mx, const float* v)
+{
+	mx[0] = rcMax(mx[0], v[0]);
+	mx[1] = rcMax(mx[1], v[1]);
+	mx[2] = rcMax(mx[2], v[2]);
+}
+
+/// Performs a vector copy.
+///  @param[out]	dest	The result. [(x, y, z)]
+///  @param[in]		v		The vector to copy. [(x, y, z)]
+inline void rcVcopy(float* dest, const float* v)
+{
+	dest[0] = v[0];
+	dest[1] = v[1];
+	dest[2] = v[2];
+}
+
+/// Returns the distance between two points.
+///  @param[in]		v1	A point. [(x, y, z)]
+///  @param[in]		v2	A point. [(x, y, z)]
+/// @return The distance between the two points.
+inline float rcVdist(const float* v1, const float* v2)
+{
+	float dx = v2[0] - v1[0];
+	float dy = v2[1] - v1[1];
+	float dz = v2[2] - v1[2];
+	return rcSqrt(dx*dx + dy*dy + dz*dz);
+}
+
+/// Returns the square of the distance between two points.
+///  @param[in]		v1	A point. [(x, y, z)]
+///  @param[in]		v2	A point. [(x, y, z)]
+/// @return The square of the distance between the two points.
+inline float rcVdistSqr(const float* v1, const float* v2)
+{
+	float dx = v2[0] - v1[0];
+	float dy = v2[1] - v1[1];
+	float dz = v2[2] - v1[2];
+	return dx*dx + dy*dy + dz*dz;
+}
+
+/// Normalizes the vector.
+///  @param[in,out]	v	The vector to normalize. [(x, y, z)]
+inline void rcVnormalize(float* v)
+{
+	float d = 1.0f / rcSqrt(rcSqr(v[0]) + rcSqr(v[1]) + rcSqr(v[2]));
+	v[0] *= d;
+	v[1] *= d;
+	v[2] *= d;
+}
+
+/// @}
+/// @name Heightfield Functions
+/// @see rcHeightfield
+/// @{
+
+/// Calculates the bounding box of an array of vertices.
+///  @ingroup recast
+///  @param[in]		verts	An array of vertices. [(x, y, z) * @p nv]
+///  @param[in]		nv		The number of vertices in the @p verts array.
+///  @param[out]	bmin	The minimum bounds of the AABB. [(x, y, z)] [Units: wu]
+///  @param[out]	bmax	The maximum bounds of the AABB. [(x, y, z)] [Units: wu]
+void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax);
+
+/// Calculates the grid size based on the bounding box and grid cell size.
+///  @ingroup recast
+///  @param[in]		bmin	The minimum bounds of the AABB. [(x, y, z)] [Units: wu]
+///  @param[in]		bmax	The maximum bounds of the AABB. [(x, y, z)] [Units: wu]
+///  @param[in]		cs		The xz-plane cell size. [Limit: > 0] [Units: wu]
+///  @param[out]	w		The width along the x-axis. [Limit: >= 0] [Units: vx]
+///  @param[out]	h		The height along the z-axis. [Limit: >= 0] [Units: vx]
+void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h);
+
+/// Initializes a new heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in,out]	hf		The allocated heightfield to initialize.
+///  @param[in]		width	The width of the field along the x-axis. [Limit: >= 0] [Units: vx]
+///  @param[in]		height	The height of the field along the z-axis. [Limit: >= 0] [Units: vx]
+///  @param[in]		bmin	The minimum bounds of the field's AABB. [(x, y, z)] [Units: wu]
+///  @param[in]		bmax	The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu]
+///  @param[in]		cs		The xz-plane cell size to use for the field. [Limit: > 0] [Units: wu]
+///  @param[in]		ch		The y-axis cell size to use for field. [Limit: > 0] [Units: wu]
+///  @returns True if the operation completed successfully.
+bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height,
+						 const float* bmin, const float* bmax,
+						 float cs, float ch);
+
+/// Sets the area id of all triangles with a slope below the specified value
+/// to #RC_WALKABLE_AREA.
+///  @ingroup recast
+///  @param[in,out]	ctx					The build context to use during the operation.
+///  @param[in]		walkableSlopeAngle	The maximum slope that is considered walkable.
+///  									[Limits: 0 <= value < 90] [Units: Degrees]
+///  @param[in]		verts				The vertices. [(x, y, z) * @p nv]
+///  @param[in]		nv					The number of vertices.
+///  @param[in]		tris				The triangle vertex indices. [(vertA, vertB, vertC) * @p nt]
+///  @param[in]		nt					The number of triangles.
+///  @param[out]	areas				The triangle area ids. [Length: >= @p nt]
+void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const float* verts, int nv,
+							 const int* tris, int nt, unsigned char* areas); 
+
+/// Sets the area id of all triangles with a slope greater than or equal to the specified value to #RC_NULL_AREA.
+///  @ingroup recast
+///  @param[in,out]	ctx					The build context to use during the operation.
+///  @param[in]		walkableSlopeAngle	The maximum slope that is considered walkable.
+///  									[Limits: 0 <= value < 90] [Units: Degrees]
+///  @param[in]		verts				The vertices. [(x, y, z) * @p nv]
+///  @param[in]		nv					The number of vertices.
+///  @param[in]		tris				The triangle vertex indices. [(vertA, vertB, vertC) * @p nt]
+///  @param[in]		nt					The number of triangles.
+///  @param[out]	areas				The triangle area ids. [Length: >= @p nt]
+void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const float* verts, int nv,
+								const int* tris, int nt, unsigned char* areas); 
+
+/// Adds a span to the specified heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in,out]	hf				An initialized heightfield.
+///  @param[in]		x				The width index where the span is to be added.
+///  								[Limits: 0 <= value < rcHeightfield::width]
+///  @param[in]		y				The height index where the span is to be added.
+///  								[Limits: 0 <= value < rcHeightfield::height]
+///  @param[in]		smin			The minimum height of the span. [Limit: < @p smax] [Units: vx]
+///  @param[in]		smax			The maximum height of the span. [Limit: <= #RC_SPAN_MAX_HEIGHT] [Units: vx]
+///  @param[in]		area			The area id of the span. [Limit: <= #RC_WALKABLE_AREA)
+///  @param[in]		flagMergeThr	The merge theshold. [Limit: >= 0] [Units: vx]
+///  @returns True if the operation completed successfully.
+bool rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y,
+			   const unsigned short smin, const unsigned short smax,
+			   const unsigned char area, const int flagMergeThr);
+
+/// Rasterizes a triangle into the specified heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		v0				Triangle vertex 0 [(x, y, z)]
+///  @param[in]		v1				Triangle vertex 1 [(x, y, z)]
+///  @param[in]		v2				Triangle vertex 2 [(x, y, z)]
+///  @param[in]		area			The area id of the triangle. [Limit: <= #RC_WALKABLE_AREA]
+///  @param[in,out]	solid			An initialized heightfield.
+///  @param[in]		flagMergeThr	The distance where the walkable flag is favored over the non-walkable flag.
+///  								[Limit: >= 0] [Units: vx]
+///  @returns True if the operation completed successfully.
+bool rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2,
+						 const unsigned char area, rcHeightfield& solid,
+						 const int flagMergeThr = 1);
+
+/// Rasterizes an indexed triangle mesh into the specified heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		verts			The vertices. [(x, y, z) * @p nv]
+///  @param[in]		nv				The number of vertices.
+///  @param[in]		tris			The triangle indices. [(vertA, vertB, vertC) * @p nt]
+///  @param[in]		areas			The area id's of the triangles. [Limit: <= #RC_WALKABLE_AREA] [Size: @p nt]
+///  @param[in]		nt				The number of triangles.
+///  @param[in,out]	solid			An initialized heightfield.
+///  @param[in]		flagMergeThr	The distance where the walkable flag is favored over the non-walkable flag. 
+///  								[Limit: >= 0] [Units: vx]
+///  @returns True if the operation completed successfully.
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int nv,
+						  const int* tris, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr = 1);
+
+/// Rasterizes an indexed triangle mesh into the specified heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx			The build context to use during the operation.
+///  @param[in]		verts		The vertices. [(x, y, z) * @p nv]
+///  @param[in]		nv			The number of vertices.
+///  @param[in]		tris		The triangle indices. [(vertA, vertB, vertC) * @p nt]
+///  @param[in]		areas		The area id's of the triangles. [Limit: <= #RC_WALKABLE_AREA] [Size: @p nt]
+///  @param[in]		nt			The number of triangles.
+///  @param[in,out]	solid		An initialized heightfield.
+///  @param[in]		flagMergeThr	The distance where the walkable flag is favored over the non-walkable flag. 
+///  							[Limit: >= 0] [Units: vx]
+///  @returns True if the operation completed successfully.
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int nv,
+						  const unsigned short* tris, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr = 1);
+
+/// Rasterizes triangles into the specified heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		verts			The triangle vertices. [(ax, ay, az, bx, by, bz, cx, by, cx) * @p nt]
+///  @param[in]		areas			The area id's of the triangles. [Limit: <= #RC_WALKABLE_AREA] [Size: @p nt]
+///  @param[in]		nt				The number of triangles.
+///  @param[in,out]	solid			An initialized heightfield.
+///  @param[in]		flagMergeThr	The distance where the walkable flag is favored over the non-walkable flag. 
+///  								[Limit: >= 0] [Units: vx]
+///  @returns True if the operation completed successfully.
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr = 1);
+
+/// Marks non-walkable spans as walkable if their maximum is within @p walkableClimp of a walkable neighbor. 
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		walkableClimb	Maximum ledge height that is considered to still be traversable. 
+///  								[Limit: >=0] [Units: vx]
+///  @param[in,out]	solid			A fully built heightfield.  (All spans have been added.)
+void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb, rcHeightfield& solid);
+
+/// Marks spans that are ledges as not-walkable. 
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		walkableHeight	Minimum floor to 'ceiling' height that will still allow the floor area to 
+///  								be considered walkable. [Limit: >= 3] [Units: vx]
+///  @param[in]		walkableClimb	Maximum ledge height that is considered to still be traversable. 
+///  								[Limit: >=0] [Units: vx]
+///  @param[in,out]	solid			A fully built heightfield.  (All spans have been added.)
+void rcFilterLedgeSpans(rcContext* ctx, const int walkableHeight,
+						const int walkableClimb, rcHeightfield& solid);
+
+/// Marks walkable spans as not walkable if the clearence above the span is less than the specified height. 
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		walkableHeight	Minimum floor to 'ceiling' height that will still allow the floor area to 
+///  								be considered walkable. [Limit: >= 3] [Units: vx]
+///  @param[in,out]	solid			A fully built heightfield.  (All spans have been added.)
+void rcFilterWalkableLowHeightSpans(rcContext* ctx, int walkableHeight, rcHeightfield& solid);
+
+/// Returns the number of spans contained in the specified heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		hf		An initialized heightfield.
+///  @returns The number of spans in the heightfield.
+int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf);
+
+/// @}
+/// @name Compact Heightfield Functions
+/// @see rcCompactHeightfield
+/// @{
+
+/// Builds a compact heightfield representing open space, from a heightfield representing solid space.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		walkableHeight	Minimum floor to 'ceiling' height that will still allow the floor area 
+///  								to be considered walkable. [Limit: >= 3] [Units: vx]
+///  @param[in]		walkableClimb	Maximum ledge height that is considered to still be traversable. 
+///  								[Limit: >=0] [Units: vx]
+///  @param[in]		hf				The heightfield to be compacted.
+///  @param[out]	chf				The resulting compact heightfield. (Must be pre-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb,
+							   rcHeightfield& hf, rcCompactHeightfield& chf);
+
+/// Erodes the walkable area within the heightfield by the specified radius. 
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		radius	The radius of erosion. [Limits: 0 < value < 255] [Units: vx]
+///  @param[in,out]	chf		The populated compact heightfield to erode.
+///  @returns True if the operation completed successfully.
+bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf);
+
+/// Applies a median filter to walkable area types (based on area id), removing noise.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in,out]	chf		A populated compact heightfield.
+///  @returns True if the operation completed successfully.
+bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf);
+
+/// Applies an area id to all spans within the specified bounding box. (AABB) 
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		bmin	The minimum of the bounding box. [(x, y, z)]
+///  @param[in]		bmax	The maximum of the bounding box. [(x, y, z)]
+///  @param[in]		areaId	The area id to apply. [Limit: <= #RC_WALKABLE_AREA]
+///  @param[in,out]	chf		A populated compact heightfield.
+void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId,
+				   rcCompactHeightfield& chf);
+
+/// Applies the area id to the all spans within the specified convex polygon. 
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		verts	The vertices of the polygon [Fomr: (x, y, z) * @p nverts]
+///  @param[in]		nverts	The number of vertices in the polygon.
+///  @param[in]		hmin	The height of the base of the polygon.
+///  @param[in]		hmax	The height of the top of the polygon.
+///  @param[in]		areaId	The area id to apply. [Limit: <= #RC_WALKABLE_AREA]
+///  @param[in,out]	chf		A populated compact heightfield.
+void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
+						  const float hmin, const float hmax, unsigned char areaId,
+						  rcCompactHeightfield& chf);
+
+/// Helper function to offset voncex polygons for rcMarkConvexPolyArea.
+///  @ingroup recast
+///  @param[in]		verts		The vertices of the polygon [Form: (x, y, z) * @p nverts]
+///  @param[in]		nverts		The number of vertices in the polygon.
+///  @param[out]	outVerts	The offset vertices (should hold up to 2 * @p nverts) [Form: (x, y, z) * return value]
+///  @param[in]		maxOutVerts	The max number of vertices that can be stored to @p outVerts.
+///  @returns Number of vertices in the offset polygon or 0 if too few vertices in @p outVerts.
+int rcOffsetPoly(const float* verts, const int nverts, const float offset,
+				 float* outVerts, const int maxOutVerts);
+
+/// Applies the area id to all spans within the specified cylinder.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		pos		The center of the base of the cylinder. [Form: (x, y, z)] 
+///  @param[in]		r		The radius of the cylinder.
+///  @param[in]		h		The height of the cylinder.
+///  @param[in]		areaId	The area id to apply. [Limit: <= #RC_WALKABLE_AREA]
+///  @param[in,out]	chf	A populated compact heightfield.
+void rcMarkCylinderArea(rcContext* ctx, const float* pos,
+						const float r, const float h, unsigned char areaId,
+						rcCompactHeightfield& chf);
+
+/// Builds the distance field for the specified compact heightfield. 
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in,out]	chf		A populated compact heightfield.
+///  @returns True if the operation completed successfully.
+bool rcBuildDistanceField(rcContext* ctx, rcCompactHeightfield& chf);
+
+/// Builds region data for the heightfield using watershed partitioning.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in,out]	chf				A populated compact heightfield.
+///  @param[in]		borderSize		The size of the non-navigable border around the heightfield.
+///  								[Limit: >=0] [Units: vx]
+///  @param[in]		minRegionArea	The minimum number of cells allowed to form isolated island areas.
+///  								[Limit: >=0] [Units: vx].
+///  @param[in]		mergeRegionArea		Any regions with a span count smaller than this value will, if possible,
+///  								be merged with larger regions. [Limit: >=0] [Units: vx] 
+///  @returns True if the operation completed successfully.
+bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
+					const int borderSize, const int minRegionArea, const int mergeRegionArea);
+
+/// Builds region data for the heightfield by partitioning the heightfield in non-overlapping layers.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in,out]	chf				A populated compact heightfield.
+///  @param[in]		borderSize		The size of the non-navigable border around the heightfield.
+///  								[Limit: >=0] [Units: vx]
+///  @param[in]		minRegionArea	The minimum number of cells allowed to form isolated island areas.
+///  								[Limit: >=0] [Units: vx].
+///  @returns True if the operation completed successfully.
+bool rcBuildLayerRegions(rcContext* ctx, rcCompactHeightfield& chf,
+						 const int borderSize, const int minRegionArea);
+
+/// Builds region data for the heightfield using simple monotone partitioning.
+///  @ingroup recast 
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in,out]	chf				A populated compact heightfield.
+///  @param[in]		borderSize		The size of the non-navigable border around the heightfield.
+///  								[Limit: >=0] [Units: vx]
+///  @param[in]		minRegionArea	The minimum number of cells allowed to form isolated island areas.
+///  								[Limit: >=0] [Units: vx].
+///  @param[in]		mergeRegionArea	Any regions with a span count smaller than this value will, if possible, 
+///  								be merged with larger regions. [Limit: >=0] [Units: vx] 
+///  @returns True if the operation completed successfully.
+bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf,
+							const int borderSize, const int minRegionArea, const int mergeRegionArea);
+
+/// Sets the neighbor connection data for the specified direction.
+///  @param[in]		s		The span to update.
+///  @param[in]		dir		The direction to set. [Limits: 0 <= value < 4]
+///  @param[in]		i		The index of the neighbor span.
+inline void rcSetCon(rcCompactSpan& s, int dir, int i)
+{
+	const unsigned int shift = (unsigned int)dir*6;
+	unsigned int con = s.con;
+	s.con = (con & ~(0x3f << shift)) | (((unsigned int)i & 0x3f) << shift);
+}
+
+/// Gets neighbor connection data for the specified direction.
+///  @param[in]		s		The span to check.
+///  @param[in]		dir		The direction to check. [Limits: 0 <= value < 4]
+///  @return The neighbor connection data for the specified direction,
+///  	or #RC_NOT_CONNECTED if there is no connection.
+inline int rcGetCon(const rcCompactSpan& s, int dir)
+{
+	const unsigned int shift = (unsigned int)dir*6;
+	return (s.con >> shift) & 0x3f;
+}
+
+/// Gets the standard width (x-axis) offset for the specified direction.
+///  @param[in]		dir		The direction. [Limits: 0 <= value < 4]
+///  @return The width offset to apply to the current cell position to move
+///  	in the direction.
+inline int rcGetDirOffsetX(int dir)
+{
+	static const int offset[4] = { -1, 0, 1, 0, };
+	return offset[dir&0x03];
+}
+
+/// Gets the standard height (z-axis) offset for the specified direction.
+///  @param[in]		dir		The direction. [Limits: 0 <= value < 4]
+///  @return The height offset to apply to the current cell position to move
+///  	in the direction.
+inline int rcGetDirOffsetY(int dir)
+{
+	static const int offset[4] = { 0, 1, 0, -1 };
+	return offset[dir&0x03];
+}
+
+/// Gets the direction for the specified offset. One of x and y should be 0.
+///  @param[in]		x		The x offset. [Limits: -1 <= value <= 1]
+///  @param[in]		y		The y offset. [Limits: -1 <= value <= 1]
+///  @return The direction that represents the offset.
+inline int rcGetDirForOffset(int x, int y)
+{
+	static const int dirs[5] = { 3, 0, -1, 2, 1 };
+	return dirs[((y+1)<<1)+x];
+}
+
+/// @}
+/// @name Layer, Contour, Polymesh, and Detail Mesh Functions
+/// @see rcHeightfieldLayer, rcContourSet, rcPolyMesh, rcPolyMeshDetail
+/// @{
+
+/// Builds a layer set from the specified compact heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx			The build context to use during the operation.
+///  @param[in]		chf			A fully built compact heightfield.
+///  @param[in]		borderSize	The size of the non-navigable border around the heightfield. [Limit: >=0] 
+///  							[Units: vx]
+///  @param[in]		walkableHeight	Minimum floor to 'ceiling' height that will still allow the floor area 
+///  							to be considered walkable. [Limit: >= 3] [Units: vx]
+///  @param[out]	lset		The resulting layer set. (Must be pre-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, 
+							  const int borderSize, const int walkableHeight,
+							  rcHeightfieldLayerSet& lset);
+
+/// Builds a contour set from the region outlines in the provided compact heightfield.
+///  @ingroup recast
+///  @param[in,out]	ctx			The build context to use during the operation.
+///  @param[in]		chf			A fully built compact heightfield.
+///  @param[in]		maxError	The maximum distance a simplfied contour's border edges should deviate 
+///  							the original raw contour. [Limit: >=0] [Units: wu]
+///  @param[in]		maxEdgeLen	The maximum allowed length for contour edges along the border of the mesh. 
+///  							[Limit: >=0] [Units: vx]
+///  @param[out]	cset		The resulting contour set. (Must be pre-allocated.)
+///  @param[in]		buildFlags	The build flags. (See: #rcBuildContoursFlags)
+///  @returns True if the operation completed successfully.
+bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
+					 const float maxError, const int maxEdgeLen,
+					 rcContourSet& cset, const int buildFlags = RC_CONTOUR_TESS_WALL_EDGES);
+
+/// Builds a polygon mesh from the provided contours.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		cset	A fully built contour set.
+///  @param[in]		nvp		The maximum number of vertices allowed for polygons generated during the 
+///  						contour to polygon conversion process. [Limit: >= 3] 
+///  @param[out]	mesh	The resulting polygon mesh. (Must be re-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh);
+
+/// Merges multiple polygon meshes into a single mesh.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		meshes	An array of polygon meshes to merge. [Size: @p nmeshes]
+///  @param[in]		nmeshes	The number of polygon meshes in the meshes array.
+///  @param[in]		mesh	The resulting polygon mesh. (Must be pre-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh);
+
+/// Builds a detail mesh from the provided polygon mesh.
+///  @ingroup recast
+///  @param[in,out]	ctx				The build context to use during the operation.
+///  @param[in]		mesh			A fully built polygon mesh.
+///  @param[in]		chf				The compact heightfield used to build the polygon mesh.
+///  @param[in]		sampleDist		Sets the distance to use when samping the heightfield. [Limit: >=0] [Units: wu]
+///  @param[in]		sampleMaxError	The maximum distance the detail mesh surface should deviate from 
+///  								heightfield data. [Limit: >=0] [Units: wu]
+///  @param[out]	dmesh			The resulting detail mesh.  (Must be pre-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompactHeightfield& chf,
+						   const float sampleDist, const float sampleMaxError,
+						   rcPolyMeshDetail& dmesh);
+
+/// Copies the poly mesh data from src to dst.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		src		The source mesh to copy from.
+///  @param[out]	dst		The resulting detail mesh. (Must be pre-allocated, must be empty mesh.)
+///  @returns True if the operation completed successfully.
+bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst);
+
+/// Merges multiple detail meshes into a single detail mesh.
+///  @ingroup recast
+///  @param[in,out]	ctx		The build context to use during the operation.
+///  @param[in]		meshes	An array of detail meshes to merge. [Size: @p nmeshes]
+///  @param[in]		nmeshes	The number of detail meshes in the meshes array.
+///  @param[out]	mesh	The resulting detail mesh. (Must be pre-allocated.)
+///  @returns True if the operation completed successfully.
+bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh);
+
+/// @}
+
+#endif // RECAST_H
+
+///////////////////////////////////////////////////////////////////////////
+
+// Due to the large amount of detail documentation for this file, 
+// the content normally located at the end of the header file has been separated
+// out to a file in /Docs/Extern.

panda/src/recastdetour/RecastAlloc.cpp

@@ -0,0 +1,60 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <string.h>
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+static void *rcAllocDefault(size_t size, rcAllocHint)
+{
+	return malloc(size);
+}
+
+static void rcFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static rcAllocFunc* sRecastAllocFunc = rcAllocDefault;
+static rcFreeFunc* sRecastFreeFunc = rcFreeDefault;
+
+/// @see rcAlloc, rcFree
+void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc)
+{
+	sRecastAllocFunc = allocFunc ? allocFunc : rcAllocDefault;
+	sRecastFreeFunc = freeFunc ? freeFunc : rcFreeDefault;
+}
+
+/// @see rcAllocSetCustom
+void* rcAlloc(size_t size, rcAllocHint hint)
+{
+	return sRecastAllocFunc(size, hint);
+}
+
+/// @par
+///
+/// @warning This function leaves the value of @p ptr unchanged.  So it still
+/// points to the same (now invalid) location, and not to null.
+/// 
+/// @see rcAllocSetCustom
+void rcFree(void* ptr)
+{
+	if (ptr)
+		sRecastFreeFunc(ptr);
+}

panda/src/recastdetour/RecastAlloc.h

@@ -0,0 +1,342 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef RECASTALLOC_H
+#define RECASTALLOC_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include <RecastAssert.h>
+
+/// Provides hint values to the memory allocator on how long the
+/// memory is expected to be used.
+enum rcAllocHint
+{
+	RC_ALLOC_PERM,		///< Memory will persist after a function call.
+	RC_ALLOC_TEMP		///< Memory used temporarily within a function.
+};
+
+/// A memory allocation function.
+//  @param[in]		size			The size, in bytes of memory, to allocate.
+//  @param[in]		rcAllocHint	A hint to the allocator on how long the memory is expected to be in use.
+//  @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
+///  @see rcAllocSetCustom
+typedef void* (rcAllocFunc)(size_t size, rcAllocHint hint);
+
+/// A memory deallocation function.
+///  @param[in]		ptr		A pointer to a memory block previously allocated using #rcAllocFunc.
+/// @see rcAllocSetCustom
+typedef void (rcFreeFunc)(void* ptr);
+
+/// Sets the base custom allocation functions to be used by Recast.
+///  @param[in]		allocFunc	The memory allocation function to be used by #rcAlloc
+///  @param[in]		freeFunc	The memory de-allocation function to be used by #rcFree
+void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc);
+
+/// Allocates a memory block.
+///  @param[in]		size	The size, in bytes of memory, to allocate.
+///  @param[in]		hint	A hint to the allocator on how long the memory is expected to be in use.
+///  @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
+/// @see rcFree
+void* rcAlloc(size_t size, rcAllocHint hint);
+
+/// Deallocates a memory block.
+///  @param[in]		ptr		A pointer to a memory block previously allocated using #rcAlloc.
+/// @see rcAlloc
+void rcFree(void* ptr);
+
+/// An implementation of operator new usable for placement new. The default one is part of STL (which we don't use).
+/// rcNewTag is a dummy type used to differentiate our operator from the STL one, in case users import both Recast
+/// and STL.
+struct rcNewTag {};
+inline void* operator new(size_t, const rcNewTag&, void* p) { return p; }
+inline void operator delete(void*, const rcNewTag&, void*) {}
+
+/// Signed to avoid warnnings when comparing to int loop indexes, and common error with comparing to zero.
+/// MSVC2010 has a bug where ssize_t is unsigned (!!!).
+typedef intptr_t rcSizeType;
+#define RC_SIZE_MAX INTPTR_MAX
+
+/// Macros to hint to the compiler about the likeliest branch. Please add a benchmark that demonstrates a performance
+/// improvement before introducing use cases.
+#if defined(__GNUC__) || defined(__clang__)
+#define rcLikely(x) __builtin_expect((x), true)
+#define rcUnlikely(x) __builtin_expect((x), false)
+#else
+#define rcLikely(x) (x)
+#define rcUnlikely(x) (x)
+#endif
+
+/// Variable-sized storage type. Mimics the interface of std::vector<T> with some notable differences:
+///  * Uses rcAlloc()/rcFree() to handle storage.
+///  * No support for a custom allocator.
+///  * Uses signed size instead of size_t to avoid warnings in for loops: "for (int i = 0; i < foo.size(); i++)"
+///  * Omits methods of limited utility: insert/erase, (bad performance), at (we don't use exceptions), operator=.
+///  * assign() and the pre-sizing constructor follow C++11 semantics -- they don't construct a temporary if no value is provided.
+///  * push_back() and resize() support adding values from the current vector. Range-based constructors and assign(begin, end) do not.
+///  * No specialization for bool.
+template <typename T, rcAllocHint H>
+class rcVectorBase {
+	rcSizeType m_size;
+	rcSizeType m_cap;
+	T* m_data;
+	// Constructs a T at the give address with either the copy constructor or the default.
+	static void construct(T* p, const T& v) { ::new(rcNewTag(), (void*)p) T(v); }
+	static void construct(T* p) { ::new(rcNewTag(), (void*)p) T; }
+	static void construct_range(T* begin, T* end);
+	static void construct_range(T* begin, T* end, const T& value);
+	static void copy_range(T* dst, const T* begin, const T* end);
+	void destroy_range(rcSizeType begin, rcSizeType end);
+	// Creates an array of the given size, copies all of this vector's data into it, and returns it.
+	T* allocate_and_copy(rcSizeType size);
+	void resize_impl(rcSizeType size, const T* value);
+ public:
+	typedef rcSizeType size_type;
+	typedef T value_type;
+
+	rcVectorBase() : m_size(0), m_cap(0), m_data(0) {};
+	rcVectorBase(const rcVectorBase<T, H>& other) : m_size(0), m_cap(0), m_data(0) { assign(other.begin(), other.end()); }
+	explicit rcVectorBase(rcSizeType count) : m_size(0), m_cap(0), m_data(0) { resize(count); }
+	rcVectorBase(rcSizeType count, const T& value) : m_size(0), m_cap(0), m_data(0) { resize(count, value); }
+	rcVectorBase(const T* begin, const T* end) : m_size(0), m_cap(0), m_data(0) { assign(begin, end); }
+	~rcVectorBase() { destroy_range(0, m_size); rcFree(m_data); }
+
+	// Unlike in std::vector, we return a bool to indicate whether the alloc was successful.
+	bool reserve(rcSizeType size);
+
+	void assign(rcSizeType count, const T& value) { clear(); resize(count, value); }
+	void assign(const T* begin, const T* end);
+
+	void resize(rcSizeType size) { resize_impl(size, NULL); }
+	void resize(rcSizeType size, const T& value) { resize_impl(size, &value); }
+	// Not implemented as resize(0) because resize requires T to be default-constructible.
+	void clear() { destroy_range(0, m_size); m_size = 0; }
+
+	void push_back(const T& value);
+	void pop_back() { rcAssert(m_size > 0); back().~T(); m_size--; }
+
+	rcSizeType size() const { return m_size; }
+	rcSizeType capacity() const { return m_cap; }
+	bool empty() const { return size() == 0; }
+
+	const T& operator[](rcSizeType i) const { rcAssert(i >= 0 && i < m_size); return m_data[i]; }
+	T& operator[](rcSizeType i) { rcAssert(i >= 0 && i < m_size); return m_data[i]; }
+
+	const T& front() const { rcAssert(m_size); return m_data[0]; }
+	T& front() { rcAssert(m_size); return m_data[0]; }
+	const T& back() const { rcAssert(m_size); return m_data[m_size - 1]; };
+	T& back() { rcAssert(m_size); return m_data[m_size - 1]; };
+	const T* data() const { return m_data; }
+	T* data() { return m_data; }
+
+	T* begin() { return m_data; }
+	T* end() { return m_data + m_size; }
+	const T* begin() const { return m_data; }
+	const T* end() const { return m_data + m_size; }
+
+	void swap(rcVectorBase<T, H>& other);
+
+	// Explicitly deleted.
+	rcVectorBase& operator=(const rcVectorBase<T, H>& other);
+};
+
+template<typename T, rcAllocHint H>
+bool rcVectorBase<T, H>::reserve(rcSizeType count) {
+	if (count <= m_cap) {
+		return true;
+	}
+	T* new_data = allocate_and_copy(count);
+	if (!new_data) {
+	  return false;
+	}
+	destroy_range(0, m_size);
+	rcFree(m_data);
+	m_data = new_data;
+	m_cap = count;
+	return true;
+}
+template <typename T, rcAllocHint H>
+T* rcVectorBase<T, H>::allocate_and_copy(rcSizeType size) {
+	rcAssert(RC_SIZE_MAX / static_cast<rcSizeType>(sizeof(T)) >= size);
+	T* new_data = static_cast<T*>(rcAlloc(sizeof(T) * size, H));
+	if (new_data) {
+		copy_range(new_data, m_data, m_data + m_size);
+	}
+	return new_data;
+}
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::assign(const T* begin, const T* end) {
+	clear();
+	reserve(end - begin);
+	m_size = end - begin;
+	copy_range(m_data, begin, end);
+}
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::push_back(const T& value) {
+	// rcLikely increases performance by ~50% on BM_rcVector_PushPreallocated,
+	// and by ~2-5% on BM_rcVector_Push.
+	if (rcLikely(m_size < m_cap)) {
+		construct(m_data + m_size++, value);
+		return;
+	}
+
+	rcAssert(RC_SIZE_MAX / 2 >= m_size);
+	rcSizeType new_cap = m_size ? 2*m_size : 1;
+	T* data = allocate_and_copy(new_cap);
+	// construct between allocate and destroy+free in case value is
+	// in this vector.
+	construct(data + m_size, value);
+	destroy_range(0, m_size);
+	m_size++;
+	m_cap = new_cap;
+	rcFree(m_data);
+	m_data = data;
+}
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::resize_impl(rcSizeType size, const T* value) {
+	if (size < m_size) {
+		destroy_range(size, m_size);
+		m_size = size;
+	} else if (size > m_size) {
+		T* new_data = allocate_and_copy(size);
+		// We defer deconstructing/freeing old data until after constructing
+		// new elements in case "value" is there.
+		if (value) {
+			construct_range(new_data + m_size, new_data + size, *value);
+		} else {
+			construct_range(new_data + m_size, new_data + size);
+		}
+		destroy_range(0, m_size);
+		rcFree(m_data);
+		m_data = new_data;
+		m_cap = size;
+		m_size = size;
+	}
+}
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::swap(rcVectorBase<T, H>& other) {
+	// TODO: Reorganize headers so we can use rcSwap here.
+	rcSizeType tmp_cap = other.m_cap;
+	rcSizeType tmp_size = other.m_size;
+	T* tmp_data = other.m_data;
+
+	other.m_cap = m_cap;
+	other.m_size = m_size;
+	other.m_data = m_data;
+
+	m_cap = tmp_cap;
+	m_size = tmp_size;
+	m_data = tmp_data;
+}
+// static
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::construct_range(T* begin, T* end) {
+	for (T* p = begin; p < end; p++) {
+		construct(p);
+	}
+}
+// static
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::construct_range(T* begin, T* end, const T& value) {
+	for (T* p = begin; p < end; p++) {
+		construct(p, value);
+	}
+}
+// static
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::copy_range(T* dst, const T* begin, const T* end) {
+	for (rcSizeType i = 0 ; i < end - begin; i++) {
+		construct(dst + i, begin[i]);
+	}
+}
+template <typename T, rcAllocHint H>
+void rcVectorBase<T, H>::destroy_range(rcSizeType begin, rcSizeType end) {
+	for (rcSizeType i = begin; i < end; i++) {
+		m_data[i].~T();
+	}
+}
+
+template <typename T>
+class rcTempVector : public rcVectorBase<T, RC_ALLOC_TEMP> {
+	typedef rcVectorBase<T, RC_ALLOC_TEMP> Base;
+public:
+	rcTempVector() : Base() {}
+	explicit rcTempVector(rcSizeType size) : Base(size) {}
+	rcTempVector(rcSizeType size, const T& value) : Base(size, value) {}
+	rcTempVector(const rcTempVector<T>& other) : Base(other) {}
+	rcTempVector(const T* begin, const T* end) : Base(begin, end) {}
+};
+template <typename T>
+class rcPermVector : public rcVectorBase<T, RC_ALLOC_PERM> {
+	typedef rcVectorBase<T, RC_ALLOC_PERM> Base;
+public:
+	rcPermVector() : Base() {}
+	explicit rcPermVector(rcSizeType size) : Base(size) {}
+	rcPermVector(rcSizeType size, const T& value) : Base(size, value) {}
+	rcPermVector(const rcPermVector<T>& other) : Base(other) {}
+	rcPermVector(const T* begin, const T* end) : Base(begin, end) {}
+};
+
+
+/// Legacy class. Prefer rcVector<int>.
+class rcIntArray
+{
+	rcTempVector<int> m_impl;
+public:
+	rcIntArray() {}
+	rcIntArray(int n) : m_impl(n, 0) {}
+	void push(int item) { m_impl.push_back(item); }
+	void resize(int size) { m_impl.resize(size); }
+	int pop()
+	{
+		int v = m_impl.back();
+		m_impl.pop_back();
+		return v;
+	}
+	int size() const { return static_cast<int>(m_impl.size()); }
+	int& operator[](int index) { return m_impl[index]; }
+	int operator[](int index) const { return m_impl[index]; }
+};
+
+/// A simple helper class used to delete an array when it goes out of scope.
+/// @note This class is rarely if ever used by the end user.
+template<class T> class rcScopedDelete
+{
+	T* ptr;
+public:
+
+	/// Constructs an instance with a null pointer.
+	inline rcScopedDelete() : ptr(0) {}
+
+	/// Constructs an instance with the specified pointer.
+	///  @param[in]		p	An pointer to an allocated array.
+	inline rcScopedDelete(T* p) : ptr(p) {}
+	inline ~rcScopedDelete() { rcFree(ptr); }
+
+	/// The root array pointer.
+	///  @return The root array pointer.
+	inline operator T*() { return ptr; }
+	
+private:
+	// Explicitly disabled copy constructor and copy assignment operator.
+	rcScopedDelete(const rcScopedDelete&);
+	rcScopedDelete& operator=(const rcScopedDelete&);
+};
+
+#endif

panda/src/recastdetour/RecastArea.cpp

@@ -0,0 +1,591 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+/// @par 
+/// 
+/// Basically, any spans that are closer to a boundary or obstruction than the specified radius 
+/// are marked as unwalkable.
+///
+/// This method is usually called immediately after the heightfield has been built.
+///
+/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius
+bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedTimer timer(ctx, RC_TIMER_ERODE_AREA);
+	
+	unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
+	if (!dist)
+	{
+		ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount);
+		return false;
+	}
+	
+	// Init distance.
+	memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount);
+	
+	// Mark boundary cells.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (chf.areas[i] == RC_NULL_AREA)
+				{
+					dist[i] = 0;
+				}
+				else
+				{
+					const rcCompactSpan& s = chf.spans[i];
+					int nc = 0;
+					for (int dir = 0; dir < 4; ++dir)
+					{
+						if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+						{
+							const int nx = x + rcGetDirOffsetX(dir);
+							const int ny = y + rcGetDirOffsetY(dir);
+							const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir);
+							if (chf.areas[nidx] != RC_NULL_AREA)
+							{
+								nc++;
+							}
+						}
+					}
+					// At least one missing neighbour.
+					if (nc != 4)
+						dist[i] = 0;
+				}
+			}
+		}
+	}
+	
+	unsigned char nd;
+	
+	// Pass 1
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					// (-1,0)
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (-1,-1)
+					if (rcGetCon(as, 3) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(3);
+						const int aay = ay + rcGetDirOffsetY(3);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+				if (rcGetCon(s, 3) != RC_NOT_CONNECTED)
+				{
+					// (0,-1)
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (1,-1)
+					if (rcGetCon(as, 2) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(2);
+						const int aay = ay + rcGetDirOffsetY(2);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+			}
+		}
+	}
+	
+	// Pass 2
+	for (int y = h-1; y >= 0; --y)
+	{
+		for (int x = w-1; x >= 0; --x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				
+				if (rcGetCon(s, 2) != RC_NOT_CONNECTED)
+				{
+					// (1,0)
+					const int ax = x + rcGetDirOffsetX(2);
+					const int ay = y + rcGetDirOffsetY(2);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (1,1)
+					if (rcGetCon(as, 1) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(1);
+						const int aay = ay + rcGetDirOffsetY(1);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+				if (rcGetCon(s, 1) != RC_NOT_CONNECTED)
+				{
+					// (0,1)
+					const int ax = x + rcGetDirOffsetX(1);
+					const int ay = y + rcGetDirOffsetY(1);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1);
+					const rcCompactSpan& as = chf.spans[ai];
+					nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
+					if (nd < dist[i])
+						dist[i] = nd;
+					
+					// (-1,1)
+					if (rcGetCon(as, 0) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(0);
+						const int aay = ay + rcGetDirOffsetY(0);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0);
+						nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
+						if (nd < dist[i])
+							dist[i] = nd;
+					}
+				}
+			}
+		}
+	}
+	
+	const unsigned char thr = (unsigned char)(radius*2);
+	for (int i = 0; i < chf.spanCount; ++i)
+		if (dist[i] < thr)
+			chf.areas[i] = RC_NULL_AREA;
+	
+	rcFree(dist);
+	
+	return true;
+}
+
+static void insertSort(unsigned char* a, const int n)
+{
+	int i, j;
+	for (i = 1; i < n; i++)
+	{
+		const unsigned char value = a[i];
+		for (j = i - 1; j >= 0 && a[j] > value; j--)
+			a[j+1] = a[j];
+		a[j+1] = value;
+	}
+}
+
+/// @par
+///
+/// This filter is usually applied after applying area id's using functions
+/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea.
+/// 
+/// @see rcCompactHeightfield
+bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MEDIAN_AREA);
+	
+	unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
+	if (!areas)
+	{
+		ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount);
+		return false;
+	}
+	
+	// Init distance.
+	memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount);
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA)
+				{
+					areas[i] = chf.areas[i];
+					continue;
+				}
+				
+				unsigned char nei[9];
+				for (int j = 0; j < 9; ++j)
+					nei[j] = chf.areas[i];
+				
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						if (chf.areas[ai] != RC_NULL_AREA)
+							nei[dir*2+0] = chf.areas[ai];
+						
+						const rcCompactSpan& as = chf.spans[ai];
+						const int dir2 = (dir+1) & 0x3;
+						if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
+						{
+							const int ax2 = ax + rcGetDirOffsetX(dir2);
+							const int ay2 = ay + rcGetDirOffsetY(dir2);
+							const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
+							if (chf.areas[ai2] != RC_NULL_AREA)
+								nei[dir*2+1] = chf.areas[ai2];
+						}
+					}
+				}
+				insertSort(nei, 9);
+				areas[i] = nei[4];
+			}
+		}
+	}
+	
+	memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount);
+	
+	rcFree(areas);
+	
+	return true;
+}
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId,
+				   rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MARK_BOX_AREA);
+
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					if (chf.areas[i] != RC_NULL_AREA)
+						chf.areas[i] = areaId;
+				}
+			}
+		}
+	}
+}
+
+
+static int pointInPoly(int nvert, const float* verts, const float* p)
+{
+	int i, j, c = 0;
+	for (i = 0, j = nvert-1; i < nvert; j = i++)
+	{
+		const float* vi = &verts[i*3];
+		const float* vj = &verts[j*3];
+		if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
+			(p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
+			c = !c;
+	}
+	return c;
+}
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// The y-values of the polygon vertices are ignored. So the polygon is effectively 
+/// projected onto the xz-plane at @p hmin, then extruded to @p hmax.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
+						  const float hmin, const float hmax, unsigned char areaId,
+						  rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA);
+
+	float bmin[3], bmax[3];
+	rcVcopy(bmin, verts);
+	rcVcopy(bmax, verts);
+	for (int i = 1; i < nverts; ++i)
+	{
+		rcVmin(bmin, &verts[i*3]);
+		rcVmax(bmax, &verts[i*3]);
+	}
+	bmin[1] = hmin;
+	bmax[1] = hmax;
+
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+	
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	
+	// TODO: Optimize.
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA)
+					continue;
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					float p[3];
+					p[0] = chf.bmin[0] + (x+0.5f)*chf.cs; 
+					p[1] = 0;
+					p[2] = chf.bmin[2] + (z+0.5f)*chf.cs; 
+
+					if (pointInPoly(nverts, verts, p))
+					{
+						chf.areas[i] = areaId;
+					}
+				}
+			}
+		}
+	}
+}
+
+int rcOffsetPoly(const float* verts, const int nverts, const float offset,
+				 float* outVerts, const int maxOutVerts)
+{
+	const float	MITER_LIMIT = 1.20f;
+
+	int n = 0;
+
+	for (int i = 0; i < nverts; i++)
+	{
+		const int a = (i+nverts-1) % nverts;
+		const int b = i;
+		const int c = (i+1) % nverts;
+		const float* va = &verts[a*3];
+		const float* vb = &verts[b*3];
+		const float* vc = &verts[c*3];
+		float dx0 = vb[0] - va[0];
+		float dy0 = vb[2] - va[2];
+		float d0 = dx0*dx0 + dy0*dy0;
+		if (d0 > 1e-6f)
+		{
+			d0 = 1.0f/rcSqrt(d0);
+			dx0 *= d0;
+			dy0 *= d0;
+		}
+		float dx1 = vc[0] - vb[0];
+		float dy1 = vc[2] - vb[2];
+		float d1 = dx1*dx1 + dy1*dy1;
+		if (d1 > 1e-6f)
+		{
+			d1 = 1.0f/rcSqrt(d1);
+			dx1 *= d1;
+			dy1 *= d1;
+		}
+		const float dlx0 = -dy0;
+		const float dly0 = dx0;
+		const float dlx1 = -dy1;
+		const float dly1 = dx1;
+		float cross = dx1*dy0 - dx0*dy1;
+		float dmx = (dlx0 + dlx1) * 0.5f;
+		float dmy = (dly0 + dly1) * 0.5f;
+		float dmr2 = dmx*dmx + dmy*dmy;
+		bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f;
+		if (dmr2 > 1e-6f)
+		{
+			const float scale = 1.0f / dmr2;
+			dmx *= scale;
+			dmy *= scale;
+		}
+
+		if (bevel && cross < 0.0f)
+		{
+			if (n+2 >= maxOutVerts)
+				return 0;
+			float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f;
+			outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset;
+			n++;
+			outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset;
+			n++;
+		}
+		else
+		{
+			if (n+1 >= maxOutVerts)
+				return 0;
+			outVerts[n*3+0] = vb[0] - dmx*offset;
+			outVerts[n*3+1] = vb[1];
+			outVerts[n*3+2] = vb[2] - dmy*offset;
+			n++;
+		}
+	}
+	
+	return n;
+}
+
+
+/// @par
+///
+/// The value of spacial parameters are in world units.
+/// 
+/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
+void rcMarkCylinderArea(rcContext* ctx, const float* pos,
+						const float r, const float h, unsigned char areaId,
+						rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MARK_CYLINDER_AREA);
+	
+	float bmin[3], bmax[3];
+	bmin[0] = pos[0] - r;
+	bmin[1] = pos[1];
+	bmin[2] = pos[2] - r;
+	bmax[0] = pos[0] + r;
+	bmax[1] = pos[1] + h;
+	bmax[2] = pos[2] + r;
+	const float r2 = r*r;
+	
+	int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
+	int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
+	int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
+	int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
+	int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
+	int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
+	
+	if (maxx < 0) return;
+	if (minx >= chf.width) return;
+	if (maxz < 0) return;
+	if (minz >= chf.height) return;
+	
+	if (minx < 0) minx = 0;
+	if (maxx >= chf.width) maxx = chf.width-1;
+	if (minz < 0) minz = 0;
+	if (maxz >= chf.height) maxz = chf.height-1;	
+	
+	
+	for (int z = minz; z <= maxz; ++z)
+	{
+		for (int x = minx; x <= maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+z*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				rcCompactSpan& s = chf.spans[i];
+				
+				if (chf.areas[i] == RC_NULL_AREA)
+					continue;
+				
+				if ((int)s.y >= miny && (int)s.y <= maxy)
+				{
+					const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; 
+					const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; 
+					const float dx = sx - pos[0];
+					const float dz = sz - pos[2];
+					
+					if (dx*dx + dz*dz < r2)
+					{
+						chf.areas[i] = areaId;
+					}
+				}
+			}
+		}
+	}
+}

panda/src/recastdetour/RecastAssert.cpp

@@ -0,0 +1,35 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 "RecastAssert.h"
+
+#ifndef NDEBUG
+
+static rcAssertFailFunc* sRecastAssertFailFunc = 0;
+
+void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc)
+{
+	sRecastAssertFailFunc = assertFailFunc;
+}
+
+rcAssertFailFunc* rcAssertFailGetCustom()
+{
+	return sRecastAssertFailFunc;
+}
+
+#endif

panda/src/recastdetour/RecastAssert.h

@@ -0,0 +1,56 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef RECASTASSERT_H
+#define RECASTASSERT_H
+
+// Note: This header file's only purpose is to include define assert.
+// Feel free to change the file and include your own implementation instead.
+
+#ifdef NDEBUG
+
+// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/
+#	define rcAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false)
+
+#else
+
+/// An assertion failure function.
+//  @param[in]		expression  asserted expression.
+//  @param[in]		file  Filename of the failed assertion.
+//  @param[in]		line  Line number of the failed assertion.
+///  @see rcAssertFailSetCustom
+typedef void (rcAssertFailFunc)(const char* expression, const char* file, int line);
+
+/// Sets the base custom assertion failure function to be used by Recast.
+///  @param[in]		assertFailFunc	The function to be used in case of failure of #dtAssert
+void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc);
+
+/// Gets the base custom assertion failure function to be used by Recast.
+rcAssertFailFunc* rcAssertFailGetCustom();
+
+#	include <assert.h> 
+#	define rcAssert(expression) \
+		{ \
+			rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \
+			if(failFunc == NULL) { assert(expression); } \
+			else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \
+		}
+
+#endif
+
+#endif // RECASTASSERT_H

panda/src/recastdetour/RecastContour.cpp

@@ -0,0 +1,1105 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+
+static int getCornerHeight(int x, int y, int i, int dir,
+						   const rcCompactHeightfield& chf,
+						   bool& isBorderVertex)
+{
+	const rcCompactSpan& s = chf.spans[i];
+	int ch = (int)s.y;
+	int dirp = (dir+1) & 0x3;
+	
+	unsigned int regs[4] = {0,0,0,0};
+	
+	// Combine region and area codes in order to prevent
+	// border vertices which are in between two areas to be removed.
+	regs[0] = chf.spans[i].reg | (chf.areas[i] << 16);
+	
+	if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+	{
+		const int ax = x + rcGetDirOffsetX(dir);
+		const int ay = y + rcGetDirOffsetY(dir);
+		const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
+		const rcCompactSpan& as = chf.spans[ai];
+		ch = rcMax(ch, (int)as.y);
+		regs[1] = chf.spans[ai].reg | (chf.areas[ai] << 16);
+		if (rcGetCon(as, dirp) != RC_NOT_CONNECTED)
+		{
+			const int ax2 = ax + rcGetDirOffsetX(dirp);
+			const int ay2 = ay + rcGetDirOffsetY(dirp);
+			const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dirp);
+			const rcCompactSpan& as2 = chf.spans[ai2];
+			ch = rcMax(ch, (int)as2.y);
+			regs[2] = chf.spans[ai2].reg | (chf.areas[ai2] << 16);
+		}
+	}
+	if (rcGetCon(s, dirp) != RC_NOT_CONNECTED)
+	{
+		const int ax = x + rcGetDirOffsetX(dirp);
+		const int ay = y + rcGetDirOffsetY(dirp);
+		const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dirp);
+		const rcCompactSpan& as = chf.spans[ai];
+		ch = rcMax(ch, (int)as.y);
+		regs[3] = chf.spans[ai].reg | (chf.areas[ai] << 16);
+		if (rcGetCon(as, dir) != RC_NOT_CONNECTED)
+		{
+			const int ax2 = ax + rcGetDirOffsetX(dir);
+			const int ay2 = ay + rcGetDirOffsetY(dir);
+			const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dir);
+			const rcCompactSpan& as2 = chf.spans[ai2];
+			ch = rcMax(ch, (int)as2.y);
+			regs[2] = chf.spans[ai2].reg | (chf.areas[ai2] << 16);
+		}
+	}
+
+	// Check if the vertex is special edge vertex, these vertices will be removed later.
+	for (int j = 0; j < 4; ++j)
+	{
+		const int a = j;
+		const int b = (j+1) & 0x3;
+		const int c = (j+2) & 0x3;
+		const int d = (j+3) & 0x3;
+		
+		// The vertex is a border vertex there are two same exterior cells in a row,
+		// followed by two interior cells and none of the regions are out of bounds.
+		const bool twoSameExts = (regs[a] & regs[b] & RC_BORDER_REG) != 0 && regs[a] == regs[b];
+		const bool twoInts = ((regs[c] | regs[d]) & RC_BORDER_REG) == 0;
+		const bool intsSameArea = (regs[c]>>16) == (regs[d]>>16);
+		const bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0;
+		if (twoSameExts && twoInts && intsSameArea && noZeros)
+		{
+			isBorderVertex = true;
+			break;
+		}
+	}
+	
+	return ch;
+}
+
+static void walkContour(int x, int y, int i,
+						rcCompactHeightfield& chf,
+						unsigned char* flags, rcIntArray& points)
+{
+	// Choose the first non-connected edge
+	unsigned char dir = 0;
+	while ((flags[i] & (1 << dir)) == 0)
+		dir++;
+	
+	unsigned char startDir = dir;
+	int starti = i;
+	
+	const unsigned char area = chf.areas[i];
+	
+	int iter = 0;
+	while (++iter < 40000)
+	{
+		if (flags[i] & (1 << dir))
+		{
+			// Choose the edge corner
+			bool isBorderVertex = false;
+			bool isAreaBorder = false;
+			int px = x;
+			int py = getCornerHeight(x, y, i, dir, chf, isBorderVertex);
+			int pz = y;
+			switch(dir)
+			{
+				case 0: pz++; break;
+				case 1: px++; pz++; break;
+				case 2: px++; break;
+			}
+			int r = 0;
+			const rcCompactSpan& s = chf.spans[i];
+			if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+			{
+				const int ax = x + rcGetDirOffsetX(dir);
+				const int ay = y + rcGetDirOffsetY(dir);
+				const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
+				r = (int)chf.spans[ai].reg;
+				if (area != chf.areas[ai])
+					isAreaBorder = true;
+			}
+			if (isBorderVertex)
+				r |= RC_BORDER_VERTEX;
+			if (isAreaBorder)
+				r |= RC_AREA_BORDER;
+			points.push(px);
+			points.push(py);
+			points.push(pz);
+			points.push(r);
+			
+			flags[i] &= ~(1 << dir); // Remove visited edges
+			dir = (dir+1) & 0x3;  // Rotate CW
+		}
+		else
+		{
+			int ni = -1;
+			const int nx = x + rcGetDirOffsetX(dir);
+			const int ny = y + rcGetDirOffsetY(dir);
+			const rcCompactSpan& s = chf.spans[i];
+			if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+			{
+				const rcCompactCell& nc = chf.cells[nx+ny*chf.width];
+				ni = (int)nc.index + rcGetCon(s, dir);
+			}
+			if (ni == -1)
+			{
+				// Should not happen.
+				return;
+			}
+			x = nx;
+			y = ny;
+			i = ni;
+			dir = (dir+3) & 0x3;	// Rotate CCW
+		}
+		
+		if (starti == i && startDir == dir)
+		{
+			break;
+		}
+	}
+}
+
+static float distancePtSeg(const int x, const int z,
+						   const int px, const int pz,
+						   const int qx, const int qz)
+{
+	float pqx = (float)(qx - px);
+	float pqz = (float)(qz - pz);
+	float dx = (float)(x - px);
+	float dz = (float)(z - pz);
+	float d = pqx*pqx + pqz*pqz;
+	float t = pqx*dx + pqz*dz;
+	if (d > 0)
+		t /= d;
+	if (t < 0)
+		t = 0;
+	else if (t > 1)
+		t = 1;
+	
+	dx = px + t*pqx - x;
+	dz = pz + t*pqz - z;
+	
+	return dx*dx + dz*dz;
+}
+
+static void simplifyContour(rcIntArray& points, rcIntArray& simplified,
+							const float maxError, const int maxEdgeLen, const int buildFlags)
+{
+	// Add initial points.
+	bool hasConnections = false;
+	for (int i = 0; i < points.size(); i += 4)
+	{
+		if ((points[i+3] & RC_CONTOUR_REG_MASK) != 0)
+		{
+			hasConnections = true;
+			break;
+		}
+	}
+	
+	if (hasConnections)
+	{
+		// The contour has some portals to other regions.
+		// Add a new point to every location where the region changes.
+		for (int i = 0, ni = points.size()/4; i < ni; ++i)
+		{
+			int ii = (i+1) % ni;
+			const bool differentRegs = (points[i*4+3] & RC_CONTOUR_REG_MASK) != (points[ii*4+3] & RC_CONTOUR_REG_MASK);
+			const bool areaBorders = (points[i*4+3] & RC_AREA_BORDER) != (points[ii*4+3] & RC_AREA_BORDER);
+			if (differentRegs || areaBorders)
+			{
+				simplified.push(points[i*4+0]);
+				simplified.push(points[i*4+1]);
+				simplified.push(points[i*4+2]);
+				simplified.push(i);
+			}
+		}
+	}
+	
+	if (simplified.size() == 0)
+	{
+		// If there is no connections at all,
+		// create some initial points for the simplification process.
+		// Find lower-left and upper-right vertices of the contour.
+		int llx = points[0];
+		int lly = points[1];
+		int llz = points[2];
+		int lli = 0;
+		int urx = points[0];
+		int ury = points[1];
+		int urz = points[2];
+		int uri = 0;
+		for (int i = 0; i < points.size(); i += 4)
+		{
+			int x = points[i+0];
+			int y = points[i+1];
+			int z = points[i+2];
+			if (x < llx || (x == llx && z < llz))
+			{
+				llx = x;
+				lly = y;
+				llz = z;
+				lli = i/4;
+			}
+			if (x > urx || (x == urx && z > urz))
+			{
+				urx = x;
+				ury = y;
+				urz = z;
+				uri = i/4;
+			}
+		}
+		simplified.push(llx);
+		simplified.push(lly);
+		simplified.push(llz);
+		simplified.push(lli);
+		
+		simplified.push(urx);
+		simplified.push(ury);
+		simplified.push(urz);
+		simplified.push(uri);
+	}
+	
+	// Add points until all raw points are within
+	// error tolerance to the simplified shape.
+	const int pn = points.size()/4;
+	for (int i = 0; i < simplified.size()/4; )
+	{
+		int ii = (i+1) % (simplified.size()/4);
+		
+		int ax = simplified[i*4+0];
+		int az = simplified[i*4+2];
+		int ai = simplified[i*4+3];
+
+		int bx = simplified[ii*4+0];
+		int bz = simplified[ii*4+2];
+		int bi = simplified[ii*4+3];
+
+		// Find maximum deviation from the segment.
+		float maxd = 0;
+		int maxi = -1;
+		int ci, cinc, endi;
+
+		// Traverse the segment in lexilogical order so that the
+		// max deviation is calculated similarly when traversing
+		// opposite segments.
+		if (bx > ax || (bx == ax && bz > az))
+		{
+			cinc = 1;
+			ci = (ai+cinc) % pn;
+			endi = bi;
+		}
+		else
+		{
+			cinc = pn-1;
+			ci = (bi+cinc) % pn;
+			endi = ai;
+			rcSwap(ax, bx);
+			rcSwap(az, bz);
+		}
+		
+		// Tessellate only outer edges or edges between areas.
+		if ((points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0 ||
+			(points[ci*4+3] & RC_AREA_BORDER))
+		{
+			while (ci != endi)
+			{
+				float d = distancePtSeg(points[ci*4+0], points[ci*4+2], ax, az, bx, bz);
+				if (d > maxd)
+				{
+					maxd = d;
+					maxi = ci;
+				}
+				ci = (ci+cinc) % pn;
+			}
+		}
+		
+		
+		// If the max deviation is larger than accepted error,
+		// add new point, else continue to next segment.
+		if (maxi != -1 && maxd > (maxError*maxError))
+		{
+			// Add space for the new point.
+			simplified.resize(simplified.size()+4);
+			const int n = simplified.size()/4;
+			for (int j = n-1; j > i; --j)
+			{
+				simplified[j*4+0] = simplified[(j-1)*4+0];
+				simplified[j*4+1] = simplified[(j-1)*4+1];
+				simplified[j*4+2] = simplified[(j-1)*4+2];
+				simplified[j*4+3] = simplified[(j-1)*4+3];
+			}
+			// Add the point.
+			simplified[(i+1)*4+0] = points[maxi*4+0];
+			simplified[(i+1)*4+1] = points[maxi*4+1];
+			simplified[(i+1)*4+2] = points[maxi*4+2];
+			simplified[(i+1)*4+3] = maxi;
+		}
+		else
+		{
+			++i;
+		}
+	}
+	
+	// Split too long edges.
+	if (maxEdgeLen > 0 && (buildFlags & (RC_CONTOUR_TESS_WALL_EDGES|RC_CONTOUR_TESS_AREA_EDGES)) != 0)
+	{
+		for (int i = 0; i < simplified.size()/4; )
+		{
+			const int ii = (i+1) % (simplified.size()/4);
+			
+			const int ax = simplified[i*4+0];
+			const int az = simplified[i*4+2];
+			const int ai = simplified[i*4+3];
+			
+			const int bx = simplified[ii*4+0];
+			const int bz = simplified[ii*4+2];
+			const int bi = simplified[ii*4+3];
+			
+			// Find maximum deviation from the segment.
+			int maxi = -1;
+			int ci = (ai+1) % pn;
+			
+			// Tessellate only outer edges or edges between areas.
+			bool tess = false;
+			// Wall edges.
+			if ((buildFlags & RC_CONTOUR_TESS_WALL_EDGES) && (points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0)
+				tess = true;
+			// Edges between areas.
+			if ((buildFlags & RC_CONTOUR_TESS_AREA_EDGES) && (points[ci*4+3] & RC_AREA_BORDER))
+				tess = true;
+			
+			if (tess)
+			{
+				int dx = bx - ax;
+				int dz = bz - az;
+				if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen)
+				{
+					// Round based on the segments in lexilogical order so that the
+					// max tesselation is consistent regardles in which direction
+					// segments are traversed.
+					const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
+					if (n > 1)
+					{
+						if (bx > ax || (bx == ax && bz > az))
+							maxi = (ai + n/2) % pn;
+						else
+							maxi = (ai + (n+1)/2) % pn;
+					}
+				}
+			}
+			
+			// If the max deviation is larger than accepted error,
+			// add new point, else continue to next segment.
+			if (maxi != -1)
+			{
+				// Add space for the new point.
+				simplified.resize(simplified.size()+4);
+				const int n = simplified.size()/4;
+				for (int j = n-1; j > i; --j)
+				{
+					simplified[j*4+0] = simplified[(j-1)*4+0];
+					simplified[j*4+1] = simplified[(j-1)*4+1];
+					simplified[j*4+2] = simplified[(j-1)*4+2];
+					simplified[j*4+3] = simplified[(j-1)*4+3];
+				}
+				// Add the point.
+				simplified[(i+1)*4+0] = points[maxi*4+0];
+				simplified[(i+1)*4+1] = points[maxi*4+1];
+				simplified[(i+1)*4+2] = points[maxi*4+2];
+				simplified[(i+1)*4+3] = maxi;
+			}
+			else
+			{
+				++i;
+			}
+		}
+	}
+	
+	for (int i = 0; i < simplified.size()/4; ++i)
+	{
+		// The edge vertex flag is take from the current raw point,
+		// and the neighbour region is take from the next raw point.
+		const int ai = (simplified[i*4+3]+1) % pn;
+		const int bi = simplified[i*4+3];
+		simplified[i*4+3] = (points[ai*4+3] & (RC_CONTOUR_REG_MASK|RC_AREA_BORDER)) | (points[bi*4+3] & RC_BORDER_VERTEX);
+	}
+	
+}
+
+static int calcAreaOfPolygon2D(const int* verts, const int nverts)
+{
+	int area = 0;
+	for (int i = 0, j = nverts-1; i < nverts; j=i++)
+	{
+		const int* vi = &verts[i*4];
+		const int* vj = &verts[j*4];
+		area += vi[0] * vj[2] - vj[0] * vi[2];
+	}
+	return (area+1) / 2;
+}
+
+// TODO: these are the same as in RecastMesh.cpp, consider using the same.
+// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv).
+inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; }
+inline int next(int i, int n) { return i+1 < n ? i+1 : 0; }
+
+inline int area2(const int* a, const int* b, const int* c)
+{
+	return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
+}
+
+//	Exclusive or: true iff exactly one argument is true.
+//	The arguments are negated to ensure that they are 0/1
+//	values.  Then the bitwise Xor operator may apply.
+//	(This idea is due to Michael Baldwin.)
+inline bool xorb(bool x, bool y)
+{
+	return !x ^ !y;
+}
+
+// Returns true iff c is strictly to the left of the directed
+// line through a to b.
+inline bool left(const int* a, const int* b, const int* c)
+{
+	return area2(a, b, c) < 0;
+}
+
+inline bool leftOn(const int* a, const int* b, const int* c)
+{
+	return area2(a, b, c) <= 0;
+}
+
+inline bool collinear(const int* a, const int* b, const int* c)
+{
+	return area2(a, b, c) == 0;
+}
+
+//	Returns true iff ab properly intersects cd: they share
+//	a point interior to both segments.  The properness of the
+//	intersection is ensured by using strict leftness.
+static bool intersectProp(const int* a, const int* b, const int* c, const int* d)
+{
+	// Eliminate improper cases.
+	if (collinear(a,b,c) || collinear(a,b,d) ||
+		collinear(c,d,a) || collinear(c,d,b))
+		return false;
+	
+	return xorb(left(a,b,c), left(a,b,d)) && xorb(left(c,d,a), left(c,d,b));
+}
+
+// Returns T iff (a,b,c) are collinear and point c lies
+// on the closed segement ab.
+static bool between(const int* a, const int* b, const int* c)
+{
+	if (!collinear(a, b, c))
+		return false;
+	// If ab not vertical, check betweenness on x; else on y.
+	if (a[0] != b[0])
+		return	((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0]));
+	else
+		return	((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2]));
+}
+
+// Returns true iff segments ab and cd intersect, properly or improperly.
+static bool intersect(const int* a, const int* b, const int* c, const int* d)
+{
+	if (intersectProp(a, b, c, d))
+		return true;
+	else if (between(a, b, c) || between(a, b, d) ||
+			 between(c, d, a) || between(c, d, b))
+		return true;
+	else
+		return false;
+}
+
+static bool vequal(const int* a, const int* b)
+{
+	return a[0] == b[0] && a[2] == b[2];
+}
+
+static bool intersectSegCountour(const int* d0, const int* d1, int i, int n, const int* verts)
+{
+	// For each edge (k,k+1) of P
+	for (int k = 0; k < n; k++)
+	{
+		int k1 = next(k, n);
+		// Skip edges incident to i.
+		if (i == k || i == k1)
+			continue;
+		const int* p0 = &verts[k * 4];
+		const int* p1 = &verts[k1 * 4];
+		if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1))
+			continue;
+		
+		if (intersect(d0, d1, p0, p1))
+			return true;
+	}
+	return false;
+}
+
+static bool	inCone(int i, int n, const int* verts, const int* pj)
+{
+	const int* pi = &verts[i * 4];
+	const int* pi1 = &verts[next(i, n) * 4];
+	const int* pin1 = &verts[prev(i, n) * 4];
+	
+	// If P[i] is a convex vertex [ i+1 left or on (i-1,i) ].
+	if (leftOn(pin1, pi, pi1))
+		return left(pi, pj, pin1) && left(pj, pi, pi1);
+	// Assume (i-1,i,i+1) not collinear.
+	// else P[i] is reflex.
+	return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1));
+}
+
+
+static void removeDegenerateSegments(rcIntArray& simplified)
+{
+	// Remove adjacent vertices which are equal on xz-plane,
+	// or else the triangulator will get confused.
+	int npts = simplified.size()/4;
+	for (int i = 0; i < npts; ++i)
+	{
+		int ni = next(i, npts);
+		
+		if (vequal(&simplified[i*4], &simplified[ni*4]))
+		{
+			// Degenerate segment, remove.
+			for (int j = i; j < simplified.size()/4-1; ++j)
+			{
+				simplified[j*4+0] = simplified[(j+1)*4+0];
+				simplified[j*4+1] = simplified[(j+1)*4+1];
+				simplified[j*4+2] = simplified[(j+1)*4+2];
+				simplified[j*4+3] = simplified[(j+1)*4+3];
+			}
+			simplified.resize(simplified.size()-4);
+			npts--;
+		}
+	}
+}
+
+
+static bool mergeContours(rcContour& ca, rcContour& cb, int ia, int ib)
+{
+	const int maxVerts = ca.nverts + cb.nverts + 2;
+	int* verts = (int*)rcAlloc(sizeof(int)*maxVerts*4, RC_ALLOC_PERM);
+	if (!verts)
+		return false;
+	
+	int nv = 0;
+	
+	// Copy contour A.
+	for (int i = 0; i <= ca.nverts; ++i)
+	{
+		int* dst = &verts[nv*4];
+		const int* src = &ca.verts[((ia+i)%ca.nverts)*4];
+		dst[0] = src[0];
+		dst[1] = src[1];
+		dst[2] = src[2];
+		dst[3] = src[3];
+		nv++;
+	}
+
+	// Copy contour B
+	for (int i = 0; i <= cb.nverts; ++i)
+	{
+		int* dst = &verts[nv*4];
+		const int* src = &cb.verts[((ib+i)%cb.nverts)*4];
+		dst[0] = src[0];
+		dst[1] = src[1];
+		dst[2] = src[2];
+		dst[3] = src[3];
+		nv++;
+	}
+	
+	rcFree(ca.verts);
+	ca.verts = verts;
+	ca.nverts = nv;
+	
+	rcFree(cb.verts);
+	cb.verts = 0;
+	cb.nverts = 0;
+	
+	return true;
+}
+
+struct rcContourHole
+{
+	rcContour* contour;
+	int minx, minz, leftmost;
+};
+
+struct rcContourRegion
+{
+	rcContour* outline;
+	rcContourHole* holes;
+	int nholes;
+};
+
+struct rcPotentialDiagonal
+{
+	int vert;
+	int dist;
+};
+
+// Finds the lowest leftmost vertex of a contour.
+static void findLeftMostVertex(rcContour* contour, int* minx, int* minz, int* leftmost)
+{
+	*minx = contour->verts[0];
+	*minz = contour->verts[2];
+	*leftmost = 0;
+	for (int i = 1; i < contour->nverts; i++)
+	{
+		const int x = contour->verts[i*4+0];
+		const int z = contour->verts[i*4+2];
+		if (x < *minx || (x == *minx && z < *minz))
+		{
+			*minx = x;
+			*minz = z;
+			*leftmost = i;
+		}
+	}
+}
+
+static int compareHoles(const void* va, const void* vb)
+{
+	const rcContourHole* a = (const rcContourHole*)va;
+	const rcContourHole* b = (const rcContourHole*)vb;
+	if (a->minx == b->minx)
+	{
+		if (a->minz < b->minz)
+			return -1;
+		if (a->minz > b->minz)
+			return 1;
+	}
+	else
+	{
+		if (a->minx < b->minx)
+			return -1;
+		if (a->minx > b->minx)
+			return 1;
+	}
+	return 0;
+}
+
+
+static int compareDiagDist(const void* va, const void* vb)
+{
+	const rcPotentialDiagonal* a = (const rcPotentialDiagonal*)va;
+	const rcPotentialDiagonal* b = (const rcPotentialDiagonal*)vb;
+	if (a->dist < b->dist)
+		return -1;
+	if (a->dist > b->dist)
+		return 1;
+	return 0;
+}
+
+
+static void mergeRegionHoles(rcContext* ctx, rcContourRegion& region)
+{
+	// Sort holes from left to right.
+	for (int i = 0; i < region.nholes; i++)
+		findLeftMostVertex(region.holes[i].contour, &region.holes[i].minx, &region.holes[i].minz, &region.holes[i].leftmost);
+	
+	qsort(region.holes, region.nholes, sizeof(rcContourHole), compareHoles);
+	
+	int maxVerts = region.outline->nverts;
+	for (int i = 0; i < region.nholes; i++)
+		maxVerts += region.holes[i].contour->nverts;
+	
+	rcScopedDelete<rcPotentialDiagonal> diags((rcPotentialDiagonal*)rcAlloc(sizeof(rcPotentialDiagonal)*maxVerts, RC_ALLOC_TEMP));
+	if (!diags)
+	{
+		ctx->log(RC_LOG_WARNING, "mergeRegionHoles: Failed to allocated diags %d.", maxVerts);
+		return;
+	}
+	
+	rcContour* outline = region.outline;
+	
+	// Merge holes into the outline one by one.
+	for (int i = 0; i < region.nholes; i++)
+	{
+		rcContour* hole = region.holes[i].contour;
+		
+		int index = -1;
+		int bestVertex = region.holes[i].leftmost;
+		for (int iter = 0; iter < hole->nverts; iter++)
+		{
+			// Find potential diagonals.
+			// The 'best' vertex must be in the cone described by 3 cosequtive vertices of the outline.
+			// ..o j-1
+			//   |
+			//   |   * best
+			//   |
+			// j o-----o j+1
+			//         :
+			int ndiags = 0;
+			const int* corner = &hole->verts[bestVertex*4];
+			for (int j = 0; j < outline->nverts; j++)
+			{
+				if (inCone(j, outline->nverts, outline->verts, corner))
+				{
+					int dx = outline->verts[j*4+0] - corner[0];
+					int dz = outline->verts[j*4+2] - corner[2];
+					diags[ndiags].vert = j;
+					diags[ndiags].dist = dx*dx + dz*dz;
+					ndiags++;
+				}
+			}
+			// Sort potential diagonals by distance, we want to make the connection as short as possible.
+			qsort(diags, ndiags, sizeof(rcPotentialDiagonal), compareDiagDist);
+			
+			// Find a diagonal that is not intersecting the outline not the remaining holes.
+			index = -1;
+			for (int j = 0; j < ndiags; j++)
+			{
+				const int* pt = &outline->verts[diags[j].vert*4];
+				bool intersect = intersectSegCountour(pt, corner, diags[i].vert, outline->nverts, outline->verts);
+				for (int k = i; k < region.nholes && !intersect; k++)
+					intersect |= intersectSegCountour(pt, corner, -1, region.holes[k].contour->nverts, region.holes[k].contour->verts);
+				if (!intersect)
+				{
+					index = diags[j].vert;
+					break;
+				}
+			}
+			// If found non-intersecting diagonal, stop looking.
+			if (index != -1)
+				break;
+			// All the potential diagonals for the current vertex were intersecting, try next vertex.
+			bestVertex = (bestVertex + 1) % hole->nverts;
+		}
+		
+		if (index == -1)
+		{
+			ctx->log(RC_LOG_WARNING, "mergeHoles: Failed to find merge points for %p and %p.", region.outline, hole);
+			continue;
+		}
+		if (!mergeContours(*region.outline, *hole, index, bestVertex))
+		{
+			ctx->log(RC_LOG_WARNING, "mergeHoles: Failed to merge contours %p and %p.", region.outline, hole);
+			continue;
+		}
+	}
+}
+
+
+/// @par
+///
+/// The raw contours will match the region outlines exactly. The @p maxError and @p maxEdgeLen
+/// parameters control how closely the simplified contours will match the raw contours.
+///
+/// Simplified contours are generated such that the vertices for portals between areas match up.
+/// (They are considered mandatory vertices.)
+///
+/// Setting @p maxEdgeLength to zero will disabled the edge length feature.
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcAllocContourSet, rcCompactHeightfield, rcContourSet, rcConfig
+bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
+					 const float maxError, const int maxEdgeLen,
+					 rcContourSet& cset, const int buildFlags)
+{
+	rcAssert(ctx);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	const int borderSize = chf.borderSize;
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_CONTOURS);
+	
+	rcVcopy(cset.bmin, chf.bmin);
+	rcVcopy(cset.bmax, chf.bmax);
+	if (borderSize > 0)
+	{
+		// If the heightfield was build with bordersize, remove the offset.
+		const float pad = borderSize*chf.cs;
+		cset.bmin[0] += pad;
+		cset.bmin[2] += pad;
+		cset.bmax[0] -= pad;
+		cset.bmax[2] -= pad;
+	}
+	cset.cs = chf.cs;
+	cset.ch = chf.ch;
+	cset.width = chf.width - chf.borderSize*2;
+	cset.height = chf.height - chf.borderSize*2;
+	cset.borderSize = chf.borderSize;
+	cset.maxError = maxError;
+	
+	int maxContours = rcMax((int)chf.maxRegions, 8);
+	cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM);
+	if (!cset.conts)
+		return false;
+	cset.nconts = 0;
+	
+	rcScopedDelete<unsigned char> flags((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP));
+	if (!flags)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'flags' (%d).", chf.spanCount);
+		return false;
+	}
+	
+	ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
+	
+	// Mark boundaries.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				unsigned char res = 0;
+				const rcCompactSpan& s = chf.spans[i];
+				if (!chf.spans[i].reg || (chf.spans[i].reg & RC_BORDER_REG))
+				{
+					flags[i] = 0;
+					continue;
+				}
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					unsigned short r = 0;
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						r = chf.spans[ai].reg;
+					}
+					if (r == chf.spans[i].reg)
+						res |= (1 << dir);
+				}
+				flags[i] = res ^ 0xf; // Inverse, mark non connected edges.
+			}
+		}
+	}
+	
+	ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
+	
+	rcIntArray verts(256);
+	rcIntArray simplified(64);
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (flags[i] == 0 || flags[i] == 0xf)
+				{
+					flags[i] = 0;
+					continue;
+				}
+				const unsigned short reg = chf.spans[i].reg;
+				if (!reg || (reg & RC_BORDER_REG))
+					continue;
+				const unsigned char area = chf.areas[i];
+				
+				verts.resize(0);
+				simplified.resize(0);
+				
+				ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
+				walkContour(x, y, i, chf, flags, verts);
+				ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
+				
+				ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
+				simplifyContour(verts, simplified, maxError, maxEdgeLen, buildFlags);
+				removeDegenerateSegments(simplified);
+				ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
+				
+				
+				// Store region->contour remap info.
+				// Create contour.
+				if (simplified.size()/4 >= 3)
+				{
+					if (cset.nconts >= maxContours)
+					{
+						// Allocate more contours.
+						// This happens when a region has holes.
+						const int oldMax = maxContours;
+						maxContours *= 2;
+						rcContour* newConts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM);
+						for (int j = 0; j < cset.nconts; ++j)
+						{
+							newConts[j] = cset.conts[j];
+							// Reset source pointers to prevent data deletion.
+							cset.conts[j].verts = 0;
+							cset.conts[j].rverts = 0;
+						}
+						rcFree(cset.conts);
+						cset.conts = newConts;
+						
+						ctx->log(RC_LOG_WARNING, "rcBuildContours: Expanding max contours from %d to %d.", oldMax, maxContours);
+					}
+					
+					rcContour* cont = &cset.conts[cset.nconts++];
+					
+					cont->nverts = simplified.size()/4;
+					cont->verts = (int*)rcAlloc(sizeof(int)*cont->nverts*4, RC_ALLOC_PERM);
+					if (!cont->verts)
+					{
+						ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'verts' (%d).", cont->nverts);
+						return false;
+					}
+					memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4);
+					if (borderSize > 0)
+					{
+						// If the heightfield was build with bordersize, remove the offset.
+						for (int j = 0; j < cont->nverts; ++j)
+						{
+							int* v = &cont->verts[j*4];
+							v[0] -= borderSize;
+							v[2] -= borderSize;
+						}
+					}
+					
+					cont->nrverts = verts.size()/4;
+					cont->rverts = (int*)rcAlloc(sizeof(int)*cont->nrverts*4, RC_ALLOC_PERM);
+					if (!cont->rverts)
+					{
+						ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'rverts' (%d).", cont->nrverts);
+						return false;
+					}
+					memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4);
+					if (borderSize > 0)
+					{
+						// If the heightfield was build with bordersize, remove the offset.
+						for (int j = 0; j < cont->nrverts; ++j)
+						{
+							int* v = &cont->rverts[j*4];
+							v[0] -= borderSize;
+							v[2] -= borderSize;
+						}
+					}
+					
+					cont->reg = reg;
+					cont->area = area;
+				}
+			}
+		}
+	}
+	
+	// Merge holes if needed.
+	if (cset.nconts > 0)
+	{
+		// Calculate winding of all polygons.
+		rcScopedDelete<signed char> winding((signed char*)rcAlloc(sizeof(signed char)*cset.nconts, RC_ALLOC_TEMP));
+		if (!winding)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'hole' (%d).", cset.nconts);
+			return false;
+		}
+		int nholes = 0;
+		for (int i = 0; i < cset.nconts; ++i)
+		{
+			rcContour& cont = cset.conts[i];
+			// If the contour is wound backwards, it is a hole.
+			winding[i] = calcAreaOfPolygon2D(cont.verts, cont.nverts) < 0 ? -1 : 1;
+			if (winding[i] < 0)
+				nholes++;
+		}
+		
+		if (nholes > 0)
+		{
+			// Collect outline contour and holes contours per region.
+			// We assume that there is one outline and multiple holes.
+			const int nregions = chf.maxRegions+1;
+			rcScopedDelete<rcContourRegion> regions((rcContourRegion*)rcAlloc(sizeof(rcContourRegion)*nregions, RC_ALLOC_TEMP));
+			if (!regions)
+			{
+				ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'regions' (%d).", nregions);
+				return false;
+			}
+			memset(regions, 0, sizeof(rcContourRegion)*nregions);
+			
+			rcScopedDelete<rcContourHole> holes((rcContourHole*)rcAlloc(sizeof(rcContourHole)*cset.nconts, RC_ALLOC_TEMP));
+			if (!holes)
+			{
+				ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'holes' (%d).", cset.nconts);
+				return false;
+			}
+			memset(holes, 0, sizeof(rcContourHole)*cset.nconts);
+			
+			for (int i = 0; i < cset.nconts; ++i)
+			{
+				rcContour& cont = cset.conts[i];
+				// Positively would contours are outlines, negative holes.
+				if (winding[i] > 0)
+				{
+					if (regions[cont.reg].outline)
+						ctx->log(RC_LOG_ERROR, "rcBuildContours: Multiple outlines for region %d.", cont.reg);
+					regions[cont.reg].outline = &cont;
+				}
+				else
+				{
+					regions[cont.reg].nholes++;
+				}
+			}
+			int index = 0;
+			for (int i = 0; i < nregions; i++)
+			{
+				if (regions[i].nholes > 0)
+				{
+					regions[i].holes = &holes[index];
+					index += regions[i].nholes;
+					regions[i].nholes = 0;
+				}
+			}
+			for (int i = 0; i < cset.nconts; ++i)
+			{
+				rcContour& cont = cset.conts[i];
+				rcContourRegion& reg = regions[cont.reg];
+				if (winding[i] < 0)
+					reg.holes[reg.nholes++].contour = &cont;
+			}
+			
+			// Finally merge each regions holes into the outline.
+			for (int i = 0; i < nregions; i++)
+			{
+				rcContourRegion& reg = regions[i];
+				if (!reg.nholes) continue;
+				
+				if (reg.outline)
+				{
+					mergeRegionHoles(ctx, reg);
+				}
+				else
+				{
+					// The region does not have an outline.
+					// This can happen if the contour becaomes selfoverlapping because of
+					// too aggressive simplification settings.
+					ctx->log(RC_LOG_ERROR, "rcBuildContours: Bad outline for region %d, contour simplification is likely too aggressive.", i);
+				}
+			}
+		}
+		
+	}
+	
+	return true;
+}

panda/src/recastdetour/RecastDebugDraw.cpp

@@ -0,0 +1,1064 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include "DebugDraw.h"
+#include "RecastDebugDraw.h"
+#include "Recast.h"
+
+void duDebugDrawTriMesh(duDebugDraw* dd, const float* verts, int /*nverts*/,
+						const int* tris, const float* normals, int ntris,
+						const unsigned char* flags, const float texScale)
+{
+	if (!dd) return;
+	if (!verts) return;
+	if (!tris) return;
+	if (!normals) return;
+
+	float uva[2];
+	float uvb[2];
+	float uvc[2];
+
+	const unsigned int unwalkable = duRGBA(192,128,0,255);
+
+	dd->texture(true);
+
+	dd->begin(DU_DRAW_TRIS);
+	for (int i = 0; i < ntris*3; i += 3)
+	{
+		const float* norm = &normals[i];
+		unsigned int color;
+		unsigned char a = (unsigned char)(220*(2+norm[0]+norm[1])/4);
+		if (flags && !flags[i/3])
+			color = duLerpCol(duRGBA(a,a,a,255), unwalkable, 64);
+		else
+			color = duRGBA(a,a,a,255);
+
+		const float* va = &verts[tris[i+0]*3];
+		const float* vb = &verts[tris[i+1]*3];
+		const float* vc = &verts[tris[i+2]*3];
+		
+		int ax = 0, ay = 0;
+		if (rcAbs(norm[1]) > rcAbs(norm[ax]))
+			ax = 1;
+		if (rcAbs(norm[2]) > rcAbs(norm[ax]))
+			ax = 2;
+		ax = (1<<ax)&3; // +1 mod 3
+		ay = (1<<ax)&3; // +1 mod 3
+
+		uva[0] = va[ax]*texScale;
+		uva[1] = va[ay]*texScale;
+		uvb[0] = vb[ax]*texScale;
+		uvb[1] = vb[ay]*texScale;
+		uvc[0] = vc[ax]*texScale;
+		uvc[1] = vc[ay]*texScale;
+		
+		dd->vertex(va, color, uva);
+		dd->vertex(vb, color, uvb);
+		dd->vertex(vc, color, uvc);
+	}
+	dd->end();
+	dd->texture(false);
+}
+
+void duDebugDrawTriMeshSlope(duDebugDraw* dd, const float* verts, int /*nverts*/,
+							 const int* tris, const float* normals, int ntris,
+							 const float walkableSlopeAngle, const float texScale)
+{
+	if (!dd) return;
+	if (!verts) return;
+	if (!tris) return;
+	if (!normals) return;
+	
+	const float walkableThr = cosf(walkableSlopeAngle/180.0f*DU_PI);
+	
+	float uva[2];
+	float uvb[2];
+	float uvc[2];
+	
+	dd->texture(true);
+
+	const unsigned int unwalkable = duRGBA(192,128,0,255);
+	
+	dd->begin(DU_DRAW_TRIS);
+	for (int i = 0; i < ntris*3; i += 3)
+	{
+		const float* norm = &normals[i];
+		unsigned int color;
+		unsigned char a = (unsigned char)(220*(2+norm[0]+norm[1])/4);
+		if (norm[1] < walkableThr)
+			color = duLerpCol(duRGBA(a,a,a,255), unwalkable, 64);
+		else
+			color = duRGBA(a,a,a,255);
+		
+		const float* va = &verts[tris[i+0]*3];
+		const float* vb = &verts[tris[i+1]*3];
+		const float* vc = &verts[tris[i+2]*3];
+		
+		int ax = 0, ay = 0;
+		if (rcAbs(norm[1]) > rcAbs(norm[ax]))
+			ax = 1;
+		if (rcAbs(norm[2]) > rcAbs(norm[ax]))
+			ax = 2;
+		ax = (1<<ax)&3; // +1 mod 3
+		ay = (1<<ax)&3; // +1 mod 3
+		
+		uva[0] = va[ax]*texScale;
+		uva[1] = va[ay]*texScale;
+		uvb[0] = vb[ax]*texScale;
+		uvb[1] = vb[ay]*texScale;
+		uvc[0] = vc[ax]*texScale;
+		uvc[1] = vc[ay]*texScale;
+		
+		dd->vertex(va, color, uva);
+		dd->vertex(vb, color, uvb);
+		dd->vertex(vc, color, uvc);
+	}
+	dd->end();
+
+	dd->texture(false);
+}
+
+void duDebugDrawHeightfieldSolid(duDebugDraw* dd, const rcHeightfield& hf)
+{
+	if (!dd) return;
+
+	const float* orig = hf.bmin;
+	const float cs = hf.cs;
+	const float ch = hf.ch;
+	
+	const int w = hf.width;
+	const int h = hf.height;
+		
+	unsigned int fcol[6];
+	duCalcBoxColors(fcol, duRGBA(255,255,255,255), duRGBA(255,255,255,255));
+	
+	dd->begin(DU_DRAW_QUADS);
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			float fx = orig[0] + x*cs;
+			float fz = orig[2] + y*cs;
+			const rcSpan* s = hf.spans[x + y*w];
+			while (s)
+			{
+				duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol);
+				s = s->next;
+			}
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawHeightfieldWalkable(duDebugDraw* dd, const rcHeightfield& hf)
+{
+	if (!dd) return;
+
+	const float* orig = hf.bmin;
+	const float cs = hf.cs;
+	const float ch = hf.ch;
+	
+	const int w = hf.width;
+	const int h = hf.height;
+	
+	unsigned int fcol[6];
+	duCalcBoxColors(fcol, duRGBA(255,255,255,255), duRGBA(217,217,217,255));
+
+	dd->begin(DU_DRAW_QUADS);
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			float fx = orig[0] + x*cs;
+			float fz = orig[2] + y*cs;
+			const rcSpan* s = hf.spans[x + y*w];
+			while (s)
+			{
+				if (s->area == RC_WALKABLE_AREA)
+					fcol[0] = duRGBA(64,128,160,255);
+				else if (s->area == RC_NULL_AREA)
+					fcol[0] = duRGBA(64,64,64,255);
+				else
+					fcol[0] = duMultCol(dd->areaToCol(s->area), 200);
+				
+				duAppendBox(dd, fx, orig[1]+s->smin*ch, fz, fx+cs, orig[1] + s->smax*ch, fz+cs, fcol);
+				s = s->next;
+			}
+		}
+	}
+	
+	dd->end();
+}
+
+void duDebugDrawCompactHeightfieldSolid(duDebugDraw* dd, const rcCompactHeightfield& chf)
+{
+	if (!dd) return;
+
+	const float cs = chf.cs;
+	const float ch = chf.ch;
+
+	dd->begin(DU_DRAW_QUADS);
+	
+	for (int y = 0; y < chf.height; ++y)
+	{
+		for (int x = 0; x < chf.width; ++x)
+		{
+			const float fx = chf.bmin[0] + x*cs;
+			const float fz = chf.bmin[2] + y*cs;
+			const rcCompactCell& c = chf.cells[x+y*chf.width];
+
+			for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+
+				const unsigned char area = chf.areas[i];
+				unsigned int color;
+				if (area == RC_WALKABLE_AREA)
+					color = duRGBA(0,192,255,64);
+				else if (area == RC_NULL_AREA)
+					color = duRGBA(0,0,0,64);
+				else
+					color = dd->areaToCol(area);
+				
+				const float fy = chf.bmin[1] + (s.y+1)*ch;
+				dd->vertex(fx, fy, fz, color);
+				dd->vertex(fx, fy, fz+cs, color);
+				dd->vertex(fx+cs, fy, fz+cs, color);
+				dd->vertex(fx+cs, fy, fz, color);
+			}
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawCompactHeightfieldRegions(duDebugDraw* dd, const rcCompactHeightfield& chf)
+{
+	if (!dd) return;
+
+	const float cs = chf.cs;
+	const float ch = chf.ch;
+
+	dd->begin(DU_DRAW_QUADS);
+
+	for (int y = 0; y < chf.height; ++y)
+	{
+		for (int x = 0; x < chf.width; ++x)
+		{
+			const float fx = chf.bmin[0] + x*cs;
+			const float fz = chf.bmin[2] + y*cs;
+			const rcCompactCell& c = chf.cells[x+y*chf.width];
+			
+			for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const float fy = chf.bmin[1] + (s.y)*ch;
+				unsigned int color;
+				if (s.reg)
+					color = duIntToCol(s.reg, 192);
+				else
+					color = duRGBA(0,0,0,64);
+
+				dd->vertex(fx, fy, fz, color);
+				dd->vertex(fx, fy, fz+cs, color);
+				dd->vertex(fx+cs, fy, fz+cs, color);
+				dd->vertex(fx+cs, fy, fz, color);
+			}
+		}
+	}
+	
+	dd->end();
+}
+
+
+void duDebugDrawCompactHeightfieldDistance(duDebugDraw* dd, const rcCompactHeightfield& chf)
+{
+	if (!dd) return;
+	if (!chf.dist) return;
+		
+	const float cs = chf.cs;
+	const float ch = chf.ch;
+			
+	float maxd = chf.maxDistance;
+	if (maxd < 1.0f) maxd = 1;
+	const float dscale = 255.0f / maxd;
+	
+	dd->begin(DU_DRAW_QUADS);
+	
+	for (int y = 0; y < chf.height; ++y)
+	{
+		for (int x = 0; x < chf.width; ++x)
+		{
+			const float fx = chf.bmin[0] + x*cs;
+			const float fz = chf.bmin[2] + y*cs;
+			const rcCompactCell& c = chf.cells[x+y*chf.width];
+			
+			for (unsigned i = c.index, ni = c.index+c.count; i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const float fy = chf.bmin[1] + (s.y+1)*ch;
+				const unsigned char cd = (unsigned char)(chf.dist[i] * dscale);
+				const unsigned int color = duRGBA(cd,cd,cd,255);
+				dd->vertex(fx, fy, fz, color);
+				dd->vertex(fx, fy, fz+cs, color);
+				dd->vertex(fx+cs, fy, fz+cs, color);
+				dd->vertex(fx+cs, fy, fz, color);
+			}
+		}
+	}
+	dd->end();
+}
+
+static void drawLayerPortals(duDebugDraw* dd, const rcHeightfieldLayer* layer)
+{
+	const float cs = layer->cs;
+	const float ch = layer->ch;
+	const int w = layer->width;
+	const int h = layer->height;
+	
+	unsigned int pcol = duRGBA(255,255,255,255);
+	
+	const int segs[4*4] = {0,0,0,1, 0,1,1,1, 1,1,1,0, 1,0,0,0};
+	
+	// Layer portals
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int idx = x+y*w;
+			const int lh = (int)layer->heights[idx];
+			if (lh == 255) continue;
+			
+			for (int dir = 0; dir < 4; ++dir)
+			{
+				if (layer->cons[idx] & (1<<(dir+4)))
+				{
+					const int* seg = &segs[dir*4];
+					const float ax = layer->bmin[0] + (x+seg[0])*cs;
+					const float ay = layer->bmin[1] + (lh+2)*ch;
+					const float az = layer->bmin[2] + (y+seg[1])*cs;
+					const float bx = layer->bmin[0] + (x+seg[2])*cs;
+					const float by = layer->bmin[1] + (lh+2)*ch;
+					const float bz = layer->bmin[2] + (y+seg[3])*cs;
+					dd->vertex(ax, ay, az, pcol);
+					dd->vertex(bx, by, bz, pcol);
+				}
+			}
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLayer& layer, const int idx)
+{
+	const float cs = layer.cs;
+	const float ch = layer.ch;
+	const int w = layer.width;
+	const int h = layer.height;
+	
+	unsigned int color = duIntToCol(idx+1, 255);
+	
+	// Layer bounds
+	float bmin[3], bmax[3];
+	bmin[0] = layer.bmin[0] + layer.minx*cs;
+	bmin[1] = layer.bmin[1];
+	bmin[2] = layer.bmin[2] + layer.miny*cs;
+	bmax[0] = layer.bmin[0] + (layer.maxx+1)*cs;
+	bmax[1] = layer.bmax[1];
+	bmax[2] = layer.bmin[2] + (layer.maxy+1)*cs;
+	duDebugDrawBoxWire(dd, bmin[0],bmin[1],bmin[2], bmax[0],bmax[1],bmax[2], duTransCol(color,128), 2.0f);
+	
+	// Layer height
+	dd->begin(DU_DRAW_QUADS);
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const int lidx = x+y*w;
+			const int lh = (int)layer.heights[lidx];
+			if (h == 0xff) continue;
+			const unsigned char area = layer.areas[lidx];
+			
+			unsigned int col;
+			if (area == RC_WALKABLE_AREA)
+				col = duLerpCol(color, duRGBA(0,192,255,64), 32);
+			else if (area == RC_NULL_AREA)
+				col = duLerpCol(color, duRGBA(0,0,0,64), 32);
+			else
+				col = duLerpCol(color, dd->areaToCol(area), 32);
+			
+			const float fx = layer.bmin[0] + x*cs;
+			const float fy = layer.bmin[1] + (lh+1)*ch;
+			const float fz = layer.bmin[2] + y*cs;
+			
+			dd->vertex(fx, fy, fz, col);
+			dd->vertex(fx, fy, fz+cs, col);
+			dd->vertex(fx+cs, fy, fz+cs, col);
+			dd->vertex(fx+cs, fy, fz, col);
+		}
+	}
+	dd->end();
+	
+	// Portals
+	drawLayerPortals(dd, &layer);
+}
+
+void duDebugDrawHeightfieldLayers(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset)
+{
+	if (!dd) return;
+	for (int i = 0; i < lset.nlayers; ++i)
+		duDebugDrawHeightfieldLayer(dd, lset.layers[i], i);
+}
+
+/*
+void duDebugDrawLayerContours(duDebugDraw* dd, const struct rcLayerContourSet& lcset)
+{
+	if (!dd) return;
+	
+	const float* orig = lcset.bmin;
+	const float cs = lcset.cs;
+	const float ch = lcset.ch;
+	
+	const unsigned char a = 255;// (unsigned char)(alpha*255.0f);
+	
+	const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1};
+
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		const rcLayerContour& c = lcset.conts[i];
+		unsigned int color = 0;
+
+		color = duIntToCol(i, a);
+
+		for (int j = 0; j < c.nverts; ++j)
+		{
+			const int k = (j+1) % c.nverts;
+			const unsigned char* va = &c.verts[j*4];
+			const unsigned char* vb = &c.verts[k*4];
+			const float ax = orig[0] + va[0]*cs;
+			const float ay = orig[1] + (va[1]+1+(i&1))*ch;
+			const float az = orig[2] + va[2]*cs;
+			const float bx = orig[0] + vb[0]*cs;
+			const float by = orig[1] + (vb[1]+1+(i&1))*ch;
+			const float bz = orig[2] + vb[2]*cs;
+			unsigned int col = color;
+			if ((va[3] & 0xf) != 0xf)
+			{
+				col = duRGBA(255,255,255,128);
+				int d = va[3] & 0xf;
+				
+				const float cx = (ax+bx)*0.5f;
+				const float cy = (ay+by)*0.5f;
+				const float cz = (az+bz)*0.5f;
+				
+				const float dx = cx + offs[d*2+0]*2*cs;
+				const float dy = cy;
+				const float dz = cz + offs[d*2+1]*2*cs;
+				
+				dd->vertex(cx,cy,cz,duRGBA(255,0,0,255));
+				dd->vertex(dx,dy,dz,duRGBA(255,0,0,255));
+			}
+			
+			duAppendArrow(dd, ax,ay,az, bx,by,bz, 0.0f, cs*0.5f, col);
+		}
+	}
+	dd->end();
+	
+	dd->begin(DU_DRAW_POINTS, 4.0f);	
+	
+	for (int i = 0; i < lcset.nconts; ++i)
+	{
+		const rcLayerContour& c = lcset.conts[i];
+		unsigned int color = 0;
+		
+		for (int j = 0; j < c.nverts; ++j)
+		{
+			const unsigned char* va = &c.verts[j*4];
+
+			color = duDarkenCol(duIntToCol(i, a));
+			if (va[3] & 0x80)
+				color = duRGBA(255,0,0,255);
+
+			float fx = orig[0] + va[0]*cs;
+			float fy = orig[1] + (va[1]+1+(i&1))*ch;
+			float fz = orig[2] + va[2]*cs;
+			dd->vertex(fx,fy,fz, color);
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawLayerPolyMesh(duDebugDraw* dd, const struct rcLayerPolyMesh& lmesh)
+{
+	if (!dd) return;
+	
+	const int nvp = lmesh.nvp;
+	const float cs = lmesh.cs;
+	const float ch = lmesh.ch;
+	const float* orig = lmesh.bmin;
+	
+	const int offs[2*4] = {-1,0, 0,1, 1,0, 0,-1};
+
+	dd->begin(DU_DRAW_TRIS);
+	
+	for (int i = 0; i < lmesh.npolys; ++i)
+	{
+		const unsigned short* p = &lmesh.polys[i*nvp*2];
+		
+		unsigned int color;
+		if (lmesh.areas[i] == RC_WALKABLE_AREA)
+			color = duRGBA(0,192,255,64);
+		else if (lmesh.areas[i] == RC_NULL_AREA)
+			color = duRGBA(0,0,0,64);
+		else
+			color = duIntToCol(lmesh.areas[i], 255);
+		
+		unsigned short vi[3];
+		for (int j = 2; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			vi[0] = p[0];
+			vi[1] = p[j-1];
+			vi[2] = p[j];
+			for (int k = 0; k < 3; ++k)
+			{
+				const unsigned short* v = &lmesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x,y,z, color);
+			}
+		}
+	}
+	dd->end();
+	
+	// Draw neighbours edges
+	const unsigned int coln = duRGBA(0,48,64,32);
+	dd->begin(DU_DRAW_LINES, 1.5f);
+	for (int i = 0; i < lmesh.npolys; ++i)
+	{
+		const unsigned short* p = &lmesh.polys[i*nvp*2];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			if (p[nvp+j] & 0x8000) continue;
+			const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; 
+			int vi[2] = {p[j], p[nj]};
+			
+			for (int k = 0; k < 2; ++k)
+			{
+				const unsigned short* v = &lmesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x, y, z, coln);
+			}
+		}
+	}
+	dd->end();
+	
+	// Draw boundary edges
+	const unsigned int colb = duRGBA(0,48,64,220);
+	dd->begin(DU_DRAW_LINES, 2.5f);
+	for (int i = 0; i < lmesh.npolys; ++i)
+	{
+		const unsigned short* p = &lmesh.polys[i*nvp*2];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			if ((p[nvp+j] & 0x8000) == 0) continue;
+			const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; 
+			int vi[2] = {p[j], p[nj]};
+			
+			unsigned int col = colb;
+			if ((p[nvp+j] & 0xf) != 0xf)
+			{
+				const unsigned short* va = &lmesh.verts[vi[0]*3];
+				const unsigned short* vb = &lmesh.verts[vi[1]*3];
+
+				const float ax = orig[0] + va[0]*cs;
+				const float ay = orig[1] + (va[1]+1+(i&1))*ch;
+				const float az = orig[2] + va[2]*cs;
+				const float bx = orig[0] + vb[0]*cs;
+				const float by = orig[1] + (vb[1]+1+(i&1))*ch;
+				const float bz = orig[2] + vb[2]*cs;
+				
+				const float cx = (ax+bx)*0.5f;
+				const float cy = (ay+by)*0.5f;
+				const float cz = (az+bz)*0.5f;
+				
+				int d = p[nvp+j] & 0xf;
+				
+				const float dx = cx + offs[d*2+0]*2*cs;
+				const float dy = cy;
+				const float dz = cz + offs[d*2+1]*2*cs;
+				
+				dd->vertex(cx,cy,cz,duRGBA(255,0,0,255));
+				dd->vertex(dx,dy,dz,duRGBA(255,0,0,255));
+				
+				col = duRGBA(255,255,255,128);
+			}
+							 
+			for (int k = 0; k < 2; ++k)
+			{
+				const unsigned short* v = &lmesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x, y, z, col);
+			}
+		}
+	}
+	dd->end();
+	
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	const unsigned int colv = duRGBA(0,0,0,220);
+	for (int i = 0; i < lmesh.nverts; ++i)
+	{
+		const unsigned short* v = &lmesh.verts[i*3];
+		const float x = orig[0] + v[0]*cs;
+		const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+		const float z = orig[2] + v[2]*cs;
+		dd->vertex(x,y,z, colv);
+	}
+	dd->end();
+}
+*/
+
+static void getContourCenter(const rcContour* cont, const float* orig, float cs, float ch, float* center)
+{
+	center[0] = 0;
+	center[1] = 0;
+	center[2] = 0;
+	if (!cont->nverts)
+		return;
+	for (int i = 0; i < cont->nverts; ++i)
+	{
+		const int* v = &cont->verts[i*4];
+		center[0] += (float)v[0];
+		center[1] += (float)v[1];
+		center[2] += (float)v[2];
+	}
+	const float s = 1.0f / cont->nverts;
+	center[0] *= s * cs;
+	center[1] *= s * ch;
+	center[2] *= s * cs;
+	center[0] += orig[0];
+	center[1] += orig[1] + 4*ch;
+	center[2] += orig[2];
+}
+
+static const rcContour* findContourFromSet(const rcContourSet& cset, unsigned short reg)
+{
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		if (cset.conts[i].reg == reg)
+			return &cset.conts[i];
+	}
+	return 0;
+}
+
+void duDebugDrawRegionConnections(duDebugDraw* dd, const rcContourSet& cset, const float alpha)
+{
+	if (!dd) return;
+	
+	const float* orig = cset.bmin;
+	const float cs = cset.cs;
+	const float ch = cset.ch;
+	
+	// Draw centers
+	float pos[3], pos2[3];
+
+	unsigned int color = duRGBA(0,0,0,196);
+
+	dd->begin(DU_DRAW_LINES, 2.0f);
+
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour* cont = &cset.conts[i];
+		getContourCenter(cont, orig, cs, ch, pos);
+		for (int j = 0; j < cont->nverts; ++j)
+		{
+			const int* v = &cont->verts[j*4];
+			if (v[3] == 0 || (unsigned short)v[3] < cont->reg) continue;
+			const rcContour* cont2 = findContourFromSet(cset, (unsigned short)v[3]);
+			if (cont2)
+			{
+				getContourCenter(cont2, orig, cs, ch, pos2);
+				duAppendArc(dd, pos[0],pos[1],pos[2], pos2[0],pos2[1],pos2[2], 0.25f, 0.6f, 0.6f, color);
+			}
+		}
+	}
+	
+	dd->end();
+
+	unsigned char a = (unsigned char)(alpha * 255.0f);
+
+	dd->begin(DU_DRAW_POINTS, 7.0f);
+
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour* cont = &cset.conts[i];
+		unsigned int col = duDarkenCol(duIntToCol(cont->reg,a));
+		getContourCenter(cont, orig, cs, ch, pos);
+		dd->vertex(pos, col);
+	}
+	dd->end();
+}
+
+void duDebugDrawRawContours(duDebugDraw* dd, const rcContourSet& cset, const float alpha)
+{
+	if (!dd) return;
+
+	const float* orig = cset.bmin;
+	const float cs = cset.cs;
+	const float ch = cset.ch;
+	
+	const unsigned char a = (unsigned char)(alpha*255.0f);
+	
+	dd->begin(DU_DRAW_LINES, 2.0f);
+			
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour& c = cset.conts[i];
+		unsigned int color = duIntToCol(c.reg, a);
+
+		for (int j = 0; j < c.nrverts; ++j)
+		{
+			const int* v = &c.rverts[j*4];
+			float fx = orig[0] + v[0]*cs;
+			float fy = orig[1] + (v[1]+1+(i&1))*ch;
+			float fz = orig[2] + v[2]*cs;
+			dd->vertex(fx,fy,fz,color);
+			if (j > 0)
+				dd->vertex(fx,fy,fz,color);
+		}
+		// Loop last segment.
+		const int* v = &c.rverts[0];
+		float fx = orig[0] + v[0]*cs;
+		float fy = orig[1] + (v[1]+1+(i&1))*ch;
+		float fz = orig[2] + v[2]*cs;
+		dd->vertex(fx,fy,fz,color);
+	}
+	dd->end();
+
+	dd->begin(DU_DRAW_POINTS, 2.0f);	
+
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour& c = cset.conts[i];
+		unsigned int color = duDarkenCol(duIntToCol(c.reg, a));
+		
+		for (int j = 0; j < c.nrverts; ++j)
+		{
+			const int* v = &c.rverts[j*4];
+			float off = 0;
+			unsigned int colv = color;
+			if (v[3] & RC_BORDER_VERTEX)
+			{
+				colv = duRGBA(255,255,255,a);
+				off = ch*2;
+			}
+			
+			float fx = orig[0] + v[0]*cs;
+			float fy = orig[1] + (v[1]+1+(i&1))*ch + off;
+			float fz = orig[2] + v[2]*cs;
+			dd->vertex(fx,fy,fz, colv);
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawContours(duDebugDraw* dd, const rcContourSet& cset, const float alpha)
+{
+	if (!dd) return;
+
+	const float* orig = cset.bmin;
+	const float cs = cset.cs;
+	const float ch = cset.ch;
+	
+	const unsigned char a = (unsigned char)(alpha*255.0f);
+	
+	dd->begin(DU_DRAW_LINES, 2.5f);
+	
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour& c = cset.conts[i];
+		if (!c.nverts)
+			continue;
+		const unsigned int color = duIntToCol(c.reg, a);
+		const unsigned int bcolor = duLerpCol(color,duRGBA(255,255,255,a),128);
+		for (int j = 0, k = c.nverts-1; j < c.nverts; k=j++)
+		{
+			const int* va = &c.verts[k*4];
+			const int* vb = &c.verts[j*4];
+			unsigned int col = (va[3] & RC_AREA_BORDER) ? bcolor : color; 
+			float fx,fy,fz;
+			fx = orig[0] + va[0]*cs;
+			fy = orig[1] + (va[1]+1+(i&1))*ch;
+			fz = orig[2] + va[2]*cs;
+			dd->vertex(fx,fy,fz, col);
+			fx = orig[0] + vb[0]*cs;
+			fy = orig[1] + (vb[1]+1+(i&1))*ch;
+			fz = orig[2] + vb[2]*cs;
+			dd->vertex(fx,fy,fz, col);
+		}
+	}
+	dd->end();
+
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour& c = cset.conts[i];
+		unsigned int color = duDarkenCol(duIntToCol(c.reg, a));
+		for (int j = 0; j < c.nverts; ++j)
+		{
+			const int* v = &c.verts[j*4];
+			float off = 0;
+			unsigned int colv = color;
+			if (v[3] & RC_BORDER_VERTEX)
+			{
+				colv = duRGBA(255,255,255,a);
+				off = ch*2;
+			}
+
+			float fx = orig[0] + v[0]*cs;
+			float fy = orig[1] + (v[1]+1+(i&1))*ch + off;
+			float fz = orig[2] + v[2]*cs;
+			dd->vertex(fx,fy,fz, colv);
+		}
+	}
+	dd->end();
+}
+
+void duDebugDrawPolyMesh(duDebugDraw* dd, const struct rcPolyMesh& mesh)
+{
+	if (!dd) return;
+
+	const int nvp = mesh.nvp;
+	const float cs = mesh.cs;
+	const float ch = mesh.ch;
+	const float* orig = mesh.bmin;
+	
+	dd->begin(DU_DRAW_TRIS);
+	
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		const unsigned short* p = &mesh.polys[i*nvp*2];
+		const unsigned char area = mesh.areas[i];
+		
+		unsigned int color;
+		if (area == RC_WALKABLE_AREA)
+			color = duRGBA(0,192,255,64);
+		else if (area == RC_NULL_AREA)
+			color = duRGBA(0,0,0,64);
+		else
+			color = dd->areaToCol(area);
+		
+		unsigned short vi[3];
+		for (int j = 2; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			vi[0] = p[0];
+			vi[1] = p[j-1];
+			vi[2] = p[j];
+			for (int k = 0; k < 3; ++k)
+			{
+				const unsigned short* v = &mesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x,y,z, color);
+			}
+		}
+	}
+	dd->end();
+
+	// Draw neighbours edges
+	const unsigned int coln = duRGBA(0,48,64,32);
+	dd->begin(DU_DRAW_LINES, 1.5f);
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		const unsigned short* p = &mesh.polys[i*nvp*2];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			if (p[nvp+j] & 0x8000) continue;
+			const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; 
+			const int vi[2] = {p[j], p[nj]};
+			
+			for (int k = 0; k < 2; ++k)
+			{
+				const unsigned short* v = &mesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x, y, z, coln);
+			}
+		}
+	}
+	dd->end();
+	
+	// Draw boundary edges
+	const unsigned int colb = duRGBA(0,48,64,220);
+	dd->begin(DU_DRAW_LINES, 2.5f);
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		const unsigned short* p = &mesh.polys[i*nvp*2];
+		for (int j = 0; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			if ((p[nvp+j] & 0x8000) == 0) continue;
+			const int nj = (j+1 >= nvp || p[j+1] == RC_MESH_NULL_IDX) ? 0 : j+1; 
+			const int vi[2] = {p[j], p[nj]};
+			
+			unsigned int col = colb;
+			if ((p[nvp+j] & 0xf) != 0xf)
+				col = duRGBA(255,255,255,128);
+			for (int k = 0; k < 2; ++k)
+			{
+				const unsigned short* v = &mesh.verts[vi[k]*3];
+				const float x = orig[0] + v[0]*cs;
+				const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+				const float z = orig[2] + v[2]*cs;
+				dd->vertex(x, y, z, col);
+			}
+		}
+	}
+	dd->end();
+	
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	const unsigned int colv = duRGBA(0,0,0,220);
+	for (int i = 0; i < mesh.nverts; ++i)
+	{
+		const unsigned short* v = &mesh.verts[i*3];
+		const float x = orig[0] + v[0]*cs;
+		const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+		const float z = orig[2] + v[2]*cs;
+		dd->vertex(x,y,z, colv);
+	}
+	dd->end();
+}
+
+void duDebugDrawPolyMeshDetail(duDebugDraw* dd, const struct rcPolyMeshDetail& dmesh)
+{
+	if (!dd) return;
+
+	dd->begin(DU_DRAW_TRIS);
+	
+	for (int i = 0; i < dmesh.nmeshes; ++i)
+	{
+		const unsigned int* m = &dmesh.meshes[i*4];
+		const unsigned int bverts = m[0];
+		const unsigned int btris = m[2];
+		const int ntris = (int)m[3];
+		const float* verts = &dmesh.verts[bverts*3];
+		const unsigned char* tris = &dmesh.tris[btris*4];
+
+		unsigned int color = duIntToCol(i, 192);
+
+		for (int j = 0; j < ntris; ++j)
+		{
+			dd->vertex(&verts[tris[j*4+0]*3], color);
+			dd->vertex(&verts[tris[j*4+1]*3], color);
+			dd->vertex(&verts[tris[j*4+2]*3], color);
+		}
+	}
+	dd->end();
+
+	// Internal edges.
+	dd->begin(DU_DRAW_LINES, 1.0f);
+	const unsigned int coli = duRGBA(0,0,0,64);
+	for (int i = 0; i < dmesh.nmeshes; ++i)
+	{
+		const unsigned int* m = &dmesh.meshes[i*4];
+		const unsigned int bverts = m[0];
+		const unsigned int btris = m[2];
+		const int ntris = (int)m[3];
+		const float* verts = &dmesh.verts[bverts*3];
+		const unsigned char* tris = &dmesh.tris[btris*4];
+		
+		for (int j = 0; j < ntris; ++j)
+		{
+			const unsigned char* t = &tris[j*4];
+			for (int k = 0, kp = 2; k < 3; kp=k++)
+			{
+				unsigned char ef = (t[3] >> (kp*2)) & 0x3;
+				if (ef == 0)
+				{
+					// Internal edge
+					if (t[kp] < t[k])
+					{
+						dd->vertex(&verts[t[kp]*3], coli);
+						dd->vertex(&verts[t[k]*3], coli);
+					}
+				}
+			}
+		}
+	}
+	dd->end();
+	
+	// External edges.
+	dd->begin(DU_DRAW_LINES, 2.0f);
+	const unsigned int cole = duRGBA(0,0,0,64);
+	for (int i = 0; i < dmesh.nmeshes; ++i)
+	{
+		const unsigned int* m = &dmesh.meshes[i*4];
+		const unsigned int bverts = m[0];
+		const unsigned int btris = m[2];
+		const int ntris = (int)m[3];
+		const float* verts = &dmesh.verts[bverts*3];
+		const unsigned char* tris = &dmesh.tris[btris*4];
+		
+		for (int j = 0; j < ntris; ++j)
+		{
+			const unsigned char* t = &tris[j*4];
+			for (int k = 0, kp = 2; k < 3; kp=k++)
+			{
+				unsigned char ef = (t[3] >> (kp*2)) & 0x3;
+				if (ef != 0)
+				{
+					// Ext edge
+					dd->vertex(&verts[t[kp]*3], cole);
+					dd->vertex(&verts[t[k]*3], cole);
+				}
+			}
+		}
+	}
+	dd->end();
+	
+	dd->begin(DU_DRAW_POINTS, 3.0f);
+	const unsigned int colv = duRGBA(0,0,0,64);
+	for (int i = 0; i < dmesh.nmeshes; ++i)
+	{
+		const unsigned int* m = &dmesh.meshes[i*4];
+		const unsigned int bverts = m[0];
+		const int nverts = (int)m[1];
+		const float* verts = &dmesh.verts[bverts*3];
+		for (int j = 0; j < nverts; ++j)
+			dd->vertex(&verts[j*3], colv);
+	}
+	dd->end();
+}

panda/src/recastdetour/RecastDebugDraw.h

@@ -0,0 +1,42 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef RECAST_DEBUGDRAW_H
+#define RECAST_DEBUGDRAW_H
+
+void duDebugDrawTriMesh(struct duDebugDraw* dd, const float* verts, int nverts, const int* tris, const float* normals, int ntris, const unsigned char* flags, const float texScale);
+void duDebugDrawTriMeshSlope(struct duDebugDraw* dd, const float* verts, int nverts, const int* tris, const float* normals, int ntris, const float walkableSlopeAngle, const float texScale);
+
+void duDebugDrawHeightfieldSolid(struct duDebugDraw* dd, const struct rcHeightfield& hf);
+void duDebugDrawHeightfieldWalkable(struct duDebugDraw* dd, const struct rcHeightfield& hf);
+
+void duDebugDrawCompactHeightfieldSolid(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf);
+void duDebugDrawCompactHeightfieldRegions(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf);
+void duDebugDrawCompactHeightfieldDistance(struct duDebugDraw* dd, const struct rcCompactHeightfield& chf);
+
+void duDebugDrawHeightfieldLayer(duDebugDraw* dd, const struct rcHeightfieldLayer& layer, const int idx);
+void duDebugDrawHeightfieldLayers(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset);
+void duDebugDrawHeightfieldLayersRegions(duDebugDraw* dd, const struct rcHeightfieldLayerSet& lset);
+
+void duDebugDrawRegionConnections(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f);
+void duDebugDrawRawContours(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f);
+void duDebugDrawContours(struct duDebugDraw* dd, const struct rcContourSet& cset, const float alpha = 1.0f);
+void duDebugDrawPolyMesh(struct duDebugDraw* dd, const struct rcPolyMesh& mesh);
+void duDebugDrawPolyMeshDetail(struct duDebugDraw* dd, const struct rcPolyMeshDetail& dmesh);
+
+#endif // RECAST_DEBUGDRAW_H

panda/src/recastdetour/RecastDump.cpp

@@ -0,0 +1,451 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastDump.h"
+
+
+duFileIO::~duFileIO()
+{
+	// Empty
+}
+	
+static void ioprintf(duFileIO* io, const char* format, ...)
+{
+	char line[256];
+	va_list ap;
+	va_start(ap, format);
+	const int n = vsnprintf(line, sizeof(line), format, ap);
+	va_end(ap);
+	if (n > 0)
+		io->write(line, sizeof(char)*n);
+}
+
+bool duDumpPolyMeshToObj(rcPolyMesh& pmesh, duFileIO* io)
+{
+	if (!io)
+	{
+		printf("duDumpPolyMeshToObj: input IO is null.\n"); 
+		return false;
+	}
+	if (!io->isWriting())
+	{
+		printf("duDumpPolyMeshToObj: input IO not writing.\n"); 
+		return false;
+	}
+	
+	const int nvp = pmesh.nvp;
+	const float cs = pmesh.cs;
+	const float ch = pmesh.ch;
+	const float* orig = pmesh.bmin;
+	
+	ioprintf(io, "# Recast Navmesh\n");
+	ioprintf(io, "o NavMesh\n");
+
+	ioprintf(io, "\n");
+	
+	for (int i = 0; i < pmesh.nverts; ++i)
+	{
+		const unsigned short* v = &pmesh.verts[i*3];
+		const float x = orig[0] + v[0]*cs;
+		const float y = orig[1] + (v[1]+1)*ch + 0.1f;
+		const float z = orig[2] + v[2]*cs;
+		ioprintf(io, "v %f %f %f\n", x,y,z);
+	}
+
+	ioprintf(io, "\n");
+
+	for (int i = 0; i < pmesh.npolys; ++i)
+	{
+		const unsigned short* p = &pmesh.polys[i*nvp*2];
+		for (int j = 2; j < nvp; ++j)
+		{
+			if (p[j] == RC_MESH_NULL_IDX) break;
+			ioprintf(io, "f %d %d %d\n", p[0]+1, p[j-1]+1, p[j]+1); 
+		}
+	}
+	
+	return true;
+}
+
+bool duDumpPolyMeshDetailToObj(rcPolyMeshDetail& dmesh, duFileIO* io)
+{
+	if (!io)
+	{
+		printf("duDumpPolyMeshDetailToObj: input IO is null.\n"); 
+		return false;
+	}
+	if (!io->isWriting())
+	{
+		printf("duDumpPolyMeshDetailToObj: input IO not writing.\n"); 
+		return false;
+	}
+	
+	ioprintf(io, "# Recast Navmesh\n");
+	ioprintf(io, "o NavMesh\n");
+	
+	ioprintf(io, "\n");
+
+	for (int i = 0; i < dmesh.nverts; ++i)
+	{
+		const float* v = &dmesh.verts[i*3];
+		ioprintf(io, "v %f %f %f\n", v[0],v[1],v[2]);
+	}
+	
+	ioprintf(io, "\n");
+	
+	for (int i = 0; i < dmesh.nmeshes; ++i)
+	{
+		const unsigned int* m = &dmesh.meshes[i*4];
+		const unsigned int bverts = m[0];
+		const unsigned int btris = m[2];
+		const unsigned int ntris = m[3];
+		const unsigned char* tris = &dmesh.tris[btris*4];
+		for (unsigned int j = 0; j < ntris; ++j)
+		{
+			ioprintf(io, "f %d %d %d\n",
+					(int)(bverts+tris[j*4+0])+1,
+					(int)(bverts+tris[j*4+1])+1,
+					(int)(bverts+tris[j*4+2])+1);
+		}
+	}
+	
+	return true;
+}
+
+static const int CSET_MAGIC = ('c' << 24) | ('s' << 16) | ('e' << 8) | 't';
+static const int CSET_VERSION = 2;
+
+bool duDumpContourSet(struct rcContourSet& cset, duFileIO* io)
+{
+	if (!io)
+	{
+		printf("duDumpContourSet: input IO is null.\n"); 
+		return false;
+	}
+	if (!io->isWriting())
+	{
+		printf("duDumpContourSet: input IO not writing.\n"); 
+		return false;
+	}
+	
+	io->write(&CSET_MAGIC, sizeof(CSET_MAGIC));
+	io->write(&CSET_VERSION, sizeof(CSET_VERSION));
+
+	io->write(&cset.nconts, sizeof(cset.nconts));
+	
+	io->write(cset.bmin, sizeof(cset.bmin));
+	io->write(cset.bmax, sizeof(cset.bmax));
+	
+	io->write(&cset.cs, sizeof(cset.cs));
+	io->write(&cset.ch, sizeof(cset.ch));
+
+	io->write(&cset.width, sizeof(cset.width));
+	io->write(&cset.height, sizeof(cset.height));
+	io->write(&cset.borderSize, sizeof(cset.borderSize));
+
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		const rcContour& cont = cset.conts[i];
+		io->write(&cont.nverts, sizeof(cont.nverts));
+		io->write(&cont.nrverts, sizeof(cont.nrverts));
+		io->write(&cont.reg, sizeof(cont.reg));
+		io->write(&cont.area, sizeof(cont.area));
+		io->write(cont.verts, sizeof(int)*4*cont.nverts);
+		io->write(cont.rverts, sizeof(int)*4*cont.nrverts);
+	}
+
+	return true;
+}
+
+bool duReadContourSet(struct rcContourSet& cset, duFileIO* io)
+{
+	if (!io)
+	{
+		printf("duReadContourSet: input IO is null.\n"); 
+		return false;
+	}
+	if (!io->isReading())
+	{
+		printf("duReadContourSet: input IO not reading.\n"); 
+		return false;
+	}
+	
+	int magic = 0;
+	int version = 0;
+	
+	io->read(&magic, sizeof(magic));
+	io->read(&version, sizeof(version));
+	
+	if (magic != CSET_MAGIC)
+	{
+		printf("duReadContourSet: Bad voodoo.\n");
+		return false;
+	}
+	if (version != CSET_VERSION)
+	{
+		printf("duReadContourSet: Bad version.\n");
+		return false;
+	}
+	
+	io->read(&cset.nconts, sizeof(cset.nconts));
+
+	cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*cset.nconts, RC_ALLOC_PERM);
+	if (!cset.conts)
+	{
+		printf("duReadContourSet: Could not alloc contours (%d)\n", cset.nconts);
+		return false;
+	}
+	memset(cset.conts, 0, sizeof(rcContour)*cset.nconts);
+	
+	io->read(cset.bmin, sizeof(cset.bmin));
+	io->read(cset.bmax, sizeof(cset.bmax));
+	
+	io->read(&cset.cs, sizeof(cset.cs));
+	io->read(&cset.ch, sizeof(cset.ch));
+	
+	io->read(&cset.width, sizeof(cset.width));
+	io->read(&cset.height, sizeof(cset.height));
+	io->read(&cset.borderSize, sizeof(cset.borderSize));
+	
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		rcContour& cont = cset.conts[i];
+		io->read(&cont.nverts, sizeof(cont.nverts));
+		io->read(&cont.nrverts, sizeof(cont.nrverts));
+		io->read(&cont.reg, sizeof(cont.reg));
+		io->read(&cont.area, sizeof(cont.area));
+
+		cont.verts = (int*)rcAlloc(sizeof(int)*4*cont.nverts, RC_ALLOC_PERM);
+		if (!cont.verts)
+		{
+			printf("duReadContourSet: Could not alloc contour verts (%d)\n", cont.nverts);
+			return false;
+		}
+		cont.rverts = (int*)rcAlloc(sizeof(int)*4*cont.nrverts, RC_ALLOC_PERM);
+		if (!cont.rverts)
+		{
+			printf("duReadContourSet: Could not alloc contour rverts (%d)\n", cont.nrverts);
+			return false;
+		}
+		
+		io->read(cont.verts, sizeof(int)*4*cont.nverts);
+		io->read(cont.rverts, sizeof(int)*4*cont.nrverts);
+	}
+	
+	return true;
+}
+	
+
+static const int CHF_MAGIC = ('r' << 24) | ('c' << 16) | ('h' << 8) | 'f';
+static const int CHF_VERSION = 3;
+
+bool duDumpCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io)
+{
+	if (!io)
+	{
+		printf("duDumpCompactHeightfield: input IO is null.\n"); 
+		return false;
+	}
+	if (!io->isWriting())
+	{
+		printf("duDumpCompactHeightfield: input IO not writing.\n"); 
+		return false;
+	}
+	
+	io->write(&CHF_MAGIC, sizeof(CHF_MAGIC));
+	io->write(&CHF_VERSION, sizeof(CHF_VERSION));
+	
+	io->write(&chf.width, sizeof(chf.width));
+	io->write(&chf.height, sizeof(chf.height));
+	io->write(&chf.spanCount, sizeof(chf.spanCount));
+
+	io->write(&chf.walkableHeight, sizeof(chf.walkableHeight));
+	io->write(&chf.walkableClimb, sizeof(chf.walkableClimb));
+	io->write(&chf.borderSize, sizeof(chf.borderSize));
+
+	io->write(&chf.maxDistance, sizeof(chf.maxDistance));
+	io->write(&chf.maxRegions, sizeof(chf.maxRegions));
+
+	io->write(chf.bmin, sizeof(chf.bmin));
+	io->write(chf.bmax, sizeof(chf.bmax));
+
+	io->write(&chf.cs, sizeof(chf.cs));
+	io->write(&chf.ch, sizeof(chf.ch));
+
+	int tmp = 0;
+	if (chf.cells) tmp |= 1;
+	if (chf.spans) tmp |= 2;
+	if (chf.dist) tmp |= 4;
+	if (chf.areas) tmp |= 8;
+
+	io->write(&tmp, sizeof(tmp));
+
+	if (chf.cells)
+		io->write(chf.cells, sizeof(rcCompactCell)*chf.width*chf.height);
+	if (chf.spans)
+		io->write(chf.spans, sizeof(rcCompactSpan)*chf.spanCount);
+	if (chf.dist)
+		io->write(chf.dist, sizeof(unsigned short)*chf.spanCount);
+	if (chf.areas)
+		io->write(chf.areas, sizeof(unsigned char)*chf.spanCount);
+
+	return true;
+}
+
+bool duReadCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io)
+{
+	if (!io)
+	{
+		printf("duReadCompactHeightfield: input IO is null.\n"); 
+		return false;
+	}
+	if (!io->isReading())
+	{
+		printf("duReadCompactHeightfield: input IO not reading.\n"); 
+		return false;
+	}
+
+	int magic = 0;
+	int version = 0;
+	
+	io->read(&magic, sizeof(magic));
+	io->read(&version, sizeof(version));
+	
+	if (magic != CHF_MAGIC)
+	{
+		printf("duReadCompactHeightfield: Bad voodoo.\n");
+		return false;
+	}
+	if (version != CHF_VERSION)
+	{
+		printf("duReadCompactHeightfield: Bad version.\n");
+		return false;
+	}
+	
+	io->read(&chf.width, sizeof(chf.width));
+	io->read(&chf.height, sizeof(chf.height));
+	io->read(&chf.spanCount, sizeof(chf.spanCount));
+	
+	io->read(&chf.walkableHeight, sizeof(chf.walkableHeight));
+	io->read(&chf.walkableClimb, sizeof(chf.walkableClimb));
+	io->read(&chf.borderSize, sizeof(chf.borderSize));
+
+	io->read(&chf.maxDistance, sizeof(chf.maxDistance));
+	io->read(&chf.maxRegions, sizeof(chf.maxRegions));
+	
+	io->read(chf.bmin, sizeof(chf.bmin));
+	io->read(chf.bmax, sizeof(chf.bmax));
+	
+	io->read(&chf.cs, sizeof(chf.cs));
+	io->read(&chf.ch, sizeof(chf.ch));
+	
+	int tmp = 0;
+	io->read(&tmp, sizeof(tmp));
+	
+	if (tmp & 1)
+	{
+		chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*chf.width*chf.height, RC_ALLOC_PERM);
+		if (!chf.cells)
+		{
+			printf("duReadCompactHeightfield: Could not alloc cells (%d)\n", chf.width*chf.height);
+			return false;
+		}
+		io->read(chf.cells, sizeof(rcCompactCell)*chf.width*chf.height);
+	}
+	if (tmp & 2)
+	{
+		chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*chf.spanCount, RC_ALLOC_PERM);
+		if (!chf.spans)
+		{
+			printf("duReadCompactHeightfield: Could not alloc spans (%d)\n", chf.spanCount);
+			return false;
+		}
+		io->read(chf.spans, sizeof(rcCompactSpan)*chf.spanCount);
+	}
+	if (tmp & 4)
+	{
+		chf.dist = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_PERM);
+		if (!chf.dist)
+		{
+			printf("duReadCompactHeightfield: Could not alloc dist (%d)\n", chf.spanCount);
+			return false;
+		}
+		io->read(chf.dist, sizeof(unsigned short)*chf.spanCount);
+	}
+	if (tmp & 8)
+	{
+		chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_PERM);
+		if (!chf.areas)
+		{
+			printf("duReadCompactHeightfield: Could not alloc areas (%d)\n", chf.spanCount);
+			return false;
+		}
+		io->read(chf.areas, sizeof(unsigned char)*chf.spanCount);
+	}
+	
+	return true;
+}
+
+
+static void logLine(rcContext& ctx, rcTimerLabel label, const char* name, const float pc)
+{
+	const int t = ctx.getAccumulatedTime(label);
+	if (t < 0) return;
+	ctx.log(RC_LOG_PROGRESS, "%s:\t%.2fms\t(%.1f%%)", name, t/1000.0f, t*pc);
+}
+
+void duLogBuildTimes(rcContext& ctx, const int totalTimeUsec)
+{
+	const float pc = 100.0f / totalTimeUsec;
+ 
+	ctx.log(RC_LOG_PROGRESS, "Build Times");
+	logLine(ctx, RC_TIMER_RASTERIZE_TRIANGLES,		"- Rasterize", pc);
+	logLine(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD,	"- Build Compact", pc);
+	logLine(ctx, RC_TIMER_FILTER_BORDER,				"- Filter Border", pc);
+	logLine(ctx, RC_TIMER_FILTER_WALKABLE,			"- Filter Walkable", pc);
+	logLine(ctx, RC_TIMER_ERODE_AREA,				"- Erode Area", pc);
+	logLine(ctx, RC_TIMER_MEDIAN_AREA,				"- Median Area", pc);
+	logLine(ctx, RC_TIMER_MARK_BOX_AREA,				"- Mark Box Area", pc);
+	logLine(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA,		"- Mark Convex Area", pc);
+	logLine(ctx, RC_TIMER_MARK_CYLINDER_AREA,		"- Mark Cylinder Area", pc);
+	logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD,		"- Build Distance Field", pc);
+	logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD_DIST,	"    - Distance", pc);
+	logLine(ctx, RC_TIMER_BUILD_DISTANCEFIELD_BLUR,	"    - Blur", pc);
+	logLine(ctx, RC_TIMER_BUILD_REGIONS,				"- Build Regions", pc);
+	logLine(ctx, RC_TIMER_BUILD_REGIONS_WATERSHED,	"    - Watershed", pc);
+	logLine(ctx, RC_TIMER_BUILD_REGIONS_EXPAND,		"      - Expand", pc);
+	logLine(ctx, RC_TIMER_BUILD_REGIONS_FLOOD,		"      - Find Basins", pc);
+	logLine(ctx, RC_TIMER_BUILD_REGIONS_FILTER,		"    - Filter", pc);
+	logLine(ctx, RC_TIMER_BUILD_LAYERS,				"- Build Layers", pc);
+	logLine(ctx, RC_TIMER_BUILD_CONTOURS,			"- Build Contours", pc);
+	logLine(ctx, RC_TIMER_BUILD_CONTOURS_TRACE,		"    - Trace", pc);
+	logLine(ctx, RC_TIMER_BUILD_CONTOURS_SIMPLIFY,	"    - Simplify", pc);
+	logLine(ctx, RC_TIMER_BUILD_POLYMESH,			"- Build Polymesh", pc);
+	logLine(ctx, RC_TIMER_BUILD_POLYMESHDETAIL,		"- Build Polymesh Detail", pc);
+	logLine(ctx, RC_TIMER_MERGE_POLYMESH,			"- Merge Polymeshes", pc);
+	logLine(ctx, RC_TIMER_MERGE_POLYMESHDETAIL,		"- Merge Polymesh Details", pc);
+	ctx.log(RC_LOG_PROGRESS, "=== TOTAL:\t%.2fms", totalTimeUsec/1000.0f);
+}
+

panda/src/recastdetour/RecastDump.h

@@ -0,0 +1,43 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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.
+//
+
+#ifndef RECAST_DUMP_H
+#define RECAST_DUMP_H
+
+struct duFileIO
+{
+	virtual ~duFileIO() = 0;
+	virtual bool isWriting() const = 0;
+	virtual bool isReading() const = 0;
+	virtual bool write(const void* ptr, const size_t size) = 0;
+	virtual bool read(void* ptr, const size_t size) = 0;
+};
+
+bool duDumpPolyMeshToObj(struct rcPolyMesh& pmesh, duFileIO* io);
+bool duDumpPolyMeshDetailToObj(struct rcPolyMeshDetail& dmesh, duFileIO* io);
+
+bool duDumpContourSet(struct rcContourSet& cset, duFileIO* io);
+bool duReadContourSet(struct rcContourSet& cset, duFileIO* io);
+
+bool duDumpCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io);
+bool duReadCompactHeightfield(struct rcCompactHeightfield& chf, duFileIO* io);
+
+void duLogBuildTimes(rcContext& ctx, const int totalTileUsec);
+
+
+#endif // RECAST_DUMP_H

panda/src/recastdetour/RecastFilter.cpp

@@ -0,0 +1,202 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAssert.h"
+
+/// @par
+///
+/// Allows the formation of walkable regions that will flow over low lying 
+/// objects such as curbs, and up structures such as stairways. 
+/// 
+/// Two neighboring spans are walkable if: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) < waklableClimb</tt>
+/// 
+/// @warning Will override the effect of #rcFilterLedgeSpans.  So if both filters are used, call
+/// #rcFilterLedgeSpans after calling this filter. 
+///
+/// @see rcHeightfield, rcConfig
+void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb, rcHeightfield& solid)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_FILTER_LOW_OBSTACLES);
+	
+	const int w = solid.width;
+	const int h = solid.height;
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			rcSpan* ps = 0;
+			bool previousWalkable = false;
+			unsigned char previousArea = RC_NULL_AREA;
+			
+			for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next)
+			{
+				const bool walkable = s->area != RC_NULL_AREA;
+				// If current span is not walkable, but there is walkable
+				// span just below it, mark the span above it walkable too.
+				if (!walkable && previousWalkable)
+				{
+					if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb)
+						s->area = previousArea;
+				}
+				// Copy walkable flag so that it cannot propagate
+				// past multiple non-walkable objects.
+				previousWalkable = walkable;
+				previousArea = s->area;
+			}
+		}
+	}
+}
+
+/// @par
+///
+/// A ledge is a span with one or more neighbors whose maximum is further away than @p walkableClimb
+/// from the current span's maximum.
+/// This method removes the impact of the overestimation of conservative voxelization 
+/// so the resulting mesh will not have regions hanging in the air over ledges.
+/// 
+/// A span is a ledge if: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) > walkableClimb</tt>
+/// 
+/// @see rcHeightfield, rcConfig
+void rcFilterLedgeSpans(rcContext* ctx, const int walkableHeight, const int walkableClimb,
+						rcHeightfield& solid)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_FILTER_BORDER);
+
+	const int w = solid.width;
+	const int h = solid.height;
+	const int MAX_HEIGHT = 0xffff;
+	
+	// Mark border spans.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next)
+			{
+				// Skip non walkable spans.
+				if (s->area == RC_NULL_AREA)
+					continue;
+				
+				const int bot = (int)(s->smax);
+				const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT;
+				
+				// Find neighbours minimum height.
+				int minh = MAX_HEIGHT;
+
+				// Min and max height of accessible neighbours.
+				int asmin = s->smax;
+				int asmax = s->smax;
+
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					int dx = x + rcGetDirOffsetX(dir);
+					int dy = y + rcGetDirOffsetY(dir);
+					// Skip neighbours which are out of bounds.
+					if (dx < 0 || dy < 0 || dx >= w || dy >= h)
+					{
+						minh = rcMin(minh, -walkableClimb - bot);
+						continue;
+					}
+
+					// From minus infinity to the first span.
+					rcSpan* ns = solid.spans[dx + dy*w];
+					int nbot = -walkableClimb;
+					int ntop = ns ? (int)ns->smin : MAX_HEIGHT;
+					// Skip neightbour if the gap between the spans is too small.
+					if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight)
+						minh = rcMin(minh, nbot - bot);
+					
+					// Rest of the spans.
+					for (ns = solid.spans[dx + dy*w]; ns; ns = ns->next)
+					{
+						nbot = (int)ns->smax;
+						ntop = ns->next ? (int)ns->next->smin : MAX_HEIGHT;
+						// Skip neightbour if the gap between the spans is too small.
+						if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight)
+						{
+							minh = rcMin(minh, nbot - bot);
+						
+							// Find min/max accessible neighbour height. 
+							if (rcAbs(nbot - bot) <= walkableClimb)
+							{
+								if (nbot < asmin) asmin = nbot;
+								if (nbot > asmax) asmax = nbot;
+							}
+							
+						}
+					}
+				}
+				
+				// The current span is close to a ledge if the drop to any
+				// neighbour span is less than the walkableClimb.
+				if (minh < -walkableClimb)
+				{
+					s->area = RC_NULL_AREA;
+				}
+				// If the difference between all neighbours is too large,
+				// we are at steep slope, mark the span as ledge.
+				else if ((asmax - asmin) > walkableClimb)
+				{
+					s->area = RC_NULL_AREA;
+				}
+			}
+		}
+	}
+}
+
+/// @par
+///
+/// For this filter, the clearance above the span is the distance from the span's 
+/// maximum to the next higher span's minimum. (Same grid column.)
+/// 
+/// @see rcHeightfield, rcConfig
+void rcFilterWalkableLowHeightSpans(rcContext* ctx, int walkableHeight, rcHeightfield& solid)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_FILTER_WALKABLE);
+	
+	const int w = solid.width;
+	const int h = solid.height;
+	const int MAX_HEIGHT = 0xffff;
+	
+	// Remove walkable flag from spans which do not have enough
+	// space above them for the agent to stand there.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next)
+			{
+				const int bot = (int)(s->smax);
+				const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT;
+				if ((top - bot) <= walkableHeight)
+					s->area = RC_NULL_AREA;
+			}
+		}
+	}
+}

panda/src/recastdetour/RecastLayers.cpp

@@ -0,0 +1,644 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+
+// Must be 255 or smaller (not 256) because layer IDs are stored as
+// a byte where 255 is a special value.
+static const int RC_MAX_LAYERS = 63;
+static const int RC_MAX_NEIS = 16;
+
+struct rcLayerRegion
+{
+	unsigned char layers[RC_MAX_LAYERS];
+	unsigned char neis[RC_MAX_NEIS];
+	unsigned short ymin, ymax;
+	unsigned char layerId;		// Layer ID
+	unsigned char nlayers;		// Layer count
+	unsigned char nneis;		// Neighbour count
+	unsigned char base;		// Flag indicating if the region is the base of merged regions.
+};
+
+
+static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v)
+{
+	const int n = (int)an;
+	for (int i = 0; i < n; ++i)
+	{
+		if (a[i] == v)
+			return true;
+	}
+	return false;
+}
+
+static bool addUnique(unsigned char* a, unsigned char& an, int anMax, unsigned char v)
+{
+	if (contains(a, an, v))
+		return true;
+
+	if ((int)an >= anMax)
+		return false;
+
+	a[an] = v;
+	an++;
+	return true;
+}
+
+
+inline bool overlapRange(const unsigned short amin, const unsigned short amax,
+						 const unsigned short bmin, const unsigned short bmax)
+{
+	return (amin > bmax || amax < bmin) ? false : true;
+}
+
+
+
+struct rcLayerSweepSpan
+{
+	unsigned short ns;	// number samples
+	unsigned char id;	// region id
+	unsigned char nei;	// neighbour id
+};
+
+/// @par
+/// 
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// @see rcAllocHeightfieldLayerSet, rcCompactHeightfield, rcHeightfieldLayerSet, rcConfig
+bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf,
+							  const int borderSize, const int walkableHeight,
+							  rcHeightfieldLayerSet& lset)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_LAYERS);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedDelete<unsigned char> srcReg((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP));
+	if (!srcReg)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'srcReg' (%d).", chf.spanCount);
+		return false;
+	}
+	memset(srcReg,0xff,sizeof(unsigned char)*chf.spanCount);
+	
+	const int nsweeps = chf.width;
+	rcScopedDelete<rcLayerSweepSpan> sweeps((rcLayerSweepSpan*)rcAlloc(sizeof(rcLayerSweepSpan)*nsweeps, RC_ALLOC_TEMP));
+	if (!sweeps)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'sweeps' (%d).", nsweeps);
+		return false;
+	}
+	
+	
+	// Partition walkable area into monotone regions.
+	int prevCount[256];
+	unsigned char regId = 0;
+
+	for (int y = borderSize; y < h-borderSize; ++y)
+	{
+		memset(prevCount,0,sizeof(int)*regId);
+		unsigned char sweepId = 0;
+		
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA) continue;
+
+				unsigned char sid = 0xff;
+
+				// -x
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					if (chf.areas[ai] != RC_NULL_AREA && srcReg[ai] != 0xff)
+						sid = srcReg[ai];
+				}
+				
+				if (sid == 0xff)
+				{
+					sid = sweepId++;
+					sweeps[sid].nei = 0xff;
+					sweeps[sid].ns = 0;
+				}
+				
+				// -y
+				if (rcGetCon(s,3) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					const unsigned char nr = srcReg[ai];
+					if (nr != 0xff)
+					{
+						// Set neighbour when first valid neighbour is encoutered.
+						if (sweeps[sid].ns == 0)
+							sweeps[sid].nei = nr;
+						
+						if (sweeps[sid].nei == nr)
+						{
+							// Update existing neighbour
+							sweeps[sid].ns++;
+							prevCount[nr]++;
+						}
+						else
+						{
+							// This is hit if there is nore than one neighbour.
+							// Invalidate the neighbour.
+							sweeps[sid].nei = 0xff;
+						}
+					}
+				}
+				
+				srcReg[i] = sid;
+			}
+		}
+		
+		// Create unique ID.
+		for (int i = 0; i < sweepId; ++i)
+		{
+			// If the neighbour is set and there is only one continuous connection to it,
+			// the sweep will be merged with the previous one, else new region is created.
+			if (sweeps[i].nei != 0xff && prevCount[sweeps[i].nei] == (int)sweeps[i].ns)
+			{
+				sweeps[i].id = sweeps[i].nei;
+			}
+			else
+			{
+				if (regId == 255)
+				{
+					ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Region ID overflow.");
+					return false;
+				}
+				sweeps[i].id = regId++;
+			}
+		}
+		
+		// Remap local sweep ids to region ids.
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (srcReg[i] != 0xff)
+					srcReg[i] = sweeps[srcReg[i]].id;
+			}
+		}
+	}
+
+	// Allocate and init layer regions.
+	const int nregs = (int)regId;
+	rcScopedDelete<rcLayerRegion> regs((rcLayerRegion*)rcAlloc(sizeof(rcLayerRegion)*nregs, RC_ALLOC_TEMP));
+	if (!regs)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'regs' (%d).", nregs);
+		return false;
+	}
+	memset(regs, 0, sizeof(rcLayerRegion)*nregs);
+	for (int i = 0; i < nregs; ++i)
+	{
+		regs[i].layerId = 0xff;
+		regs[i].ymin = 0xffff;
+		regs[i].ymax = 0;
+	}
+	
+	// Find region neighbours and overlapping regions.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			unsigned char lregs[RC_MAX_LAYERS];
+			int nlregs = 0;
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const unsigned char ri = srcReg[i];
+				if (ri == 0xff) continue;
+				
+				regs[ri].ymin = rcMin(regs[ri].ymin, s.y);
+				regs[ri].ymax = rcMax(regs[ri].ymax, s.y);
+				
+				// Collect all region layers.
+				if (nlregs < RC_MAX_LAYERS)
+					lregs[nlregs++] = ri;
+				
+				// Update neighbours
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						const unsigned char rai = srcReg[ai];
+						if (rai != 0xff && rai != ri)
+						{
+							// Don't check return value -- if we cannot add the neighbor
+							// it will just cause a few more regions to be created, which
+							// is fine.
+							addUnique(regs[ri].neis, regs[ri].nneis, RC_MAX_NEIS, rai);
+						}
+					}
+				}
+				
+			}
+			
+			// Update overlapping regions.
+			for (int i = 0; i < nlregs-1; ++i)
+			{
+				for (int j = i+1; j < nlregs; ++j)
+				{
+					if (lregs[i] != lregs[j])
+					{
+						rcLayerRegion& ri = regs[lregs[i]];
+						rcLayerRegion& rj = regs[lregs[j]];
+
+						if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, lregs[j]) ||
+							!addUnique(rj.layers, rj.nlayers, RC_MAX_LAYERS, lregs[i]))
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
+							return false;
+						}
+					}
+				}
+			}
+			
+		}
+	}
+	
+	// Create 2D layers from regions.
+	unsigned char layerId = 0;
+	
+	static const int MAX_STACK = 64;
+	unsigned char stack[MAX_STACK];
+	int nstack = 0;
+	
+	for (int i = 0; i < nregs; ++i)
+	{
+		rcLayerRegion& root = regs[i];
+		// Skip already visited.
+		if (root.layerId != 0xff)
+			continue;
+
+		// Start search.
+		root.layerId = layerId;
+		root.base = 1;
+		
+		nstack = 0;
+		stack[nstack++] = (unsigned char)i;
+		
+		while (nstack)
+		{
+			// Pop front
+			rcLayerRegion& reg = regs[stack[0]];
+			nstack--;
+			for (int j = 0; j < nstack; ++j)
+				stack[j] = stack[j+1];
+			
+			const int nneis = (int)reg.nneis;
+			for (int j = 0; j < nneis; ++j)
+			{
+				const unsigned char nei = reg.neis[j];
+				rcLayerRegion& regn = regs[nei];
+				// Skip already visited.
+				if (regn.layerId != 0xff)
+					continue;
+				// Skip if the neighbour is overlapping root region.
+				if (contains(root.layers, root.nlayers, nei))
+					continue;
+				// Skip if the height range would become too large.
+				const int ymin = rcMin(root.ymin, regn.ymin);
+				const int ymax = rcMax(root.ymax, regn.ymax);
+				if ((ymax - ymin) >= 255)
+					 continue;
+
+				if (nstack < MAX_STACK)
+				{
+					// Deepen
+					stack[nstack++] = (unsigned char)nei;
+					
+					// Mark layer id
+					regn.layerId = layerId;
+					// Merge current layers to root.
+					for (int k = 0; k < regn.nlayers; ++k)
+					{
+						if (!addUnique(root.layers, root.nlayers, RC_MAX_LAYERS, regn.layers[k]))
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
+							return false;
+						}
+					}
+					root.ymin = rcMin(root.ymin, regn.ymin);
+					root.ymax = rcMax(root.ymax, regn.ymax);
+				}
+			}
+		}
+		
+		layerId++;
+	}
+	
+	// Merge non-overlapping regions that are close in height.
+	const unsigned short mergeHeight = (unsigned short)walkableHeight * 4;
+	
+	for (int i = 0; i < nregs; ++i)
+	{
+		rcLayerRegion& ri = regs[i];
+		if (!ri.base) continue;
+		
+		unsigned char newId = ri.layerId;
+		
+		for (;;)
+		{
+			unsigned char oldId = 0xff;
+			
+			for (int j = 0; j < nregs; ++j)
+			{
+				if (i == j) continue;
+				rcLayerRegion& rj = regs[j];
+				if (!rj.base) continue;
+				
+				// Skip if the regions are not close to each other.
+				if (!overlapRange(ri.ymin,ri.ymax+mergeHeight, rj.ymin,rj.ymax+mergeHeight))
+					continue;
+				// Skip if the height range would become too large.
+				const int ymin = rcMin(ri.ymin, rj.ymin);
+				const int ymax = rcMax(ri.ymax, rj.ymax);
+				if ((ymax - ymin) >= 255)
+				  continue;
+						  
+				// Make sure that there is no overlap when merging 'ri' and 'rj'.
+				bool overlap = false;
+				// Iterate over all regions which have the same layerId as 'rj'
+				for (int k = 0; k < nregs; ++k)
+				{
+					if (regs[k].layerId != rj.layerId)
+						continue;
+					// Check if region 'k' is overlapping region 'ri'
+					// Index to 'regs' is the same as region id.
+					if (contains(ri.layers,ri.nlayers, (unsigned char)k))
+					{
+						overlap = true;
+						break;
+					}
+				}
+				// Cannot merge of regions overlap.
+				if (overlap)
+					continue;
+				
+				// Can merge i and j.
+				oldId = rj.layerId;
+				break;
+			}
+			
+			// Could not find anything to merge with, stop.
+			if (oldId == 0xff)
+				break;
+			
+			// Merge
+			for (int j = 0; j < nregs; ++j)
+			{
+				rcLayerRegion& rj = regs[j];
+				if (rj.layerId == oldId)
+				{
+					rj.base = 0;
+					// Remap layerIds.
+					rj.layerId = newId;
+					// Add overlaid layers from 'rj' to 'ri'.
+					for (int k = 0; k < rj.nlayers; ++k)
+					{
+						if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, rj.layers[k]))
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
+							return false;
+						}
+					}
+
+					// Update height bounds.
+					ri.ymin = rcMin(ri.ymin, rj.ymin);
+					ri.ymax = rcMax(ri.ymax, rj.ymax);
+				}
+			}
+		}
+	}
+	
+	// Compact layerIds
+	unsigned char remap[256];
+	memset(remap, 0, 256);
+
+	// Find number of unique layers.
+	layerId = 0;
+	for (int i = 0; i < nregs; ++i)
+		remap[regs[i].layerId] = 1;
+	for (int i = 0; i < 256; ++i)
+	{
+		if (remap[i])
+			remap[i] = layerId++;
+		else
+			remap[i] = 0xff;
+	}
+	// Remap ids.
+	for (int i = 0; i < nregs; ++i)
+		regs[i].layerId = remap[regs[i].layerId];
+	
+	// No layers, return empty.
+	if (layerId == 0)
+		return true;
+	
+	// Create layers.
+	rcAssert(lset.layers == 0);
+	
+	const int lw = w - borderSize*2;
+	const int lh = h - borderSize*2;
+
+	// Build contracted bbox for layers.
+	float bmin[3], bmax[3];
+	rcVcopy(bmin, chf.bmin);
+	rcVcopy(bmax, chf.bmax);
+	bmin[0] += borderSize*chf.cs;
+	bmin[2] += borderSize*chf.cs;
+	bmax[0] -= borderSize*chf.cs;
+	bmax[2] -= borderSize*chf.cs;
+	
+	lset.nlayers = (int)layerId;
+	
+	lset.layers = (rcHeightfieldLayer*)rcAlloc(sizeof(rcHeightfieldLayer)*lset.nlayers, RC_ALLOC_PERM);
+	if (!lset.layers)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'layers' (%d).", lset.nlayers);
+		return false;
+	}
+	memset(lset.layers, 0, sizeof(rcHeightfieldLayer)*lset.nlayers);
+
+	
+	// Store layers.
+	for (int i = 0; i < lset.nlayers; ++i)
+	{
+		unsigned char curId = (unsigned char)i;
+
+		rcHeightfieldLayer* layer = &lset.layers[i];
+
+		const int gridSize = sizeof(unsigned char)*lw*lh;
+
+		layer->heights = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
+		if (!layer->heights)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'heights' (%d).", gridSize);
+			return false;
+		}
+		memset(layer->heights, 0xff, gridSize);
+
+		layer->areas = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
+		if (!layer->areas)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'areas' (%d).", gridSize);
+			return false;
+		}
+		memset(layer->areas, 0, gridSize);
+
+		layer->cons = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
+		if (!layer->cons)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'cons' (%d).", gridSize);
+			return false;
+		}
+		memset(layer->cons, 0, gridSize);
+		
+		// Find layer height bounds.
+		int hmin = 0, hmax = 0;
+		for (int j = 0; j < nregs; ++j)
+		{
+			if (regs[j].base && regs[j].layerId == curId)
+			{
+				hmin = (int)regs[j].ymin;
+				hmax = (int)regs[j].ymax;
+			}
+		}
+
+		layer->width = lw;
+		layer->height = lh;
+		layer->cs = chf.cs;
+		layer->ch = chf.ch;
+		
+		// Adjust the bbox to fit the heightfield.
+		rcVcopy(layer->bmin, bmin);
+		rcVcopy(layer->bmax, bmax);
+		layer->bmin[1] = bmin[1] + hmin*chf.ch;
+		layer->bmax[1] = bmin[1] + hmax*chf.ch;
+		layer->hmin = hmin;
+		layer->hmax = hmax;
+
+		// Update usable data region.
+		layer->minx = layer->width;
+		layer->maxx = 0;
+		layer->miny = layer->height;
+		layer->maxy = 0;
+		
+		// Copy height and area from compact heightfield. 
+		for (int y = 0; y < lh; ++y)
+		{
+			for (int x = 0; x < lw; ++x)
+			{
+				const int cx = borderSize+x;
+				const int cy = borderSize+y;
+				const rcCompactCell& c = chf.cells[cx+cy*w];
+				for (int j = (int)c.index, nj = (int)(c.index+c.count); j < nj; ++j)
+				{
+					const rcCompactSpan& s = chf.spans[j];
+					// Skip unassigned regions.
+					if (srcReg[j] == 0xff)
+						continue;
+					// Skip of does nto belong to current layer.
+					unsigned char lid = regs[srcReg[j]].layerId;
+					if (lid != curId)
+						continue;
+					
+					// Update data bounds.
+					layer->minx = rcMin(layer->minx, x);
+					layer->maxx = rcMax(layer->maxx, x);
+					layer->miny = rcMin(layer->miny, y);
+					layer->maxy = rcMax(layer->maxy, y);
+					
+					// Store height and area type.
+					const int idx = x+y*lw;
+					layer->heights[idx] = (unsigned char)(s.y - hmin);
+					layer->areas[idx] = chf.areas[j];
+					
+					// Check connection.
+					unsigned char portal = 0;
+					unsigned char con = 0;
+					for (int dir = 0; dir < 4; ++dir)
+					{
+						if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+						{
+							const int ax = cx + rcGetDirOffsetX(dir);
+							const int ay = cy + rcGetDirOffsetY(dir);
+							const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+							unsigned char alid = srcReg[ai] != 0xff ? regs[srcReg[ai]].layerId : 0xff;
+							// Portal mask
+							if (chf.areas[ai] != RC_NULL_AREA && lid != alid)
+							{
+								portal |= (unsigned char)(1<<dir);
+								// Update height so that it matches on both sides of the portal.
+								const rcCompactSpan& as = chf.spans[ai];
+								if (as.y > hmin)
+									layer->heights[idx] = rcMax(layer->heights[idx], (unsigned char)(as.y - hmin));
+							}
+							// Valid connection mask
+							if (chf.areas[ai] != RC_NULL_AREA && lid == alid)
+							{
+								const int nx = ax - borderSize;
+								const int ny = ay - borderSize;
+								if (nx >= 0 && ny >= 0 && nx < lw && ny < lh)
+									con |= (unsigned char)(1<<dir);
+							}
+						}
+					}
+					
+					layer->cons[idx] = (portal << 4) | con;
+				}
+			}
+		}
+		
+		if (layer->minx > layer->maxx)
+			layer->minx = layer->maxx = 0;
+		if (layer->miny > layer->maxy)
+			layer->miny = layer->maxy = 0;
+	}
+	
+	return true;
+}

panda/src/recastdetour/RecastMesh.cpp

@@ -0,0 +1,1552 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+struct rcEdge
+{
+	unsigned short vert[2];
+	unsigned short polyEdge[2];
+	unsigned short poly[2];
+};
+
+static bool buildMeshAdjacency(unsigned short* polys, const int npolys,
+							   const int nverts, const int vertsPerPoly)
+{
+	// Based on code by Eric Lengyel from:
+	// http://www.terathon.com/code/edges.php
+	
+	int maxEdgeCount = npolys*vertsPerPoly;
+	unsigned short* firstEdge = (unsigned short*)rcAlloc(sizeof(unsigned short)*(nverts + maxEdgeCount), RC_ALLOC_TEMP);
+	if (!firstEdge)
+		return false;
+	unsigned short* nextEdge = firstEdge + nverts;
+	int edgeCount = 0;
+	
+	rcEdge* edges = (rcEdge*)rcAlloc(sizeof(rcEdge)*maxEdgeCount, RC_ALLOC_TEMP);
+	if (!edges)
+	{
+		rcFree(firstEdge);
+		return false;
+	}
+	
+	for (int i = 0; i < nverts; i++)
+		firstEdge[i] = RC_MESH_NULL_IDX;
+	
+	for (int i = 0; i < npolys; ++i)
+	{
+		unsigned short* t = &polys[i*vertsPerPoly*2];
+		for (int j = 0; j < vertsPerPoly; ++j)
+		{
+			if (t[j] == RC_MESH_NULL_IDX) break;
+			unsigned short v0 = t[j];
+			unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1];
+			if (v0 < v1)
+			{
+				rcEdge& edge = edges[edgeCount];
+				edge.vert[0] = v0;
+				edge.vert[1] = v1;
+				edge.poly[0] = (unsigned short)i;
+				edge.polyEdge[0] = (unsigned short)j;
+				edge.poly[1] = (unsigned short)i;
+				edge.polyEdge[1] = 0;
+				// Insert edge
+				nextEdge[edgeCount] = firstEdge[v0];
+				firstEdge[v0] = (unsigned short)edgeCount;
+				edgeCount++;
+			}
+		}
+	}
+	
+	for (int i = 0; i < npolys; ++i)
+	{
+		unsigned short* t = &polys[i*vertsPerPoly*2];
+		for (int j = 0; j < vertsPerPoly; ++j)
+		{
+			if (t[j] == RC_MESH_NULL_IDX) break;
+			unsigned short v0 = t[j];
+			unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1];
+			if (v0 > v1)
+			{
+				for (unsigned short e = firstEdge[v1]; e != RC_MESH_NULL_IDX; e = nextEdge[e])
+				{
+					rcEdge& edge = edges[e];
+					if (edge.vert[1] == v0 && edge.poly[0] == edge.poly[1])
+					{
+						edge.poly[1] = (unsigned short)i;
+						edge.polyEdge[1] = (unsigned short)j;
+						break;
+					}
+				}
+			}
+		}
+	}
+	
+	// Store adjacency
+	for (int i = 0; i < edgeCount; ++i)
+	{
+		const rcEdge& e = edges[i];
+		if (e.poly[0] != e.poly[1])
+		{
+			unsigned short* p0 = &polys[e.poly[0]*vertsPerPoly*2];
+			unsigned short* p1 = &polys[e.poly[1]*vertsPerPoly*2];
+			p0[vertsPerPoly + e.polyEdge[0]] = e.poly[1];
+			p1[vertsPerPoly + e.polyEdge[1]] = e.poly[0];
+		}
+	}
+	
+	rcFree(firstEdge);
+	rcFree(edges);
+	
+	return true;
+}
+
+
+static const int VERTEX_BUCKET_COUNT = (1<<12);
+
+inline int computeVertexHash(int x, int y, int z)
+{
+	const unsigned int h1 = 0x8da6b343; // Large multiplicative constants;
+	const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes
+	const unsigned int h3 = 0xcb1ab31f;
+	unsigned int n = h1 * x + h2 * y + h3 * z;
+	return (int)(n & (VERTEX_BUCKET_COUNT-1));
+}
+
+static unsigned short addVertex(unsigned short x, unsigned short y, unsigned short z,
+								unsigned short* verts, int* firstVert, int* nextVert, int& nv)
+{
+	int bucket = computeVertexHash(x, 0, z);
+	int i = firstVert[bucket];
+	
+	while (i != -1)
+	{
+		const unsigned short* v = &verts[i*3];
+		if (v[0] == x && (rcAbs(v[1] - y) <= 2) && v[2] == z)
+			return (unsigned short)i;
+		i = nextVert[i]; // next
+	}
+	
+	// Could not find, create new.
+	i = nv; nv++;
+	unsigned short* v = &verts[i*3];
+	v[0] = x;
+	v[1] = y;
+	v[2] = z;
+	nextVert[i] = firstVert[bucket];
+	firstVert[bucket] = i;
+	
+	return (unsigned short)i;
+}
+
+// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv).
+inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; }
+inline int next(int i, int n) { return i+1 < n ? i+1 : 0; }
+
+inline int area2(const int* a, const int* b, const int* c)
+{
+	return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
+}
+
+//	Exclusive or: true iff exactly one argument is true.
+//	The arguments are negated to ensure that they are 0/1
+//	values.  Then the bitwise Xor operator may apply.
+//	(This idea is due to Michael Baldwin.)
+inline bool xorb(bool x, bool y)
+{
+	return !x ^ !y;
+}
+
+// Returns true iff c is strictly to the left of the directed
+// line through a to b.
+inline bool left(const int* a, const int* b, const int* c)
+{
+	return area2(a, b, c) < 0;
+}
+
+inline bool leftOn(const int* a, const int* b, const int* c)
+{
+	return area2(a, b, c) <= 0;
+}
+
+inline bool collinear(const int* a, const int* b, const int* c)
+{
+	return area2(a, b, c) == 0;
+}
+
+//	Returns true iff ab properly intersects cd: they share
+//	a point interior to both segments.  The properness of the
+//	intersection is ensured by using strict leftness.
+static bool intersectProp(const int* a, const int* b, const int* c, const int* d)
+{
+	// Eliminate improper cases.
+	if (collinear(a,b,c) || collinear(a,b,d) ||
+		collinear(c,d,a) || collinear(c,d,b))
+		return false;
+	
+	return xorb(left(a,b,c), left(a,b,d)) && xorb(left(c,d,a), left(c,d,b));
+}
+
+// Returns T iff (a,b,c) are collinear and point c lies 
+// on the closed segement ab.
+static bool between(const int* a, const int* b, const int* c)
+{
+	if (!collinear(a, b, c))
+		return false;
+	// If ab not vertical, check betweenness on x; else on y.
+	if (a[0] != b[0])
+		return	((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0]));
+	else
+		return	((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2]));
+}
+
+// Returns true iff segments ab and cd intersect, properly or improperly.
+static bool intersect(const int* a, const int* b, const int* c, const int* d)
+{
+	if (intersectProp(a, b, c, d))
+		return true;
+	else if (between(a, b, c) || between(a, b, d) ||
+			 between(c, d, a) || between(c, d, b))
+		return true;
+	else
+		return false;
+}
+
+static bool vequal(const int* a, const int* b)
+{
+	return a[0] == b[0] && a[2] == b[2];
+}
+
+// Returns T iff (v_i, v_j) is a proper internal *or* external
+// diagonal of P, *ignoring edges incident to v_i and v_j*.
+static bool diagonalie(int i, int j, int n, const int* verts, int* indices)
+{
+	const int* d0 = &verts[(indices[i] & 0x0fffffff) * 4];
+	const int* d1 = &verts[(indices[j] & 0x0fffffff) * 4];
+	
+	// For each edge (k,k+1) of P
+	for (int k = 0; k < n; k++)
+	{
+		int k1 = next(k, n);
+		// Skip edges incident to i or j
+		if (!((k == i) || (k1 == i) || (k == j) || (k1 == j)))
+		{
+			const int* p0 = &verts[(indices[k] & 0x0fffffff) * 4];
+			const int* p1 = &verts[(indices[k1] & 0x0fffffff) * 4];
+
+			if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1))
+				continue;
+			
+			if (intersect(d0, d1, p0, p1))
+				return false;
+		}
+	}
+	return true;
+}
+
+// Returns true iff the diagonal (i,j) is strictly internal to the 
+// polygon P in the neighborhood of the i endpoint.
+static bool	inCone(int i, int j, int n, const int* verts, int* indices)
+{
+	const int* pi = &verts[(indices[i] & 0x0fffffff) * 4];
+	const int* pj = &verts[(indices[j] & 0x0fffffff) * 4];
+	const int* pi1 = &verts[(indices[next(i, n)] & 0x0fffffff) * 4];
+	const int* pin1 = &verts[(indices[prev(i, n)] & 0x0fffffff) * 4];
+
+	// If P[i] is a convex vertex [ i+1 left or on (i-1,i) ].
+	if (leftOn(pin1, pi, pi1))
+		return left(pi, pj, pin1) && left(pj, pi, pi1);
+	// Assume (i-1,i,i+1) not collinear.
+	// else P[i] is reflex.
+	return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1));
+}
+
+// Returns T iff (v_i, v_j) is a proper internal
+// diagonal of P.
+static bool diagonal(int i, int j, int n, const int* verts, int* indices)
+{
+	return inCone(i, j, n, verts, indices) && diagonalie(i, j, n, verts, indices);
+}
+
+
+static bool diagonalieLoose(int i, int j, int n, const int* verts, int* indices)
+{
+	const int* d0 = &verts[(indices[i] & 0x0fffffff) * 4];
+	const int* d1 = &verts[(indices[j] & 0x0fffffff) * 4];
+	
+	// For each edge (k,k+1) of P
+	for (int k = 0; k < n; k++)
+	{
+		int k1 = next(k, n);
+		// Skip edges incident to i or j
+		if (!((k == i) || (k1 == i) || (k == j) || (k1 == j)))
+		{
+			const int* p0 = &verts[(indices[k] & 0x0fffffff) * 4];
+			const int* p1 = &verts[(indices[k1] & 0x0fffffff) * 4];
+			
+			if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1))
+				continue;
+			
+			if (intersectProp(d0, d1, p0, p1))
+				return false;
+		}
+	}
+	return true;
+}
+
+static bool	inConeLoose(int i, int j, int n, const int* verts, int* indices)
+{
+	const int* pi = &verts[(indices[i] & 0x0fffffff) * 4];
+	const int* pj = &verts[(indices[j] & 0x0fffffff) * 4];
+	const int* pi1 = &verts[(indices[next(i, n)] & 0x0fffffff) * 4];
+	const int* pin1 = &verts[(indices[prev(i, n)] & 0x0fffffff) * 4];
+	
+	// If P[i] is a convex vertex [ i+1 left or on (i-1,i) ].
+	if (leftOn(pin1, pi, pi1))
+		return leftOn(pi, pj, pin1) && leftOn(pj, pi, pi1);
+	// Assume (i-1,i,i+1) not collinear.
+	// else P[i] is reflex.
+	return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1));
+}
+
+static bool diagonalLoose(int i, int j, int n, const int* verts, int* indices)
+{
+	return inConeLoose(i, j, n, verts, indices) && diagonalieLoose(i, j, n, verts, indices);
+}
+
+
+static int triangulate(int n, const int* verts, int* indices, int* tris)
+{
+	int ntris = 0;
+	int* dst = tris;
+	
+	// The last bit of the index is used to indicate if the vertex can be removed.
+	for (int i = 0; i < n; i++)
+	{
+		int i1 = next(i, n);
+		int i2 = next(i1, n);
+		if (diagonal(i, i2, n, verts, indices))
+			indices[i1] |= 0x80000000;
+	}
+	
+	while (n > 3)
+	{
+		int minLen = -1;
+		int mini = -1;
+		for (int i = 0; i < n; i++)
+		{
+			int i1 = next(i, n);
+			if (indices[i1] & 0x80000000)
+			{
+				const int* p0 = &verts[(indices[i] & 0x0fffffff) * 4];
+				const int* p2 = &verts[(indices[next(i1, n)] & 0x0fffffff) * 4];
+				
+				int dx = p2[0] - p0[0];
+				int dy = p2[2] - p0[2];
+				int len = dx*dx + dy*dy;
+				
+				if (minLen < 0 || len < minLen)
+				{
+					minLen = len;
+					mini = i;
+				}
+			}
+		}
+		
+		if (mini == -1)
+		{
+			// We might get here because the contour has overlapping segments, like this:
+			//
+			//  A o-o=====o---o B
+			//   /  |C   D|    \.
+			//  o   o     o     o
+			//  :   :     :     :
+			// We'll try to recover by loosing up the inCone test a bit so that a diagonal
+			// like A-B or C-D can be found and we can continue.
+			minLen = -1;
+			mini = -1;
+			for (int i = 0; i < n; i++)
+			{
+				int i1 = next(i, n);
+				int i2 = next(i1, n);
+				if (diagonalLoose(i, i2, n, verts, indices))
+				{
+					const int* p0 = &verts[(indices[i] & 0x0fffffff) * 4];
+					const int* p2 = &verts[(indices[next(i2, n)] & 0x0fffffff) * 4];
+					int dx = p2[0] - p0[0];
+					int dy = p2[2] - p0[2];
+					int len = dx*dx + dy*dy;
+					
+					if (minLen < 0 || len < minLen)
+					{
+						minLen = len;
+						mini = i;
+					}
+				}
+			}
+			if (mini == -1)
+			{
+				// The contour is messed up. This sometimes happens
+				// if the contour simplification is too aggressive.
+				return -ntris;
+			}
+		}
+		
+		int i = mini;
+		int i1 = next(i, n);
+		int i2 = next(i1, n);
+		
+		*dst++ = indices[i] & 0x0fffffff;
+		*dst++ = indices[i1] & 0x0fffffff;
+		*dst++ = indices[i2] & 0x0fffffff;
+		ntris++;
+		
+		// Removes P[i1] by copying P[i+1]...P[n-1] left one index.
+		n--;
+		for (int k = i1; k < n; k++)
+			indices[k] = indices[k+1];
+		
+		if (i1 >= n) i1 = 0;
+		i = prev(i1,n);
+		// Update diagonal flags.
+		if (diagonal(prev(i, n), i1, n, verts, indices))
+			indices[i] |= 0x80000000;
+		else
+			indices[i] &= 0x0fffffff;
+		
+		if (diagonal(i, next(i1, n), n, verts, indices))
+			indices[i1] |= 0x80000000;
+		else
+			indices[i1] &= 0x0fffffff;
+	}
+	
+	// Append the remaining triangle.
+	*dst++ = indices[0] & 0x0fffffff;
+	*dst++ = indices[1] & 0x0fffffff;
+	*dst++ = indices[2] & 0x0fffffff;
+	ntris++;
+	
+	return ntris;
+}
+
+static int countPolyVerts(const unsigned short* p, const int nvp)
+{
+	for (int i = 0; i < nvp; ++i)
+		if (p[i] == RC_MESH_NULL_IDX)
+			return i;
+	return nvp;
+}
+
+inline bool uleft(const unsigned short* a, const unsigned short* b, const unsigned short* c)
+{
+	return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) -
+		   ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) < 0;
+}
+
+static int getPolyMergeValue(unsigned short* pa, unsigned short* pb,
+							 const unsigned short* verts, int& ea, int& eb,
+							 const int nvp)
+{
+	const int na = countPolyVerts(pa, nvp);
+	const int nb = countPolyVerts(pb, nvp);
+	
+	// If the merged polygon would be too big, do not merge.
+	if (na+nb-2 > nvp)
+		return -1;
+	
+	// Check if the polygons share an edge.
+	ea = -1;
+	eb = -1;
+	
+	for (int i = 0; i < na; ++i)
+	{
+		unsigned short va0 = pa[i];
+		unsigned short va1 = pa[(i+1) % na];
+		if (va0 > va1)
+			rcSwap(va0, va1);
+		for (int j = 0; j < nb; ++j)
+		{
+			unsigned short vb0 = pb[j];
+			unsigned short vb1 = pb[(j+1) % nb];
+			if (vb0 > vb1)
+				rcSwap(vb0, vb1);
+			if (va0 == vb0 && va1 == vb1)
+			{
+				ea = i;
+				eb = j;
+				break;
+			}
+		}
+	}
+	
+	// No common edge, cannot merge.
+	if (ea == -1 || eb == -1)
+		return -1;
+	
+	// Check to see if the merged polygon would be convex.
+	unsigned short va, vb, vc;
+	
+	va = pa[(ea+na-1) % na];
+	vb = pa[ea];
+	vc = pb[(eb+2) % nb];
+	if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3]))
+		return -1;
+	
+	va = pb[(eb+nb-1) % nb];
+	vb = pb[eb];
+	vc = pa[(ea+2) % na];
+	if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3]))
+		return -1;
+	
+	va = pa[ea];
+	vb = pa[(ea+1)%na];
+	
+	int dx = (int)verts[va*3+0] - (int)verts[vb*3+0];
+	int dy = (int)verts[va*3+2] - (int)verts[vb*3+2];
+	
+	return dx*dx + dy*dy;
+}
+
+static void mergePolyVerts(unsigned short* pa, unsigned short* pb, int ea, int eb,
+						   unsigned short* tmp, const int nvp)
+{
+	const int na = countPolyVerts(pa, nvp);
+	const int nb = countPolyVerts(pb, nvp);
+	
+	// Merge polygons.
+	memset(tmp, 0xff, sizeof(unsigned short)*nvp);
+	int n = 0;
+	// Add pa
+	for (int i = 0; i < na-1; ++i)
+		tmp[n++] = pa[(ea+1+i) % na];
+	// Add pb
+	for (int i = 0; i < nb-1; ++i)
+		tmp[n++] = pb[(eb+1+i) % nb];
+	
+	memcpy(pa, tmp, sizeof(unsigned short)*nvp);
+}
+
+
+static void pushFront(int v, int* arr, int& an)
+{
+	an++;
+	for (int i = an-1; i > 0; --i) arr[i] = arr[i-1];
+	arr[0] = v;
+}
+
+static void pushBack(int v, int* arr, int& an)
+{
+	arr[an] = v;
+	an++;
+}
+
+static bool canRemoveVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short rem)
+{
+	const int nvp = mesh.nvp;
+	
+	// Count number of polygons to remove.
+	int numRemovedVerts = 0;
+	int numTouchedVerts = 0;
+	int numRemainingEdges = 0;
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*nvp*2];
+		const int nv = countPolyVerts(p, nvp);
+		int numRemoved = 0;
+		int numVerts = 0;
+		for (int j = 0; j < nv; ++j)
+		{
+			if (p[j] == rem)
+			{
+				numTouchedVerts++;
+				numRemoved++;
+			}
+			numVerts++;
+		}
+		if (numRemoved)
+		{
+			numRemovedVerts += numRemoved;
+			numRemainingEdges += numVerts-(numRemoved+1);
+		}
+	}
+	
+	// There would be too few edges remaining to create a polygon.
+	// This can happen for example when a tip of a triangle is marked
+	// as deletion, but there are no other polys that share the vertex.
+	// In this case, the vertex should not be removed.
+	if (numRemainingEdges <= 2)
+		return false;
+	
+	// Find edges which share the removed vertex.
+	const int maxEdges = numTouchedVerts*2;
+	int nedges = 0;
+	rcScopedDelete<int> edges((int*)rcAlloc(sizeof(int)*maxEdges*3, RC_ALLOC_TEMP));
+	if (!edges)
+	{
+		ctx->log(RC_LOG_WARNING, "canRemoveVertex: Out of memory 'edges' (%d).", maxEdges*3);
+		return false;
+	}
+		
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*nvp*2];
+		const int nv = countPolyVerts(p, nvp);
+
+		// Collect edges which touches the removed vertex.
+		for (int j = 0, k = nv-1; j < nv; k = j++)
+		{
+			if (p[j] == rem || p[k] == rem)
+			{
+				// Arrange edge so that a=rem.
+				int a = p[j], b = p[k];
+				if (b == rem)
+					rcSwap(a,b);
+					
+				// Check if the edge exists
+				bool exists = false;
+				for (int m = 0; m < nedges; ++m)
+				{
+					int* e = &edges[m*3];
+					if (e[1] == b)
+					{
+						// Exists, increment vertex share count.
+						e[2]++;
+						exists = true;
+					}
+				}
+				// Add new edge.
+				if (!exists)
+				{
+					int* e = &edges[nedges*3];
+					e[0] = a;
+					e[1] = b;
+					e[2] = 1;
+					nedges++;
+				}
+			}
+		}
+	}
+
+	// There should be no more than 2 open edges.
+	// This catches the case that two non-adjacent polygons
+	// share the removed vertex. In that case, do not remove the vertex.
+	int numOpenEdges = 0;
+	for (int i = 0; i < nedges; ++i)
+	{
+		if (edges[i*3+2] < 2)
+			numOpenEdges++;
+	}
+	if (numOpenEdges > 2)
+		return false;
+	
+	return true;
+}
+
+static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short rem, const int maxTris)
+{
+	const int nvp = mesh.nvp;
+
+	// Count number of polygons to remove.
+	int numRemovedVerts = 0;
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*nvp*2];
+		const int nv = countPolyVerts(p, nvp);
+		for (int j = 0; j < nv; ++j)
+		{
+			if (p[j] == rem)
+				numRemovedVerts++;
+		}
+	}
+	
+	int nedges = 0;
+	rcScopedDelete<int> edges((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp*4, RC_ALLOC_TEMP));
+	if (!edges)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'edges' (%d).", numRemovedVerts*nvp*4);
+		return false;
+	}
+
+	int nhole = 0;
+	rcScopedDelete<int> hole((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp, RC_ALLOC_TEMP));
+	if (!hole)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hole' (%d).", numRemovedVerts*nvp);
+		return false;
+	}
+
+	int nhreg = 0;
+	rcScopedDelete<int> hreg((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp, RC_ALLOC_TEMP));
+	if (!hreg)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hreg' (%d).", numRemovedVerts*nvp);
+		return false;
+	}
+
+	int nharea = 0;
+	rcScopedDelete<int> harea((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp, RC_ALLOC_TEMP));
+	if (!harea)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'harea' (%d).", numRemovedVerts*nvp);
+		return false;
+	}
+	
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*nvp*2];
+		const int nv = countPolyVerts(p, nvp);
+		bool hasRem = false;
+		for (int j = 0; j < nv; ++j)
+			if (p[j] == rem) hasRem = true;
+		if (hasRem)
+		{
+			// Collect edges which does not touch the removed vertex.
+			for (int j = 0, k = nv-1; j < nv; k = j++)
+			{
+				if (p[j] != rem && p[k] != rem)
+				{
+					int* e = &edges[nedges*4];
+					e[0] = p[k];
+					e[1] = p[j];
+					e[2] = mesh.regs[i];
+					e[3] = mesh.areas[i];
+					nedges++;
+				}
+			}
+			// Remove the polygon.
+			unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2];
+			if (p != p2)
+				memcpy(p,p2,sizeof(unsigned short)*nvp);
+			memset(p+nvp,0xff,sizeof(unsigned short)*nvp);
+			mesh.regs[i] = mesh.regs[mesh.npolys-1];
+			mesh.areas[i] = mesh.areas[mesh.npolys-1];
+			mesh.npolys--;
+			--i;
+		}
+	}
+	
+	// Remove vertex.
+	for (int i = (int)rem; i < mesh.nverts - 1; ++i)
+	{
+		mesh.verts[i*3+0] = mesh.verts[(i+1)*3+0];
+		mesh.verts[i*3+1] = mesh.verts[(i+1)*3+1];
+		mesh.verts[i*3+2] = mesh.verts[(i+1)*3+2];
+	}
+	mesh.nverts--;
+
+	// Adjust indices to match the removed vertex layout.
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		unsigned short* p = &mesh.polys[i*nvp*2];
+		const int nv = countPolyVerts(p, nvp);
+		for (int j = 0; j < nv; ++j)
+			if (p[j] > rem) p[j]--;
+	}
+	for (int i = 0; i < nedges; ++i)
+	{
+		if (edges[i*4+0] > rem) edges[i*4+0]--;
+		if (edges[i*4+1] > rem) edges[i*4+1]--;
+	}
+
+	if (nedges == 0)
+		return true;
+
+	// Start with one vertex, keep appending connected
+	// segments to the start and end of the hole.
+	pushBack(edges[0], hole, nhole);
+	pushBack(edges[2], hreg, nhreg);
+	pushBack(edges[3], harea, nharea);
+	
+	while (nedges)
+	{
+		bool match = false;
+		
+		for (int i = 0; i < nedges; ++i)
+		{
+			const int ea = edges[i*4+0];
+			const int eb = edges[i*4+1];
+			const int r = edges[i*4+2];
+			const int a = edges[i*4+3];
+			bool add = false;
+			if (hole[0] == eb)
+			{
+				// The segment matches the beginning of the hole boundary.
+				pushFront(ea, hole, nhole);
+				pushFront(r, hreg, nhreg);
+				pushFront(a, harea, nharea);
+				add = true;
+			}
+			else if (hole[nhole-1] == ea)
+			{
+				// The segment matches the end of the hole boundary.
+				pushBack(eb, hole, nhole);
+				pushBack(r, hreg, nhreg);
+				pushBack(a, harea, nharea);
+				add = true;
+			}
+			if (add)
+			{
+				// The edge segment was added, remove it.
+				edges[i*4+0] = edges[(nedges-1)*4+0];
+				edges[i*4+1] = edges[(nedges-1)*4+1];
+				edges[i*4+2] = edges[(nedges-1)*4+2];
+				edges[i*4+3] = edges[(nedges-1)*4+3];
+				--nedges;
+				match = true;
+				--i;
+			}
+		}
+		
+		if (!match)
+			break;
+	}
+
+	rcScopedDelete<int> tris((int*)rcAlloc(sizeof(int)*nhole*3, RC_ALLOC_TEMP));
+	if (!tris)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tris' (%d).", nhole*3);
+		return false;
+	}
+
+	rcScopedDelete<int> tverts((int*)rcAlloc(sizeof(int)*nhole*4, RC_ALLOC_TEMP));
+	if (!tverts)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tverts' (%d).", nhole*4);
+		return false;
+	}
+
+	rcScopedDelete<int> thole((int*)rcAlloc(sizeof(int)*nhole, RC_ALLOC_TEMP));
+	if (!thole)
+	{
+		ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'thole' (%d).", nhole);
+		return false;
+	}
+
+	// Generate temp vertex array for triangulation.
+	for (int i = 0; i < nhole; ++i)
+	{
+		const int pi = hole[i];
+		tverts[i*4+0] = mesh.verts[pi*3+0];
+		tverts[i*4+1] = mesh.verts[pi*3+1];
+		tverts[i*4+2] = mesh.verts[pi*3+2];
+		tverts[i*4+3] = 0;
+		thole[i] = i;
+	}
+
+	// Triangulate the hole.
+	int ntris = triangulate(nhole, &tverts[0], &thole[0], tris);
+	if (ntris < 0)
+	{
+		ntris = -ntris;
+		ctx->log(RC_LOG_WARNING, "removeVertex: triangulate() returned bad results.");
+	}
+	
+	// Merge the hole triangles back to polygons.
+	rcScopedDelete<unsigned short> polys((unsigned short*)rcAlloc(sizeof(unsigned short)*(ntris+1)*nvp, RC_ALLOC_TEMP));
+	if (!polys)
+	{
+		ctx->log(RC_LOG_ERROR, "removeVertex: Out of memory 'polys' (%d).", (ntris+1)*nvp);
+		return false;
+	}
+	rcScopedDelete<unsigned short> pregs((unsigned short*)rcAlloc(sizeof(unsigned short)*ntris, RC_ALLOC_TEMP));
+	if (!pregs)
+	{
+		ctx->log(RC_LOG_ERROR, "removeVertex: Out of memory 'pregs' (%d).", ntris);
+		return false;
+	}
+	rcScopedDelete<unsigned char> pareas((unsigned char*)rcAlloc(sizeof(unsigned char)*ntris, RC_ALLOC_TEMP));
+	if (!pareas)
+	{
+		ctx->log(RC_LOG_ERROR, "removeVertex: Out of memory 'pareas' (%d).", ntris);
+		return false;
+	}
+	
+	unsigned short* tmpPoly = &polys[ntris*nvp];
+			
+	// Build initial polygons.
+	int npolys = 0;
+	memset(polys, 0xff, ntris*nvp*sizeof(unsigned short));
+	for (int j = 0; j < ntris; ++j)
+	{
+		int* t = &tris[j*3];
+		if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2])
+		{
+			polys[npolys*nvp+0] = (unsigned short)hole[t[0]];
+			polys[npolys*nvp+1] = (unsigned short)hole[t[1]];
+			polys[npolys*nvp+2] = (unsigned short)hole[t[2]];
+
+			// If this polygon covers multiple region types then
+			// mark it as such
+			if (hreg[t[0]] != hreg[t[1]] || hreg[t[1]] != hreg[t[2]])
+				pregs[npolys] = RC_MULTIPLE_REGS;
+			else
+				pregs[npolys] = (unsigned short)hreg[t[0]];
+
+			pareas[npolys] = (unsigned char)harea[t[0]];
+			npolys++;
+		}
+	}
+	if (!npolys)
+		return true;
+	
+	// Merge polygons.
+	if (nvp > 3)
+	{
+		for (;;)
+		{
+			// Find best polygons to merge.
+			int bestMergeVal = 0;
+			int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0;
+			
+			for (int j = 0; j < npolys-1; ++j)
+			{
+				unsigned short* pj = &polys[j*nvp];
+				for (int k = j+1; k < npolys; ++k)
+				{
+					unsigned short* pk = &polys[k*nvp];
+					int ea, eb;
+					int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp);
+					if (v > bestMergeVal)
+					{
+						bestMergeVal = v;
+						bestPa = j;
+						bestPb = k;
+						bestEa = ea;
+						bestEb = eb;
+					}
+				}
+			}
+			
+			if (bestMergeVal > 0)
+			{
+				// Found best, merge.
+				unsigned short* pa = &polys[bestPa*nvp];
+				unsigned short* pb = &polys[bestPb*nvp];
+				mergePolyVerts(pa, pb, bestEa, bestEb, tmpPoly, nvp);
+				if (pregs[bestPa] != pregs[bestPb])
+					pregs[bestPa] = RC_MULTIPLE_REGS;
+
+				unsigned short* last = &polys[(npolys-1)*nvp];
+				if (pb != last)
+					memcpy(pb, last, sizeof(unsigned short)*nvp);
+				pregs[bestPb] = pregs[npolys-1];
+				pareas[bestPb] = pareas[npolys-1];
+				npolys--;
+			}
+			else
+			{
+				// Could not merge any polygons, stop.
+				break;
+			}
+		}
+	}
+	
+	// Store polygons.
+	for (int i = 0; i < npolys; ++i)
+	{
+		if (mesh.npolys >= maxTris) break;
+		unsigned short* p = &mesh.polys[mesh.npolys*nvp*2];
+		memset(p,0xff,sizeof(unsigned short)*nvp*2);
+		for (int j = 0; j < nvp; ++j)
+			p[j] = polys[i*nvp+j];
+		mesh.regs[mesh.npolys] = pregs[i];
+		mesh.areas[mesh.npolys] = pareas[i];
+		mesh.npolys++;
+		if (mesh.npolys > maxTris)
+		{
+			ctx->log(RC_LOG_ERROR, "removeVertex: Too many polygons %d (max:%d).", mesh.npolys, maxTris);
+			return false;
+		}
+	}
+	
+	return true;
+}
+
+/// @par
+///
+/// @note If the mesh data is to be used to construct a Detour navigation mesh, then the upper 
+/// limit must be retricted to <= #DT_VERTS_PER_POLYGON.
+///
+/// @see rcAllocPolyMesh, rcContourSet, rcPolyMesh, rcConfig
+bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_POLYMESH);
+
+	rcVcopy(mesh.bmin, cset.bmin);
+	rcVcopy(mesh.bmax, cset.bmax);
+	mesh.cs = cset.cs;
+	mesh.ch = cset.ch;
+	mesh.borderSize = cset.borderSize;
+	mesh.maxEdgeError = cset.maxError;
+	
+	int maxVertices = 0;
+	int maxTris = 0;
+	int maxVertsPerCont = 0;
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		// Skip null contours.
+		if (cset.conts[i].nverts < 3) continue;
+		maxVertices += cset.conts[i].nverts;
+		maxTris += cset.conts[i].nverts - 2;
+		maxVertsPerCont = rcMax(maxVertsPerCont, cset.conts[i].nverts);
+	}
+	
+	if (maxVertices >= 0xfffe)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many vertices %d.", maxVertices);
+		return false;
+	}
+		
+	rcScopedDelete<unsigned char> vflags((unsigned char*)rcAlloc(sizeof(unsigned char)*maxVertices, RC_ALLOC_TEMP));
+	if (!vflags)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'vflags' (%d).", maxVertices);
+		return false;
+	}
+	memset(vflags, 0, maxVertices);
+	
+	mesh.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxVertices*3, RC_ALLOC_PERM);
+	if (!mesh.verts)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices);
+		return false;
+	}
+	mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris*nvp*2, RC_ALLOC_PERM);
+	if (!mesh.polys)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2);
+		return false;
+	}
+	mesh.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris, RC_ALLOC_PERM);
+	if (!mesh.regs)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.regs' (%d).", maxTris);
+		return false;
+	}
+	mesh.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxTris, RC_ALLOC_PERM);
+	if (!mesh.areas)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.areas' (%d).", maxTris);
+		return false;
+	}
+	
+	mesh.nverts = 0;
+	mesh.npolys = 0;
+	mesh.nvp = nvp;
+	mesh.maxpolys = maxTris;
+	
+	memset(mesh.verts, 0, sizeof(unsigned short)*maxVertices*3);
+	memset(mesh.polys, 0xff, sizeof(unsigned short)*maxTris*nvp*2);
+	memset(mesh.regs, 0, sizeof(unsigned short)*maxTris);
+	memset(mesh.areas, 0, sizeof(unsigned char)*maxTris);
+	
+	rcScopedDelete<int> nextVert((int*)rcAlloc(sizeof(int)*maxVertices, RC_ALLOC_TEMP));
+	if (!nextVert)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'nextVert' (%d).", maxVertices);
+		return false;
+	}
+	memset(nextVert, 0, sizeof(int)*maxVertices);
+	
+	rcScopedDelete<int> firstVert((int*)rcAlloc(sizeof(int)*VERTEX_BUCKET_COUNT, RC_ALLOC_TEMP));
+	if (!firstVert)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT);
+		return false;
+	}
+	for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i)
+		firstVert[i] = -1;
+	
+	rcScopedDelete<int> indices((int*)rcAlloc(sizeof(int)*maxVertsPerCont, RC_ALLOC_TEMP));
+	if (!indices)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'indices' (%d).", maxVertsPerCont);
+		return false;
+	}
+	rcScopedDelete<int> tris((int*)rcAlloc(sizeof(int)*maxVertsPerCont*3, RC_ALLOC_TEMP));
+	if (!tris)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'tris' (%d).", maxVertsPerCont*3);
+		return false;
+	}
+	rcScopedDelete<unsigned short> polys((unsigned short*)rcAlloc(sizeof(unsigned short)*(maxVertsPerCont+1)*nvp, RC_ALLOC_TEMP));
+	if (!polys)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'polys' (%d).", maxVertsPerCont*nvp);
+		return false;
+	}
+	unsigned short* tmpPoly = &polys[maxVertsPerCont*nvp];
+
+	for (int i = 0; i < cset.nconts; ++i)
+	{
+		rcContour& cont = cset.conts[i];
+		
+		// Skip null contours.
+		if (cont.nverts < 3)
+			continue;
+		
+		// Triangulate contour
+		for (int j = 0; j < cont.nverts; ++j)
+			indices[j] = j;
+			
+		int ntris = triangulate(cont.nverts, cont.verts, &indices[0], &tris[0]);
+		if (ntris <= 0)
+		{
+			// Bad triangulation, should not happen.
+/*			printf("\tconst float bmin[3] = {%ff,%ff,%ff};\n", cset.bmin[0], cset.bmin[1], cset.bmin[2]);
+			printf("\tconst float cs = %ff;\n", cset.cs);
+			printf("\tconst float ch = %ff;\n", cset.ch);
+			printf("\tconst int verts[] = {\n");
+			for (int k = 0; k < cont.nverts; ++k)
+			{
+				const int* v = &cont.verts[k*4];
+				printf("\t\t%d,%d,%d,%d,\n", v[0], v[1], v[2], v[3]);
+			}
+			printf("\t};\n\tconst int nverts = sizeof(verts)/(sizeof(int)*4);\n");*/
+			ctx->log(RC_LOG_WARNING, "rcBuildPolyMesh: Bad triangulation Contour %d.", i);
+			ntris = -ntris;
+		}
+				
+		// Add and merge vertices.
+		for (int j = 0; j < cont.nverts; ++j)
+		{
+			const int* v = &cont.verts[j*4];
+			indices[j] = addVertex((unsigned short)v[0], (unsigned short)v[1], (unsigned short)v[2],
+								   mesh.verts, firstVert, nextVert, mesh.nverts);
+			if (v[3] & RC_BORDER_VERTEX)
+			{
+				// This vertex should be removed.
+				vflags[indices[j]] = 1;
+			}
+		}
+
+		// Build initial polygons.
+		int npolys = 0;
+		memset(polys, 0xff, maxVertsPerCont*nvp*sizeof(unsigned short));
+		for (int j = 0; j < ntris; ++j)
+		{
+			int* t = &tris[j*3];
+			if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2])
+			{
+				polys[npolys*nvp+0] = (unsigned short)indices[t[0]];
+				polys[npolys*nvp+1] = (unsigned short)indices[t[1]];
+				polys[npolys*nvp+2] = (unsigned short)indices[t[2]];
+				npolys++;
+			}
+		}
+		if (!npolys)
+			continue;
+		
+		// Merge polygons.
+		if (nvp > 3)
+		{
+			for(;;)
+			{
+				// Find best polygons to merge.
+				int bestMergeVal = 0;
+				int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0;
+				
+				for (int j = 0; j < npolys-1; ++j)
+				{
+					unsigned short* pj = &polys[j*nvp];
+					for (int k = j+1; k < npolys; ++k)
+					{
+						unsigned short* pk = &polys[k*nvp];
+						int ea, eb;
+						int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp);
+						if (v > bestMergeVal)
+						{
+							bestMergeVal = v;
+							bestPa = j;
+							bestPb = k;
+							bestEa = ea;
+							bestEb = eb;
+						}
+					}
+				}
+				
+				if (bestMergeVal > 0)
+				{
+					// Found best, merge.
+					unsigned short* pa = &polys[bestPa*nvp];
+					unsigned short* pb = &polys[bestPb*nvp];
+					mergePolyVerts(pa, pb, bestEa, bestEb, tmpPoly, nvp);
+					unsigned short* lastPoly = &polys[(npolys-1)*nvp];
+					if (pb != lastPoly)
+						memcpy(pb, lastPoly, sizeof(unsigned short)*nvp);
+					npolys--;
+				}
+				else
+				{
+					// Could not merge any polygons, stop.
+					break;
+				}
+			}
+		}
+		
+		// Store polygons.
+		for (int j = 0; j < npolys; ++j)
+		{
+			unsigned short* p = &mesh.polys[mesh.npolys*nvp*2];
+			unsigned short* q = &polys[j*nvp];
+			for (int k = 0; k < nvp; ++k)
+				p[k] = q[k];
+			mesh.regs[mesh.npolys] = cont.reg;
+			mesh.areas[mesh.npolys] = cont.area;
+			mesh.npolys++;
+			if (mesh.npolys > maxTris)
+			{
+				ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many polygons %d (max:%d).", mesh.npolys, maxTris);
+				return false;
+			}
+		}
+	}
+	
+	
+	// Remove edge vertices.
+	for (int i = 0; i < mesh.nverts; ++i)
+	{
+		if (vflags[i])
+		{
+			if (!canRemoveVertex(ctx, mesh, (unsigned short)i))
+				continue;
+			if (!removeVertex(ctx, mesh, (unsigned short)i, maxTris))
+			{
+				// Failed to remove vertex
+				ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Failed to remove edge vertex %d.", i);
+				return false;
+			}
+			// Remove vertex
+			// Note: mesh.nverts is already decremented inside removeVertex()!
+			// Fixup vertex flags
+			for (int j = i; j < mesh.nverts; ++j)
+				vflags[j] = vflags[j+1];
+			--i;
+		}
+	}
+	
+	// Calculate adjacency.
+	if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, nvp))
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed.");
+		return false;
+	}
+	
+	// Find portal edges
+	if (mesh.borderSize > 0)
+	{
+		const int w = cset.width;
+		const int h = cset.height;
+		for (int i = 0; i < mesh.npolys; ++i)
+		{
+			unsigned short* p = &mesh.polys[i*2*nvp];
+			for (int j = 0; j < nvp; ++j)
+			{
+				if (p[j] == RC_MESH_NULL_IDX) break;
+				// Skip connected edges.
+				if (p[nvp+j] != RC_MESH_NULL_IDX)
+					continue;
+				int nj = j+1;
+				if (nj >= nvp || p[nj] == RC_MESH_NULL_IDX) nj = 0;
+				const unsigned short* va = &mesh.verts[p[j]*3];
+				const unsigned short* vb = &mesh.verts[p[nj]*3];
+
+				if ((int)va[0] == 0 && (int)vb[0] == 0)
+					p[nvp+j] = 0x8000 | 0;
+				else if ((int)va[2] == h && (int)vb[2] == h)
+					p[nvp+j] = 0x8000 | 1;
+				else if ((int)va[0] == w && (int)vb[0] == w)
+					p[nvp+j] = 0x8000 | 2;
+				else if ((int)va[2] == 0 && (int)vb[2] == 0)
+					p[nvp+j] = 0x8000 | 3;
+			}
+		}
+	}
+
+	// Just allocate the mesh flags array. The user is resposible to fill it.
+	mesh.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*mesh.npolys, RC_ALLOC_PERM);
+	if (!mesh.flags)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.flags' (%d).", mesh.npolys);
+		return false;
+	}
+	memset(mesh.flags, 0, sizeof(unsigned short) * mesh.npolys);
+	
+	if (mesh.nverts > 0xffff)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff);
+	}
+	if (mesh.npolys > 0xffff)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff);
+	}
+	
+	return true;
+}
+
+/// @see rcAllocPolyMesh, rcPolyMesh
+bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh)
+{
+	rcAssert(ctx);
+	
+	if (!nmeshes || !meshes)
+		return true;
+
+	rcScopedTimer timer(ctx, RC_TIMER_MERGE_POLYMESH);
+
+	mesh.nvp = meshes[0]->nvp;
+	mesh.cs = meshes[0]->cs;
+	mesh.ch = meshes[0]->ch;
+	rcVcopy(mesh.bmin, meshes[0]->bmin);
+	rcVcopy(mesh.bmax, meshes[0]->bmax);
+
+	int maxVerts = 0;
+	int maxPolys = 0;
+	int maxVertsPerMesh = 0;
+	for (int i = 0; i < nmeshes; ++i)
+	{
+		rcVmin(mesh.bmin, meshes[i]->bmin);
+		rcVmax(mesh.bmax, meshes[i]->bmax);
+		maxVertsPerMesh = rcMax(maxVertsPerMesh, meshes[i]->nverts);
+		maxVerts += meshes[i]->nverts;
+		maxPolys += meshes[i]->npolys;
+	}
+	
+	mesh.nverts = 0;
+	mesh.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxVerts*3, RC_ALLOC_PERM);
+	if (!mesh.verts)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.verts' (%d).", maxVerts*3);
+		return false;
+	}
+
+	mesh.npolys = 0;
+	mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxPolys*2*mesh.nvp, RC_ALLOC_PERM);
+	if (!mesh.polys)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.polys' (%d).", maxPolys*2*mesh.nvp);
+		return false;
+	}
+	memset(mesh.polys, 0xff, sizeof(unsigned short)*maxPolys*2*mesh.nvp);
+
+	mesh.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxPolys, RC_ALLOC_PERM);
+	if (!mesh.regs)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.regs' (%d).", maxPolys);
+		return false;
+	}
+	memset(mesh.regs, 0, sizeof(unsigned short)*maxPolys);
+
+	mesh.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxPolys, RC_ALLOC_PERM);
+	if (!mesh.areas)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.areas' (%d).", maxPolys);
+		return false;
+	}
+	memset(mesh.areas, 0, sizeof(unsigned char)*maxPolys);
+
+	mesh.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxPolys, RC_ALLOC_PERM);
+	if (!mesh.flags)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.flags' (%d).", maxPolys);
+		return false;
+	}
+	memset(mesh.flags, 0, sizeof(unsigned short)*maxPolys);
+	
+	rcScopedDelete<int> nextVert((int*)rcAlloc(sizeof(int)*maxVerts, RC_ALLOC_TEMP));
+	if (!nextVert)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'nextVert' (%d).", maxVerts);
+		return false;
+	}
+	memset(nextVert, 0, sizeof(int)*maxVerts);
+	
+	rcScopedDelete<int> firstVert((int*)rcAlloc(sizeof(int)*VERTEX_BUCKET_COUNT, RC_ALLOC_TEMP));
+	if (!firstVert)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT);
+		return false;
+	}
+	for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i)
+		firstVert[i] = -1;
+
+	rcScopedDelete<unsigned short> vremap((unsigned short*)rcAlloc(sizeof(unsigned short)*maxVertsPerMesh, RC_ALLOC_PERM));
+	if (!vremap)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh);
+		return false;
+	}
+	memset(vremap, 0, sizeof(unsigned short)*maxVertsPerMesh);
+	
+	for (int i = 0; i < nmeshes; ++i)
+	{
+		const rcPolyMesh* pmesh = meshes[i];
+		
+		const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f);
+		const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f);
+		
+		bool isMinX = (ox == 0);
+		bool isMinZ = (oz == 0);
+		bool isMaxX = ((unsigned short)floorf((mesh.bmax[0] - pmesh->bmax[0]) / mesh.cs + 0.5f)) == 0;
+		bool isMaxZ = ((unsigned short)floorf((mesh.bmax[2] - pmesh->bmax[2]) / mesh.cs + 0.5f)) == 0;
+		bool isOnBorder = (isMinX || isMinZ || isMaxX || isMaxZ);
+
+		for (int j = 0; j < pmesh->nverts; ++j)
+		{
+			unsigned short* v = &pmesh->verts[j*3];
+			vremap[j] = addVertex(v[0]+ox, v[1], v[2]+oz,
+								  mesh.verts, firstVert, nextVert, mesh.nverts);
+		}
+		
+		for (int j = 0; j < pmesh->npolys; ++j)
+		{
+			unsigned short* tgt = &mesh.polys[mesh.npolys*2*mesh.nvp];
+			unsigned short* src = &pmesh->polys[j*2*mesh.nvp];
+			mesh.regs[mesh.npolys] = pmesh->regs[j];
+			mesh.areas[mesh.npolys] = pmesh->areas[j];
+			mesh.flags[mesh.npolys] = pmesh->flags[j];
+			mesh.npolys++;
+			for (int k = 0; k < mesh.nvp; ++k)
+			{
+				if (src[k] == RC_MESH_NULL_IDX) break;
+				tgt[k] = vremap[src[k]];
+			}
+
+			if (isOnBorder)
+			{
+				for (int k = mesh.nvp; k < mesh.nvp * 2; ++k)
+				{
+					if (src[k] & 0x8000 && src[k] != 0xffff)
+					{
+						unsigned short dir = src[k] & 0xf;
+						switch (dir)
+						{
+							case 0: // Portal x-
+								if (isMinX)
+									tgt[k] = src[k];
+								break;
+							case 1: // Portal z+
+								if (isMaxZ)
+									tgt[k] = src[k];
+								break;
+							case 2: // Portal x+
+								if (isMaxX)
+									tgt[k] = src[k];
+								break;
+							case 3: // Portal z-
+								if (isMinZ)
+									tgt[k] = src[k];
+								break;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	// Calculate adjacency.
+	if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, mesh.nvp))
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Adjacency failed.");
+		return false;
+	}
+
+	if (mesh.nverts > 0xffff)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff);
+	}
+	if (mesh.npolys > 0xffff)
+	{
+		ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff);
+	}
+	
+	return true;
+}
+
+bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst)
+{
+	rcAssert(ctx);
+	
+	// Destination must be empty.
+	rcAssert(dst.verts == 0);
+	rcAssert(dst.polys == 0);
+	rcAssert(dst.regs == 0);
+	rcAssert(dst.areas == 0);
+	rcAssert(dst.flags == 0);
+	
+	dst.nverts = src.nverts;
+	dst.npolys = src.npolys;
+	dst.maxpolys = src.npolys;
+	dst.nvp = src.nvp;
+	rcVcopy(dst.bmin, src.bmin);
+	rcVcopy(dst.bmax, src.bmax);
+	dst.cs = src.cs;
+	dst.ch = src.ch;
+	dst.borderSize = src.borderSize;
+	dst.maxEdgeError = src.maxEdgeError;
+	
+	dst.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.nverts*3, RC_ALLOC_PERM);
+	if (!dst.verts)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.verts' (%d).", src.nverts*3);
+		return false;
+	}
+	memcpy(dst.verts, src.verts, sizeof(unsigned short)*src.nverts*3);
+	
+	dst.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys*2*src.nvp, RC_ALLOC_PERM);
+	if (!dst.polys)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.polys' (%d).", src.npolys*2*src.nvp);
+		return false;
+	}
+	memcpy(dst.polys, src.polys, sizeof(unsigned short)*src.npolys*2*src.nvp);
+	
+	dst.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM);
+	if (!dst.regs)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.regs' (%d).", src.npolys);
+		return false;
+	}
+	memcpy(dst.regs, src.regs, sizeof(unsigned short)*src.npolys);
+	
+	dst.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*src.npolys, RC_ALLOC_PERM);
+	if (!dst.areas)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.areas' (%d).", src.npolys);
+		return false;
+	}
+	memcpy(dst.areas, src.areas, sizeof(unsigned char)*src.npolys);
+	
+	dst.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM);
+	if (!dst.flags)
+	{
+		ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.flags' (%d).", src.npolys);
+		return false;
+	}
+	memcpy(dst.flags, src.flags, sizeof(unsigned short)*src.npolys);
+	
+	return true;
+}

panda/src/recastdetour/RecastMeshDetail.cpp

@@ -0,0 +1,1464 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+
+static const unsigned RC_UNSET_HEIGHT = 0xffff;
+
+struct rcHeightPatch
+{
+	inline rcHeightPatch() : data(0), xmin(0), ymin(0), width(0), height(0) {}
+	inline ~rcHeightPatch() { rcFree(data); }
+	unsigned short* data;
+	int xmin, ymin, width, height;
+};
+
+
+inline float vdot2(const float* a, const float* b)
+{
+	return a[0]*b[0] + a[2]*b[2];
+}
+
+inline float vdistSq2(const float* p, const float* q)
+{
+	const float dx = q[0] - p[0];
+	const float dy = q[2] - p[2];
+	return dx*dx + dy*dy;
+}
+
+inline float vdist2(const float* p, const float* q)
+{
+	return sqrtf(vdistSq2(p,q));
+}
+
+inline float vcross2(const float* p1, const float* p2, const float* p3)
+{
+	const float u1 = p2[0] - p1[0];
+	const float v1 = p2[2] - p1[2];
+	const float u2 = p3[0] - p1[0];
+	const float v2 = p3[2] - p1[2];
+	return u1 * v2 - v1 * u2;
+}
+
+static bool circumCircle(const float* p1, const float* p2, const float* p3,
+						 float* c, float& r)
+{
+	static const float EPS = 1e-6f;
+	// Calculate the circle relative to p1, to avoid some precision issues.
+	const float v1[3] = {0,0,0};
+	float v2[3], v3[3];
+	rcVsub(v2, p2,p1);
+	rcVsub(v3, p3,p1);
+	
+	const float cp = vcross2(v1, v2, v3);
+	if (fabsf(cp) > EPS)
+	{
+		const float v1Sq = vdot2(v1,v1);
+		const float v2Sq = vdot2(v2,v2);
+		const float v3Sq = vdot2(v3,v3);
+		c[0] = (v1Sq*(v2[2]-v3[2]) + v2Sq*(v3[2]-v1[2]) + v3Sq*(v1[2]-v2[2])) / (2*cp);
+		c[1] = 0;
+		c[2] = (v1Sq*(v3[0]-v2[0]) + v2Sq*(v1[0]-v3[0]) + v3Sq*(v2[0]-v1[0])) / (2*cp);
+		r = vdist2(c, v1);
+		rcVadd(c, c, p1);
+		return true;
+	}
+	
+	rcVcopy(c, p1);
+	r = 0;
+	return false;
+}
+
+static float distPtTri(const float* p, const float* a, const float* b, const float* c)
+{
+	float v0[3], v1[3], v2[3];
+	rcVsub(v0, c,a);
+	rcVsub(v1, b,a);
+	rcVsub(v2, p,a);
+	
+	const float dot00 = vdot2(v0, v0);
+	const float dot01 = vdot2(v0, v1);
+	const float dot02 = vdot2(v0, v2);
+	const float dot11 = vdot2(v1, v1);
+	const float dot12 = vdot2(v1, v2);
+	
+	// Compute barycentric coordinates
+	const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
+	const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+	float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+	
+	// If point lies inside the triangle, return interpolated y-coord.
+	static const float EPS = 1e-4f;
+	if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
+	{
+		const float y = a[1] + v0[1]*u + v1[1]*v;
+		return fabsf(y-p[1]);
+	}
+	return FLT_MAX;
+}
+
+static float distancePtSeg(const float* pt, const float* p, const float* q)
+{
+	float pqx = q[0] - p[0];
+	float pqy = q[1] - p[1];
+	float pqz = q[2] - p[2];
+	float dx = pt[0] - p[0];
+	float dy = pt[1] - p[1];
+	float dz = pt[2] - p[2];
+	float d = pqx*pqx + pqy*pqy + pqz*pqz;
+	float t = pqx*dx + pqy*dy + 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];
+	dy = p[1] + t*pqy - pt[1];
+	dz = p[2] + t*pqz - pt[2];
+	
+	return dx*dx + dy*dy + dz*dz;
+}
+
+static float distancePtSeg2d(const float* pt, const float* p, const float* q)
+{
+	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;
+	float 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;
+}
+
+static float distToTriMesh(const float* p, const float* verts, const int /*nverts*/, const int* tris, const int ntris)
+{
+	float dmin = FLT_MAX;
+	for (int i = 0; i < ntris; ++i)
+	{
+		const float* va = &verts[tris[i*4+0]*3];
+		const float* vb = &verts[tris[i*4+1]*3];
+		const float* vc = &verts[tris[i*4+2]*3];
+		float d = distPtTri(p, va,vb,vc);
+		if (d < dmin)
+			dmin = d;
+	}
+	if (dmin == FLT_MAX) return -1;
+	return dmin;
+}
+
+static float distToPoly(int nvert, const float* verts, const float* p)
+{
+	
+	float dmin = FLT_MAX;
+	int i, j, c = 0;
+	for (i = 0, j = nvert-1; i < nvert; j = i++)
+	{
+		const float* vi = &verts[i*3];
+		const float* vj = &verts[j*3];
+		if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
+			(p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
+			c = !c;
+		dmin = rcMin(dmin, distancePtSeg2d(p, vj, vi));
+	}
+	return c ? -dmin : dmin;
+}
+
+
+static unsigned short getHeight(const float fx, const float fy, const float fz,
+								const float /*cs*/, const float ics, const float ch,
+								const int radius, const rcHeightPatch& hp)
+{
+	int ix = (int)floorf(fx*ics + 0.01f);
+	int iz = (int)floorf(fz*ics + 0.01f);
+	ix = rcClamp(ix-hp.xmin, 0, hp.width - 1);
+	iz = rcClamp(iz-hp.ymin, 0, hp.height - 1);
+	unsigned short h = hp.data[ix+iz*hp.width];
+	if (h == RC_UNSET_HEIGHT)
+	{
+		// Special case when data might be bad.
+		// Walk adjacent cells in a spiral up to 'radius', and look
+		// for a pixel which has a valid height.
+		int x = 1, z = 0, dx = 1, dz = 0;
+		int maxSize = radius * 2 + 1;
+		int maxIter = maxSize * maxSize - 1;
+
+		int nextRingIterStart = 8;
+		int nextRingIters = 16;
+
+		float dmin = FLT_MAX;
+		for (int i = 0; i < maxIter; i++)
+		{
+			const int nx = ix + x;
+			const int nz = iz + z;
+
+			if (nx >= 0 && nz >= 0 && nx < hp.width && nz < hp.height)
+			{
+				const unsigned short nh = hp.data[nx + nz*hp.width];
+				if (nh != RC_UNSET_HEIGHT)
+				{
+					const float d = fabsf(nh*ch - fy);
+					if (d < dmin)
+					{
+						h = nh;
+						dmin = d;
+					}
+				}
+			}
+
+			// We are searching in a grid which looks approximately like this:
+			//  __________
+			// |2 ______ 2|
+			// | |1 __ 1| |
+			// | | |__| | |
+			// | |______| |
+			// |__________|
+			// We want to find the best height as close to the center cell as possible. This means that
+			// if we find a height in one of the neighbor cells to the center, we don't want to
+			// expand further out than the 8 neighbors - we want to limit our search to the closest
+			// of these "rings", but the best height in the ring.
+			// For example, the center is just 1 cell. We checked that at the entrance to the function.
+			// The next "ring" contains 8 cells (marked 1 above). Those are all the neighbors to the center cell.
+			// The next one again contains 16 cells (marked 2). In general each ring has 8 additional cells, which
+			// can be thought of as adding 2 cells around the "center" of each side when we expand the ring.
+			// Here we detect if we are about to enter the next ring, and if we are and we have found
+			// a height, we abort the search.
+			if (i + 1 == nextRingIterStart)
+			{
+				if (h != RC_UNSET_HEIGHT)
+					break;
+
+				nextRingIterStart += nextRingIters;
+				nextRingIters += 8;
+			}
+
+			if ((x == z) || ((x < 0) && (x == -z)) || ((x > 0) && (x == 1 - z)))
+			{
+				int tmp = dx;
+				dx = -dz;
+				dz = tmp;
+			}
+			x += dx;
+			z += dz;
+		}
+	}
+	return h;
+}
+
+
+enum EdgeValues
+{
+	EV_UNDEF = -1,
+	EV_HULL = -2,
+};
+
+static int findEdge(const int* edges, int nedges, int s, int t)
+{
+	for (int i = 0; i < nedges; i++)
+	{
+		const int* e = &edges[i*4];
+		if ((e[0] == s && e[1] == t) || (e[0] == t && e[1] == s))
+			return i;
+	}
+	return EV_UNDEF;
+}
+
+static int addEdge(rcContext* ctx, int* edges, int& nedges, const int maxEdges, int s, int t, int l, int r)
+{
+	if (nedges >= maxEdges)
+	{
+		ctx->log(RC_LOG_ERROR, "addEdge: Too many edges (%d/%d).", nedges, maxEdges);
+		return EV_UNDEF;
+	}
+	
+	// Add edge if not already in the triangulation.
+	int e = findEdge(edges, nedges, s, t);
+	if (e == EV_UNDEF)
+	{
+		int* edge = &edges[nedges*4];
+		edge[0] = s;
+		edge[1] = t;
+		edge[2] = l;
+		edge[3] = r;
+		return nedges++;
+	}
+	else
+	{
+		return EV_UNDEF;
+	}
+}
+
+static void updateLeftFace(int* e, int s, int t, int f)
+{
+	if (e[0] == s && e[1] == t && e[2] == EV_UNDEF)
+		e[2] = f;
+	else if (e[1] == s && e[0] == t && e[3] == EV_UNDEF)
+		e[3] = f;
+}
+
+static int overlapSegSeg2d(const float* a, const float* b, const float* c, const float* d)
+{
+	const float a1 = vcross2(a, b, d);
+	const float a2 = vcross2(a, b, c);
+	if (a1*a2 < 0.0f)
+	{
+		float a3 = vcross2(c, d, a);
+		float a4 = a3 + a2 - a1;
+		if (a3 * a4 < 0.0f)
+			return 1;
+	}
+	return 0;
+}
+
+static bool overlapEdges(const float* pts, const int* edges, int nedges, int s1, int t1)
+{
+	for (int i = 0; i < nedges; ++i)
+	{
+		const int s0 = edges[i*4+0];
+		const int t0 = edges[i*4+1];
+		// Same or connected edges do not overlap.
+		if (s0 == s1 || s0 == t1 || t0 == s1 || t0 == t1)
+			continue;
+		if (overlapSegSeg2d(&pts[s0*3],&pts[t0*3], &pts[s1*3],&pts[t1*3]))
+			return true;
+	}
+	return false;
+}
+
+static void completeFacet(rcContext* ctx, const float* pts, int npts, int* edges, int& nedges, const int maxEdges, int& nfaces, int e)
+{
+	static const float EPS = 1e-5f;
+	
+	int* edge = &edges[e*4];
+	
+	// Cache s and t.
+	int s,t;
+	if (edge[2] == EV_UNDEF)
+	{
+		s = edge[0];
+		t = edge[1];
+	}
+	else if (edge[3] == EV_UNDEF)
+	{
+		s = edge[1];
+		t = edge[0];
+	}
+	else
+	{
+	    // Edge already completed.
+	    return;
+	}
+    
+	// Find best point on left of edge.
+	int pt = npts;
+	float c[3] = {0,0,0};
+	float r = -1;
+	for (int u = 0; u < npts; ++u)
+	{
+		if (u == s || u == t) continue;
+		if (vcross2(&pts[s*3], &pts[t*3], &pts[u*3]) > EPS)
+		{
+			if (r < 0)
+			{
+				// The circle is not updated yet, do it now.
+				pt = u;
+				circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
+				continue;
+			}
+			const float d = vdist2(c, &pts[u*3]);
+			const float tol = 0.001f;
+			if (d > r*(1+tol))
+			{
+				// Outside current circumcircle, skip.
+				continue;
+			}
+			else if (d < r*(1-tol))
+			{
+				// Inside safe circumcircle, update circle.
+				pt = u;
+				circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
+			}
+			else
+			{
+				// Inside epsilon circum circle, do extra tests to make sure the edge is valid.
+				// s-u and t-u cannot overlap with s-pt nor t-pt if they exists.
+				if (overlapEdges(pts, edges, nedges, s,u))
+					continue;
+				if (overlapEdges(pts, edges, nedges, t,u))
+					continue;
+				// Edge is valid.
+				pt = u;
+				circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
+			}
+		}
+	}
+	
+	// Add new triangle or update edge info if s-t is on hull.
+	if (pt < npts)
+	{
+		// Update face information of edge being completed.
+		updateLeftFace(&edges[e*4], s, t, nfaces);
+		
+		// Add new edge or update face info of old edge.
+		e = findEdge(edges, nedges, pt, s);
+		if (e == EV_UNDEF)
+		    addEdge(ctx, edges, nedges, maxEdges, pt, s, nfaces, EV_UNDEF);
+		else
+		    updateLeftFace(&edges[e*4], pt, s, nfaces);
+		
+		// Add new edge or update face info of old edge.
+		e = findEdge(edges, nedges, t, pt);
+		if (e == EV_UNDEF)
+		    addEdge(ctx, edges, nedges, maxEdges, t, pt, nfaces, EV_UNDEF);
+		else
+		    updateLeftFace(&edges[e*4], t, pt, nfaces);
+		
+		nfaces++;
+	}
+	else
+	{
+		updateLeftFace(&edges[e*4], s, t, EV_HULL);
+	}
+}
+
+static void delaunayHull(rcContext* ctx, const int npts, const float* pts,
+						 const int nhull, const int* hull,
+						 rcIntArray& tris, rcIntArray& edges)
+{
+	int nfaces = 0;
+	int nedges = 0;
+	const int maxEdges = npts*10;
+	edges.resize(maxEdges*4);
+	
+	for (int i = 0, j = nhull-1; i < nhull; j=i++)
+		addEdge(ctx, &edges[0], nedges, maxEdges, hull[j],hull[i], EV_HULL, EV_UNDEF);
+	
+	int currentEdge = 0;
+	while (currentEdge < nedges)
+	{
+		if (edges[currentEdge*4+2] == EV_UNDEF)
+			completeFacet(ctx, pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge);
+		if (edges[currentEdge*4+3] == EV_UNDEF)
+			completeFacet(ctx, pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge);
+		currentEdge++;
+	}
+	
+	// Create tris
+	tris.resize(nfaces*4);
+	for (int i = 0; i < nfaces*4; ++i)
+		tris[i] = -1;
+	
+	for (int i = 0; i < nedges; ++i)
+	{
+		const int* e = &edges[i*4];
+		if (e[3] >= 0)
+		{
+			// Left face
+			int* t = &tris[e[3]*4];
+			if (t[0] == -1)
+			{
+				t[0] = e[0];
+				t[1] = e[1];
+			}
+			else if (t[0] == e[1])
+				t[2] = e[0];
+			else if (t[1] == e[0])
+				t[2] = e[1];
+		}
+		if (e[2] >= 0)
+		{
+			// Right
+			int* t = &tris[e[2]*4];
+			if (t[0] == -1)
+			{
+				t[0] = e[1];
+				t[1] = e[0];
+			}
+			else if (t[0] == e[0])
+				t[2] = e[1];
+			else if (t[1] == e[1])
+				t[2] = e[0];
+		}
+	}
+	
+	for (int i = 0; i < tris.size()/4; ++i)
+	{
+		int* t = &tris[i*4];
+		if (t[0] == -1 || t[1] == -1 || t[2] == -1)
+		{
+			ctx->log(RC_LOG_WARNING, "delaunayHull: Removing dangling face %d [%d,%d,%d].", i, t[0],t[1],t[2]);
+			t[0] = tris[tris.size()-4];
+			t[1] = tris[tris.size()-3];
+			t[2] = tris[tris.size()-2];
+			t[3] = tris[tris.size()-1];
+			tris.resize(tris.size()-4);
+			--i;
+		}
+	}
+}
+
+// Calculate minimum extend of the polygon.
+static float polyMinExtent(const float* verts, const int nverts)
+{
+	float minDist = FLT_MAX;
+	for (int i = 0; i < nverts; i++)
+	{
+		const int ni = (i+1) % nverts;
+		const float* p1 = &verts[i*3];
+		const float* p2 = &verts[ni*3];
+		float maxEdgeDist = 0;
+		for (int j = 0; j < nverts; j++)
+		{
+			if (j == i || j == ni) continue;
+			float d = distancePtSeg2d(&verts[j*3], p1,p2);
+			maxEdgeDist = rcMax(maxEdgeDist, d);
+		}
+		minDist = rcMin(minDist, maxEdgeDist);
+	}
+	return rcSqrt(minDist);
+}
+
+// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv).
+inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; }
+inline int next(int i, int n) { return i+1 < n ? i+1 : 0; }
+
+static void triangulateHull(const int /*nverts*/, const float* verts, const int nhull, const int* hull, const int nin, rcIntArray& tris)
+{
+	int start = 0, left = 1, right = nhull-1;
+	
+	// Start from an ear with shortest perimeter.
+	// This tends to favor well formed triangles as starting point.
+	float dmin = FLT_MAX;
+	for (int i = 0; i < nhull; i++)
+	{
+		if (hull[i] >= nin) continue; // Ears are triangles with original vertices as middle vertex while others are actually line segments on edges
+		int pi = prev(i, nhull);
+		int ni = next(i, nhull);
+		const float* pv = &verts[hull[pi]*3];
+		const float* cv = &verts[hull[i]*3];
+		const float* nv = &verts[hull[ni]*3];
+		const float d = vdist2(pv,cv) + vdist2(cv,nv) + vdist2(nv,pv);
+		if (d < dmin)
+		{
+			start = i;
+			left = ni;
+			right = pi;
+			dmin = d;
+		}
+	}
+	
+	// Add first triangle
+	tris.push(hull[start]);
+	tris.push(hull[left]);
+	tris.push(hull[right]);
+	tris.push(0);
+	
+	// Triangulate the polygon by moving left or right,
+	// depending on which triangle has shorter perimeter.
+	// This heuristic was chose emprically, since it seems
+	// handle tesselated straight edges well.
+	while (next(left, nhull) != right)
+	{
+		// Check to see if se should advance left or right.
+		int nleft = next(left, nhull);
+		int nright = prev(right, nhull);
+		
+		const float* cvleft = &verts[hull[left]*3];
+		const float* nvleft = &verts[hull[nleft]*3];
+		const float* cvright = &verts[hull[right]*3];
+		const float* nvright = &verts[hull[nright]*3];
+		const float dleft = vdist2(cvleft, nvleft) + vdist2(nvleft, cvright);
+		const float dright = vdist2(cvright, nvright) + vdist2(cvleft, nvright);
+		
+		if (dleft < dright)
+		{
+			tris.push(hull[left]);
+			tris.push(hull[nleft]);
+			tris.push(hull[right]);
+			tris.push(0);
+			left = nleft;
+		}
+		else
+		{
+			tris.push(hull[left]);
+			tris.push(hull[nright]);
+			tris.push(hull[right]);
+			tris.push(0);
+			right = nright;
+		}
+	}
+}
+
+
+inline float getJitterX(const int i)
+{
+	return (((i * 0x8da6b343) & 0xffff) / 65535.0f * 2.0f) - 1.0f;
+}
+
+inline float getJitterY(const int i)
+{
+	return (((i * 0xd8163841) & 0xffff) / 65535.0f * 2.0f) - 1.0f;
+}
+
+static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin,
+							const float sampleDist, const float sampleMaxError,
+							const int heightSearchRadius, const rcCompactHeightfield& chf,
+							const rcHeightPatch& hp, float* verts, int& nverts,
+							rcIntArray& tris, rcIntArray& edges, rcIntArray& samples)
+{
+	static const int MAX_VERTS = 127;
+	static const int MAX_TRIS = 255;	// Max tris for delaunay is 2n-2-k (n=num verts, k=num hull verts).
+	static const int MAX_VERTS_PER_EDGE = 32;
+	float edge[(MAX_VERTS_PER_EDGE+1)*3];
+	int hull[MAX_VERTS];
+	int nhull = 0;
+	
+	nverts = nin;
+	
+	for (int i = 0; i < nin; ++i)
+		rcVcopy(&verts[i*3], &in[i*3]);
+	
+	edges.resize(0);
+	tris.resize(0);
+	
+	const float cs = chf.cs;
+	const float ics = 1.0f/cs;
+	
+	// Calculate minimum extents of the polygon based on input data.
+	float minExtent = polyMinExtent(verts, nverts);
+	
+	// Tessellate outlines.
+	// This is done in separate pass in order to ensure
+	// seamless height values across the ply boundaries.
+	if (sampleDist > 0)
+	{
+		for (int i = 0, j = nin-1; i < nin; j=i++)
+		{
+			const float* vj = &in[j*3];
+			const float* vi = &in[i*3];
+			bool swapped = false;
+			// Make sure the segments are always handled in same order
+			// using lexological sort or else there will be seams.
+			if (fabsf(vj[0]-vi[0]) < 1e-6f)
+			{
+				if (vj[2] > vi[2])
+				{
+					rcSwap(vj,vi);
+					swapped = true;
+				}
+			}
+			else
+			{
+				if (vj[0] > vi[0])
+				{
+					rcSwap(vj,vi);
+					swapped = true;
+				}
+			}
+			// Create samples along the edge.
+			float dx = vi[0] - vj[0];
+			float dy = vi[1] - vj[1];
+			float dz = vi[2] - vj[2];
+			float d = sqrtf(dx*dx + dz*dz);
+			int nn = 1 + (int)floorf(d/sampleDist);
+			if (nn >= MAX_VERTS_PER_EDGE) nn = MAX_VERTS_PER_EDGE-1;
+			if (nverts+nn >= MAX_VERTS)
+				nn = MAX_VERTS-1-nverts;
+			
+			for (int k = 0; k <= nn; ++k)
+			{
+				float u = (float)k/(float)nn;
+				float* pos = &edge[k*3];
+				pos[0] = vj[0] + dx*u;
+				pos[1] = vj[1] + dy*u;
+				pos[2] = vj[2] + dz*u;
+				pos[1] = getHeight(pos[0],pos[1],pos[2], cs, ics, chf.ch, heightSearchRadius, hp)*chf.ch;
+			}
+			// Simplify samples.
+			int idx[MAX_VERTS_PER_EDGE] = {0,nn};
+			int nidx = 2;
+			for (int k = 0; k < nidx-1; )
+			{
+				const int a = idx[k];
+				const int b = idx[k+1];
+				const float* va = &edge[a*3];
+				const float* vb = &edge[b*3];
+				// Find maximum deviation along the segment.
+				float maxd = 0;
+				int maxi = -1;
+				for (int m = a+1; m < b; ++m)
+				{
+					float dev = distancePtSeg(&edge[m*3],va,vb);
+					if (dev > maxd)
+					{
+						maxd = dev;
+						maxi = m;
+					}
+				}
+				// If the max deviation is larger than accepted error,
+				// add new point, else continue to next segment.
+				if (maxi != -1 && maxd > rcSqr(sampleMaxError))
+				{
+					for (int m = nidx; m > k; --m)
+						idx[m] = idx[m-1];
+					idx[k+1] = maxi;
+					nidx++;
+				}
+				else
+				{
+					++k;
+				}
+			}
+			
+			hull[nhull++] = j;
+			// Add new vertices.
+			if (swapped)
+			{
+				for (int k = nidx-2; k > 0; --k)
+				{
+					rcVcopy(&verts[nverts*3], &edge[idx[k]*3]);
+					hull[nhull++] = nverts;
+					nverts++;
+				}
+			}
+			else
+			{
+				for (int k = 1; k < nidx-1; ++k)
+				{
+					rcVcopy(&verts[nverts*3], &edge[idx[k]*3]);
+					hull[nhull++] = nverts;
+					nverts++;
+				}
+			}
+		}
+	}
+	
+	// If the polygon minimum extent is small (sliver or small triangle), do not try to add internal points.
+	if (minExtent < sampleDist*2)
+	{
+		triangulateHull(nverts, verts, nhull, hull, nin, tris);
+		return true;
+	}
+	
+	// Tessellate the base mesh.
+	// We're using the triangulateHull instead of delaunayHull as it tends to
+	// create a bit better triangulation for long thin triangles when there
+	// are no internal points.
+	triangulateHull(nverts, verts, nhull, hull, nin, tris);
+	
+	if (tris.size() == 0)
+	{
+		// Could not triangulate the poly, make sure there is some valid data there.
+		ctx->log(RC_LOG_WARNING, "buildPolyDetail: Could not triangulate polygon (%d verts).", nverts);
+		return true;
+	}
+	
+	if (sampleDist > 0)
+	{
+		// Create sample locations in a grid.
+		float bmin[3], bmax[3];
+		rcVcopy(bmin, in);
+		rcVcopy(bmax, in);
+		for (int i = 1; i < nin; ++i)
+		{
+			rcVmin(bmin, &in[i*3]);
+			rcVmax(bmax, &in[i*3]);
+		}
+		int x0 = (int)floorf(bmin[0]/sampleDist);
+		int x1 = (int)ceilf(bmax[0]/sampleDist);
+		int z0 = (int)floorf(bmin[2]/sampleDist);
+		int z1 = (int)ceilf(bmax[2]/sampleDist);
+		samples.resize(0);
+		for (int z = z0; z < z1; ++z)
+		{
+			for (int x = x0; x < x1; ++x)
+			{
+				float pt[3];
+				pt[0] = x*sampleDist;
+				pt[1] = (bmax[1]+bmin[1])*0.5f;
+				pt[2] = z*sampleDist;
+				// Make sure the samples are not too close to the edges.
+				if (distToPoly(nin,in,pt) > -sampleDist/2) continue;
+				samples.push(x);
+				samples.push(getHeight(pt[0], pt[1], pt[2], cs, ics, chf.ch, heightSearchRadius, hp));
+				samples.push(z);
+				samples.push(0); // Not added
+			}
+		}
+		
+		// Add the samples starting from the one that has the most
+		// error. The procedure stops when all samples are added
+		// or when the max error is within treshold.
+		const int nsamples = samples.size()/4;
+		for (int iter = 0; iter < nsamples; ++iter)
+		{
+			if (nverts >= MAX_VERTS)
+				break;
+			
+			// Find sample with most error.
+			float bestpt[3] = {0,0,0};
+			float bestd = 0;
+			int besti = -1;
+			for (int i = 0; i < nsamples; ++i)
+			{
+				const int* s = &samples[i*4];
+				if (s[3]) continue; // skip added.
+				float pt[3];
+				// The sample location is jittered to get rid of some bad triangulations
+				// which are cause by symmetrical data from the grid structure.
+				pt[0] = s[0]*sampleDist + getJitterX(i)*cs*0.1f;
+				pt[1] = s[1]*chf.ch;
+				pt[2] = s[2]*sampleDist + getJitterY(i)*cs*0.1f;
+				float d = distToTriMesh(pt, verts, nverts, &tris[0], tris.size()/4);
+				if (d < 0) continue; // did not hit the mesh.
+				if (d > bestd)
+				{
+					bestd = d;
+					besti = i;
+					rcVcopy(bestpt,pt);
+				}
+			}
+			// If the max error is within accepted threshold, stop tesselating.
+			if (bestd <= sampleMaxError || besti == -1)
+				break;
+			// Mark sample as added.
+			samples[besti*4+3] = 1;
+			// Add the new sample point.
+			rcVcopy(&verts[nverts*3],bestpt);
+			nverts++;
+			
+			// Create new triangulation.
+			// TODO: Incremental add instead of full rebuild.
+			edges.resize(0);
+			tris.resize(0);
+			delaunayHull(ctx, nverts, verts, nhull, hull, tris, edges);
+		}
+	}
+	
+	const int ntris = tris.size()/4;
+	if (ntris > MAX_TRIS)
+	{
+		tris.resize(MAX_TRIS*4);
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Shrinking triangle count from %d to max %d.", ntris, MAX_TRIS);
+	}
+	
+	return true;
+}
+
+static void seedArrayWithPolyCenter(rcContext* ctx, const rcCompactHeightfield& chf,
+									const unsigned short* poly, const int npoly,
+									const unsigned short* verts, const int bs,
+									rcHeightPatch& hp, rcIntArray& array)
+{
+	// Note: Reads to the compact heightfield are offset by border size (bs)
+	// since border size offset is already removed from the polymesh vertices.
+	
+	static const int offset[9*2] =
+	{
+		0,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1, -1,0,
+	};
+	
+	// Find cell closest to a poly vertex
+	int startCellX = 0, startCellY = 0, startSpanIndex = -1;
+	int dmin = RC_UNSET_HEIGHT;
+	for (int j = 0; j < npoly && dmin > 0; ++j)
+	{
+		for (int k = 0; k < 9 && dmin > 0; ++k)
+		{
+			const int ax = (int)verts[poly[j]*3+0] + offset[k*2+0];
+			const int ay = (int)verts[poly[j]*3+1];
+			const int az = (int)verts[poly[j]*3+2] + offset[k*2+1];
+			if (ax < hp.xmin || ax >= hp.xmin+hp.width ||
+				az < hp.ymin || az >= hp.ymin+hp.height)
+				continue;
+			
+			const rcCompactCell& c = chf.cells[(ax+bs)+(az+bs)*chf.width];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni && dmin > 0; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				int d = rcAbs(ay - (int)s.y);
+				if (d < dmin)
+				{
+					startCellX = ax;
+					startCellY = az;
+					startSpanIndex = i;
+					dmin = d;
+				}
+			}
+		}
+	}
+	
+	rcAssert(startSpanIndex != -1);
+	// Find center of the polygon
+	int pcx = 0, pcy = 0;
+	for (int j = 0; j < npoly; ++j)
+	{
+		pcx += (int)verts[poly[j]*3+0];
+		pcy += (int)verts[poly[j]*3+2];
+	}
+	pcx /= npoly;
+	pcy /= npoly;
+	
+	// Use seeds array as a stack for DFS
+	array.resize(0);
+	array.push(startCellX);
+	array.push(startCellY);
+	array.push(startSpanIndex);
+
+	int dirs[] = { 0, 1, 2, 3 };
+	memset(hp.data, 0, sizeof(unsigned short)*hp.width*hp.height);
+	// DFS to move to the center. Note that we need a DFS here and can not just move
+	// directly towards the center without recording intermediate nodes, even though the polygons
+	// are convex. In very rare we can get stuck due to contour simplification if we do not
+	// record nodes.
+	int cx = -1, cy = -1, ci = -1;
+	while (true)
+	{
+		if (array.size() < 3)
+		{
+			ctx->log(RC_LOG_WARNING, "Walk towards polygon center failed to reach center");
+			break;
+		}
+
+		ci = array.pop();
+		cy = array.pop();
+		cx = array.pop();
+
+		if (cx == pcx && cy == pcy)
+			break;
+
+		// If we are already at the correct X-position, prefer direction
+		// directly towards the center in the Y-axis; otherwise prefer
+		// direction in the X-axis
+		int directDir;
+		if (cx == pcx)
+			directDir = rcGetDirForOffset(0, pcy > cy ? 1 : -1);
+		else
+			directDir = rcGetDirForOffset(pcx > cx ? 1 : -1, 0);
+
+		// Push the direct dir last so we start with this on next iteration
+		rcSwap(dirs[directDir], dirs[3]);
+
+		const rcCompactSpan& cs = chf.spans[ci];
+		for (int i = 0; i < 4; i++)
+		{
+			int dir = dirs[i];
+			if (rcGetCon(cs, dir) == RC_NOT_CONNECTED)
+				continue;
+
+			int newX = cx + rcGetDirOffsetX(dir);
+			int newY = cy + rcGetDirOffsetY(dir);
+
+			int hpx = newX - hp.xmin;
+			int hpy = newY - hp.ymin;
+			if (hpx < 0 || hpx >= hp.width || hpy < 0 || hpy >= hp.height)
+				continue;
+
+			if (hp.data[hpx+hpy*hp.width] != 0)
+				continue;
+
+			hp.data[hpx+hpy*hp.width] = 1;
+			array.push(newX);
+			array.push(newY);
+			array.push((int)chf.cells[(newX+bs)+(newY+bs)*chf.width].index + rcGetCon(cs, dir));
+		}
+
+		rcSwap(dirs[directDir], dirs[3]);
+	}
+
+	array.resize(0);
+	// getHeightData seeds are given in coordinates with borders
+	array.push(cx+bs);
+	array.push(cy+bs);
+	array.push(ci);
+
+	memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height);
+	const rcCompactSpan& cs = chf.spans[ci];
+	hp.data[cx-hp.xmin+(cy-hp.ymin)*hp.width] = cs.y;
+}
+
+
+static void push3(rcIntArray& queue, int v1, int v2, int v3)
+{
+	queue.resize(queue.size() + 3);
+	queue[queue.size() - 3] = v1;
+	queue[queue.size() - 2] = v2;
+	queue[queue.size() - 1] = v3;
+}
+
+static void getHeightData(rcContext* ctx, const rcCompactHeightfield& chf,
+						  const unsigned short* poly, const int npoly,
+						  const unsigned short* verts, const int bs,
+						  rcHeightPatch& hp, rcIntArray& queue,
+						  int region)
+{
+	// Note: Reads to the compact heightfield are offset by border size (bs)
+	// since border size offset is already removed from the polymesh vertices.
+	
+	queue.resize(0);
+	// Set all heights to RC_UNSET_HEIGHT.
+	memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height);
+
+	bool empty = true;
+	
+	// We cannot sample from this poly if it was created from polys
+	// of different regions. If it was then it could potentially be overlapping
+	// with polys of that region and the heights sampled here could be wrong.
+	if (region != RC_MULTIPLE_REGS)
+	{
+		// Copy the height from the same region, and mark region borders
+		// as seed points to fill the rest.
+		for (int hy = 0; hy < hp.height; hy++)
+		{
+			int y = hp.ymin + hy + bs;
+			for (int hx = 0; hx < hp.width; hx++)
+			{
+				int x = hp.xmin + hx + bs;
+				const rcCompactCell& c = chf.cells[x + y*chf.width];
+				for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; ++i)
+				{
+					const rcCompactSpan& s = chf.spans[i];
+					if (s.reg == region)
+					{
+						// Store height
+						hp.data[hx + hy*hp.width] = s.y;
+						empty = false;
+
+						// If any of the neighbours is not in same region,
+						// add the current location as flood fill start
+						bool border = false;
+						for (int dir = 0; dir < 4; ++dir)
+						{
+							if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+							{
+								const int ax = x + rcGetDirOffsetX(dir);
+								const int ay = y + rcGetDirOffsetY(dir);
+								const int ai = (int)chf.cells[ax + ay*chf.width].index + rcGetCon(s, dir);
+								const rcCompactSpan& as = chf.spans[ai];
+								if (as.reg != region)
+								{
+									border = true;
+									break;
+								}
+							}
+						}
+						if (border)
+							push3(queue, x, y, i);
+						break;
+					}
+				}
+			}
+		}
+	}
+	
+	// if the polygon does not contain any points from the current region (rare, but happens)
+	// or if it could potentially be overlapping polygons of the same region,
+	// then use the center as the seed point.
+	if (empty)
+		seedArrayWithPolyCenter(ctx, chf, poly, npoly, verts, bs, hp, queue);
+	
+	static const int RETRACT_SIZE = 256;
+	int head = 0;
+	
+	// We assume the seed is centered in the polygon, so a BFS to collect
+	// height data will ensure we do not move onto overlapping polygons and
+	// sample wrong heights.
+	while (head*3 < queue.size())
+	{
+		int cx = queue[head*3+0];
+		int cy = queue[head*3+1];
+		int ci = queue[head*3+2];
+		head++;
+		if (head >= RETRACT_SIZE)
+		{
+			head = 0;
+			if (queue.size() > RETRACT_SIZE*3)
+				memmove(&queue[0], &queue[RETRACT_SIZE*3], sizeof(int)*(queue.size()-RETRACT_SIZE*3));
+			queue.resize(queue.size()-RETRACT_SIZE*3);
+		}
+		
+		const rcCompactSpan& cs = chf.spans[ci];
+		for (int dir = 0; dir < 4; ++dir)
+		{
+			if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue;
+			
+			const int ax = cx + rcGetDirOffsetX(dir);
+			const int ay = cy + rcGetDirOffsetY(dir);
+			const int hx = ax - hp.xmin - bs;
+			const int hy = ay - hp.ymin - bs;
+			
+			if ((unsigned int)hx >= (unsigned int)hp.width || (unsigned int)hy >= (unsigned int)hp.height)
+				continue;
+			
+			if (hp.data[hx + hy*hp.width] != RC_UNSET_HEIGHT)
+				continue;
+			
+			const int ai = (int)chf.cells[ax + ay*chf.width].index + rcGetCon(cs, dir);
+			const rcCompactSpan& as = chf.spans[ai];
+			
+			hp.data[hx + hy*hp.width] = as.y;
+			
+			push3(queue, ax, ay, ai);
+		}
+	}
+}
+
+static unsigned char getEdgeFlags(const float* va, const float* vb,
+								  const float* vpoly, const int npoly)
+{
+	// The flag returned by this function matches dtDetailTriEdgeFlags in Detour.
+	// Figure out if edge (va,vb) is part of the polygon boundary.
+	static const float thrSqr = rcSqr(0.001f);
+	for (int i = 0, j = npoly-1; i < npoly; j=i++)
+	{
+		if (distancePtSeg2d(va, &vpoly[j*3], &vpoly[i*3]) < thrSqr &&
+			distancePtSeg2d(vb, &vpoly[j*3], &vpoly[i*3]) < thrSqr)
+			return 1;
+	}
+	return 0;
+}
+
+static unsigned char getTriFlags(const float* va, const float* vb, const float* vc,
+								 const float* vpoly, const int npoly)
+{
+	unsigned char flags = 0;
+	flags |= getEdgeFlags(va,vb,vpoly,npoly) << 0;
+	flags |= getEdgeFlags(vb,vc,vpoly,npoly) << 2;
+	flags |= getEdgeFlags(vc,va,vpoly,npoly) << 4;
+	return flags;
+}
+
+/// @par
+///
+/// See the #rcConfig documentation for more information on the configuration parameters.
+///
+/// @see rcAllocPolyMeshDetail, rcPolyMesh, rcCompactHeightfield, rcPolyMeshDetail, rcConfig
+bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompactHeightfield& chf,
+						   const float sampleDist, const float sampleMaxError,
+						   rcPolyMeshDetail& dmesh)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_POLYMESHDETAIL);
+	
+	if (mesh.nverts == 0 || mesh.npolys == 0)
+		return true;
+	
+	const int nvp = mesh.nvp;
+	const float cs = mesh.cs;
+	const float ch = mesh.ch;
+	const float* orig = mesh.bmin;
+	const int borderSize = mesh.borderSize;
+	const int heightSearchRadius = rcMax(1, (int)ceilf(mesh.maxEdgeError));
+	
+	rcIntArray edges(64);
+	rcIntArray tris(512);
+	rcIntArray arr(512);
+	rcIntArray samples(512);
+	float verts[256*3];
+	rcHeightPatch hp;
+	int nPolyVerts = 0;
+	int maxhw = 0, maxhh = 0;
+	
+	rcScopedDelete<int> bounds((int*)rcAlloc(sizeof(int)*mesh.npolys*4, RC_ALLOC_TEMP));
+	if (!bounds)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'bounds' (%d).", mesh.npolys*4);
+		return false;
+	}
+	rcScopedDelete<float> poly((float*)rcAlloc(sizeof(float)*nvp*3, RC_ALLOC_TEMP));
+	if (!poly)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'poly' (%d).", nvp*3);
+		return false;
+	}
+	
+	// Find max size for a polygon area.
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		const unsigned short* p = &mesh.polys[i*nvp*2];
+		int& xmin = bounds[i*4+0];
+		int& xmax = bounds[i*4+1];
+		int& ymin = bounds[i*4+2];
+		int& ymax = bounds[i*4+3];
+		xmin = chf.width;
+		xmax = 0;
+		ymin = chf.height;
+		ymax = 0;
+		for (int j = 0; j < nvp; ++j)
+		{
+			if(p[j] == RC_MESH_NULL_IDX) break;
+			const unsigned short* v = &mesh.verts[p[j]*3];
+			xmin = rcMin(xmin, (int)v[0]);
+			xmax = rcMax(xmax, (int)v[0]);
+			ymin = rcMin(ymin, (int)v[2]);
+			ymax = rcMax(ymax, (int)v[2]);
+			nPolyVerts++;
+		}
+		xmin = rcMax(0,xmin-1);
+		xmax = rcMin(chf.width,xmax+1);
+		ymin = rcMax(0,ymin-1);
+		ymax = rcMin(chf.height,ymax+1);
+		if (xmin >= xmax || ymin >= ymax) continue;
+		maxhw = rcMax(maxhw, xmax-xmin);
+		maxhh = rcMax(maxhh, ymax-ymin);
+	}
+	
+	hp.data = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxhw*maxhh, RC_ALLOC_TEMP);
+	if (!hp.data)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'hp.data' (%d).", maxhw*maxhh);
+		return false;
+	}
+	
+	dmesh.nmeshes = mesh.npolys;
+	dmesh.nverts = 0;
+	dmesh.ntris = 0;
+	dmesh.meshes = (unsigned int*)rcAlloc(sizeof(unsigned int)*dmesh.nmeshes*4, RC_ALLOC_PERM);
+	if (!dmesh.meshes)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.meshes' (%d).", dmesh.nmeshes*4);
+		return false;
+	}
+	
+	int vcap = nPolyVerts+nPolyVerts/2;
+	int tcap = vcap*2;
+	
+	dmesh.nverts = 0;
+	dmesh.verts = (float*)rcAlloc(sizeof(float)*vcap*3, RC_ALLOC_PERM);
+	if (!dmesh.verts)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", vcap*3);
+		return false;
+	}
+	dmesh.ntris = 0;
+	dmesh.tris = (unsigned char*)rcAlloc(sizeof(unsigned char)*tcap*4, RC_ALLOC_PERM);
+	if (!dmesh.tris)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", tcap*4);
+		return false;
+	}
+	
+	for (int i = 0; i < mesh.npolys; ++i)
+	{
+		const unsigned short* p = &mesh.polys[i*nvp*2];
+		
+		// Store polygon vertices for processing.
+		int npoly = 0;
+		for (int j = 0; j < nvp; ++j)
+		{
+			if(p[j] == RC_MESH_NULL_IDX) break;
+			const unsigned short* v = &mesh.verts[p[j]*3];
+			poly[j*3+0] = v[0]*cs;
+			poly[j*3+1] = v[1]*ch;
+			poly[j*3+2] = v[2]*cs;
+			npoly++;
+		}
+		
+		// Get the height data from the area of the polygon.
+		hp.xmin = bounds[i*4+0];
+		hp.ymin = bounds[i*4+2];
+		hp.width = bounds[i*4+1]-bounds[i*4+0];
+		hp.height = bounds[i*4+3]-bounds[i*4+2];
+		getHeightData(ctx, chf, p, npoly, mesh.verts, borderSize, hp, arr, mesh.regs[i]);
+		
+		// Build detail mesh.
+		int nverts = 0;
+		if (!buildPolyDetail(ctx, poly, npoly,
+							 sampleDist, sampleMaxError,
+							 heightSearchRadius, chf, hp,
+							 verts, nverts, tris,
+							 edges, samples))
+		{
+			return false;
+		}
+		
+		// Move detail verts to world space.
+		for (int j = 0; j < nverts; ++j)
+		{
+			verts[j*3+0] += orig[0];
+			verts[j*3+1] += orig[1] + chf.ch; // Is this offset necessary?
+			verts[j*3+2] += orig[2];
+		}
+		// Offset poly too, will be used to flag checking.
+		for (int j = 0; j < npoly; ++j)
+		{
+			poly[j*3+0] += orig[0];
+			poly[j*3+1] += orig[1];
+			poly[j*3+2] += orig[2];
+		}
+		
+		// Store detail submesh.
+		const int ntris = tris.size()/4;
+		
+		dmesh.meshes[i*4+0] = (unsigned int)dmesh.nverts;
+		dmesh.meshes[i*4+1] = (unsigned int)nverts;
+		dmesh.meshes[i*4+2] = (unsigned int)dmesh.ntris;
+		dmesh.meshes[i*4+3] = (unsigned int)ntris;
+		
+		// Store vertices, allocate more memory if necessary.
+		if (dmesh.nverts+nverts > vcap)
+		{
+			while (dmesh.nverts+nverts > vcap)
+				vcap += 256;
+			
+			float* newv = (float*)rcAlloc(sizeof(float)*vcap*3, RC_ALLOC_PERM);
+			if (!newv)
+			{
+				ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newv' (%d).", vcap*3);
+				return false;
+			}
+			if (dmesh.nverts)
+				memcpy(newv, dmesh.verts, sizeof(float)*3*dmesh.nverts);
+			rcFree(dmesh.verts);
+			dmesh.verts = newv;
+		}
+		for (int j = 0; j < nverts; ++j)
+		{
+			dmesh.verts[dmesh.nverts*3+0] = verts[j*3+0];
+			dmesh.verts[dmesh.nverts*3+1] = verts[j*3+1];
+			dmesh.verts[dmesh.nverts*3+2] = verts[j*3+2];
+			dmesh.nverts++;
+		}
+		
+		// Store triangles, allocate more memory if necessary.
+		if (dmesh.ntris+ntris > tcap)
+		{
+			while (dmesh.ntris+ntris > tcap)
+				tcap += 256;
+			unsigned char* newt = (unsigned char*)rcAlloc(sizeof(unsigned char)*tcap*4, RC_ALLOC_PERM);
+			if (!newt)
+			{
+				ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newt' (%d).", tcap*4);
+				return false;
+			}
+			if (dmesh.ntris)
+				memcpy(newt, dmesh.tris, sizeof(unsigned char)*4*dmesh.ntris);
+			rcFree(dmesh.tris);
+			dmesh.tris = newt;
+		}
+		for (int j = 0; j < ntris; ++j)
+		{
+			const int* t = &tris[j*4];
+			dmesh.tris[dmesh.ntris*4+0] = (unsigned char)t[0];
+			dmesh.tris[dmesh.ntris*4+1] = (unsigned char)t[1];
+			dmesh.tris[dmesh.ntris*4+2] = (unsigned char)t[2];
+			dmesh.tris[dmesh.ntris*4+3] = getTriFlags(&verts[t[0]*3], &verts[t[1]*3], &verts[t[2]*3], poly, npoly);
+			dmesh.ntris++;
+		}
+	}
+	
+	return true;
+}
+
+/// @see rcAllocPolyMeshDetail, rcPolyMeshDetail
+bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_MERGE_POLYMESHDETAIL);
+	
+	int maxVerts = 0;
+	int maxTris = 0;
+	int maxMeshes = 0;
+	
+	for (int i = 0; i < nmeshes; ++i)
+	{
+		if (!meshes[i]) continue;
+		maxVerts += meshes[i]->nverts;
+		maxTris += meshes[i]->ntris;
+		maxMeshes += meshes[i]->nmeshes;
+	}
+	
+	mesh.nmeshes = 0;
+	mesh.meshes = (unsigned int*)rcAlloc(sizeof(unsigned int)*maxMeshes*4, RC_ALLOC_PERM);
+	if (!mesh.meshes)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'pmdtl.meshes' (%d).", maxMeshes*4);
+		return false;
+	}
+	
+	mesh.ntris = 0;
+	mesh.tris = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxTris*4, RC_ALLOC_PERM);
+	if (!mesh.tris)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", maxTris*4);
+		return false;
+	}
+	
+	mesh.nverts = 0;
+	mesh.verts = (float*)rcAlloc(sizeof(float)*maxVerts*3, RC_ALLOC_PERM);
+	if (!mesh.verts)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", maxVerts*3);
+		return false;
+	}
+	
+	// Merge datas.
+	for (int i = 0; i < nmeshes; ++i)
+	{
+		rcPolyMeshDetail* dm = meshes[i];
+		if (!dm) continue;
+		for (int j = 0; j < dm->nmeshes; ++j)
+		{
+			unsigned int* dst = &mesh.meshes[mesh.nmeshes*4];
+			unsigned int* src = &dm->meshes[j*4];
+			dst[0] = (unsigned int)mesh.nverts+src[0];
+			dst[1] = src[1];
+			dst[2] = (unsigned int)mesh.ntris+src[2];
+			dst[3] = src[3];
+			mesh.nmeshes++;
+		}
+		
+		for (int k = 0; k < dm->nverts; ++k)
+		{
+			rcVcopy(&mesh.verts[mesh.nverts*3], &dm->verts[k*3]);
+			mesh.nverts++;
+		}
+		for (int k = 0; k < dm->ntris; ++k)
+		{
+			mesh.tris[mesh.ntris*4+0] = dm->tris[k*4+0];
+			mesh.tris[mesh.ntris*4+1] = dm->tris[k*4+1];
+			mesh.tris[mesh.ntris*4+2] = dm->tris[k*4+2];
+			mesh.tris[mesh.ntris*4+3] = dm->tris[k*4+3];
+			mesh.ntris++;
+		}
+	}
+	
+	return true;
+}

panda/src/recastdetour/RecastRasterization.cpp

@@ -0,0 +1,454 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// This software is provided 'as-is', without any express or implied
+// warranty.  In no event will the authors be held liable for any damages
+// arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+//
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+inline bool overlapBounds(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 overlapInterval(unsigned short amin, unsigned short amax,
+							unsigned short bmin, unsigned short bmax)
+{
+	if (amax < bmin) return false;
+	if (amin > bmax) return false;
+	return true;
+}
+
+
+static rcSpan* allocSpan(rcHeightfield& hf)
+{
+	// If running out of memory, allocate new page and update the freelist.
+	if (!hf.freelist || !hf.freelist->next)
+	{
+		// Create new page.
+		// Allocate memory for the new pool.
+		rcSpanPool* pool = (rcSpanPool*)rcAlloc(sizeof(rcSpanPool), RC_ALLOC_PERM);
+		if (!pool) return 0;
+
+		// Add the pool into the list of pools.
+		pool->next = hf.pools;
+		hf.pools = pool;
+		// Add new items to the free list.
+		rcSpan* freelist = hf.freelist;
+		rcSpan* head = &pool->items[0];
+		rcSpan* it = &pool->items[RC_SPANS_PER_POOL];
+		do
+		{
+			--it;
+			it->next = freelist;
+			freelist = it;
+		}
+		while (it != head);
+		hf.freelist = it;
+	}
+	
+	// Pop item from in front of the free list.
+	rcSpan* it = hf.freelist;
+	hf.freelist = hf.freelist->next;
+	return it;
+}
+
+static void freeSpan(rcHeightfield& hf, rcSpan* ptr)
+{
+	if (!ptr) return;
+	// Add the node in front of the free list.
+	ptr->next = hf.freelist;
+	hf.freelist = ptr;
+}
+
+static bool addSpan(rcHeightfield& hf, const int x, const int y,
+					const unsigned short smin, const unsigned short smax,
+					const unsigned char area, const int flagMergeThr)
+{
+	
+	int idx = x + y*hf.width;
+	
+	rcSpan* s = allocSpan(hf);
+	if (!s)
+		return false;
+	s->smin = smin;
+	s->smax = smax;
+	s->area = area;
+	s->next = 0;
+	
+	// Empty cell, add the first span.
+	if (!hf.spans[idx])
+	{
+		hf.spans[idx] = s;
+		return true;
+	}
+	rcSpan* prev = 0;
+	rcSpan* cur = hf.spans[idx];
+	
+	// Insert and merge spans.
+	while (cur)
+	{
+		if (cur->smin > s->smax)
+		{
+			// Current span is further than the new span, break.
+			break;
+		}
+		else if (cur->smax < s->smin)
+		{
+			// Current span is before the new span advance.
+			prev = cur;
+			cur = cur->next;
+		}
+		else
+		{
+			// Merge spans.
+			if (cur->smin < s->smin)
+				s->smin = cur->smin;
+			if (cur->smax > s->smax)
+				s->smax = cur->smax;
+			
+			// Merge flags.
+			if (rcAbs((int)s->smax - (int)cur->smax) <= flagMergeThr)
+				s->area = rcMax(s->area, cur->area);
+			
+			// Remove current span.
+			rcSpan* next = cur->next;
+			freeSpan(hf, cur);
+			if (prev)
+				prev->next = next;
+			else
+				hf.spans[idx] = next;
+			cur = next;
+		}
+	}
+	
+	// Insert new span.
+	if (prev)
+	{
+		s->next = prev->next;
+		prev->next = s;
+	}
+	else
+	{
+		s->next = hf.spans[idx];
+		hf.spans[idx] = s;
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// The span addition can be set to favor flags. If the span is merged to
+/// another span and the new @p smax is within @p flagMergeThr units
+/// from the existing span, the span flags are merged.
+///
+/// @see rcHeightfield, rcSpan.
+bool rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y,
+			   const unsigned short smin, const unsigned short smax,
+			   const unsigned char area, const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	if (!addSpan(hf, x, y, smin, smax, area, flagMergeThr))
+	{
+		ctx->log(RC_LOG_ERROR, "rcAddSpan: Out of memory.");
+		return false;
+	}
+
+	return true;
+}
+
+// divides a convex polygons into two convex polygons on both sides of a line
+static void dividePoly(const float* in, int nin,
+					  float* out1, int* nout1,
+					  float* out2, int* nout2,
+					  float x, int axis)
+{
+	float d[12];
+	for (int i = 0; i < nin; ++i)
+		d[i] = x - in[i*3+axis];
+
+	int m = 0, n = 0;
+	for (int i = 0, j = nin-1; i < nin; j=i, ++i)
+	{
+		bool ina = d[j] >= 0;
+		bool inb = d[i] >= 0;
+		if (ina != inb)
+		{
+			float s = d[j] / (d[j] - d[i]);
+			out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
+			out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
+			out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
+			rcVcopy(out2 + n*3, out1 + m*3);
+			m++;
+			n++;
+			// add the i'th point to the right polygon. Do NOT add points that are on the dividing line
+			// since these were already added above
+			if (d[i] > 0)
+			{
+				rcVcopy(out1 + m*3, in + i*3);
+				m++;
+			}
+			else if (d[i] < 0)
+			{
+				rcVcopy(out2 + n*3, in + i*3);
+				n++;
+			}
+		}
+		else // same side
+		{
+			// add the i'th point to the right polygon. Addition is done even for points on the dividing line
+			if (d[i] >= 0)
+			{
+				rcVcopy(out1 + m*3, in + i*3);
+				m++;
+				if (d[i] != 0)
+					continue;
+			}
+			rcVcopy(out2 + n*3, in + i*3);
+			n++;
+		}
+	}
+
+	*nout1 = m;
+	*nout2 = n;
+}
+
+
+
+static bool rasterizeTri(const float* v0, const float* v1, const float* v2,
+						 const unsigned char area, rcHeightfield& hf,
+						 const float* bmin, const float* bmax,
+						 const float cs, const float ics, const float ich,
+						 const int flagMergeThr)
+{
+	const int w = hf.width;
+	const int h = hf.height;
+	float tmin[3], tmax[3];
+	const float by = bmax[1] - bmin[1];
+	
+	// Calculate the bounding box of the triangle.
+	rcVcopy(tmin, v0);
+	rcVcopy(tmax, v0);
+	rcVmin(tmin, v1);
+	rcVmin(tmin, v2);
+	rcVmax(tmax, v1);
+	rcVmax(tmax, v2);
+	
+	// If the triangle does not touch the bbox of the heightfield, skip the triagle.
+	if (!overlapBounds(bmin, bmax, tmin, tmax))
+		return true;
+	
+	// Calculate the footprint of the triangle on the grid's y-axis
+	int y0 = (int)((tmin[2] - bmin[2])*ics);
+	int y1 = (int)((tmax[2] - bmin[2])*ics);
+	y0 = rcClamp(y0, 0, h-1);
+	y1 = rcClamp(y1, 0, h-1);
+	
+	// Clip the triangle into all grid cells it touches.
+	float buf[7*3*4];
+	float *in = buf, *inrow = buf+7*3, *p1 = inrow+7*3, *p2 = p1+7*3;
+
+	rcVcopy(&in[0], v0);
+	rcVcopy(&in[1*3], v1);
+	rcVcopy(&in[2*3], v2);
+	int nvrow, nvIn = 3;
+	
+	for (int y = y0; y <= y1; ++y)
+	{
+		// Clip polygon to row. Store the remaining polygon as well
+		const float cz = bmin[2] + y*cs;
+		dividePoly(in, nvIn, inrow, &nvrow, p1, &nvIn, cz+cs, 2);
+		rcSwap(in, p1);
+		if (nvrow < 3) continue;
+		
+		// find the horizontal bounds in the row
+		float minX = inrow[0], maxX = inrow[0];
+		for (int i=1; i<nvrow; ++i)
+		{
+			if (minX > inrow[i*3])	minX = inrow[i*3];
+			if (maxX < inrow[i*3])	maxX = inrow[i*3];
+		}
+		int x0 = (int)((minX - bmin[0])*ics);
+		int x1 = (int)((maxX - bmin[0])*ics);
+		x0 = rcClamp(x0, 0, w-1);
+		x1 = rcClamp(x1, 0, w-1);
+
+		int nv, nv2 = nvrow;
+
+		for (int x = x0; x <= x1; ++x)
+		{
+			// Clip polygon to column. store the remaining polygon as well
+			const float cx = bmin[0] + x*cs;
+			dividePoly(inrow, nv2, p1, &nv, p2, &nv2, cx+cs, 0);
+			rcSwap(inrow, p2);
+			if (nv < 3) continue;
+			
+			// Calculate min and max of the span.
+			float smin = p1[1], smax = p1[1];
+			for (int i = 1; i < nv; ++i)
+			{
+				smin = rcMin(smin, p1[i*3+1]);
+				smax = rcMax(smax, p1[i*3+1]);
+			}
+			smin -= bmin[1];
+			smax -= bmin[1];
+			// Skip the span if it is outside the heightfield bbox
+			if (smax < 0.0f) continue;
+			if (smin > by) continue;
+			// Clamp the span to the heightfield bbox.
+			if (smin < 0.0f) smin = 0;
+			if (smax > by) smax = by;
+			
+			// Snap the span to the heightfield height grid.
+			unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, RC_SPAN_MAX_HEIGHT);
+			unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), (int)ismin+1, RC_SPAN_MAX_HEIGHT);
+			
+			if (!addSpan(hf, x, y, ismin, ismax, area, flagMergeThr))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// No spans will be added if the triangle does not overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2,
+						 const unsigned char area, rcHeightfield& solid,
+						 const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	if (!rasterizeTri(v0, v1, v2, area, solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+	{
+		ctx->log(RC_LOG_ERROR, "rcRasterizeTriangle: Out of memory.");
+		return false;
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// Spans will only be added for triangles that overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
+						  const int* tris, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+	
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	// Rasterize triangles.
+	for (int i = 0; i < nt; ++i)
+	{
+		const float* v0 = &verts[tris[i*3+0]*3];
+		const float* v1 = &verts[tris[i*3+1]*3];
+		const float* v2 = &verts[tris[i*3+2]*3];
+		// Rasterize.
+		if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+		{
+			ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// Spans will only be added for triangles that overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
+						  const unsigned short* tris, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr)
+{
+	rcAssert(ctx);
+
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+	
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	// Rasterize triangles.
+	for (int i = 0; i < nt; ++i)
+	{
+		const float* v0 = &verts[tris[i*3+0]*3];
+		const float* v1 = &verts[tris[i*3+1]*3];
+		const float* v2 = &verts[tris[i*3+2]*3];
+		// Rasterize.
+		if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+		{
+			ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/// @par
+///
+/// Spans will only be added for triangles that overlap the heightfield grid.
+///
+/// @see rcHeightfield
+bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt,
+						  rcHeightfield& solid, const int flagMergeThr)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
+	
+	const float ics = 1.0f/solid.cs;
+	const float ich = 1.0f/solid.ch;
+	// Rasterize triangles.
+	for (int i = 0; i < nt; ++i)
+	{
+		const float* v0 = &verts[(i*3+0)*3];
+		const float* v1 = &verts[(i*3+1)*3];
+		const float* v2 = &verts[(i*3+2)*3];
+		// Rasterize.
+		if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
+		{
+			ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
+			return false;
+		}
+	}
+
+	return true;
+}

panda/src/recastdetour/RecastRegion.cpp

@@ -0,0 +1,1812 @@
+//
+// Copyright (c) 2009-2010 Mikko Mononen [email protected]
+//
+// 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 <float.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "Recast.h"
+#include "RecastAlloc.h"
+#include "RecastAssert.h"
+
+namespace
+{
+struct LevelStackEntry
+{
+	LevelStackEntry(int x_, int y_, int index_) : x(x_), y(y_), index(index_) {}
+	int x;
+	int y;
+	int index;
+};
+}  // namespace
+
+static void calculateDistanceField(rcCompactHeightfield& chf, unsigned short* src, unsigned short& maxDist)
+{
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	// Init distance and points.
+	for (int i = 0; i < chf.spanCount; ++i)
+		src[i] = 0xffff;
+	
+	// Mark boundary cells.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const unsigned char area = chf.areas[i];
+				
+				int nc = 0;
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						if (area == chf.areas[ai])
+							nc++;
+					}
+				}
+				if (nc != 4)
+					src[i] = 0;
+			}
+		}
+	}
+	
+			
+	// Pass 1
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					// (-1,0)
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					const rcCompactSpan& as = chf.spans[ai];
+					if (src[ai]+2 < src[i])
+						src[i] = src[ai]+2;
+					
+					// (-1,-1)
+					if (rcGetCon(as, 3) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(3);
+						const int aay = ay + rcGetDirOffsetY(3);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3);
+						if (src[aai]+3 < src[i])
+							src[i] = src[aai]+3;
+					}
+				}
+				if (rcGetCon(s, 3) != RC_NOT_CONNECTED)
+				{
+					// (0,-1)
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					const rcCompactSpan& as = chf.spans[ai];
+					if (src[ai]+2 < src[i])
+						src[i] = src[ai]+2;
+					
+					// (1,-1)
+					if (rcGetCon(as, 2) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(2);
+						const int aay = ay + rcGetDirOffsetY(2);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2);
+						if (src[aai]+3 < src[i])
+							src[i] = src[aai]+3;
+					}
+				}
+			}
+		}
+	}
+	
+	// Pass 2
+	for (int y = h-1; y >= 0; --y)
+	{
+		for (int x = w-1; x >= 0; --x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				
+				if (rcGetCon(s, 2) != RC_NOT_CONNECTED)
+				{
+					// (1,0)
+					const int ax = x + rcGetDirOffsetX(2);
+					const int ay = y + rcGetDirOffsetY(2);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2);
+					const rcCompactSpan& as = chf.spans[ai];
+					if (src[ai]+2 < src[i])
+						src[i] = src[ai]+2;
+					
+					// (1,1)
+					if (rcGetCon(as, 1) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(1);
+						const int aay = ay + rcGetDirOffsetY(1);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1);
+						if (src[aai]+3 < src[i])
+							src[i] = src[aai]+3;
+					}
+				}
+				if (rcGetCon(s, 1) != RC_NOT_CONNECTED)
+				{
+					// (0,1)
+					const int ax = x + rcGetDirOffsetX(1);
+					const int ay = y + rcGetDirOffsetY(1);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1);
+					const rcCompactSpan& as = chf.spans[ai];
+					if (src[ai]+2 < src[i])
+						src[i] = src[ai]+2;
+					
+					// (-1,1)
+					if (rcGetCon(as, 0) != RC_NOT_CONNECTED)
+					{
+						const int aax = ax + rcGetDirOffsetX(0);
+						const int aay = ay + rcGetDirOffsetY(0);
+						const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0);
+						if (src[aai]+3 < src[i])
+							src[i] = src[aai]+3;
+					}
+				}
+			}
+		}
+	}	
+	
+	maxDist = 0;
+	for (int i = 0; i < chf.spanCount; ++i)
+		maxDist = rcMax(src[i], maxDist);
+	
+}
+
+static unsigned short* boxBlur(rcCompactHeightfield& chf, int thr,
+							   unsigned short* src, unsigned short* dst)
+{
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	thr *= 2;
+	
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const unsigned short cd = src[i];
+				if (cd <= thr)
+				{
+					dst[i] = cd;
+					continue;
+				}
+
+				int d = (int)cd;
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						d += (int)src[ai];
+						
+						const rcCompactSpan& as = chf.spans[ai];
+						const int dir2 = (dir+1) & 0x3;
+						if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
+						{
+							const int ax2 = ax + rcGetDirOffsetX(dir2);
+							const int ay2 = ay + rcGetDirOffsetY(dir2);
+							const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
+							d += (int)src[ai2];
+						}
+						else
+						{
+							d += cd;
+						}
+					}
+					else
+					{
+						d += cd*2;
+					}
+				}
+				dst[i] = (unsigned short)((d+5)/9);
+			}
+		}
+	}
+	return dst;
+}
+
+
+static bool floodRegion(int x, int y, int i,
+						unsigned short level, unsigned short r,
+						rcCompactHeightfield& chf,
+						unsigned short* srcReg, unsigned short* srcDist,
+						rcTempVector<LevelStackEntry>& stack)
+{
+	const int w = chf.width;
+	
+	const unsigned char area = chf.areas[i];
+	
+	// Flood fill mark region.
+	stack.clear();
+	stack.push_back(LevelStackEntry(x, y, i));
+	srcReg[i] = r;
+	srcDist[i] = 0;
+	
+	unsigned short lev = level >= 2 ? level-2 : 0;
+	int count = 0;
+	
+	while (stack.size() > 0)
+	{
+		LevelStackEntry& back = stack.back();
+		int cx = back.x;
+		int cy = back.y;
+		int ci = back.index;
+		stack.pop_back();
+		
+		const rcCompactSpan& cs = chf.spans[ci];
+		
+		// Check if any of the neighbours already have a valid region set.
+		unsigned short ar = 0;
+		for (int dir = 0; dir < 4; ++dir)
+		{
+			// 8 connected
+			if (rcGetCon(cs, dir) != RC_NOT_CONNECTED)
+			{
+				const int ax = cx + rcGetDirOffsetX(dir);
+				const int ay = cy + rcGetDirOffsetY(dir);
+				const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir);
+				if (chf.areas[ai] != area)
+					continue;
+				unsigned short nr = srcReg[ai];
+				if (nr & RC_BORDER_REG) // Do not take borders into account.
+					continue;
+				if (nr != 0 && nr != r)
+				{
+					ar = nr;
+					break;
+				}
+				
+				const rcCompactSpan& as = chf.spans[ai];
+				
+				const int dir2 = (dir+1) & 0x3;
+				if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
+				{
+					const int ax2 = ax + rcGetDirOffsetX(dir2);
+					const int ay2 = ay + rcGetDirOffsetY(dir2);
+					const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
+					if (chf.areas[ai2] != area)
+						continue;
+					unsigned short nr2 = srcReg[ai2];
+					if (nr2 != 0 && nr2 != r)
+					{
+						ar = nr2;
+						break;
+					}
+				}				
+			}
+		}
+		if (ar != 0)
+		{
+			srcReg[ci] = 0;
+			continue;
+		}
+		
+		count++;
+		
+		// Expand neighbours.
+		for (int dir = 0; dir < 4; ++dir)
+		{
+			if (rcGetCon(cs, dir) != RC_NOT_CONNECTED)
+			{
+				const int ax = cx + rcGetDirOffsetX(dir);
+				const int ay = cy + rcGetDirOffsetY(dir);
+				const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir);
+				if (chf.areas[ai] != area)
+					continue;
+				if (chf.dist[ai] >= lev && srcReg[ai] == 0)
+				{
+					srcReg[ai] = r;
+					srcDist[ai] = 0;
+					stack.push_back(LevelStackEntry(ax, ay, ai));
+				}
+			}
+		}
+	}
+	
+	return count > 0;
+}
+
+// Struct to keep track of entries in the region table that have been changed.
+struct DirtyEntry
+{
+	DirtyEntry(int index_, unsigned short region_, unsigned short distance2_)
+		: index(index_), region(region_), distance2(distance2_) {}
+	int index;
+	unsigned short region;
+	unsigned short distance2;
+};
+static void expandRegions(int maxIter, unsigned short level,
+					      rcCompactHeightfield& chf,
+					      unsigned short* srcReg, unsigned short* srcDist,
+					      rcTempVector<LevelStackEntry>& stack,
+					      bool fillStack)
+{
+	const int w = chf.width;
+	const int h = chf.height;
+
+	if (fillStack)
+	{
+		// Find cells revealed by the raised level.
+		stack.clear();
+		for (int y = 0; y < h; ++y)
+		{
+			for (int x = 0; x < w; ++x)
+			{
+				const rcCompactCell& c = chf.cells[x+y*w];
+				for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+				{
+					if (chf.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA)
+					{
+						stack.push_back(LevelStackEntry(x, y, i));
+					}
+				}
+			}
+		}
+	}
+	else // use cells in the input stack
+	{
+		// mark all cells which already have a region
+		for (int j=0; j<stack.size(); j++)
+		{
+			int i = stack[j].index;
+			if (srcReg[i] != 0)
+				stack[j].index = -1;
+		}
+	}
+
+	rcTempVector<DirtyEntry> dirtyEntries;
+	int iter = 0;
+	while (stack.size() > 0)
+	{
+		int failed = 0;
+		dirtyEntries.clear();
+		
+		for (int j = 0; j < stack.size(); j++)
+		{
+			int x = stack[j].x;
+			int y = stack[j].y;
+			int i = stack[j].index;
+			if (i < 0)
+			{
+				failed++;
+				continue;
+			}
+			
+			unsigned short r = srcReg[i];
+			unsigned short d2 = 0xffff;
+			const unsigned char area = chf.areas[i];
+			const rcCompactSpan& s = chf.spans[i];
+			for (int dir = 0; dir < 4; ++dir)
+			{
+				if (rcGetCon(s, dir) == RC_NOT_CONNECTED) continue;
+				const int ax = x + rcGetDirOffsetX(dir);
+				const int ay = y + rcGetDirOffsetY(dir);
+				const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+				if (chf.areas[ai] != area) continue;
+				if (srcReg[ai] > 0 && (srcReg[ai] & RC_BORDER_REG) == 0)
+				{
+					if ((int)srcDist[ai]+2 < (int)d2)
+					{
+						r = srcReg[ai];
+						d2 = srcDist[ai]+2;
+					}
+				}
+			}
+			if (r)
+			{
+				stack[j].index = -1; // mark as used
+				dirtyEntries.push_back(DirtyEntry(i, r, d2));
+			}
+			else
+			{
+				failed++;
+			}
+		}
+		
+		// Copy entries that differ between src and dst to keep them in sync.
+		for (int i = 0; i < dirtyEntries.size(); i++) {
+			int idx = dirtyEntries[i].index;
+			srcReg[idx] = dirtyEntries[i].region;
+			srcDist[idx] = dirtyEntries[i].distance2;
+		}
+		
+		if (failed == stack.size())
+			break;
+		
+		if (level > 0)
+		{
+			++iter;
+			if (iter >= maxIter)
+				break;
+		}
+	}
+}
+
+
+
+static void sortCellsByLevel(unsigned short startLevel,
+							  rcCompactHeightfield& chf,
+							  const unsigned short* srcReg,
+							  unsigned int nbStacks, rcTempVector<LevelStackEntry>* stacks,
+							  unsigned short loglevelsPerStack) // the levels per stack (2 in our case) as a bit shift
+{
+	const int w = chf.width;
+	const int h = chf.height;
+	startLevel = startLevel >> loglevelsPerStack;
+
+	for (unsigned int j=0; j<nbStacks; ++j)
+		stacks[j].clear();
+
+	// put all cells in the level range into the appropriate stacks
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (chf.areas[i] == RC_NULL_AREA || srcReg[i] != 0)
+					continue;
+
+				int level = chf.dist[i] >> loglevelsPerStack;
+				int sId = startLevel - level;
+				if (sId >= (int)nbStacks)
+					continue;
+				if (sId < 0)
+					sId = 0;
+
+				stacks[sId].push_back(LevelStackEntry(x, y, i));
+			}
+		}
+	}
+}
+
+
+static void appendStacks(const rcTempVector<LevelStackEntry>& srcStack,
+						 rcTempVector<LevelStackEntry>& dstStack,
+						 const unsigned short* srcReg)
+{
+	for (int j=0; j<srcStack.size(); j++)
+	{
+		int i = srcStack[j].index;
+		if ((i < 0) || (srcReg[i] != 0))
+			continue;
+		dstStack.push_back(srcStack[j]);
+	}
+}
+
+struct rcRegion
+{
+	inline rcRegion(unsigned short i) :
+		spanCount(0),
+		id(i),
+		areaType(0),
+		remap(false),
+		visited(false),
+		overlap(false),
+		connectsToBorder(false),
+		ymin(0xffff),
+		ymax(0)
+	{}
+	
+	int spanCount;					// Number of spans belonging to this region
+	unsigned short id;				// ID of the region
+	unsigned char areaType;			// Are type.
+	bool remap;
+	bool visited;
+	bool overlap;
+	bool connectsToBorder;
+	unsigned short ymin, ymax;
+	rcIntArray connections;
+	rcIntArray floors;
+};
+
+static void removeAdjacentNeighbours(rcRegion& reg)
+{
+	// Remove adjacent duplicates.
+	for (int i = 0; i < reg.connections.size() && reg.connections.size() > 1; )
+	{
+		int ni = (i+1) % reg.connections.size();
+		if (reg.connections[i] == reg.connections[ni])
+		{
+			// Remove duplicate
+			for (int j = i; j < reg.connections.size()-1; ++j)
+				reg.connections[j] = reg.connections[j+1];
+			reg.connections.pop();
+		}
+		else
+			++i;
+	}
+}
+
+static void replaceNeighbour(rcRegion& reg, unsigned short oldId, unsigned short newId)
+{
+	bool neiChanged = false;
+	for (int i = 0; i < reg.connections.size(); ++i)
+	{
+		if (reg.connections[i] == oldId)
+		{
+			reg.connections[i] = newId;
+			neiChanged = true;
+		}
+	}
+	for (int i = 0; i < reg.floors.size(); ++i)
+	{
+		if (reg.floors[i] == oldId)
+			reg.floors[i] = newId;
+	}
+	if (neiChanged)
+		removeAdjacentNeighbours(reg);
+}
+
+static bool canMergeWithRegion(const rcRegion& rega, const rcRegion& regb)
+{
+	if (rega.areaType != regb.areaType)
+		return false;
+	int n = 0;
+	for (int i = 0; i < rega.connections.size(); ++i)
+	{
+		if (rega.connections[i] == regb.id)
+			n++;
+	}
+	if (n > 1)
+		return false;
+	for (int i = 0; i < rega.floors.size(); ++i)
+	{
+		if (rega.floors[i] == regb.id)
+			return false;
+	}
+	return true;
+}
+
+static void addUniqueFloorRegion(rcRegion& reg, int n)
+{
+	for (int i = 0; i < reg.floors.size(); ++i)
+		if (reg.floors[i] == n)
+			return;
+	reg.floors.push(n);
+}
+
+static bool mergeRegions(rcRegion& rega, rcRegion& regb)
+{
+	unsigned short aid = rega.id;
+	unsigned short bid = regb.id;
+	
+	// Duplicate current neighbourhood.
+	rcIntArray acon;
+	acon.resize(rega.connections.size());
+	for (int i = 0; i < rega.connections.size(); ++i)
+		acon[i] = rega.connections[i];
+	rcIntArray& bcon = regb.connections;
+	
+	// Find insertion point on A.
+	int insa = -1;
+	for (int i = 0; i < acon.size(); ++i)
+	{
+		if (acon[i] == bid)
+		{
+			insa = i;
+			break;
+		}
+	}
+	if (insa == -1)
+		return false;
+	
+	// Find insertion point on B.
+	int insb = -1;
+	for (int i = 0; i < bcon.size(); ++i)
+	{
+		if (bcon[i] == aid)
+		{
+			insb = i;
+			break;
+		}
+	}
+	if (insb == -1)
+		return false;
+	
+	// Merge neighbours.
+	rega.connections.resize(0);
+	for (int i = 0, ni = acon.size(); i < ni-1; ++i)
+		rega.connections.push(acon[(insa+1+i) % ni]);
+		
+	for (int i = 0, ni = bcon.size(); i < ni-1; ++i)
+		rega.connections.push(bcon[(insb+1+i) % ni]);
+	
+	removeAdjacentNeighbours(rega);
+	
+	for (int j = 0; j < regb.floors.size(); ++j)
+		addUniqueFloorRegion(rega, regb.floors[j]);
+	rega.spanCount += regb.spanCount;
+	regb.spanCount = 0;
+	regb.connections.resize(0);
+
+	return true;
+}
+
+static bool isRegionConnectedToBorder(const rcRegion& reg)
+{
+	// Region is connected to border if
+	// one of the neighbours is null id.
+	for (int i = 0; i < reg.connections.size(); ++i)
+	{
+		if (reg.connections[i] == 0)
+			return true;
+	}
+	return false;
+}
+
+static bool isSolidEdge(rcCompactHeightfield& chf, const unsigned short* srcReg,
+						int x, int y, int i, int dir)
+{
+	const rcCompactSpan& s = chf.spans[i];
+	unsigned short r = 0;
+	if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+	{
+		const int ax = x + rcGetDirOffsetX(dir);
+		const int ay = y + rcGetDirOffsetY(dir);
+		const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
+		r = srcReg[ai];
+	}
+	if (r == srcReg[i])
+		return false;
+	return true;
+}
+
+static void walkContour(int x, int y, int i, int dir,
+						rcCompactHeightfield& chf,
+						const unsigned short* srcReg,
+						rcIntArray& cont)
+{
+	int startDir = dir;
+	int starti = i;
+
+	const rcCompactSpan& ss = chf.spans[i];
+	unsigned short curReg = 0;
+	if (rcGetCon(ss, dir) != RC_NOT_CONNECTED)
+	{
+		const int ax = x + rcGetDirOffsetX(dir);
+		const int ay = y + rcGetDirOffsetY(dir);
+		const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(ss, dir);
+		curReg = srcReg[ai];
+	}
+	cont.push(curReg);
+			
+	int iter = 0;
+	while (++iter < 40000)
+	{
+		const rcCompactSpan& s = chf.spans[i];
+		
+		if (isSolidEdge(chf, srcReg, x, y, i, dir))
+		{
+			// Choose the edge corner
+			unsigned short r = 0;
+			if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+			{
+				const int ax = x + rcGetDirOffsetX(dir);
+				const int ay = y + rcGetDirOffsetY(dir);
+				const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
+				r = srcReg[ai];
+			}
+			if (r != curReg)
+			{
+				curReg = r;
+				cont.push(curReg);
+			}
+			
+			dir = (dir+1) & 0x3;  // Rotate CW
+		}
+		else
+		{
+			int ni = -1;
+			const int nx = x + rcGetDirOffsetX(dir);
+			const int ny = y + rcGetDirOffsetY(dir);
+			if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+			{
+				const rcCompactCell& nc = chf.cells[nx+ny*chf.width];
+				ni = (int)nc.index + rcGetCon(s, dir);
+			}
+			if (ni == -1)
+			{
+				// Should not happen.
+				return;
+			}
+			x = nx;
+			y = ny;
+			i = ni;
+			dir = (dir+3) & 0x3;	// Rotate CCW
+		}
+		
+		if (starti == i && startDir == dir)
+		{
+			break;
+		}
+	}
+
+	// Remove adjacent duplicates.
+	if (cont.size() > 1)
+	{
+		for (int j = 0; j < cont.size(); )
+		{
+			int nj = (j+1) % cont.size();
+			if (cont[j] == cont[nj])
+			{
+				for (int k = j; k < cont.size()-1; ++k)
+					cont[k] = cont[k+1];
+				cont.pop();
+			}
+			else
+				++j;
+		}
+	}
+}
+
+
+static bool mergeAndFilterRegions(rcContext* ctx, int minRegionArea, int mergeRegionSize,
+								  unsigned short& maxRegionId,
+								  rcCompactHeightfield& chf,
+								  unsigned short* srcReg, rcIntArray& overlaps)
+{
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	const int nreg = maxRegionId+1;
+	rcTempVector<rcRegion> regions;
+	if (!regions.reserve(nreg)) {
+		ctx->log(RC_LOG_ERROR, "mergeAndFilterRegions: Out of memory 'regions' (%d).", nreg);
+		return false;
+	}
+
+	// Construct regions
+	for (int i = 0; i < nreg; ++i)
+		regions.push_back(rcRegion((unsigned short) i));
+	
+	// Find edge of a region and find connections around the contour.
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				unsigned short r = srcReg[i];
+				if (r == 0 || r >= nreg)
+					continue;
+				
+				rcRegion& reg = regions[r];
+				reg.spanCount++;
+				
+				// Update floors.
+				for (int j = (int)c.index; j < ni; ++j)
+				{
+					if (i == j) continue;
+					unsigned short floorId = srcReg[j];
+					if (floorId == 0 || floorId >= nreg)
+						continue;
+					if (floorId == r)
+						reg.overlap = true;
+					addUniqueFloorRegion(reg, floorId);
+				}
+				
+				// Have found contour
+				if (reg.connections.size() > 0)
+					continue;
+				
+				reg.areaType = chf.areas[i];
+				
+				// Check if this cell is next to a border.
+				int ndir = -1;
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (isSolidEdge(chf, srcReg, x, y, i, dir))
+					{
+						ndir = dir;
+						break;
+					}
+				}
+				
+				if (ndir != -1)
+				{
+					// The cell is at border.
+					// Walk around the contour to find all the neighbours.
+					walkContour(x, y, i, ndir, chf, srcReg, reg.connections);
+				}
+			}
+		}
+	}
+
+	// Remove too small regions.
+	rcIntArray stack(32);
+	rcIntArray trace(32);
+	for (int i = 0; i < nreg; ++i)
+	{
+		rcRegion& reg = regions[i];
+		if (reg.id == 0 || (reg.id & RC_BORDER_REG))
+			continue;                       
+		if (reg.spanCount == 0)
+			continue;
+		if (reg.visited)
+			continue;
+		
+		// Count the total size of all the connected regions.
+		// Also keep track of the regions connects to a tile border.
+		bool connectsToBorder = false;
+		int spanCount = 0;
+		stack.resize(0);
+		trace.resize(0);
+
+		reg.visited = true;
+		stack.push(i);
+		
+		while (stack.size())
+		{
+			// Pop
+			int ri = stack.pop();
+			
+			rcRegion& creg = regions[ri];
+
+			spanCount += creg.spanCount;
+			trace.push(ri);
+
+			for (int j = 0; j < creg.connections.size(); ++j)
+			{
+				if (creg.connections[j] & RC_BORDER_REG)
+				{
+					connectsToBorder = true;
+					continue;
+				}
+				rcRegion& neireg = regions[creg.connections[j]];
+				if (neireg.visited)
+					continue;
+				if (neireg.id == 0 || (neireg.id & RC_BORDER_REG))
+					continue;
+				// Visit
+				stack.push(neireg.id);
+				neireg.visited = true;
+			}
+		}
+		
+		// If the accumulated regions size is too small, remove it.
+		// Do not remove areas which connect to tile borders
+		// as their size cannot be estimated correctly and removing them
+		// can potentially remove necessary areas.
+		if (spanCount < minRegionArea && !connectsToBorder)
+		{
+			// Kill all visited regions.
+			for (int j = 0; j < trace.size(); ++j)
+			{
+				regions[trace[j]].spanCount = 0;
+				regions[trace[j]].id = 0;
+			}
+		}
+	}
+	
+	// Merge too small regions to neighbour regions.
+	int mergeCount = 0 ;
+	do
+	{
+		mergeCount = 0;
+		for (int i = 0; i < nreg; ++i)
+		{
+			rcRegion& reg = regions[i];
+			if (reg.id == 0 || (reg.id & RC_BORDER_REG))
+				continue;
+			if (reg.overlap)
+				continue;
+			if (reg.spanCount == 0)
+				continue;
+			
+			// Check to see if the region should be merged.
+			if (reg.spanCount > mergeRegionSize && isRegionConnectedToBorder(reg))
+				continue;
+			
+			// Small region with more than 1 connection.
+			// Or region which is not connected to a border at all.
+			// Find smallest neighbour region that connects to this one.
+			int smallest = 0xfffffff;
+			unsigned short mergeId = reg.id;
+			for (int j = 0; j < reg.connections.size(); ++j)
+			{
+				if (reg.connections[j] & RC_BORDER_REG) continue;
+				rcRegion& mreg = regions[reg.connections[j]];
+				if (mreg.id == 0 || (mreg.id & RC_BORDER_REG) || mreg.overlap) continue;
+				if (mreg.spanCount < smallest &&
+					canMergeWithRegion(reg, mreg) &&
+					canMergeWithRegion(mreg, reg))
+				{
+					smallest = mreg.spanCount;
+					mergeId = mreg.id;
+				}
+			}
+			// Found new id.
+			if (mergeId != reg.id)
+			{
+				unsigned short oldId = reg.id;
+				rcRegion& target = regions[mergeId];
+				
+				// Merge neighbours.
+				if (mergeRegions(target, reg))
+				{
+					// Fixup regions pointing to current region.
+					for (int j = 0; j < nreg; ++j)
+					{
+						if (regions[j].id == 0 || (regions[j].id & RC_BORDER_REG)) continue;
+						// If another region was already merged into current region
+						// change the nid of the previous region too.
+						if (regions[j].id == oldId)
+							regions[j].id = mergeId;
+						// Replace the current region with the new one if the
+						// current regions is neighbour.
+						replaceNeighbour(regions[j], oldId, mergeId);
+					}
+					mergeCount++;
+				}
+			}
+		}
+	}
+	while (mergeCount > 0);
+	
+	// Compress region Ids.
+	for (int i = 0; i < nreg; ++i)
+	{
+		regions[i].remap = false;
+		if (regions[i].id == 0) continue;       // Skip nil regions.
+		if (regions[i].id & RC_BORDER_REG) continue;    // Skip external regions.
+		regions[i].remap = true;
+	}
+	
+	unsigned short regIdGen = 0;
+	for (int i = 0; i < nreg; ++i)
+	{
+		if (!regions[i].remap)
+			continue;
+		unsigned short oldId = regions[i].id;
+		unsigned short newId = ++regIdGen;
+		for (int j = i; j < nreg; ++j)
+		{
+			if (regions[j].id == oldId)
+			{
+				regions[j].id = newId;
+				regions[j].remap = false;
+			}
+		}
+	}
+	maxRegionId = regIdGen;
+	
+	// Remap regions.
+	for (int i = 0; i < chf.spanCount; ++i)
+	{
+		if ((srcReg[i] & RC_BORDER_REG) == 0)
+			srcReg[i] = regions[srcReg[i]].id;
+	}
+
+	// Return regions that we found to be overlapping.
+	for (int i = 0; i < nreg; ++i)
+		if (regions[i].overlap)
+			overlaps.push(regions[i].id);
+
+	return true;
+}
+
+
+static void addUniqueConnection(rcRegion& reg, int n)
+{
+	for (int i = 0; i < reg.connections.size(); ++i)
+		if (reg.connections[i] == n)
+			return;
+	reg.connections.push(n);
+}
+
+static bool mergeAndFilterLayerRegions(rcContext* ctx, int minRegionArea,
+									   unsigned short& maxRegionId,
+									   rcCompactHeightfield& chf,
+									   unsigned short* srcReg)
+{
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	const int nreg = maxRegionId+1;
+	rcTempVector<rcRegion> regions;
+	
+	// Construct regions
+	if (!regions.reserve(nreg)) {
+		ctx->log(RC_LOG_ERROR, "mergeAndFilterLayerRegions: Out of memory 'regions' (%d).", nreg);
+		return false;
+	}
+	for (int i = 0; i < nreg; ++i)
+		regions.push_back(rcRegion((unsigned short) i));
+	
+	// Find region neighbours and overlapping regions.
+	rcIntArray lregs(32);
+	for (int y = 0; y < h; ++y)
+	{
+		for (int x = 0; x < w; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+
+			lregs.resize(0);
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				const unsigned short ri = srcReg[i];
+				if (ri == 0 || ri >= nreg) continue;
+				rcRegion& reg = regions[ri];
+				
+				reg.spanCount++;
+				
+				reg.ymin = rcMin(reg.ymin, s.y);
+				reg.ymax = rcMax(reg.ymax, s.y);
+				
+				// Collect all region layers.
+				lregs.push(ri);
+				
+				// Update neighbours
+				for (int dir = 0; dir < 4; ++dir)
+				{
+					if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
+					{
+						const int ax = x + rcGetDirOffsetX(dir);
+						const int ay = y + rcGetDirOffsetY(dir);
+						const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
+						const unsigned short rai = srcReg[ai];
+						if (rai > 0 && rai < nreg && rai != ri)
+							addUniqueConnection(reg, rai);
+						if (rai & RC_BORDER_REG)
+							reg.connectsToBorder = true;
+					}
+				}
+				
+			}
+			
+			// Update overlapping regions.
+			for (int i = 0; i < lregs.size()-1; ++i)
+			{
+				for (int j = i+1; j < lregs.size(); ++j)
+				{
+					if (lregs[i] != lregs[j])
+					{
+						rcRegion& ri = regions[lregs[i]];
+						rcRegion& rj = regions[lregs[j]];
+						addUniqueFloorRegion(ri, lregs[j]);
+						addUniqueFloorRegion(rj, lregs[i]);
+					}
+				}
+			}
+			
+		}
+	}
+
+	// Create 2D layers from regions.
+	unsigned short layerId = 1;
+
+	for (int i = 0; i < nreg; ++i)
+		regions[i].id = 0;
+
+	// Merge montone regions to create non-overlapping areas.
+	rcIntArray stack(32);
+	for (int i = 1; i < nreg; ++i)
+	{
+		rcRegion& root = regions[i];
+		// Skip already visited.
+		if (root.id != 0)
+			continue;
+		
+		// Start search.
+		root.id = layerId;
+
+		stack.resize(0);
+		stack.push(i);
+		
+		while (stack.size() > 0)
+		{
+			// Pop front
+			rcRegion& reg = regions[stack[0]];
+			for (int j = 0; j < stack.size()-1; ++j)
+				stack[j] = stack[j+1];
+			stack.resize(stack.size()-1);
+			
+			const int ncons = (int)reg.connections.size();
+			for (int j = 0; j < ncons; ++j)
+			{
+				const int nei = reg.connections[j];
+				rcRegion& regn = regions[nei];
+				// Skip already visited.
+				if (regn.id != 0)
+					continue;
+				// Skip if the neighbour is overlapping root region.
+				bool overlap = false;
+				for (int k = 0; k < root.floors.size(); k++)
+				{
+					if (root.floors[k] == nei)
+					{
+						overlap = true;
+						break;
+					}
+				}
+				if (overlap)
+					continue;
+					
+				// Deepen
+				stack.push(nei);
+					
+				// Mark layer id
+				regn.id = layerId;
+				// Merge current layers to root.
+				for (int k = 0; k < regn.floors.size(); ++k)
+					addUniqueFloorRegion(root, regn.floors[k]);
+				root.ymin = rcMin(root.ymin, regn.ymin);
+				root.ymax = rcMax(root.ymax, regn.ymax);
+				root.spanCount += regn.spanCount;
+				regn.spanCount = 0;
+				root.connectsToBorder = root.connectsToBorder || regn.connectsToBorder;
+			}
+		}
+		
+		layerId++;
+	}
+	
+	// Remove small regions
+	for (int i = 0; i < nreg; ++i)
+	{
+		if (regions[i].spanCount > 0 && regions[i].spanCount < minRegionArea && !regions[i].connectsToBorder)
+		{
+			unsigned short reg = regions[i].id;
+			for (int j = 0; j < nreg; ++j)
+				if (regions[j].id == reg)
+					regions[j].id = 0;
+		}
+	}
+	
+	// Compress region Ids.
+	for (int i = 0; i < nreg; ++i)
+	{
+		regions[i].remap = false;
+		if (regions[i].id == 0) continue;				// Skip nil regions.
+		if (regions[i].id & RC_BORDER_REG) continue;    // Skip external regions.
+		regions[i].remap = true;
+	}
+	
+	unsigned short regIdGen = 0;
+	for (int i = 0; i < nreg; ++i)
+	{
+		if (!regions[i].remap)
+			continue;
+		unsigned short oldId = regions[i].id;
+		unsigned short newId = ++regIdGen;
+		for (int j = i; j < nreg; ++j)
+		{
+			if (regions[j].id == oldId)
+			{
+				regions[j].id = newId;
+				regions[j].remap = false;
+			}
+		}
+	}
+	maxRegionId = regIdGen;
+	
+	// Remap regions.
+	for (int i = 0; i < chf.spanCount; ++i)
+	{
+		if ((srcReg[i] & RC_BORDER_REG) == 0)
+			srcReg[i] = regions[srcReg[i]].id;
+	}
+	
+	return true;
+}
+
+
+
+/// @par
+/// 
+/// This is usually the second to the last step in creating a fully built
+/// compact heightfield.  This step is required before regions are built
+/// using #rcBuildRegions or #rcBuildRegionsMonotone.
+/// 
+/// After this step, the distance data is available via the rcCompactHeightfield::maxDistance
+/// and rcCompactHeightfield::dist fields.
+///
+/// @see rcCompactHeightfield, rcBuildRegions, rcBuildRegionsMonotone
+bool rcBuildDistanceField(rcContext* ctx, rcCompactHeightfield& chf)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_DISTANCEFIELD);
+	
+	if (chf.dist)
+	{
+		rcFree(chf.dist);
+		chf.dist = 0;
+	}
+	
+	unsigned short* src = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP);
+	if (!src)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'src' (%d).", chf.spanCount);
+		return false;
+	}
+	unsigned short* dst = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP);
+	if (!dst)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'dst' (%d).", chf.spanCount);
+		rcFree(src);
+		return false;
+	}
+	
+	unsigned short maxDist = 0;
+
+	{
+		rcScopedTimer timerDist(ctx, RC_TIMER_BUILD_DISTANCEFIELD_DIST);
+
+		calculateDistanceField(chf, src, maxDist);
+		chf.maxDistance = maxDist;
+	}
+
+	{
+		rcScopedTimer timerBlur(ctx, RC_TIMER_BUILD_DISTANCEFIELD_BLUR);
+
+		// Blur
+		if (boxBlur(chf, 1, src, dst) != src)
+			rcSwap(src, dst);
+
+		// Store distance.
+		chf.dist = src;
+	}
+	
+	rcFree(dst);
+	
+	return true;
+}
+
+static void paintRectRegion(int minx, int maxx, int miny, int maxy, unsigned short regId,
+							rcCompactHeightfield& chf, unsigned short* srcReg)
+{
+	const int w = chf.width;	
+	for (int y = miny; y < maxy; ++y)
+	{
+		for (int x = minx; x < maxx; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (chf.areas[i] != RC_NULL_AREA)
+					srcReg[i] = regId;
+			}
+		}
+	}
+}
+
+
+static const unsigned short RC_NULL_NEI = 0xffff;
+
+struct rcSweepSpan
+{
+	unsigned short rid;	// row id
+	unsigned short id;	// region id
+	unsigned short ns;	// number samples
+	unsigned short nei;	// neighbour id
+};
+
+/// @par
+/// 
+/// Non-null regions will consist of connected, non-overlapping walkable spans that form a single contour.
+/// Contours will form simple polygons.
+/// 
+/// If multiple regions form an area that is smaller than @p minRegionArea, then all spans will be
+/// re-assigned to the zero (null) region.
+/// 
+/// Partitioning can result in smaller than necessary regions. @p mergeRegionArea helps 
+/// reduce unecessarily small regions.
+/// 
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// The region data will be available via the rcCompactHeightfield::maxRegions
+/// and rcCompactSpan::reg fields.
+/// 
+/// @warning The distance field must be created using #rcBuildDistanceField before attempting to build regions.
+/// 
+/// @see rcCompactHeightfield, rcCompactSpan, rcBuildDistanceField, rcBuildRegionsMonotone, rcConfig
+bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf,
+							const int borderSize, const int minRegionArea, const int mergeRegionArea)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_REGIONS);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	unsigned short id = 1;
+	
+	rcScopedDelete<unsigned short> srcReg((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP));
+	if (!srcReg)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'src' (%d).", chf.spanCount);
+		return false;
+	}
+	memset(srcReg,0,sizeof(unsigned short)*chf.spanCount);
+
+	const int nsweeps = rcMax(chf.width,chf.height);
+	rcScopedDelete<rcSweepSpan> sweeps((rcSweepSpan*)rcAlloc(sizeof(rcSweepSpan)*nsweeps, RC_ALLOC_TEMP));
+	if (!sweeps)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'sweeps' (%d).", nsweeps);
+		return false;
+	}
+	
+	
+	// Mark border regions.
+	if (borderSize > 0)
+	{
+		// Make sure border will not overflow.
+		const int bw = rcMin(w, borderSize);
+		const int bh = rcMin(h, borderSize);
+		// Paint regions
+		paintRectRegion(0, bw, 0, h, id|RC_BORDER_REG, chf, srcReg); id++;
+		paintRectRegion(w-bw, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++;
+		paintRectRegion(0, w, 0, bh, id|RC_BORDER_REG, chf, srcReg); id++;
+		paintRectRegion(0, w, h-bh, h, id|RC_BORDER_REG, chf, srcReg); id++;
+	}
+
+	chf.borderSize = borderSize;
+	
+	rcIntArray prev(256);
+
+	// Sweep one line at a time.
+	for (int y = borderSize; y < h-borderSize; ++y)
+	{
+		// Collect spans from this row.
+		prev.resize(id+1);
+		memset(&prev[0],0,sizeof(int)*id);
+		unsigned short rid = 1;
+		
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA) continue;
+				
+				// -x
+				unsigned short previd = 0;
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					if ((srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai])
+						previd = srcReg[ai];
+				}
+				
+				if (!previd)
+				{
+					previd = rid++;
+					sweeps[previd].rid = previd;
+					sweeps[previd].ns = 0;
+					sweeps[previd].nei = 0;
+				}
+
+				// -y
+				if (rcGetCon(s,3) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					if (srcReg[ai] && (srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai])
+					{
+						unsigned short nr = srcReg[ai];
+						if (!sweeps[previd].nei || sweeps[previd].nei == nr)
+						{
+							sweeps[previd].nei = nr;
+							sweeps[previd].ns++;
+							prev[nr]++;
+						}
+						else
+						{
+							sweeps[previd].nei = RC_NULL_NEI;
+						}
+					}
+				}
+
+				srcReg[i] = previd;
+			}
+		}
+		
+		// Create unique ID.
+		for (int i = 1; i < rid; ++i)
+		{
+			if (sweeps[i].nei != RC_NULL_NEI && sweeps[i].nei != 0 &&
+				prev[sweeps[i].nei] == (int)sweeps[i].ns)
+			{
+				sweeps[i].id = sweeps[i].nei;
+			}
+			else
+			{
+				sweeps[i].id = id++;
+			}
+		}
+		
+		// Remap IDs
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (srcReg[i] > 0 && srcReg[i] < rid)
+					srcReg[i] = sweeps[srcReg[i]].id;
+			}
+		}
+	}
+
+
+	{
+		rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER);
+
+		// Merge regions and filter out small regions.
+		rcIntArray overlaps;
+		chf.maxRegions = id;
+		if (!mergeAndFilterRegions(ctx, minRegionArea, mergeRegionArea, chf.maxRegions, chf, srcReg, overlaps))
+			return false;
+
+		// Monotone partitioning does not generate overlapping regions.
+	}
+	
+	// Store the result out.
+	for (int i = 0; i < chf.spanCount; ++i)
+		chf.spans[i].reg = srcReg[i];
+
+	return true;
+}
+
+/// @par
+/// 
+/// Non-null regions will consist of connected, non-overlapping walkable spans that form a single contour.
+/// Contours will form simple polygons.
+/// 
+/// If multiple regions form an area that is smaller than @p minRegionArea, then all spans will be
+/// re-assigned to the zero (null) region.
+/// 
+/// Watershed partitioning can result in smaller than necessary regions, especially in diagonal corridors. 
+/// @p mergeRegionArea helps reduce unecessarily small regions.
+/// 
+/// See the #rcConfig documentation for more information on the configuration parameters.
+/// 
+/// The region data will be available via the rcCompactHeightfield::maxRegions
+/// and rcCompactSpan::reg fields.
+/// 
+/// @warning The distance field must be created using #rcBuildDistanceField before attempting to build regions.
+/// 
+/// @see rcCompactHeightfield, rcCompactSpan, rcBuildDistanceField, rcBuildRegionsMonotone, rcConfig
+bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf,
+					const int borderSize, const int minRegionArea, const int mergeRegionArea)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_REGIONS);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	
+	rcScopedDelete<unsigned short> buf((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount*2, RC_ALLOC_TEMP));
+	if (!buf)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildRegions: Out of memory 'tmp' (%d).", chf.spanCount*4);
+		return false;
+	}
+	
+	ctx->startTimer(RC_TIMER_BUILD_REGIONS_WATERSHED);
+
+	const int LOG_NB_STACKS = 3;
+	const int NB_STACKS = 1 << LOG_NB_STACKS;
+	rcTempVector<LevelStackEntry> lvlStacks[NB_STACKS];
+	for (int i=0; i<NB_STACKS; ++i)
+		lvlStacks[i].reserve(256);
+
+	rcTempVector<LevelStackEntry> stack;
+	stack.reserve(256);
+	
+	unsigned short* srcReg = buf;
+	unsigned short* srcDist = buf+chf.spanCount;
+	
+	memset(srcReg, 0, sizeof(unsigned short)*chf.spanCount);
+	memset(srcDist, 0, sizeof(unsigned short)*chf.spanCount);
+	
+	unsigned short regionId = 1;
+	unsigned short level = (chf.maxDistance+1) & ~1;
+
+	// TODO: Figure better formula, expandIters defines how much the 
+	// watershed "overflows" and simplifies the regions. Tying it to
+	// agent radius was usually good indication how greedy it could be.
+//	const int expandIters = 4 + walkableRadius * 2;
+	const int expandIters = 8;
+
+	if (borderSize > 0)
+	{
+		// Make sure border will not overflow.
+		const int bw = rcMin(w, borderSize);
+		const int bh = rcMin(h, borderSize);
+		
+		// Paint regions
+		paintRectRegion(0, bw, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+		paintRectRegion(w-bw, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+		paintRectRegion(0, w, 0, bh, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+		paintRectRegion(0, w, h-bh, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++;
+	}
+
+	chf.borderSize = borderSize;
+	
+	int sId = -1;
+	while (level > 0)
+	{
+		level = level >= 2 ? level-2 : 0;
+		sId = (sId+1) & (NB_STACKS-1);
+
+//		ctx->startTimer(RC_TIMER_DIVIDE_TO_LEVELS);
+
+		if (sId == 0)
+			sortCellsByLevel(level, chf, srcReg, NB_STACKS, lvlStacks, 1);
+		else 
+			appendStacks(lvlStacks[sId-1], lvlStacks[sId], srcReg); // copy left overs from last level
+
+//		ctx->stopTimer(RC_TIMER_DIVIDE_TO_LEVELS);
+
+		{
+			rcScopedTimer timerExpand(ctx, RC_TIMER_BUILD_REGIONS_EXPAND);
+
+			// Expand current regions until no empty connected cells found.
+			expandRegions(expandIters, level, chf, srcReg, srcDist, lvlStacks[sId], false);
+		}
+		
+		{
+			rcScopedTimer timerFloor(ctx, RC_TIMER_BUILD_REGIONS_FLOOD);
+
+			// Mark new regions with IDs.
+			for (int j = 0; j<lvlStacks[sId].size(); j++)
+			{
+				LevelStackEntry current = lvlStacks[sId][j];
+				int x = current.x;
+				int y = current.y;
+				int i = current.index;
+				if (i >= 0 && srcReg[i] == 0)
+				{
+					if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack))
+					{
+						if (regionId == 0xFFFF)
+						{
+							ctx->log(RC_LOG_ERROR, "rcBuildRegions: Region ID overflow");
+							return false;
+						}
+						
+						regionId++;
+					}
+				}
+			}
+		}
+	}
+	
+	// Expand current regions until no empty connected cells found.
+	expandRegions(expandIters*8, 0, chf, srcReg, srcDist, stack, true);
+	
+	ctx->stopTimer(RC_TIMER_BUILD_REGIONS_WATERSHED);
+	
+	{
+		rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER);
+
+		// Merge regions and filter out smalle regions.
+		rcIntArray overlaps;
+		chf.maxRegions = regionId;
+		if (!mergeAndFilterRegions(ctx, minRegionArea, mergeRegionArea, chf.maxRegions, chf, srcReg, overlaps))
+			return false;
+
+		// If overlapping regions were found during merging, split those regions.
+		if (overlaps.size() > 0)
+		{
+			ctx->log(RC_LOG_ERROR, "rcBuildRegions: %d overlapping regions.", overlaps.size());
+		}
+	}
+		
+	// Write the result out.
+	for (int i = 0; i < chf.spanCount; ++i)
+		chf.spans[i].reg = srcReg[i];
+	
+	return true;
+}
+
+
+bool rcBuildLayerRegions(rcContext* ctx, rcCompactHeightfield& chf,
+						 const int borderSize, const int minRegionArea)
+{
+	rcAssert(ctx);
+	
+	rcScopedTimer timer(ctx, RC_TIMER_BUILD_REGIONS);
+	
+	const int w = chf.width;
+	const int h = chf.height;
+	unsigned short id = 1;
+	
+	rcScopedDelete<unsigned short> srcReg((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP));
+	if (!srcReg)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildLayerRegions: Out of memory 'src' (%d).", chf.spanCount);
+		return false;
+	}
+	memset(srcReg,0,sizeof(unsigned short)*chf.spanCount);
+	
+	const int nsweeps = rcMax(chf.width,chf.height);
+	rcScopedDelete<rcSweepSpan> sweeps((rcSweepSpan*)rcAlloc(sizeof(rcSweepSpan)*nsweeps, RC_ALLOC_TEMP));
+	if (!sweeps)
+	{
+		ctx->log(RC_LOG_ERROR, "rcBuildLayerRegions: Out of memory 'sweeps' (%d).", nsweeps);
+		return false;
+	}
+	
+	
+	// Mark border regions.
+	if (borderSize > 0)
+	{
+		// Make sure border will not overflow.
+		const int bw = rcMin(w, borderSize);
+		const int bh = rcMin(h, borderSize);
+		// Paint regions
+		paintRectRegion(0, bw, 0, h, id|RC_BORDER_REG, chf, srcReg); id++;
+		paintRectRegion(w-bw, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++;
+		paintRectRegion(0, w, 0, bh, id|RC_BORDER_REG, chf, srcReg); id++;
+		paintRectRegion(0, w, h-bh, h, id|RC_BORDER_REG, chf, srcReg); id++;
+	}
+
+	chf.borderSize = borderSize;
+	
+	rcIntArray prev(256);
+	
+	// Sweep one line at a time.
+	for (int y = borderSize; y < h-borderSize; ++y)
+	{
+		// Collect spans from this row.
+		prev.resize(id+1);
+		memset(&prev[0],0,sizeof(int)*id);
+		unsigned short rid = 1;
+		
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				const rcCompactSpan& s = chf.spans[i];
+				if (chf.areas[i] == RC_NULL_AREA) continue;
+				
+				// -x
+				unsigned short previd = 0;
+				if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(0);
+					const int ay = y + rcGetDirOffsetY(0);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
+					if ((srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai])
+						previd = srcReg[ai];
+				}
+				
+				if (!previd)
+				{
+					previd = rid++;
+					sweeps[previd].rid = previd;
+					sweeps[previd].ns = 0;
+					sweeps[previd].nei = 0;
+				}
+				
+				// -y
+				if (rcGetCon(s,3) != RC_NOT_CONNECTED)
+				{
+					const int ax = x + rcGetDirOffsetX(3);
+					const int ay = y + rcGetDirOffsetY(3);
+					const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
+					if (srcReg[ai] && (srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai])
+					{
+						unsigned short nr = srcReg[ai];
+						if (!sweeps[previd].nei || sweeps[previd].nei == nr)
+						{
+							sweeps[previd].nei = nr;
+							sweeps[previd].ns++;
+							prev[nr]++;
+						}
+						else
+						{
+							sweeps[previd].nei = RC_NULL_NEI;
+						}
+					}
+				}
+				
+				srcReg[i] = previd;
+			}
+		}
+		
+		// Create unique ID.
+		for (int i = 1; i < rid; ++i)
+		{
+			if (sweeps[i].nei != RC_NULL_NEI && sweeps[i].nei != 0 &&
+				prev[sweeps[i].nei] == (int)sweeps[i].ns)
+			{
+				sweeps[i].id = sweeps[i].nei;
+			}
+			else
+			{
+				sweeps[i].id = id++;
+			}
+		}
+		
+		// Remap IDs
+		for (int x = borderSize; x < w-borderSize; ++x)
+		{
+			const rcCompactCell& c = chf.cells[x+y*w];
+			
+			for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
+			{
+				if (srcReg[i] > 0 && srcReg[i] < rid)
+					srcReg[i] = sweeps[srcReg[i]].id;
+			}
+		}
+	}
+	
+	
+	{
+		rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER);
+
+		// Merge monotone regions to layers and remove small regions.
+		chf.maxRegions = id;
+		if (!mergeAndFilterLayerRegions(ctx, minRegionArea, chf.maxRegions, chf, srcReg))
+			return false;
+	}
+	
+	
+	// Store the result out.
+	for (int i = 0; i < chf.spanCount; ++i)
+		chf.spans[i].reg = srcReg[i];
+	
+	return true;
+}

panda/src/recastdetour/config_recastdetour.cxx

@@ -0,0 +1,38 @@
+
+
+#include "config_recastdetour.h"
+
+#include "pandaSystem.h"
+#include "dconfig.h"
+#include "InputGeom.h"
+#include "MeshLoaderObj.h"
+#include "NavMeshSample.h"
+
+#if !defined(CPPPARSER) && !defined(LINK_ALL_STATIC) && !defined(BUILDING_RECASTDETOUR)
+  #error Buildsystem error: BUILDING_RECASTDETOUR not defined
+#endif
+
+Configure(config_recastdetour);
+NotifyCategoryDef(recastdetour, "");
+
+ConfigureFn(config_recastdetour) {
+  init_librecastdetour();
+}
+
+ConfigVariableInt recastdetour_sample_config_variable
+("recastdetour-sample-config-variable", 25);
+
+/**
+ * Initializes the library.  This must be called at least once before any of
+ * the functions or classes in this library can be used.  Normally it will be
+ * called by the static initializers and need not be called explicitly, but
+ * special cases exist.
+ */
+void
+init_librecastdetour() {
+  static bool initialized = false;
+  if (initialized) {
+    return;
+  }
+  initialized = true;
+}

panda/src/recastdetour/config_recastdetour.h

@@ -0,0 +1,17 @@
+
+#ifndef CONFIG_RECASTDETOUR_H
+#define CONFIG_RECASTDETOUR_H
+
+#include "pandabase.h"
+#include "notifyCategoryProxy.h"
+#include "configVariableDouble.h"
+#include "configVariableString.h"
+#include "configVariableInt.h"
+
+NotifyCategoryDecl(recastdetour, EXPCL_RECASTDETOUR, EXPTP_RECASTDETOUR);
+
+extern ConfigVariableInt    recastdetour_sample_config_variable;
+
+extern EXPCL_RECASTDETOUR void init_librecastdetour();
+
+#endif

panda/src/recastdetour/p3recastdetour_composite1.cxx

@@ -0,0 +1,4 @@
+#include "config_recastdetour.cxx"
+#include "InputGeom.cxx"
+#include "MeshLoaderObj.cxx"
+#include "NavMeshSample.cxx"