From 02be4986e3f02c807443366cef56d33b75ad89dc Mon Sep 17 00:00:00 2001 From: Erik Wijmans Date: Mon, 10 Dec 2018 20:25:20 -0500 Subject: [PATCH] Adds multi start shortest path Adds multi-goal shortest path on bidirectional edges by searching backwards and reversing the path --- Detour/Include/DetourNavMeshQuery.h | 28 ++++++++- Detour/Source/DetourNavMeshQuery.cpp | 94 +++++++++++++++++++++------- 2 files changed, 100 insertions(+), 22 deletions(-) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 1c23e4857..91d4496e1 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -194,6 +194,25 @@ class dtNavMeshQuery const dtQueryFilter* filter, dtPolyRef* path, int* pathCount, const int maxPath) const; + /// Finds a path from the start polygon to the closest (by geodesic distance) to any of the goals. + /// Only traverse bidirectional edges. Therefore it is not gaurenteed to find the shortest path + /// or closest goal in meshes with directed offmesh connections. + /// @param[in] numGoals The number of goals + /// @param[in] startRef The refrence id of the start polygon. + /// @param[in] endRefs List of the reference ids of the end polygons. [id * @p numGoals] + /// @param[in] startPos A position within the start polygon. [(x, y, z)] + /// @param[in] endPoses List of end positions within position within the end polygons. [(x, y, z) * @p numGoals] + /// @param[in] filter The polygon filter to apply to the query. + /// @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: >= 1] + /// @param[out] foundGoalIdx Index into the list of goals specifying which goal was found. [opt] + dtStatus findBidirPathToAny(const int numGoals, dtPolyRef startRef, const dtPolyRef* endRefs, + const float* startPos, const float* endPoses, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath, int* foundGoalIdx) 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)] @@ -540,7 +559,14 @@ class dtNavMeshQuery int* straightPathCount, const int maxStraightPath, const int options) const; // Gets the path leading to the specified end node. - dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const; + dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath, int* startIdx) const; + + // Internal implementation of multi-start A* + dtStatus findPathFromAny(const int numStars, const dtPolyRef* startRefs, dtPolyRef endRef, + const float* startPoses, const float* endPos, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath, + const bool reversedSearch, int* startIdx) const; const dtNavMesh* m_nav; ///< Pointer to navmesh data. diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 90999f2f6..4c6aaaab1 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1017,6 +1017,34 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, dtPolyRef* path, int* pathCount, const int maxPath) const +{ + return findPathFromAny(1, &startRef, endRef, startPos, endPos, + filter, path, pathCount, maxPath, false, 0); +} + +dtStatus dtNavMeshQuery::findBidirPathToAny(const int numGoals, dtPolyRef startRef, const dtPolyRef* endRefs, + const float* startPos, const float* endPoses, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath, int* foundGoalIdx) const +{ + dtStatus status = findPathFromAny(numGoals, endRefs, startRef, endPoses, startPos, + filter, path, pathCount, maxPath, true, foundGoalIdx); + + // Reverse the path + for (int i = 0; i < (*pathCount)/2; ++i) + { + dtSwap(path[i], path[*pathCount - 1 - i]); + } + + return status; +} + + +dtStatus dtNavMeshQuery::findPathFromAny(const int numStarts, const dtPolyRef* startRefs, dtPolyRef endRef, + const float* startPoses, const float* endPos, + const dtQueryFilter* filter, + dtPolyRef* path, int* pathCount, const int maxPath, + const bool reversedSearch, int* startIdx) const { dtAssert(m_nav); dtAssert(m_nodePool); @@ -1026,31 +1054,45 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, *pathCount = 0; // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || - !startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount) + if (!m_nav->isValidPolyRef(endRef) || + !startPoses || !endPos || !filter || maxPath <= 0 || !path || !pathCount) return DT_FAILURE | DT_INVALID_PARAM; - if (startRef == endRef) + for (int i = 0; i < numStarts; ++i) + if (!m_nav->isValidPolyRef(startRefs[i])) + return DT_FAILURE | DT_INVALID_PARAM; + + for (int i = 0; i < numStarts; ++i) { - path[0] = startRef; - *pathCount = 1; - return DT_SUCCESS; + if (startRefs[i] == endRef) + { + path[0] = startRefs[i]; + *pathCount = 1; + if (startIdx) + *startIdx = i; + return DT_SUCCESS; + } } m_nodePool->clear(); m_openList->clear(); + for (int i = 0; i < numStarts; ++i) + { + // Put each start node at a unique state. This handles the + // case where there are multiple start positions on the same polygon + // and lets us easly know which start was used to reach the goal + dtNode* startNode = m_nodePool->getNode(startRefs[i], i); + dtVcopy(startNode->pos, startPoses + (3 * i)); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = dtVdist(startNode->pos, endPos) * H_SCALE; + startNode->id = startRefs[i]; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + } - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, startPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = dtVdist(startPos, endPos) * H_SCALE; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - dtNode* lastBestNode = startNode; - float lastBestNodeCost = startNode->total; + dtNode* lastBestNode = m_openList->top(); + float lastBestNodeCost = lastBestNode->total; bool outOfNodes = false; @@ -1091,7 +1133,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Skip invalid ids and do not expand back to where we came from. if (!neighbourRef || neighbourRef == parentRef) continue; - + // Get neighbour poly and tile. // The API input has been cheked already, skip checking internal data. const dtMeshTile* neighbourTile = 0; @@ -1101,6 +1143,13 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; + if (reversedSearch && neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const dtOffMeshConnection* con = m_nav->getOffMeshConnectionByRef(neighbourRef); + if (!con || !(con->flags & DT_OFFMESH_CON_BIDIR)) + continue; + } + // deal explicitly with crossing tile boundaries unsigned char crossSide = 0; if (bestTile->links[i].side != 0xff) @@ -1190,7 +1239,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, } } - dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath); + dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath, startIdx); if (lastBestNode->id != endRef) status |= DT_PARTIAL_RESULT; @@ -1201,7 +1250,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, return status; } -dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const +dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath, int* startIdx) const { // Find the length of the entire path. dtNode* curNode = endNode; @@ -1228,6 +1277,9 @@ dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pa dtAssert(curNode); path[i] = curNode->id; + if (i == 0 && startIdx) + *startIdx = curNode->state; + curNode = m_nodePool->getNodeAtIdx(curNode->pidx); } @@ -3056,7 +3108,7 @@ dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* (endNode->flags & DT_NODE_CLOSED) == 0) return DT_FAILURE | DT_INVALID_PARAM; - return getPathToNode(endNode, path, pathCount, maxPath); + return getPathToNode(endNode, path, pathCount, maxPath, 0); } /// @par