From 2a0c31b8efcaaf8030bb82dfcdfd155c374595ff Mon Sep 17 00:00:00 2001 From: Jakob Botsch Nielsen Date: Fri, 10 Jun 2016 12:20:27 +0200 Subject: [PATCH] Add getPathFromDijkstraSearch function (#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 | 19 ++++- Detour/Source/DetourNavMeshQuery.cpp | 103 +++++++++++++++++---------- 2 files changed, 82 insertions(+), 40 deletions(-) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 5326600..e2c775e 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -193,7 +193,7 @@ public: 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 @@ public: 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 @@ private: 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. diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 7ef4893..7368efe 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/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; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - int n = 0; - do - { - path[n++] = node->id; - if (n >= maxPath) - { - status |= DT_BUFFER_TOO_SMALL; - break; - } - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node); - - *pathCount = n; - 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() @@ -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