Skip to content

Commit

Permalink
Adds multi start shortest path
Browse files Browse the repository at this point in the history 
Adds multi-goal shortest path on bidirectional edges by searching
backwards and reversing the path
  • Loading branch information
Erik Wijmans committed Jan 4, 2019
1 parent 14b2631 commit 02be498
Show file tree
Hide file tree
Showing 2 changed files with 100 additions and 22 deletions.
28 changes: 27 additions & 1 deletion Detour/Include/DetourNavMeshQuery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)]
Expand Down Expand Up @@ -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.

Expand Down
94 changes: 73 additions & 21 deletions Detour/Source/DetourNavMeshQuery.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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);
}

Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 02be498

Please sign in to comment.