Detour: Optimized new cost code to removed redundant edge midpoint calculations.

Detour: FIxed broken raycast code on TileNavmesh.
Detour: Added more comments to public API.

Detour/Include/DetourCommon.h

@@ -40,6 +40,20 @@ inline float vdot(const float* v1, const float* v2)
 	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
 }
 
+inline void vmad(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;
+}
+
+inline void vadd(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 void vsub(float* dest, const float* v1, const float* v2)
 {
 	dest[0] = v1[0]-v2[0];
@@ -68,6 +82,14 @@ inline void vcopy(float* dest, const float* a)
 	dest[2] = a[2];
 }
 
+inline float vdist(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 sqrtf(dx*dx + dy*dy + dz*dz);
+}
+
 inline float vdistSqr(const float* v1, const float* v2)
 {
 	float dx = v2[0] - v1[0];

Detour/Include/DetourDebugDraw.h

@@ -24,7 +24,7 @@
 
 void dtDebugDrawStatNavMeshPoly(const dtStatNavMesh* mesh, dtStatPolyRef ref, const float* col);
 void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh);
-void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh);
+void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh, bool drawClosedList = false);
 
 void dtDebugDrawTiledNavMesh(const dtTiledNavMesh* mesh);
 void dtDebugDrawTiledNavMeshPoly(const dtTiledNavMesh* mesh, dtTilePolyRef ref, const float* col);

Detour/Include/DetourStatNavMesh.h

@@ -171,7 +171,7 @@ public:
 	// Returns pointer to specified vertex.
 	inline const float* getVertex(int i) const { return &m_header->verts[i*3]; }
 
-	bool isInOpenList(dtStatPolyRef ref) const;
+	bool isInClosedList(dtStatPolyRef ref) const;
 	
 	int getMemUsed() const;
 
@@ -183,11 +183,6 @@ public:
 	
 private:
 
-	float getCost(dtStatPolyRef prev, dtStatPolyRef from, dtStatPolyRef to) const;
-	float getFirstCost(const float* pos, dtStatPolyRef from, dtStatPolyRef to) const;
-	float getLastCost(dtStatPolyRef from, dtStatPolyRef to, const float* pos) const;
-	float getHeuristic(const float* from, const float* to) const;
-
 	// Copies the locations of vertices of a polygon to an array.
 	int getPolyVerts(dtStatPolyRef ref, float* verts) const;
 	// Returns portal points between two polygons.

Detour/Include/DetourTileNavMesh.h

@@ -62,28 +62,27 @@ struct dtTileLink
 
 struct dtTileHeader
 {
-	int magic;
+	int magic;					// Magic number, used to identify the data.
-	int version;
+	int version;				// Data version number.
-	int npolys;
+	int npolys;					// Number of polygons in the tile.
-	int nverts;
+	int nverts;					// Number of vertices in the tile.
-	int nlinks;
+	int nlinks;					// Number of links in the tile (will be updated when tile is added).
-	int maxlinks;
+	int maxlinks;				// Number of allocated links.
-	float cs;
+	float bmin[3], bmax[3];		// Bounding box of the tile.
-	float bmin[3], bmax[3];
+	dtTilePoly* polys;			// Pointer to the polygons (will be updated when tile is added).
-	dtTilePoly* polys;
+	float* verts;				// Pointer to the vertices (will be updated when tile added).
-	float* verts;
+	dtTileLink* links;			// Pointer to the links (will be updated when tile added).
-	dtTileLink* links;
 };
 
 struct dtTile
 {
-	int salt;
+	int salt;				// Counter describing modifications to the tile.
-	int x,y;
+	int x,y;				// Grid location of the tile.
-	dtTileHeader* header;
+	dtTileHeader* header;	// Pointer to tile header.
-	unsigned char* data;
+	unsigned char* data;	// Pointer to tile data.
-	int dataSize;
+	int dataSize;			// Size of the tile data.
-	bool ownsData;
+	bool ownsData;			// Flag indicating of the navmesh should release the data.
-	dtTile* next;
+	dtTile* next;			// Next free tile or, next tile in spatial grid.
 };
 
 // Encodes a tile id.

Detour/Source/DetourDebugDraw.cpp

@@ -103,14 +103,14 @@ void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh)
 	glEnd();
 }
 
-void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh)
+void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh, bool drawClosedList)
 {
 	glBegin(GL_TRIANGLES);
 	for (int i = 0; i < mesh->getPolyCount(); ++i)
 	{
 		const dtStatPoly* p = mesh->getPoly(i);
 		
-		if (mesh->isInOpenList(i+1))
+		if (drawClosedList && mesh->isInClosedList(i+1))
 			glColor4ub(255,196,0,64);
 		else
 			glColor4ub(0,196,255,64);

Detour/Source/DetourStatNavMesh.cpp

@@ -85,36 +85,6 @@ const dtStatPoly* dtStatNavMesh::getPolyByRef(dtStatPolyRef ref) const
 	return &m_header->polys[ref-1];
 }
 
-float dtStatNavMesh::getCost(dtStatPolyRef prev, dtStatPolyRef from, dtStatPolyRef to) const
-{
-	float midFrom[3], midTo[3];
-	if (!getEdgeMidPoint(prev,from,midFrom) || !getEdgeMidPoint(from,to,midTo))
-		return FLT_MAX;
-	return sqrtf(vdistSqr(midFrom,midTo));
-}
-
-float dtStatNavMesh::getFirstCost(const float* pos, dtStatPolyRef from, dtStatPolyRef to) const
-{
-	float mid[3];
-	if (!getEdgeMidPoint(from,to,mid))
-		return FLT_MAX;
-	return sqrtf(vdistSqr(pos,mid));
-}
-
-float dtStatNavMesh::getLastCost(dtStatPolyRef from, dtStatPolyRef to, const float* pos) const
-{
-	float mid[3];
-	if (!getEdgeMidPoint(from,to,mid))
-		return FLT_MAX;
-	return sqrtf(vdistSqr(mid,pos));
-}
-
-float dtStatNavMesh::getHeuristic(const float* from, const float* to) const
-{
-	return sqrtf(vdistSqr(from,to)) * 1.1f;
-}
-
-
 int dtStatNavMesh::findPath(dtStatPolyRef startRef, dtStatPolyRef endRef,
 							const float* startPos, const float* endPos,
 							dtStatPolyRef* path, const int maxPathSize)
@@ -136,10 +106,12 @@ int dtStatNavMesh::findPath(dtStatPolyRef startRef, dtStatPolyRef endRef,
 	m_nodePool->clear();
 	m_openList->clear();
 
+	static const float H_SCALE = 1.1f;	// Heuristic scale.
+	
 	dtNode* startNode = m_nodePool->getNode(startRef);
 	startNode->pidx = 0;
 	startNode->cost = 0;
-	startNode->total = getHeuristic(startPos, endPos);
+	startNode->total = vdist(startPos, endPos) * H_SCALE;
 	startNode->id = startRef;
 	startNode->flags = DT_NODE_OPEN;
 	m_openList->push(startNode);
@@ -171,20 +143,20 @@ int dtStatNavMesh::findPath(dtStatPolyRef startRef, dtStatPolyRef endRef,
 				newNode.pidx = m_nodePool->getNodeIdx(parent);
 				newNode.id = neighbour;
 
-				newNode.cost = parent->cost;
+				// Calculate cost.
+				float p0[3], p1[3];
 				if (!parent->pidx)
-					newNode.cost += getFirstCost(startPos,parent->id,newNode.id);
+					vcopy(p0, startPos);
 				else
-					newNode.cost += getCost(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, newNode.id);
+					getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
+				getEdgeMidPoint(parent->id, newNode.id, p1);
+				newNode.cost = parent->cost + vdist(p0,p1);
 				// Special case for last node.
 				if (newNode.id == endRef)
-					newNode.cost += getLastCost(parent->id,newNode.id,endPos);
+					newNode.cost += vdist(p1, endPos);
-
-				float ec[3];
-				if (!getEdgeMidPoint(parent->id,newNode.id,ec))
-					continue;
-				const float h = getHeuristic(ec, endPos);
 				
+				// Heuristic
+				const float h = vdist(p1,endPos)*H_SCALE;
 				newNode.total = newNode.cost + h;
 				
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
@@ -568,11 +540,14 @@ float dtStatNavMesh::findDistanceToWall(dtStatPolyRef centerRef, const float* ce
 			newNode.pidx = m_nodePool->getNodeIdx(parent);
 			newNode.id = neighbour;
 			
-			newNode.cost = parent->total;
+			// Cost
+			float p0[3], p1[3];
 			if (!parent->pidx)
-				newNode.cost += getFirstCost(centerPos,parent->id,newNode.id);
+				vcopy(p0, centerPos);
 			else
-				newNode.cost += getCost(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, newNode.id);
+				getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
+			getEdgeMidPoint(parent->id, newNode.id, p1);
+			newNode.total = parent->total + vdist(p0,p1);
 			
 			dtNode* actualNode = m_nodePool->getNode(newNode.id);
 			if (!actualNode)
@@ -667,11 +642,14 @@ int dtStatNavMesh::findPolysAround(dtStatPolyRef centerRef, const float* centerP
 				newNode.pidx = m_nodePool->getNodeIdx(parent);
 				newNode.id = neighbour;
 
-				newNode.cost = parent->total;
+				// Cost
+				float p0[3], p1[3];
 				if (!parent->pidx)
-					newNode.cost += getFirstCost(centerPos,parent->id,newNode.id);
+					vcopy(p0, centerPos);
 				else
-					newNode.cost += getCost(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, newNode.id);
+					getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
+				getEdgeMidPoint(parent->id, newNode.id, p1);
+				newNode.total = parent->total + vdist(p0,p1);
 				
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
 				if (!actualNode)
@@ -817,7 +795,6 @@ bool dtStatNavMesh::getPortalPoints(dtStatPolyRef from, dtStatPolyRef to, float*
 	return false;
 }
 
-// Returns edge mid point between two polygons.
 bool dtStatNavMesh::getEdgeMidPoint(dtStatPolyRef from, dtStatPolyRef to, float* mid) const
 {
 	float left[3], right[3];
@@ -828,10 +805,11 @@ bool dtStatNavMesh::getEdgeMidPoint(dtStatPolyRef from, dtStatPolyRef to, float*
 	return true;
 }
 
-bool dtStatNavMesh::isInOpenList(dtStatPolyRef ref) const
+bool dtStatNavMesh::isInClosedList(dtStatPolyRef ref) const
 {
 	if (!m_nodePool) return false;
-	return m_nodePool->findNode(ref) != 0;
+	const dtNode* node = m_nodePool->findNode(ref);
+	return node && node->flags & DT_NODE_CLOSED;
 }
 
 int dtStatNavMesh::getMemUsed() const

Detour/Source/DetourStatNavMeshBuilder.cpp

@@ -211,7 +211,7 @@ bool dtCreateNavMeshData(const unsigned short* verts, const int nverts,
 						 const float* bmin, const float* bmax, float cs, float ch,
 						 unsigned char** outData, int* outDataSize)
 {
-	if (nvp != DT_STAT_VERTS_PER_POLYGON)
+	if (nvp > DT_STAT_VERTS_PER_POLYGON)
 		return false;
 	if (nverts >= 0xffff)
 		return false;

Detour/Source/DetourTileNavMesh.cpp

@@ -372,7 +372,8 @@ bool dtTiledNavMesh::addTileAt(int x, int y, unsigned char* data, int dataSize,
 	tile->x = x;
 	tile->y = y;
 	tile->data = data;
-	tile->dataSize = ownsData ? -dataSize : dataSize;
+	tile->dataSize = dataSize;
+	tile->ownsData = ownsData;
 
 	buildIntLinks(tile);
 
@@ -458,7 +459,7 @@ bool dtTiledNavMesh::removeTileAt(int x, int y, unsigned char** data, int* dataS
 	
 	
 	// Reset tile.
-	if (tile->dataSize < 0)
+	if (tile->ownsData)
 	{
 		// Owns data
 		delete [] tile->data;
@@ -614,35 +615,6 @@ int dtTiledNavMesh::queryPolygons(const float* center, const float* extents,
 	return n;
 }
 
-float dtTiledNavMesh::getCost(dtTilePolyRef prev, dtTilePolyRef from, dtTilePolyRef to) const
-{
-	float midFrom[3], midTo[3];
-	if (!getEdgeMidPoint(prev,from,midFrom) || !getEdgeMidPoint(from,to,midTo))
-		return FLT_MAX;
-	return sqrtf(vdistSqr(midFrom,midTo));
-}
-
-float dtTiledNavMesh::getFirstCost(const float* pos, dtTilePolyRef from, dtTilePolyRef to) const
-{
-	float mid[3];
-	if (!getEdgeMidPoint(from,to,mid))
-		return FLT_MAX;
-	return sqrtf(vdistSqr(pos,mid));
-}
-
-float dtTiledNavMesh::getLastCost(dtTilePolyRef from, dtTilePolyRef to, const float* pos) const
-{
-	float mid[3];
-	if (!getEdgeMidPoint(from,to,mid))
-		return FLT_MAX;
-	return sqrtf(vdistSqr(mid,pos));
-}
-
-float dtTiledNavMesh::getHeuristic(const float* from, const float* to) const
-{
-	return sqrtf(vdistSqr(from,to)) * 1.1f;
-}
-
 int dtTiledNavMesh::findPath(dtTilePolyRef startRef, dtTilePolyRef endRef,
 							 const float* startPos, const float* endPos,
 							 dtTilePolyRef* path, const int maxPathSize)
@@ -668,10 +640,12 @@ int dtTiledNavMesh::findPath(dtTilePolyRef startRef, dtTilePolyRef endRef,
 	m_nodePool->clear();
 	m_openList->clear();
 	
+	static const float H_SCALE = 1.1f;	// Heuristic scale.
+	
 	dtNode* startNode = m_nodePool->getNode(startRef);
 	startNode->pidx = 0;
 	startNode->cost = 0;
-	startNode->total = getHeuristic(startPos, endPos);
+	startNode->total = vdist(startPos, endPos) * H_SCALE;
 	startNode->id = startRef;
 	startNode->flags = DT_NODE_OPEN;
 	m_openList->push(startNode);
@@ -709,20 +683,20 @@ int dtTiledNavMesh::findPath(dtTilePolyRef startRef, dtTilePolyRef endRef,
 				newNode.pidx = m_nodePool->getNodeIdx(parent);
 				newNode.id = neighbour;
 
-				newNode.cost = parent->cost;
+				// Calculate cost.
+				float p0[3], p1[3];
 				if (!parent->pidx)
-					newNode.cost += getFirstCost(startPos,parent->id,newNode.id);
+					vcopy(p0, startPos);
 				else
-					newNode.cost += getCost(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, newNode.id);
+					getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
+				getEdgeMidPoint(parent->id, newNode.id, p1);
+				newNode.cost = parent->cost + vdist(p0,p1);
 				// Special case for last node.
 				if (newNode.id == endRef)
-					newNode.cost += getLastCost(parent->id,newNode.id,endPos);
+					newNode.cost += vdist(p1, endPos);
-				
-				float ec[3];
-				if (!getEdgeMidPoint(parent->id,newNode.id,ec))
-					continue;
-				const float h = getHeuristic(ec, endPos);
 
+				// Heuristic
+				const float h = vdist(p1,endPos)*H_SCALE;
 				newNode.total = newNode.cost + h;
 				
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
@@ -1061,13 +1035,25 @@ int dtTiledNavMesh::raycast(dtTilePolyRef centerRef, const float* startPos, cons
 					nextRef = link->ref;
 					break;
 				}
+				
 				// If the link is at tile boundary,
-				// Check that the intersection lies inside the portal.
+				const int v0 = poly->v[link->e];
+				const int v1 = poly->v[(link->e+1) % poly->nv];
+				const float* left = &h->verts[v0*3];
+				const float* right = &h->verts[v1*3];
+				
+				// Check that the intersection lies inside the link portal.
 				if (link->side == 0 || link->side == 2)
 				{
+					// Calculate link size.
+					const float smin = min(left[2],right[2]);
+					const float smax = max(left[2],right[2]);
+					const float s = (smax-smin) / 255.0f;
+					const float lmin = smin + link->bmin*s;
+					const float lmax = smin + link->bmax*s;
 					// Find Z intersection.
 					float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
-					if (z >= link->bmin && z <= link->bmax)
+					if (z >= lmin && z <= lmax)
 					{
 						nextRef = link->ref;
 						break;
@@ -1075,9 +1061,15 @@ int dtTiledNavMesh::raycast(dtTilePolyRef centerRef, const float* startPos, cons
 				}
 				else if (link->side == 1 || link->side == 3)
 				{
+					// Calculate link size.
+					const float smin = min(left[0],right[0]);
+					const float smax = max(left[0],right[0]);
+					const float s = (smax-smin) / 255.0f;
+					const float lmin = smin + link->bmin*s;
+					const float lmax = smin + link->bmax*s;
 					// Find X intersection.
 					float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
-					if (x >= link->bmin && x <= link->bmax)
+					if (x >= lmin && x <= lmax)
 					{
 						nextRef = link->ref;
 						break;
@@ -1168,11 +1160,14 @@ int dtTiledNavMesh::findPolysAround(dtTilePolyRef centerRef, const float* center
 				newNode.pidx = m_nodePool->getNodeIdx(parent);
 				newNode.id = neighbour;
 
-				newNode.cost = parent->total;
+				// Cost
+				float p0[3], p1[3];
 				if (!parent->pidx)
-					newNode.cost += getFirstCost(centerPos,parent->id,newNode.id);
+					vcopy(p0, centerPos);
 				else
-					newNode.cost += getCost(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, newNode.id);
+					getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
+				getEdgeMidPoint(parent->id, newNode.id, p1);
+				newNode.total = parent->total + vdist(p0,p1);
 				
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
 				if (!actualNode)
@@ -1311,11 +1306,13 @@ float dtTiledNavMesh::findDistanceToWall(dtTilePolyRef centerRef, const float* c
 				newNode.pidx = m_nodePool->getNodeIdx(parent);
 				newNode.id = neighbour;
 
-				newNode.cost = parent->total;
+				float p0[3], p1[3];
 				if (!parent->pidx)
-					newNode.cost += getFirstCost(centerPos,parent->id,newNode.id);
+					vcopy(p0, centerPos);
 				else
-					newNode.cost += getCost(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, newNode.id);
+					getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
+				getEdgeMidPoint(parent->id, newNode.id, p1);
+				newNode.total = parent->total + vdist(p0,p1);
 				
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
 				if (!actualNode)

Detour/Source/DetourTileNavMeshBuilder.cpp

@@ -86,7 +86,6 @@ bool dtCreateNavMeshTileData(const unsigned short* verts, const int nverts,
 	header->npolys = npolys;
 	header->nverts = nverts;
 	header->maxlinks = nedges + nportals*2;
-	header->cs = cs;
 	header->bmin[0] = bmin[0];
 	header->bmin[1] = bmin[1];
 	header->bmin[2] = bmin[2];

Recast/Include/Recast.h

@@ -37,28 +37,33 @@ struct rcConfig
 	int maxVertsPerPoly;			// Max number of vertices per polygon.
 };
 
+// Heightfield span.
 struct rcSpan
 {
 	unsigned int smin : 15;			// Span min height.
 	unsigned int smax : 15;			// Span max height.
 	unsigned int flags : 2;			// Span flags.
-	rcSpan* next;
+	rcSpan* next;					// Next span in column.
 };
 
 static const int RC_SPANS_PER_POOL = 2048; 
 
+// Memory pool used for quick span allocation.
 struct rcSpanPool
 {
-	rcSpanPool* next;
+	rcSpanPool* next;	// Pointer to next pool.
-	rcSpan items[1];
+	rcSpan items[1];	// Array of spans (size RC_SPANS_PER_POOL).
 };
 
+// Dynamic span-heightfield.
 struct rcHeightfield
 {
 	inline rcHeightfield() : width(0), height(0), spans(0), pools(0), freelist(0) {}
 	inline ~rcHeightfield()
 	{
+		// Delete span array.
 		delete [] spans;
+		// Delete span pools.
 		while (pools)
 		{
 			rcSpanPool* next = pools->next;
@@ -66,77 +71,90 @@ struct rcHeightfield
 			pools = next;
 		}
 	}
-	int width, height;
+	int width, height;			// Dimension of the heightfield.
-	float bmin[3], bmax[3];
+	float bmin[3], bmax[3];		// Bounding box of the heightfield
-	float cs, ch;
+	float cs, ch;				// Cell size and height.
-	rcSpan** spans;
+	rcSpan** spans;				// Heightfield of spans (width*height).
-	rcSpanPool* pools;
+	rcSpanPool* pools;			// Linked list of span pools.
-	rcSpan* freelist;
+	rcSpan* freelist;			// Pointer to next free span.
 };
 
 struct rcCompactCell
 {
-	unsigned int index : 24;
+	unsigned int index : 24;	// Index to first span in column.
-	unsigned int count : 8;
+	unsigned int count : 8;		// Number of spans in this column.
 };
 
 struct rcCompactSpan
 {
-	unsigned short y;
+	unsigned short y;			// Bottom coordinate of the span.
-	unsigned short reg;
+	unsigned short reg;			// Region ID
-	unsigned short dist;
+	unsigned short dist;		// Distance to border
-	unsigned short con;
+	unsigned short con;			// Connections to neighbour cells.
-	unsigned char h;
+	unsigned char h;			// Height of the span.
-	unsigned char flags;
+	unsigned char flags;		// Flags.
 };
 
+// Compact static heightfield. 
 struct rcCompactHeightfield
 {
 	inline rcCompactHeightfield() : maxDistance(0), maxRegions(0), cells(0), spans(0) {}
 	inline ~rcCompactHeightfield() { delete [] cells; delete [] spans; }
-	int width, height;
+	int width, height;					// Width and height of the heighfield.
-	int spanCount;
+	int spanCount;						// Number of spans in the heightfield.
-	int walkableHeight, walkableClimb;
+	int walkableHeight, walkableClimb;	// Agent properties.
-	unsigned short maxDistance; 
+	unsigned short maxDistance;			// Maximum distance value stored in heightfield.
-	unsigned short maxRegions;
+	unsigned short maxRegions;			// Maximum Region Id stored in heightfield.
-	float bmin[3], bmax[3];
+	float bmin[3], bmax[3];				// Bounding box of the heightfield.
-	float cs, ch;
+	float cs, ch;						// Cell size and height.
-	rcCompactCell* cells;
+	rcCompactCell* cells;				// Pointer to width*height cells.
-	rcCompactSpan* spans;
+	rcCompactSpan* spans;				// Pointer to spans.
 };
 
 struct rcContour
 {
 	inline rcContour() : verts(0), nverts(0), rverts(0), nrverts(0) { }
 	inline ~rcContour() { delete [] verts; delete [] rverts; }
-	int* verts;
+	int* verts;			// Vertex coordinates, each vertex contains 4 components.
-	int nverts;
+	int nverts;			// Number of vertices.
-	int* rverts;
+	int* rverts;		// Raw vertex coordinates, each vertex contains 4 components.
-	int nrverts;
+	int nrverts;		// Number of raw vertices.
-	unsigned short reg;
+	unsigned short reg;	// Region ID of the contour.
 };
 
 struct rcContourSet
 {
 	inline rcContourSet() : conts(0), nconts(0) {}
 	inline ~rcContourSet() { delete [] conts; }
-	rcContour* conts;
+	rcContour* conts;	// Pointer to all contours.
-	int nconts;
+	int nconts;			// Number of contours.
 };
 
+// Polymesh store a connected mesh of polygons.
+// The polygons are store in an array where each polygons takes
+// 'nvp*2' elements. The first 'nvp' elements are indices to vertices
+// and the second 'nvp' elements are indices to neighbour polygons.
+// If a polygona has less than 'bvp' vertices, the remaining indices
+// are set os 0xffff. If an polygon edge does not have a neighbour
+// the neighbour index is set to 0xffff.
+// Vertices can be transformed into world space as follows:
+// x = bmin[0] + verts[i*3+0]*cs;
+// y = bmin[1] + verts[i*3+1]*ch;
+// z = bmin[2] + verts[i*3+2]*cs;
 struct rcPolyMesh
 {
 	inline rcPolyMesh() : verts(0), polys(0), nverts(0), npolys(0), nvp(3) {}
 	inline ~rcPolyMesh() { delete [] verts; delete [] polys; }
-	unsigned short* verts;
+	unsigned short* verts;	// Vertices of the mesh, 3 elements per vertex.
-	unsigned short* polys;
+	unsigned short* polys;	// Polygons of the mesh, nvp*2 elements per polygon.
-	int nverts;
+	int nverts;				// Number of vertices.
-	int npolys;
+	int npolys;				// Number of polygons.
-	int nvp;
+	int nvp;				// Max number of vertices per polygon.
-	float bmin[3], bmax[3];
+	float bmin[3], bmax[3];	// Bounding box of the mesh.
-	float cs, ch;
+	float cs, ch;			// Cell size and height.
 };
 
+// Simple dynamic array ints.
 class rcIntArray
 {
 	int* m_data;
@@ -159,7 +177,7 @@ enum rcSpanFlags
 	RC_REACHABLE = 0x02,
 };
 
-// Comppact span neighbour helpers.
+// Compact span neighbour helpers.
 inline int rcGetCon(const rcCompactSpan& s, int dir)
 {
 	return (s.con >> (dir*4)) & 0xf;
@@ -198,6 +216,20 @@ inline float vdot(const float* v1, const float* v2)
 	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
 }
 
+inline void vmad(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;
+}
+
+inline void vadd(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 void vsub(float* dest, const float* v1, const float* v2)
 {
 	dest[0] = v1[0]-v2[0];
@@ -226,6 +258,14 @@ inline void vcopy(float* dest, const float* v)
 	dest[2] = v[2];
 }
 
+inline float vdist(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 sqrtf(dx*dx + dy*dy + dz*dz);
+}
+
 inline float vdistSqr(const float* v1, const float* v2)
 {
 	float dx = v2[0] - v1[0];

RecastDemo/Build/Xcode/Recast.xcodeproj/memon.perspectivev3

@@ -279,14 +279,14 @@
 							<key>PBXSmartGroupTreeModuleOutlineStateSelectionKey</key>
 							<array>
 								<array>
-									<integer>7</integer>
+									<integer>10</integer>
 									<integer>3</integer>
 									<integer>1</integer>
 									<integer>0</integer>
 								</array>
 							</array>
 							<key>PBXSmartGroupTreeModuleOutlineStateVisibleRectKey</key>
-							<string>{{0, 2}, {282, 628}}</string>
+							<string>{{0, 33}, {282, 628}}</string>
 						</dict>
 						<key>PBXTopSmartGroupGIDs</key>
 						<array/>
@@ -321,7 +321,7 @@
 								<key>PBXProjectModuleGUID</key>
 								<string>6B8632A30F78115100E2684A</string>
 								<key>PBXProjectModuleLabel</key>
-								<string>DetourTileNavMesh.cpp</string>
+								<string>DetourTileNavMesh.h</string>
 								<key>PBXSplitModuleInNavigatorKey</key>
 								<dict>
 									<key>Split0</key>
@@ -329,11 +329,11 @@
 										<key>PBXProjectModuleGUID</key>
 										<string>6B8632A40F78115100E2684A</string>
 										<key>PBXProjectModuleLabel</key>
-										<string>DetourTileNavMesh.cpp</string>
+										<string>DetourTileNavMesh.h</string>
 										<key>_historyCapacity</key>
 										<integer>0</integer>
 										<key>bookmark</key>
-										<string>6B996062100F42AF00D7BF5A</string>
+										<string>6BD4DBDB10145C42003FF199</string>
 										<key>history</key>
 										<array>
 											<string>6B7707F00FBD90F100D21BAE</string>
@@ -348,7 +348,6 @@
 											<string>6B2AECED0FFB8B41005BE9CC</string>
 											<string>6B092B4F0FFCA0A20088D3A5</string>
 											<string>6B092B500FFCA0A20088D3A5</string>
-											<string>6B092C5F0FFCFF790088D3A5</string>
 											<string>6B092CC10FFE40160088D3A5</string>
 											<string>6B0249BE1003793900CF7107</string>
 											<string>6B024BD31006059C00CF7107</string>
@@ -356,9 +355,7 @@
 											<string>6B024C1110060C7600CF7107</string>
 											<string>6B1186211006945C0018F96F</string>
 											<string>6B1186D1100699A00018F96F</string>
-											<string>6B1186E610069E200018F96F</string>
 											<string>6B7EBB69100721310066EF8C</string>
-											<string>6B555D23100B136A00247EA3</string>
 											<string>6B555D24100B136A00247EA3</string>
 											<string>6B555D30100B143200247EA3</string>
 											<string>6B555E01100B285300247EA3</string>
@@ -369,35 +366,34 @@
 											<string>6B555F0D100B473F00247EA3</string>
 											<string>6B555F0E100B473F00247EA3</string>
 											<string>6B555F0F100B473F00247EA3</string>
-											<string>6B555F42100B4C5800247EA3</string>
 											<string>6B92CE68100E0577003DA304</string>
 											<string>6B92CE69100E0577003DA304</string>
 											<string>6B92CE6A100E0577003DA304</string>
-											<string>6B92CE6B100E0577003DA304</string>
 											<string>6B92CE6F100E0577003DA304</string>
 											<string>6B92CE70100E0577003DA304</string>
-											<string>6B92CE71100E0577003DA304</string>
 											<string>6B92CE72100E0577003DA304</string>
-											<string>6B92CE74100E0577003DA304</string>
-											<string>6B92CE89100E0739003DA304</string>
 											<string>6B92CE8A100E0739003DA304</string>
 											<string>6B995F9F100F336B00D7BF5A</string>
 											<string>6B995FA2100F336B00D7BF5A</string>
-											<string>6B995FA4100F336B00D7BF5A</string>
-											<string>6B995FDE100F387200D7BF5A</string>
 											<string>6B995FDF100F387200D7BF5A</string>
-											<string>6B995FF4100F39EA00D7BF5A</string>
-											<string>6B996047100F41DB00D7BF5A</string>
-											<string>6B996049100F41DB00D7BF5A</string>
 											<string>6B996059100F42AF00D7BF5A</string>
-											<string>6B99605A100F42AF00D7BF5A</string>
+											<string>6B8AE8DA10121C6000FF1D07</string>
-											<string>6B99605B100F42AF00D7BF5A</string>
+											<string>6B8AE8FA10123B5700FF1D07</string>
-											<string>6B99605C100F42AF00D7BF5A</string>
+											<string>6B8AE8FB10123B5700FF1D07</string>
-											<string>6B99605D100F42AF00D7BF5A</string>
+											<string>6B8AE8FC10123B5700FF1D07</string>
-										</array>
+											<string>6B8AE8FE10123B5700FF1D07</string>
-										<key>nextStack</key>
+											<string>6B8AE90010123B5700FF1D07</string>
-										<array>
+											<string>6B8AE90210123B5700FF1D07</string>
-											<string>6B996061100F42AF00D7BF5A</string>
+											<string>6B8AE90310123B5700FF1D07</string>
+											<string>6B8AE90410123B5700FF1D07</string>
+											<string>6B8AE90510123B5700FF1D07</string>
+											<string>6B8AE90610123B5700FF1D07</string>
+											<string>6B8AE90710123B5700FF1D07</string>
+											<string>6BD4DBB910145A50003FF199</string>
+											<string>6BD4DBBA10145A50003FF199</string>
+											<string>6BD4DBC710145C42003FF199</string>
+											<string>6BD4DBC810145C42003FF199</string>
+											<string>6BD4DBC910145C42003FF199</string>
 										</array>
 										<key>prevStack</key>
 										<array>
@@ -426,7 +422,6 @@
 											<string>6B25B6250FFA63C8004F1BC4</string>
 											<string>6B2AEC740FFB8AB0005BE9CC</string>
 											<string>6B2AEC750FFB8AB0005BE9CC</string>
-											<string>6B2AECB50FFB8AB0005BE9CC</string>
 											<string>6B2AED930FFBA45B005BE9CC</string>
 											<string>6B092B1A0FFC98FF0088D3A5</string>
 											<string>6B092B530FFCA0A20088D3A5</string>
@@ -441,34 +436,40 @@
 											<string>6B11862D1006945C0018F96F</string>
 											<string>6B1186301006945C0018F96F</string>
 											<string>6B1186311006945C0018F96F</string>
-											<string>6B1186401006945C0018F96F</string>
 											<string>6B1186411006945C0018F96F</string>
 											<string>6B555D26100B136A00247EA3</string>
 											<string>6B555DC9100B236A00247EA3</string>
-											<string>6B555E12100B285300247EA3</string>
 											<string>6B555E13100B285300247EA3</string>
 											<string>6B555EE0100B39A600247EA3</string>
 											<string>6B555EF9100B42E600247EA3</string>
 											<string>6B555FB4100B595C00247EA3</string>
-											<string>6B995FA9100F336B00D7BF5A</string>
+											<string>6B8AE8DF10121C6000FF1D07</string>
-											<string>6B995FB4100F336B00D7BF5A</string>
+											<string>6B8AE90D10123B5700FF1D07</string>
-											<string>6B995FB5100F336B00D7BF5A</string>
+											<string>6B8AE90E10123B5700FF1D07</string>
-											<string>6B995FBE100F336B00D7BF5A</string>
+											<string>6B8AE90F10123B5700FF1D07</string>
-											<string>6B995FC1100F336B00D7BF5A</string>
+											<string>6B8AE91010123B5700FF1D07</string>
-											<string>6B995FCF100F345100D7BF5A</string>
+											<string>6B8AE91810123B5700FF1D07</string>
-											<string>6B995FE3100F387200D7BF5A</string>
+											<string>6B8AE91910123B5700FF1D07</string>
-											<string>6B995FE4100F387200D7BF5A</string>
+											<string>6BD4DBBD10145A50003FF199</string>
-											<string>6B995FF7100F39EA00D7BF5A</string>
+											<string>6BD4DBBE10145A50003FF199</string>
-											<string>6B99604D100F41DB00D7BF5A</string>
+											<string>6BD4DBBF10145A50003FF199</string>
-											<string>6B99604E100F41DB00D7BF5A</string>
+											<string>6BD4DBCA10145C42003FF199</string>
-											<string>6B99604F100F41DB00D7BF5A</string>
+											<string>6BD4DBCB10145C42003FF199</string>
-											<string>6B996050100F41DB00D7BF5A</string>
+											<string>6BD4DBCC10145C42003FF199</string>
-											<string>6B996051100F41DB00D7BF5A</string>
+											<string>6BD4DBCD10145C42003FF199</string>
-											<string>6B996053100F41DB00D7BF5A</string>
+											<string>6BD4DBCE10145C42003FF199</string>
-											<string>6B996055100F41DB00D7BF5A</string>
+											<string>6BD4DBCF10145C42003FF199</string>
-											<string>6B99605E100F42AF00D7BF5A</string>
+											<string>6BD4DBD010145C42003FF199</string>
-											<string>6B99605F100F42AF00D7BF5A</string>
+											<string>6BD4DBD110145C42003FF199</string>
-											<string>6B996060100F42AF00D7BF5A</string>
+											<string>6BD4DBD210145C42003FF199</string>
+											<string>6BD4DBD310145C42003FF199</string>
+											<string>6BD4DBD410145C42003FF199</string>
+											<string>6BD4DBD510145C42003FF199</string>
+											<string>6BD4DBD610145C42003FF199</string>
+											<string>6BD4DBD710145C42003FF199</string>
+											<string>6BD4DBD810145C42003FF199</string>
+											<string>6BD4DBD910145C42003FF199</string>
+											<string>6BD4DBDA10145C42003FF199</string>
 										</array>
 									</dict>
 									<key>SplitCount</key>
@@ -482,18 +483,18 @@
 							<key>GeometryConfiguration</key>
 							<dict>
 								<key>Frame</key>
-								<string>{{0, 0}, {976, 548}}</string>
+								<string>{{0, 0}, {976, 449}}</string>
 								<key>RubberWindowFrame</key>
 								<string>0 91 1280 687 0 0 1280 778 </string>
 							</dict>
 							<key>Module</key>
 							<string>PBXNavigatorGroup</string>
 							<key>Proportion</key>
-							<string>548pt</string>
+							<string>449pt</string>
 						</dict>
 						<dict>
 							<key>Proportion</key>
-							<string>93pt</string>
+							<string>192pt</string>
 							<key>Tabs</key>
 							<array>
 								<dict>
@@ -507,7 +508,7 @@
 									<key>GeometryConfiguration</key>
 									<dict>
 										<key>Frame</key>
-										<string>{{10, 27}, {976, 68}}</string>
+										<string>{{10, 27}, {976, -27}}</string>
 									</dict>
 									<key>Module</key>
 									<string>XCDetailModule</string>
@@ -523,7 +524,7 @@
 									<key>GeometryConfiguration</key>
 									<dict>
 										<key>Frame</key>
-										<string>{{10, 27}, {976, 196}}</string>
+										<string>{{10, 27}, {976, -27}}</string>
 									</dict>
 									<key>Module</key>
 									<string>PBXProjectFindModule</string>
@@ -561,7 +562,7 @@
 									<key>GeometryConfiguration</key>
 									<dict>
 										<key>Frame</key>
-										<string>{{10, 27}, {976, 66}}</string>
+										<string>{{10, 27}, {976, 165}}</string>
 										<key>RubberWindowFrame</key>
 										<string>0 91 1280 687 0 0 1280 778 </string>
 									</dict>
@@ -591,11 +592,11 @@
 			</array>
 			<key>TableOfContents</key>
 			<array>
-				<string>6B995F7A100F14BB00D7BF5A</string>
+				<string>6BD4DBB6101456DE003FF199</string>
 				<string>1CA23ED40692098700951B8B</string>
-				<string>6B995F7B100F14BB00D7BF5A</string>
+				<string>6BD4DBB7101456DE003FF199</string>
 				<string>6B8632A30F78115100E2684A</string>
-				<string>6B995F7C100F14BB00D7BF5A</string>
+				<string>6BD4DBB8101456DE003FF199</string>
 				<string>1CA23EDF0692099D00951B8B</string>
 				<string>1CA23EE00692099D00951B8B</string>
 				<string>1CA23EE10692099D00951B8B</string>
@@ -742,14 +743,14 @@
 			</array>
 			<key>TableOfContents</key>
 			<array>
-				<string>6B995FC4100F336D00D7BF5A</string>
+				<string>6B8AE8E410121C6000FF1D07</string>
 				<string>1CCC7628064C1048000F2A68</string>
 				<string>1CCC7629064C1048000F2A68</string>
-				<string>6B995FC5100F336D00D7BF5A</string>
+				<string>6B8AE8E510121C6000FF1D07</string>
-				<string>6B995FC6100F336D00D7BF5A</string>
+				<string>6B8AE8E610121C6000FF1D07</string>
-				<string>6B995FC7100F336D00D7BF5A</string>
+				<string>6B8AE8E710121C6000FF1D07</string>
-				<string>6B995FC8100F336D00D7BF5A</string>
+				<string>6B8AE8E810121C6000FF1D07</string>
-				<string>6B8632A30F78115100E2684A</string>
+				<string>6B8AE8E910121C6000FF1D07</string>
 			</array>
 			<key>ToolbarConfiguration</key>
 			<string>xcode.toolbar.config.debugV3</string>
@@ -779,8 +780,6 @@
 	<integer>5</integer>
 	<key>WindowOrderList</key>
 	<array>
-		<string>6B996027100F3E4200D7BF5A</string>
-		<string>6B996028100F3E4200D7BF5A</string>
 		<string>/Users/memon/Code/recastnavigation/RecastDemo/Build/Xcode/Recast.xcodeproj</string>
 	</array>
 	<key>WindowString</key>

RecastDemo/Source/Sample_StatMesh.cpp

@@ -200,7 +200,7 @@ void Sample_StatMesh::toolRender(int flags)
 	glDepthMask(GL_FALSE);
 
 	if (flags & NAVMESH_POLYS)
-		dtDebugDrawStatNavMesh(m_navMesh);
+		dtDebugDrawStatNavMesh(m_navMesh, m_toolMode == TOOLMODE_PATHFIND);
 	
 	if (flags & NAVMESH_BVTREE)
 		dtDebugDrawStatNavMeshBVTree(m_navMesh);

RecastDemo/Source/Sample_StatMeshSimple.cpp

@@ -18,6 +18,7 @@
 #	define snprintf _snprintf
 #endif
 
+
 Sample_StatMeshSimple::Sample_StatMeshSimple() :
 	m_keepInterResults(false),
 	m_triflags(0),
@@ -263,6 +264,10 @@ bool Sample_StatMeshSimple::handleBuild()
 	cleanup();
 	toolCleanup();
 	
+	//
+	// Step 1. Initialize build config.
+	//
+	
 	// Init build configuration from GUI
 	memset(&m_cfg, 0, sizeof(m_cfg));
 	m_cfg.cs = m_cellSize;
@@ -298,6 +303,10 @@ bool Sample_StatMeshSimple::handleBuild()
 		rcGetLog()->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", m_nverts/1000.0f, m_ntris/1000.0f);
 	}
 	
+	//
+	// Step 2. Rasterize input polygon soup.
+	//
+	
 	// Allocate voxel heighfield where we rasterize our input data to.
 	m_solid = new rcHeightfield;
 	if (!m_solid)
@@ -324,7 +333,6 @@ bool Sample_StatMeshSimple::handleBuild()
 		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 flags for each of the meshes and rasterize them.
@@ -338,12 +346,20 @@ bool Sample_StatMeshSimple::handleBuild()
 		m_triflags = 0;
 	}
 	
+	//
+	// 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.
 	rcFilterLedgeSpans(m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
 	rcFilterWalkableLowHeightSpans(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.
@@ -382,6 +398,10 @@ bool Sample_StatMeshSimple::handleBuild()
 			rcGetLog()->log(RC_LOG_ERROR, "buildNavigation: Could not build regions.");
 	}
 	
+	//
+	// Step 5. Trace and simplify region contours.
+	//
+	
 	// Create contours.
 	m_cset = new rcContourSet;
 	if (!m_cset)
@@ -403,6 +423,10 @@ bool Sample_StatMeshSimple::handleBuild()
 		m_chf = 0;
 	}
 	
+	//
+	// Step 6. Build polygons mesh from contours.
+	//
+	
 	// Build polygon navmesh from the contours.
 	m_polyMesh = new rcPolyMesh;
 	if (!m_polyMesh)
@@ -424,7 +448,16 @@ bool Sample_StatMeshSimple::handleBuild()
 		m_cset = 0;
 	}
 
-	if (m_cfg.maxVertsPerPoly == DT_STAT_VERTS_PER_POLYGON)
+	// At this point the navigation mesh data is ready, you can access it from m_polyMesh.
+	// See rcDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
+	
+	//
+	// (Optional) Step 7. Create Detour data from detour poly mesh.
+	//
+	
+	// The GUI may allow more max points per polygon than Detour can handle.
+	// Only build the detour navmesh if we do not exceed the limit.
+	if (m_cfg.maxVertsPerPoly <= DT_STAT_VERTS_PER_POLYGON)
 	{
 		unsigned char* navData = 0;
 		int navDataSize = 0;

RecastDemo/Source/Sample_StatMeshTiled.cpp

@@ -898,7 +898,7 @@ bool Sample_StatMeshTiled::handleBuild()
 		}
 	}
 	
-	if (m_cfg.maxVertsPerPoly == DT_STAT_VERTS_PER_POLYGON)
+	if (m_cfg.maxVertsPerPoly <= DT_STAT_VERTS_PER_POLYGON)
 	{
 		unsigned char* navData = 0;
 		int navDataSize = 0;