Add getPathFromDijkstraSearch function (recastnavigation#211)

This adds a method to dtNavMeshQuery that allows users to retrieve paths
from the nodes explored by the previous Dijkstra search. This is useful
after Dijkstra searches to retrieve arbitrary paths inside the explored
area.

Detour/Include/DetourNavMeshQuery.h

@@ -193,7 +193,7 @@ class dtNavMeshQuery
 					  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)]
@@ -296,6 +296,20 @@ class dtNavMeshQuery
 								  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
 	///@{
@@ -524,6 +538,9 @@ class dtNavMeshQuery
 	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(class dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const;
 
 	const dtNavMesh* m_nav;				///< Pointer to navmesh data.
 

Detour/Source/DetourNavMeshQuery.cpp

@@ -1022,18 +1022,14 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	dtAssert(m_nodePool);
 	dtAssert(m_openList);
 
-	*pathCount = 0;
-
-	if (!startRef || !endRef)
-		return DT_FAILURE | DT_INVALID_PARAM;
-
-	if (!maxPath)
-		return DT_FAILURE | DT_INVALID_PARAM;
+	if (pathCount)
+		*pathCount = 0;
 
 	// Validate input
-	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
+	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
+		!startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount)
 		return DT_FAILURE | DT_INVALID_PARAM;
-	
+
 	if (startRef == endRef)
 	{
 		path[0] = startRef;
@@ -1193,42 +1189,56 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 			}
 		}
 	}
-
+
+	status = getPathToNode(lastBestNode, path, pathCount, maxPath);
+
 	if (lastBestNode->id != endRef)
 		status |= DT_PARTIAL_RESULT;
 
-	// Reverse the path.
-	dtNode* prev = 0;
-	dtNode* node = lastBestNode;
+	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
 	{
-		dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
-		node->pidx = m_nodePool->getNodeIdx(prev);
-		prev = node;
-		node = next;
+		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);
 	}
-	while (node);
-
-	// Store path
-	node = prev;
-	int n = 0;
-	do
+
+	// Write path
+	for (int i = writeCount - 1; i >= 0; i--)
 	{
-		path[n++] = node->id;
-		if (n >= maxPath)
-		{
-			status |= DT_BUFFER_TOO_SMALL;
-			break;
-		}
-		node = m_nodePool->getNodeAtIdx(node->pidx);
+		dtAssert(curNode);
+
+		path[i] = curNode->id;
+		curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
 	}
-	while (node);
-
-	*pathCount = n;
-
-	return status;
+
+	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() 
@@ -3031,6 +3041,21 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 	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