Slightly better heuristic for path finder (Mr. Ericsson would not approve!)

Visualize A* open list.
Visualize parent nodes when querying nodes around.

Detour/Include/DetourStatNavMesh.h

@@ -148,7 +148,7 @@ public:
 	// Returns: Number of results.
 	int	findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius,
 						dtPolyRef* resultRef, dtPolyRef* resultParent,
-						unsigned short* resultCost, unsigned short* resultDepth,
+						float* resultCost, unsigned short* resultDepth,
 						const int maxResult);
 
 	// Returns closest point on navigation polygon.
@@ -159,9 +159,6 @@ public:
 	// Returns: true if closest point found.
 	bool closestPointToPoly(dtPolyRef ref, const float* pos, float* closest) const;
 
-	// Returns cost between two polygons.
-	unsigned short getCost(dtPolyRef from, dtPolyRef to) const;
-
 	// Returns pointer to a polygon based on ref.
 	const dtPoly* getPolyByRef(dtPolyRef ref) const;
 	// Returns number of navigation polygons.
@@ -184,10 +181,15 @@ public:
 	
 private:
 
+	float getCost(dtPolyRef prev, dtPolyRef from, dtPolyRef to) const;
+	float getHeuristic(dtPolyRef from, dtPolyRef to) const;
+
+	bool getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const;
+	
 	// Copies the locations of vertices of a polygon to an array.
-	int getPolyVerts(dtPolyRef ref, float* verts);
+	int getPolyVerts(dtPolyRef ref, float* verts) const;
 	// Returns portal points between two polygons.
-	bool getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right);
+	bool getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right) const;
 
 	unsigned char* m_data;
 	int m_dataSize;

Detour/Source/DetourDebugDraw.cpp

@@ -105,11 +105,16 @@ void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh)
 
 void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh)
 {
-	glColor4ub(0,196,255,64);
 	glBegin(GL_TRIANGLES);
 	for (int i = 0; i < mesh->getPolyCount(); ++i)
 	{
 		const dtPoly* p = mesh->getPoly(i);
+		
+		if (mesh->isInOpenList(i+1))
+			glColor4ub(255,196,0,64);
+		else
+			glColor4ub(0,196,255,64);
+		
 		unsigned short vi[3];
 		for (int j = 2; j < (int)p->nv; ++j)
 		{

Detour/Source/DetourStatNavMesh.cpp

@@ -326,8 +326,8 @@ struct dtNode
 		CLOSED = 0x02,
 	};
 	dtNode* parent;
-	unsigned short cost;
+	float cost;
-	unsigned short total;
+	float total;
 	unsigned short id;
 	unsigned short flags;
 };
@@ -606,17 +606,34 @@ bool dtStatNavMesh::init(unsigned char* data, int dataSize, bool ownsData)
 	return true;
 }
 
-unsigned short dtStatNavMesh::getCost(dtPolyRef from, dtPolyRef to) const
+float dtStatNavMesh::getCost(dtPolyRef prev, dtPolyRef from, dtPolyRef to) const
+{
+	const dtPoly* fromPoly = getPoly(prev ? prev-1 : from-1);
+	const dtPoly* toPoly = getPoly(to-1);
+	float fromPc[3], toPc[3];
+	calcPolyCenter(fromPc, fromPoly, m_verts);
+	calcPolyCenter(toPc, toPoly, m_verts);
+	
+	float dx = fromPc[0]-toPc[0];
+	float dy = fromPc[1]-toPc[1];
+	float dz = fromPc[2]-toPc[2];
+	
+	return sqrtf(dx*dx + dy*dy + dz*dz);
+}
+
+float dtStatNavMesh::getHeuristic(dtPolyRef from, dtPolyRef to) const
 {
 	const dtPoly* fromPoly = getPoly(from-1);
 	const dtPoly* toPoly = getPoly(to-1);
 	float fromPc[3], toPc[3];
 	calcPolyCenter(fromPc, fromPoly, m_verts);
 	calcPolyCenter(toPc, toPoly, m_verts);
-	int cost = (int)(sqrtf(sqr(fromPc[0]-toPc[0]) + sqr(fromPc[2]-toPc[2])) * 4.0f);
+	
-	if (cost < 1) cost = 1;
+	float dx = fromPc[0]-toPc[0];
-	if (cost > 0xffff) cost = 0xffff;
+	float dy = fromPc[1]-toPc[1];
-	return cost;
+	float dz = fromPc[2]-toPc[2];
+	
+	return sqrtf(dx*dx + dy*dy + dz*dz) * 2.0f;
 }
 
 const dtPoly* dtStatNavMesh::getPolyByRef(dtPolyRef ref) const
@@ -648,7 +665,7 @@ int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	dtNode* startNode = m_nodePool->getNode(startRef);
 	startNode->parent = 0;
 	startNode->cost = 0;
-	startNode->total = getCost(startRef, endRef);
+	startNode->total = getHeuristic(startRef, endRef);
 	startNode->id = startRef;
 	startNode->flags = dtNode::OPEN;
 	m_openList->push(startNode);
@@ -678,9 +695,9 @@ int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
 				dtNode newNode;
 				newNode.parent = bestNode;
 				newNode.id = neighbour;
-				newNode.cost = bestNode->cost + getCost(newNode.parent->id, newNode.id);
+				newNode.cost = bestNode->cost + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id);
-				unsigned short costToGoal = getCost(newNode.id, endRef);
+				float h = getHeuristic(newNode.id, endRef);
-				newNode.total = newNode.cost + costToGoal;
+				newNode.total = newNode.cost + h;
 
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
 				if (!actualNode)
@@ -694,9 +711,9 @@ int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
 					actualNode->cost = newNode.cost;
 					actualNode->total = newNode.total;
 
-					if (costToGoal < lastBestNodeCost)
+					if (h < lastBestNodeCost)
 					{
-						lastBestNodeCost = costToGoal;
+						lastBestNodeCost = h;
 						lastBestNode = actualNode;
 					}
 
@@ -911,7 +928,7 @@ int dtStatNavMesh::findStraightPath(const float* startPos, const float* endPos,
 	return straightPathSize;
 }
 
-int dtStatNavMesh::getPolyVerts(dtPolyRef ref, float* verts)
+int dtStatNavMesh::getPolyVerts(dtPolyRef ref, float* verts) const
 {
 	if (!m_header) return 0;
 	const dtPoly* poly = getPolyByRef(ref);
@@ -1061,7 +1078,7 @@ float dtStatNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* center
 			newNode.parent = bestNode;
 			newNode.id = neighbour;
 			newNode.cost = bestNode->cost + 1; // Depth
-			newNode.total = bestNode->total + getCost(newNode.parent->id, newNode.id);
+			newNode.total = bestNode->total + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id);
 			
 			dtNode* actualNode = m_nodePool->getNode(newNode.id);
 			if (!actualNode)
@@ -1097,7 +1114,7 @@ float dtStatNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* center
 
 int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius,
 								   dtPolyRef* resultRef, dtPolyRef* resultParent,
-								   unsigned short* resultCost, unsigned short* resultDepth,
+								   float* resultCost, unsigned short* resultDepth,
 								   const int maxResult)
 {
 	if (!m_header) return 0;
@@ -1158,7 +1175,7 @@ int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos,
 				newNode.parent = bestNode;
 				newNode.id = neighbour;
 				newNode.cost = bestNode->cost + 1; // Depth
-				newNode.total = bestNode->total + getCost(newNode.parent->id, newNode.id);
+				newNode.total = bestNode->total + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id);
 
 				dtNode* actualNode = m_nodePool->getNode(newNode.id);
 				if (!actualNode)
@@ -1187,7 +1204,7 @@ int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos,
 							if (resultCost)
 								resultCost[n] = actualNode->total;
 							if (resultDepth)
-								resultDepth[n] = actualNode->cost;
+								resultDepth[n] = (unsigned short)actualNode->cost;
 							++n;
 						}
 						actualNode->flags = dtNode::OPEN;
@@ -1284,7 +1301,31 @@ dtPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* exten
 	return nearest;
 }
 
-bool dtStatNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right)
+bool dtStatNavMesh::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const
+{
+	const dtPoly* fromPoly = getPolyByRef(from);
+	if (!fromPoly)
+		return false;
+	
+	// Find common edge between the polygons and returns the segment end points.
+	for (unsigned i = 0, j = (int)fromPoly->nv - 1; i < (int)fromPoly->nv; j = i++)
+	{
+		unsigned short neighbour = fromPoly->n[j];
+		if (neighbour == to)
+		{
+			const float* left = getVertex(fromPoly->v[j]);
+			const float* right = getVertex(fromPoly->v[i]);
+			mid[0] = (left[0]+right[0])*0.5f;
+			mid[1] = (left[1]+right[1])*0.5f;
+			mid[2] = (left[2]+right[2])*0.5f;
+			return true;
+		}
+	}
+	
+	return false;
+}
+
+bool dtStatNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right) const
 {
 	const dtPoly* fromPoly = getPolyByRef(from);
 	if (!fromPoly)

Recast/Include/RecastDebugDraw.h

@@ -53,5 +53,6 @@ void rcDebugDrawCylinderWire(float minx, float miny, float minz, float maxx, flo
 void rcDebugDrawBoxWire(float minx, float miny, float minz, float maxx, float maxy, float maxz, const float* col);
 void rcDebugDrawBox(float minx, float miny, float minz, float maxx, float maxy, float maxz,
 					const float* col1, const float* col2);
+void rcDrawArc(const float* p0, const float* p1);
 
 #endif // RECAST_DEBUGDRAW_H

Recast/Source/RecastDebugDraw.cpp

@@ -413,6 +413,13 @@ static void drawArc(const float* p0, const float* p1)
 	}
 }
 
+void rcDrawArc(const float* p0, const float* p1)
+{
+	glBegin(GL_LINES);
+	drawArc(p0, p1);
+	glEnd();
+}
+
 void rcDebugDrawRegionConnections(const rcContourSet& cset, const float* orig, float cs, float ch, const float alpha)
 {
 	// Draw centers
@@ -435,8 +442,6 @@ void rcDebugDrawRegionConnections(const rcContourSet& cset, const float* orig, f
 			{
 				getContourCenter(cont2, orig, cs, ch, pos2);
 				drawArc(pos, pos2);
-//				glVertex3fv(pos);
-//				glVertex3fv(pos2);
 			}
 		}
 	}

RecastDemo/Include/BuilderStatMesh.h

@@ -27,6 +27,7 @@ protected:
 	dtPolyRef m_startRef;
 	dtPolyRef m_endRef;
 	dtPolyRef m_polys[MAX_POLYS];
+	dtPolyRef m_parent[MAX_POLYS];
 	int m_npolys;
 	float m_straightPath[MAX_POLYS*3];
 	int m_nstraightPath;

RecastDemo/Source/BuilderStatMesh.cpp

@@ -159,11 +159,31 @@ void BuilderStatMesh::toolRecalc()
 			const float dx = m_epos[0] - m_spos[0];
 			const float dz = m_epos[2] - m_spos[2];
 			float dist = sqrtf(dx*dx + dz*dz);
-			m_npolys = m_navMesh->findPolysAround(m_startRef, m_spos, dist, m_polys, 0, 0, 0, MAX_POLYS);
+			m_npolys = m_navMesh->findPolysAround(m_startRef, m_spos, dist, m_polys, m_parent, 0, 0, MAX_POLYS);
 		}
 	}
 }
 
+static void getPolyCenter(dtStatNavMesh* navMesh, dtPolyRef ref, float* center)
+{
+	const dtPoly* p = navMesh->getPolyByRef(ref);
+	if (!p) return;
+	center[0] = 0;
+	center[1] = 0;
+	center[2] = 0;
+	for (int i = 0; i < (int)p->nv; ++i)
+	{
+		const float* v = navMesh->getVertex(p->v[i]);
+		center[0] += v[0];
+		center[1] += v[1];
+		center[2] += v[2];
+	}
+	const float s = 1.0f / p->nv;
+	center[0] *= s;
+	center[1] *= s;
+	center[2] *= s;
+}
+
 void BuilderStatMesh::toolRender(int flags)
 {
 	if (!m_navMesh)
@@ -249,8 +269,20 @@ void BuilderStatMesh::toolRender(int flags)
 		}
 		else if (m_toolMode == TOOLMODE_FIND_POLYS_AROUND)
 		{
+			glLineWidth(2.0f);
 			for (int i = 0; i < m_npolys; ++i)
+			{
 				dtDebugDrawStatNavMeshPoly(m_navMesh, m_polys[i], pathCol);
+				if (m_parent[i])
+				{
+					float p0[3], p1[3];
+					getPolyCenter(m_navMesh, m_polys[i], p0);
+					getPolyCenter(m_navMesh, m_parent[i], p1);
+					glColor4ub(0,0,0,128);
+					rcDrawArc(p0, p1);
+				}
+			}
+			glLineWidth(1.0f);
 			
 			const float dx = m_epos[0] - m_spos[0];
 			const float dz = m_epos[2] - m_spos[2];