changed path planner to Theta*, changed the raycast function to emit a cost based on the actual traversal cost

Detour/Include/DetourNavMeshQuery.h

@@ -318,10 +318,13 @@ public:
 	///  @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.
+	///  @param[out]	pathCost	The cost of the path until hit.
+	///  @param[in]		prevRef		[optional]: cost calculation allow for an additional parent ref. Used during pathfinding
 	/// @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;
+					 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath,
+					 float* pathCost=0, 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.

Detour/Include/DetourNode.h

@@ -25,6 +25,7 @@ enum dtNodeFlags
 {
 	DT_NODE_OPEN = 0x01,
 	DT_NODE_CLOSED = 0x02,
+	DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not connected. Found using raycast.
 };
 
 typedef unsigned short dtNodeIndex;
@@ -35,13 +36,14 @@ 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 : 28;		///< Index to parent node.
+	unsigned int pidx : 24;		///< Index to parent node.
 	unsigned int state : 2;		///< extra state information. A polyRef can have multiple nodes with different extra info.
-	unsigned int flags : 2;		///< Node flags 0/open/closed.
+	unsigned int flags : 3;		///< Node flags 0/open/closed.
 	dtPolyRef id;				///< Polygon ref the node corresponds to.
 };
 
 
+
 class dtNodePool
 {
 public:

Detour/Source/DetourNavMeshQuery.cpp

@@ -1016,20 +1016,30 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 		
 		// Get current poly and tile.
 		// The API input has been cheked already, skip checking internal data.
-		const dtPolyRef bestRef = bestNode->id;
+		dtPolyRef bestRef = bestNode->id;
 		const dtMeshTile* bestTile = 0;
 		const dtPoly* bestPoly = 0;
 		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
 		
-		// Get parent poly and tile.
+		// Get parent and grand parent poly and tile.
-		dtPolyRef parentRef = 0;
+		dtPolyRef parentRef = 0, grandpaRef = 0;
 		const dtMeshTile* parentTile = 0;
 		const dtPoly* parentPoly = 0;
+		dtNode* parentNode = 0;
 		if (bestNode->pidx)
-			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		{
+			parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
+			parentRef = parentNode->id;
+			if (parentNode->pidx)
+				grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
+		}
 		if (parentRef)
 			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
 
+		// decide whether to test raycast to previous nodes
+		const float rayCutDist = 30.0f; // TODO: maybe this needs to be on the filter?
+		bool tryLOS = (parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < dtSqr(rayCutDist));
+		
 		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
 		{
 			dtPolyRef neighbourRef = bestTile->links[i].ref;
@@ -1060,6 +1070,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 				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)
 			{
@@ -1072,30 +1086,37 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 			float cost = 0;
 			float heuristic = 0;
 
-			// Special case for last node.
+			// raycast parent
-			if (neighbourRef == endRef)
+			float t = 0;
+			if (tryLOS)
 			{
-				// Cost
+				float normal[3], curCost;
-				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
+				raycast(parentRef, parentNode->pos, neighbourNode->pos, filter, &t, normal, 0, 0, 0, &curCost, grandpaRef);
-													  parentRef, parentTile, parentPoly,
+				cost = parentNode->cost + curCost;
-													  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
+
+			if (t < 1.0f) // hit
 			{
-				// Cost
 				const float curCost = 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 == endRef)
+			{
+				const float endCost = filter->getCost(neighbourNode->pos, endPos,
+													  bestRef, bestTile, bestPoly,
+													  neighbourRef, neighbourTile, neighbourPoly,
+													  0, 0, 0);
+				
+				cost = cost + endCost;
+				heuristic = 0;
+			}
+			else
+			{
 				heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
 			}
 
@@ -1109,11 +1130,13 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 				continue;
 			
 			// Add or update the node.
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->pidx = t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx;
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED));
 			neighbourNode->cost = cost;
 			neighbourNode->total = total;
+			if (t >= 1.0f)
+				neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED);
 			
 			if (neighbourNode->flags & DT_NODE_OPEN)
 			{
@@ -1142,10 +1165,14 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	// Reverse the path.
 	dtNode* prev = 0;
 	dtNode* node = lastBestNode;
+	int prevRay = 0;
 	do
 	{
 		dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
 		node->pidx = m_nodePool->getNodeIdx(prev);
+		int nextRay = node->flags & DT_NODE_PARENT_DETACHED;
+		node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay;
+		prevRay = nextRay;
 		prev = node;
 		node = next;
 	}
@@ -1156,13 +1183,29 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	int n = 0;
 	do
 	{
-		path[n++] = node->id;
+		dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
-		if (n >= maxPath)
+		dtStatus status2 = 0;
+		if (node->flags & DT_NODE_PARENT_DETACHED)
 		{
-			status |= DT_BUFFER_TOO_SMALL;
+			float t, normal[3];
+			int m;
+			status2 = raycast(node->id, node->pos, next->pos, filter, &t, normal, path+n, &m, maxPath-n);
+			n += m;
+		}
+		else
+		{
+			path[n++] = node->id;
+			if (n >= maxPath)
+				status2 = DT_BUFFER_TOO_SMALL;
+		}
+
+		if (status2 & DT_STATUS_DETAIL_MASK)
+		{
+			status |= status2 & DT_STATUS_DETAIL_MASK;
 			break;
 		}
-		node = m_nodePool->getNodeAtIdx(node->pidx);
+
+		node = next;
 	}
 	while (node);
 	
@@ -1275,15 +1318,22 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 			return m_query.status;
 		}
 		
-		// Get parent poly and tile.
+		// Get parent and grand parent poly and tile.
-		dtPolyRef parentRef = 0;
+		dtPolyRef parentRef = 0, grandpaRef = 0;
 		const dtMeshTile* parentTile = 0;
 		const dtPoly* parentPoly = 0;
+		dtNode* parentNode = 0;
 		if (bestNode->pidx)
-			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+		{
+			parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
+			parentRef = parentNode->id;
+			if (parentNode->pidx)
+				grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
+		}
 		if (parentRef)
 		{
-			if (dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)))
+			bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly));
+			if (invalidParent || (grandpaRef && dtStatusFailed(m_nav->isValidPolyRef(grandpaRef))) )
 			{
 				// The polygon has disappeared during the sliced query, fail.
 				m_query.status = DT_FAILURE;
@@ -1293,6 +1343,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 			}
 		}
 
+		// decide whether to test raycast to previous nodes
+		const float rayCutDist = 30.0f; // TODO: maybe this needs to be on the filter?
+		bool tryLOS = (parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < dtSqr(rayCutDist));
+		
 		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
 		{
 			dtPolyRef neighbourRef = bestTile->links[i].ref;
@@ -1322,6 +1376,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 				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)
 			{
@@ -1334,30 +1392,37 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 			float cost = 0;
 			float heuristic = 0;
 			
+			// raycast parent
+			float t = 0;
+			if (tryLOS)
+			{
+				float normal[3], curCost;
+				raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, &t, normal, 0, 0, 0, &curCost, grandpaRef);
+				cost = parentNode->cost + curCost;
+			}
+
+			if (t < 1.0f) // hit
+			{
+				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)
 			{
-				// Cost
-				const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
-															  parentRef, parentTile, parentPoly,
-															  bestRef, bestTile, bestPoly,
-															  neighbourRef, neighbourTile, neighbourPoly);
 				const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
 															  bestRef, bestTile, bestPoly,
 															  neighbourRef, neighbourTile, neighbourPoly,
 															  0, 0, 0);
 				
-				cost = bestNode->cost + curCost + endCost;
+				cost = cost + endCost;
 				heuristic = 0;
 			}
 			else
 			{
-				// Cost
-				const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
-															  parentRef, parentTile, parentPoly,
-															  bestRef, bestTile, bestPoly,
-															  neighbourRef, neighbourTile, neighbourPoly);
-				cost = bestNode->cost + curCost;
 				heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
 			}
 			
@@ -1371,11 +1436,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 				continue;
 			
 			// Add or update the node.
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+			neighbourNode->pidx = t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx;
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED));
 			neighbourNode->cost = cost;
 			neighbourNode->total = total;
+			if (t >= 1.0f)
+				neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED);
 			
 			if (neighbourNode->flags & DT_NODE_OPEN)
 			{
@@ -1439,11 +1506,15 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
 		
 		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;
+			node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay;
+			prevRay = nextRay;
 			node = next;
 		}
 		while (node);
@@ -1452,13 +1523,28 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
 		node = prev;
 		do
 		{
-			path[n++] = node->id;
+			dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
-			if (n >= maxPath)
+			dtStatus status = 0;
+			if (node->flags & DT_NODE_PARENT_DETACHED)
 			{
-				m_query.status |= DT_BUFFER_TOO_SMALL;
+				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;
+			}
+			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 = m_nodePool->getNodeAtIdx(node->pidx);
+			node = next;
 		}
 		while (node);
 	}
@@ -2248,37 +2334,51 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
 ///
 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
+								 float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath,
+								 float* pathCost, dtPolyRef prevRef) const
 {
 	dtAssert(m_nav);
 	
 	*t = 0;
 	if (pathCount)
 		*pathCount = 0;
+	if (pathCost)
+		*pathCost = 0;
 
 	// Validate input
 	if (!startRef || !m_nav->isValidPolyRef(startRef))
 		return DT_FAILURE | DT_INVALID_PARAM;
+	if (prevRef && !m_nav->isValidPolyRef(prevRef))
+		return DT_FAILURE | DT_INVALID_PARAM;
 	
-	dtPolyRef curRef = startRef;
+	float dir[3], curPos[3], lastPos[3];
-	float verts[DT_VERTS_PER_POLYGON*3];	
+	float verts[DT_VERTS_PER_POLYGON*3+3];	
 	int n = 0;
 
-	hitNormal[0] = 0;
+	dtVcopy(curPos, startPos);
-	hitNormal[1] = 0;
+	dtVsub(dir, endPos, startPos);
-	hitNormal[2] = 0;
+	dtVset(hitNormal, 0, 0, 0);
 
 	dtStatus status = DT_SUCCESS;
 
+	const dtMeshTile* prevTile, *tile, *nextTile;
+	const dtPoly* prevPoly, *poly, *nextPoly;
+	dtPolyRef curRef, nextRef;
+
+	// The API input has been checked already, skip checking internal data.
+	nextRef = curRef = startRef;
+	tile = 0;
+	poly = 0;
+	m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
+	nextTile = prevTile = tile;
+	nextPoly = prevPoly = poly;
+	if (prevRef)
+		m_nav->getTileAndPolyByRefUnsafe(prevRef, &tile, &poly);
+
 	while (curRef)
 	{
 		// Cast ray against current polygon.
 		
-		// The API input has been cheked already, skip checking internal data.
-		const dtMeshTile* tile = 0;
-		const dtPoly* poly = 0;
-		m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
-		
 		// Collect vertices.
 		int nv = 0;
 		for (int i = 0; i < (int)poly->vertCount; ++i)
@@ -2286,6 +2386,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
 			nv++;
 		}
+		dtVcopy(&verts[nv*3], &tile->verts[poly->verts[0]*3]);
 		
 		float tmin, tmax;
 		int segMin, segMax;
@@ -2312,11 +2413,15 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			*t = FLT_MAX;
 			if (pathCount)
 				*pathCount = n;
+			
+			// add the cost
+			if (pathCost)
+				*pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
 			return status;
 		}
 
 		// Follow neighbours.
-		dtPolyRef nextRef = 0;
+		nextRef = 0;
 		
 		for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
 		{
@@ -2327,8 +2432,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 				continue;
 			
 			// Get pointer to the next polygon.
-			const dtMeshTile* nextTile = 0;
+			nextTile = 0;
-			const dtPoly* nextPoly = 0;
+			nextPoly = 0;
 			m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
 			
 			// Skip off-mesh connections.
@@ -2396,6 +2501,24 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			}
 		}
 		
+		// add the cost
+		if (pathCost)
+		{
+			// 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, *t);
+			float* e1 = &verts[segMax*3];
+			float* e2 = &verts[(segMax+1)*3]; // no need to modulu nv, seg[nv+1] was added earlier
+			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;
+
+			*pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
+		}
+
 		if (!nextRef)
 		{
 			// No neighbour, we hit a wall.
@@ -2418,7 +2541,12 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 		}
 
 		// No hit, advance to neighbour polygon.
+		prevRef = curRef;
 		curRef = nextRef;
+		prevTile = tile;
+		tile = nextTile;
+		prevPoly = poly;
+		poly = nextPoly;
 	}
 	
 	if (pathCount)