Skip to content

Commit

Permalink
Adds multi start shortest path
Browse files Browse the repository at this point in the history 
Adds the ability to have multiple starting locations for a shortest path
search

Reimplements the current shortest path as a special case of the multi
start variant
  • Loading branch information
Erik Wijmans committed Dec 29, 2018
1 parent 14b2631 commit 9d40208
Show file tree
Hide file tree
Showing 2 changed files with 587 additions and 518 deletions.
116 changes: 67 additions & 49 deletions Detour/Include/DetourNavMeshQuery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,10 @@
#include "DetourNavMesh.h"
#include "DetourStatus.h"


// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter.
// On certain platforms indirect or virtual function call is expensive. The default
// setting is to use non-virtual functions, the actual implementations of the functions
// are declared as inline for maximum speed.
// are declared as inline for maximum speed.

//#define DT_VIRTUAL_QUERYFILTER 1

Expand All @@ -37,14 +36,14 @@ class dtQueryFilter
float m_areaCost[DT_MAX_AREAS]; ///< Cost per area type. (Used by default implementation.)
unsigned short m_includeFlags; ///< Flags for polygons that can be visited. (Used by default implementation.)
unsigned short m_excludeFlags; ///< Flags for polygons that should not be visted. (Used by default implementation.)

public:
dtQueryFilter();

#ifdef DT_VIRTUAL_QUERYFILTER
virtual ~dtQueryFilter() { }
#endif

/// Returns true if the polygon can be visited. (I.e. Is traversable.)
/// @param[in] ref The reference id of the polygon test.
/// @param[in] tile The tile containing the polygon.
Expand Down Expand Up @@ -95,7 +94,7 @@ class dtQueryFilter
/// Sets the traversal cost of the area.
/// @param[in] i The id of the area.
/// @param[in] cost The new cost of traversing the area.
inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; }
inline void setAreaCost(const int i, const float cost) { m_areaCost[i] = cost; }

/// Returns the include flags for the filter.
/// Any polygons that include one or more of these flags will be
Expand All @@ -113,7 +112,7 @@ class dtQueryFilter

/// Sets the exclude flags for the filter.
/// @param[in] flags The new flags.
inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; }
inline void setExcludeFlags(const unsigned short flags) { m_excludeFlags = flags; }

///@}

Expand All @@ -127,17 +126,17 @@ class dtQueryFilter
struct dtRaycastHit
{
/// The hit parameter. (FLT_MAX if no wall hit.)
float t;
float t;

/// hitNormal The normal of the nearest wall hit. [(x, y, z)]
float hitNormal[3];

/// The index of the edge on the final polygon where the wall was hit.
int hitEdgeIndex;

/// Pointer to an array of reference ids of the visited polygons. [opt]
dtPolyRef* path;

/// The number of visited polygons. [opt]
int pathCount;

Expand All @@ -161,6 +160,7 @@ class dtPolyQuery
virtual void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) = 0;
};


/// Provides the ability to perform pathfinding related queries against
/// a navigation mesh.
/// @ingroup detour
Expand All @@ -169,13 +169,13 @@ class dtNavMeshQuery
public:
dtNavMeshQuery();
~dtNavMeshQuery();

/// Initializes the query object.
/// @param[in] nav Pointer to the dtNavMesh object to use for all queries.
/// @param[in] maxNodes Maximum number of search nodes. [Limits: 0 < value <= 65535]
/// @returns The status flags for the query.
dtStatus init(const dtNavMesh* nav, const int maxNodes);

/// @name Standard Pathfinding Functions
// /@{

Expand All @@ -185,7 +185,7 @@ class dtNavMeshQuery
/// @param[in] startPos A position within the start polygon. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @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.)
/// @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]
Expand All @@ -194,6 +194,23 @@ class dtNavMeshQuery
const dtQueryFilter* filter,
dtPolyRef* path, int* pathCount, const int maxPath) const;

/// Finds a path from the closest start location to the end location
/// @param[in] numStarts The number of starting positions
/// @param[in] startRefs An array of the reference ids for all the starting positions
/// @param[in] endRef The reference id of the end polygon.
/// @param[in] startPoses An array of the position within the start polygon
/// for each of the start polygons. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @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]
dtStatus 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;

/// 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 All @@ -217,7 +234,7 @@ class dtNavMeshQuery
/// -# Call initSlicedFindPath() to initialize the sliced path query.
/// -# Call updateSlicedFindPath() until it returns complete.
/// -# Call finalizeSlicedFindPath() to get the path.
///@{
///@{

/// Intializes a sliced path query.
/// @param[in] startRef The refrence id of the start polygon.
Expand All @@ -238,18 +255,18 @@ class dtNavMeshQuery
dtStatus updateSlicedFindPath(const int maxIter, int* doneIters);

/// Finalizes and returns the results of a sliced path query.
/// @param[out] path An ordered list of polygon references representing the path. (Start to end.)
/// @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 max number of polygons the path array can hold. [Limit: >= 1]
/// @returns The status flags for the query.
dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath);

/// Finalizes and returns the results of an incomplete sliced path query, returning the path to the furthest
/// polygon on the existing path that was visited during the search.
/// @param[in] existing An array of polygon references for the existing path.
/// @param[in] existingSize The number of polygon in the @p existing array.
/// @param[out] path An ordered list of polygon references representing the path. (Start to end.)
/// @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 max number of polygons the @p path array can hold. [Limit: >= 1]
Expand All @@ -259,15 +276,15 @@ class dtNavMeshQuery

///@}
/// @name Dijkstra Search Functions
/// @{
/// @{

/// Finds the polygons along the navigation graph that touch the specified circle.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
/// @param[in] radius The radius of the search circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the circle. [opt]
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// Zero if a result polygon has no parent. [opt]
/// @param[out] resultCost The search cost from @p centerPos to the polygon. [opt]
/// @param[out] resultCount The number of polygons found. [opt]
Expand All @@ -277,15 +294,15 @@ class dtNavMeshQuery
const dtQueryFilter* filter,
dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
int* resultCount, const int maxResult) const;

/// Finds the polygons along the naviation graph that touch the specified convex polygon.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] verts The vertices describing the convex polygon. (CCW)
/// @param[in] verts The vertices describing the convex polygon. (CCW)
/// [(x, y, z) * @p nverts]
/// @param[in] nverts The number of vertices in the polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt]
/// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a
/// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a
/// result polygon has no parent. [opt]
/// @param[out] resultCost The search cost from the centroid point to the polygon. [opt]
/// @param[out] resultCount The number of polygons found.
Expand All @@ -295,7 +312,7 @@ class dtNavMeshQuery
const dtQueryFilter* filter,
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.)
Expand Down Expand Up @@ -324,7 +341,7 @@ class dtNavMeshQuery
dtStatus findNearestPoly(const float* center, const float* halfExtents,
const dtQueryFilter* filter,
dtPolyRef* nearestRef, float* nearestPt) const;

/// Finds polygons that overlap the search box.
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] halfExtents The search distance along each axis. [(x, y, z)]
Expand All @@ -351,7 +368,7 @@ class dtNavMeshQuery
/// @param[in] radius The radius of the query circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the circle.
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// Zero if a result polygon has no parent. [opt]
/// @param[out] resultCount The number of polygons found.
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
Expand All @@ -374,12 +391,12 @@ class dtNavMeshQuery
dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
const dtQueryFilter* filter,
float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const;
/// Casts a 'walkability' ray along the surface of the navigation mesh from

/// Casts a 'walkability' ray along the surface of the navigation mesh from
/// the start position toward the end position.
/// @note A wrapper around raycast(..., RaycastHit*). Retained for backward compatibility.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
/// @param[in] endPos The position to cast the ray toward. [(x, y, z)]
/// @param[out] t The hit parameter. (FLT_MAX if no wall hit.)
Expand All @@ -392,11 +409,11 @@ class dtNavMeshQuery
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;
/// Casts a 'walkability' ray along the surface of the navigation mesh from

/// Casts a 'walkability' ray along the surface of the navigation mesh from
/// the start position toward the end position.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
/// @param[in] endPos The position to cast the ray toward. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
Expand All @@ -416,19 +433,19 @@ class dtNavMeshQuery
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] hitDist The distance to the nearest wall from @p centerPos.
/// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)]
/// @param[out] hitNormal The normalized ray formed from the wall point to the
/// @param[out] hitNormal The normalized ray formed from the wall point to the
/// source point. [(x, y, z)]
/// @returns The status flags for the query.
dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
const dtQueryFilter* filter,
float* hitDist, float* hitPos, float* hitNormal) const;

/// Returns the segments for the specified polygon, optionally including portals.
/// @param[in] ref The reference id of the polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount]
/// @param[out] segmentRefs The reference ids of each segment's neighbor polygon.
/// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount]
/// @param[out] segmentRefs The reference ids of each segment's neighbor polygon.
/// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount]
/// @param[out] segmentCount The number of segments returned.
/// @param[in] maxSegments The maximum number of segments the result arrays can hold.
/// @returns The status flags for the query.
Expand All @@ -441,7 +458,7 @@ class dtNavMeshQuery
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] frand Function returning a random number [0..1).
/// @param[out] randomRef The reference id of the random location.
/// @param[out] randomPt The random location.
/// @param[out] randomPt The random location.
/// @returns The status flags for the query.
dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
dtPolyRef* randomRef, float* randomPt) const;
Expand All @@ -459,23 +476,23 @@ class dtNavMeshQuery
dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius,
const dtQueryFilter* filter, float (*frand)(),
dtPolyRef* randomRef, float* randomPt) const;

/// Finds the closest point on the specified polygon.
/// @param[in] ref The reference id of the polygon.
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point on the polygon. [(x, y, z)]
/// @param[out] posOverPoly True of the position is over the polygon.
/// @returns The status flags for the query.
dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
/// Returns a point on the boundary closest to the source point if the source point is outside the

/// Returns a point on the boundary closest to the source point if the source point is outside the
/// polygon's xz-bounds.
/// @param[in] ref The reference id to the polygon.
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point. [(x, y, z)]
/// @returns The status flags for the query.
dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const;

/// Gets the height of the polygon at the provided position using the height detail. (Most accurate.)
/// @param[in] ref The reference id of the polygon.
/// @param[in] pos A position within the xz-bounds of the polygon. [(x, y, z)]
Expand All @@ -492,26 +509,26 @@ class dtNavMeshQuery
/// @param[in] filter The filter to apply.
bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const;

/// Returns true if the polygon reference is in the closed list.
/// Returns true if the polygon reference is in the closed list.
/// @param[in] ref The reference id of the polygon to check.
/// @returns True if the polygon is in closed list.
bool isInClosedList(dtPolyRef ref) const;

/// Gets the node pool.
/// @returns The node pool.
class dtNodePool* getNodePool() const { return m_nodePool; }

/// Gets the navigation mesh the query object is using.
/// @return The navigation mesh the query object is using.
const dtNavMesh* getAttachedNavMesh() const { return m_nav; }

/// @}

private:
// Explicitly disabled copy constructor and copy assignment operator
dtNavMeshQuery(const dtNavMeshQuery&);
dtNavMeshQuery& operator=(const dtNavMeshQuery&);

/// Queries polygons within a tile.
void queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
const dtQueryFilter* filter, dtPolyQuery* query) const;
Expand All @@ -522,13 +539,13 @@ class dtNavMeshQuery
dtStatus getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
float* left, float* right) const;

/// Returns edge mid point between two polygons.
dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const;
dtStatus getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
float* mid) const;

// Appends vertex to a straight path
dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
Expand All @@ -541,7 +558,7 @@ class dtNavMeshQuery

// Gets the path leading to the specified end node.
dtStatus getPathToNode(struct dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const;

const dtNavMesh* m_nav; ///< Pointer to navmesh data.

struct dtQueryData
Expand All @@ -560,6 +577,7 @@ class dtNavMeshQuery
class dtNodePool* m_tinyNodePool; ///< Pointer to small node pool.
class dtNodePool* m_nodePool; ///< Pointer to node pool.
class dtNodeQueue* m_openList; ///< Pointer to open list queue.

};

/// Allocates a query object using the Detour allocator.
Expand Down

0 comments on commit 9d40208

Please sign in to comment.