diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h
index 1c23e4857..86552575d 100644
--- a/Detour/Include/DetourNavMeshQuery.h
+++ b/Detour/Include/DetourNavMeshQuery.h
@@ -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
@@ -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.
@@ -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
@@ -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; }
///@}
@@ -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;
@@ -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
@@ -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
// /@{
@@ -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]
@@ -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)]
@@ -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.
@@ -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]
@@ -259,7 +276,7 @@ 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.
@@ -267,7 +284,7 @@ class dtNavMeshQuery
/// @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]
@@ -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.
@@ -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.)
@@ -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)]
@@ -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.
@@ -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.)
@@ -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.
@@ -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.
@@ -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;
@@ -459,7 +476,7 @@ 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)]
@@ -467,15 +484,15 @@ class dtNavMeshQuery
/// @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)]
@@ -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;
@@ -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,
@@ -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
@@ -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.
diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp
index 90999f2f6..4a21a3113 100644
--- a/Detour/Source/DetourNavMeshQuery.cpp
+++ b/Detour/Source/DetourNavMeshQuery.cpp
@@ -30,32 +30,32 @@
/// @class dtQueryFilter
///
/// The Default Implementation
-///
+///
/// At construction: All area costs default to 1.0. All flags are included
/// and none are excluded.
-///
+///
/// If a polygon has both an include and an exclude flag, it will be excluded.
-///
-/// The way filtering works, a navigation mesh polygon must have at least one flag
+///
+/// The way filtering works, a navigation mesh polygon must have at least one flag
/// set to ever be considered by a query. So a polygon with no flags will never
/// be considered.
///
/// Setting the include flags to 0 will result in all polygons being excluded.
///
/// Custom Implementations
-///
+///
/// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class.
-///
-/// Implement a custom query filter by overriding the virtual passFilter()
-/// and getCost() functions. If this is done, both functions should be as
-/// fast as possible. Use cached local copies of data rather than accessing
+///
+/// Implement a custom query filter by overriding the virtual passFilter()
+/// and getCost() functions. If this is done, both functions should be as
+/// fast as possible. Use cached local copies of data rather than accessing
/// your own objects where possible.
-///
-/// Custom implementations do not need to adhere to the flags or cost logic
-/// used by the default implementation.
-///
+///
+/// Custom implementations do not need to adhere to the flags or cost logic
+/// used by the default implementation.
+///
/// In order for A* searches to work properly, the cost should be proportional to
-/// the travel distance. Implementing a cost modifier less than 1.0 is likely
+/// the travel distance. Implementing a cost modifier less than 1.0 is likely
/// to lead to problems during pathfinding.
///
/// @see dtNavMeshQuery
@@ -98,8 +98,8 @@ inline float dtQueryFilter::getCost(const float* pa, const float* pb,
{
return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
}
-#endif
-
+#endif
+
static const float H_SCALE = 0.999f; // Search heuristic scale.
@@ -121,15 +121,15 @@ void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh)
/// @class dtNavMeshQuery
///
-/// For methods that support undersized buffers, if the buffer is too small
-/// to hold the entire result set the return status of the method will include
+/// For methods that support undersized buffers, if the buffer is too small
+/// to hold the entire result set the return status of the method will include
/// the #DT_BUFFER_TOO_SMALL flag.
///
/// Constant member functions can be used by multiple clients without side
/// effects. (E.g. No change to the closed list. No impact on an in-progress
/// sliced path query. Etc.)
-///
-/// Walls and portals: A @e wall is a polygon segment that is
+///
+/// Walls and portals: A @e wall is a polygon segment that is
/// considered impassable. A @e portal is a passable segment between polygons.
/// A portal may be treated as a wall based on the dtQueryFilter used for a query.
///
@@ -152,12 +152,14 @@ dtNavMeshQuery::~dtNavMeshQuery()
m_nodePool->~dtNodePool();
if (m_openList)
m_openList->~dtNodeQueue();
+
+
dtFree(m_tinyNodePool);
dtFree(m_nodePool);
dtFree(m_openList);
}
-/// @par
+/// @par
///
/// Must be the first function called after construction, before other
/// functions are used.
@@ -169,7 +171,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
return DT_FAILURE | DT_INVALID_PARAM;
m_nav = nav;
-
+
if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
{
if (m_nodePool)
@@ -186,7 +188,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
{
m_nodePool->clear();
}
-
+
if (!m_tinyNodePool)
{
m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32);
@@ -197,7 +199,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
{
m_tinyNodePool->clear();
}
-
+
if (!m_openList || m_openList->getCapacity() < maxNodes)
{
if (m_openList)
@@ -214,7 +216,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
{
m_openList->clear();
}
-
+
return DT_SUCCESS;
}
@@ -222,7 +224,7 @@ dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*fr
dtPolyRef* randomRef, float* randomPt) const
{
dtAssert(m_nav);
-
+
// Randomly pick one tile. Assume that all tiles cover roughly the same area.
const dtMeshTile* tile = 0;
float tsum = 0.0f;
@@ -230,7 +232,7 @@ dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*fr
{
const dtMeshTile* t = m_nav->getTile(i);
if (!t || !t->header) continue;
-
+
// Choose random tile using reservoi sampling.
const float area = 1.0f; // Could be tile area too.
tsum += area;
@@ -277,7 +279,7 @@ dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*fr
polyRef = ref;
}
}
-
+
if (!poly)
return DT_FAILURE;
@@ -291,19 +293,19 @@ dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*fr
v = &tile->verts[poly->verts[j]*3];
dtVcopy(&verts[j*3],v);
}
-
+
const float s = frand();
const float t = frand();
-
+
float pt[3];
dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
-
+
float h = 0.0f;
dtStatus status = getPolyHeight(polyRef, pt, &h);
if (dtStatusFailed(status))
return status;
pt[1] = h;
-
+
dtVcopy(randomPt, pt);
*randomRef = polyRef;
@@ -317,20 +319,20 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
dtAssert(m_nav);
dtAssert(m_nodePool);
dtAssert(m_openList);
-
+
// Validate input
if (!startRef || !m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
const dtMeshTile* startTile = 0;
const dtPoly* startPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
if (!filter->passFilter(startRef, startTile, startPoly))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
m_nodePool->clear();
m_openList->clear();
-
+
dtNode* startNode = m_nodePool->getNode(startRef);
dtVcopy(startNode->pos, centerPos);
startNode->pidx = 0;
@@ -339,9 +341,9 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
startNode->id = startRef;
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
-
+
dtStatus status = DT_SUCCESS;
-
+
const float radiusSqr = dtSqr(maxRadius);
float areaSum = 0.0f;
@@ -354,7 +356,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
dtNode* bestNode = m_openList->pop();
bestNode->flags &= ~DT_NODE_OPEN;
bestNode->flags |= DT_NODE_CLOSED;
-
+
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef bestRef = bestNode->id;
@@ -384,8 +386,8 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
randomPolyRef = bestRef;
}
}
-
-
+
+
// Get parent poly and tile.
dtPolyRef parentRef = 0;
const dtMeshTile* parentTile = 0;
@@ -394,7 +396,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
if (parentRef)
m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
-
+
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
const dtLink* link = &bestTile->links[i];
@@ -402,52 +404,52 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
// Skip invalid neighbours and do not follow back to parent.
if (!neighbourRef || neighbourRef == parentRef)
continue;
-
+
// Expand to neighbour
const dtMeshTile* neighbourTile = 0;
const dtPoly* neighbourPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+
// Do not advance if the polygon is excluded by the filter.
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
-
+
// Find edge and calc distance to the edge.
float va[3], vb[3];
if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
continue;
-
+
// If the circle is not touching the next polygon, skip it.
float tseg;
float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
if (distSqr > radiusSqr)
continue;
-
+
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
{
status |= DT_OUT_OF_NODES;
continue;
}
-
+
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
-
+
// Cost
if (neighbourNode->flags == 0)
dtVlerp(neighbourNode->pos, va, vb, 0.5f);
-
+
const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
-
+
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
-
+
neighbourNode->id = neighbourRef;
neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
-
+
if (neighbourNode->flags & DT_NODE_OPEN)
{
m_openList->modify(neighbourNode);
@@ -459,10 +461,10 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
}
}
}
-
+
if (!randomPoly)
return DT_FAILURE;
-
+
// Randomly pick point on polygon.
const float* v = &randomTile->verts[randomPoly->verts[0]*3];
float verts[3*DT_VERTS_PER_POLYGON];
@@ -473,22 +475,22 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
v = &randomTile->verts[randomPoly->verts[j]*3];
dtVcopy(&verts[j*3],v);
}
-
+
const float s = frand();
const float t = frand();
-
+
float pt[3];
dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
-
+
float h = 0.0f;
dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
if (dtStatusFailed(status))
return stat;
pt[1] = h;
-
+
dtVcopy(randomPt, pt);
*randomRef = randomPolyRef;
-
+
return DT_SUCCESS;
}
@@ -512,7 +514,7 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
return DT_FAILURE | DT_INVALID_PARAM;
if (!tile)
return DT_FAILURE | DT_INVALID_PARAM;
-
+
// Off-mesh connections don't have detail polygons.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
@@ -531,13 +533,13 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
const dtPolyDetail* pd = &tile->detailMeshes[ip];
// Clamp point to be inside the polygon.
- float verts[DT_VERTS_PER_POLYGON*3];
+ float verts[DT_VERTS_PER_POLYGON*3];
float edged[DT_VERTS_PER_POLYGON];
float edget[DT_VERTS_PER_POLYGON];
const int nv = poly->vertCount;
for (int i = 0; i < nv; ++i)
dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
-
+
dtVcopy(closest, pos);
if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
@@ -584,7 +586,7 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
break;
}
}
-
+
return DT_SUCCESS;
}
@@ -592,24 +594,24 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
///
/// Much faster than closestPointOnPoly().
///
-/// If the provided position lies within the polygon's xz-bounds (above or below),
+/// If the provided position lies within the polygon's xz-bounds (above or below),
/// then @p pos and @p closest will be equal.
///
/// The height of @p closest will be the polygon boundary. The height detail is not used.
-///
+///
/// @p pos does not have to be within the bounds of the polybon or the navigation mesh.
-///
+///
dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const
{
dtAssert(m_nav);
-
+
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
// Collect vertices.
- float verts[DT_VERTS_PER_POLYGON*3];
+ float verts[DT_VERTS_PER_POLYGON*3];
float edged[DT_VERTS_PER_POLYGON];
float edget[DT_VERTS_PER_POLYGON];
int nv = 0;
@@ -617,8 +619,8 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float*
{
dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
nv++;
- }
-
+ }
+
bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
if (inside)
{
@@ -642,15 +644,15 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float*
const float* vb = &verts[((imin+1)%nv)*3];
dtVlerp(closest, va, vb, edget[imin]);
}
-
+
return DT_SUCCESS;
}
/// @par
///
-/// Will return #DT_FAILURE if the provided position is outside the xz-bounds
+/// Will return #DT_FAILURE if the provided position is outside the xz-bounds
/// of the polygon.
-///
+///
dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
{
dtAssert(m_nav);
@@ -659,7 +661,7 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h
const dtPoly* poly = 0;
if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
const float* v0 = &tile->verts[poly->verts[0]*3];
@@ -695,7 +697,7 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h
}
}
}
-
+
return DT_FAILURE | DT_INVALID_PARAM;
}
@@ -735,13 +737,13 @@ class dtFindNearestPolyQuery : public dtPolyQuery
if (posOverPoly)
{
d = dtAbs(diff[1]) - tile->header->walkableClimb;
- d = d > 0 ? d*d : 0;
+ d = d > 0 ? d*d : 0;
}
else
{
d = dtVlenSqr(diff);
}
-
+
if (d < m_nearestDistanceSqr)
{
dtVcopy(m_nearestPoint, closestPtPoly);
@@ -753,10 +755,10 @@ class dtFindNearestPolyQuery : public dtPolyQuery
}
};
-/// @par
+/// @par
///
-/// @note If the search box does not intersect any polygons the search will
-/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check
+/// @note If the search box does not intersect any polygons the search will
+/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check
/// @p nearestRef before using @p nearestPt.
///
dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents,
@@ -767,7 +769,7 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfE
if (!nearestRef)
return DT_FAILURE | DT_INVALID_PARAM;
-
+
dtFindNearestPolyQuery query(this, center);
dtStatus status = queryPolygons(center, halfExtents, filter, &query);
@@ -779,7 +781,7 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfE
// is valid.
if (nearestPt && *nearestRef)
dtVcopy(nearestPt, query.nearestPoint());
-
+
return DT_SUCCESS;
}
@@ -934,13 +936,13 @@ class dtCollectPolysQuery : public dtPolyQuery
}
};
-/// @par
+/// @par
///
/// If no polygons are found, the function will return #DT_SUCCESS with a
/// @p polyCount of zero.
///
-/// If @p polys is too small to hold the entire result set, then the array will
-/// be filled to capacity. The method of choosing which polygons from the
+/// If @p polys is too small to hold the entire result set, then the array will
+/// be filled to capacity. The method of choosing which polygons from the
/// full set are included in the partial result set is undefined.
///
dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
@@ -960,7 +962,7 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt
return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS;
}
-/// @par
+/// @par
///
/// The query will be invoked with batches of polygons. Polygons passed
/// to the query have bounding boxes that overlap with the center and halfExtents
@@ -978,7 +980,7 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt
float bmin[3], bmax[3];
dtVsub(bmin, center, halfExtents);
dtVadd(bmax, center, halfExtents);
-
+
// Find tiles the query touches.
int minx, miny, maxx, maxy;
m_nav->calcTileLoc(bmin, &minx, &miny);
@@ -986,7 +988,7 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt
static const int MAX_NEIS = 32;
const dtMeshTile* neis[MAX_NEIS];
-
+
for (int y = miny; y <= maxy; ++y)
{
for (int x = minx; x <= maxx; ++x)
@@ -998,7 +1000,7 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt
}
}
}
-
+
return DT_SUCCESS;
}
@@ -1007,74 +1009,118 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExt
/// If the end polygon cannot be reached through the navigation graph,
/// the last polygon in the path will be the nearest the end polygon.
///
-/// If the path array is to small to hold the full result, it will be filled as
+/// If the path array is to small to hold the full result, it will be filled as
/// far as possible from the start polygon toward the end polygon.
///
-/// The start and end positions are used to calculate traversal costs.
+/// The start and end positions are used to calculate traversal costs.
/// (The y-values impact the result.)
///
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);
+}
+
+/// @par
+///
+/// If the end polygon cannot be reached through the navigation graph,
+/// the last polygon in the path will be the nearest the end polygon.
+///
+/// If the path array is to small to hold the full result, it will be filled as
+/// far as possible from the start polygon toward the end polygon.
+///
+/// The start and end positions are used to calculate traversal costs.
+/// (The y-values impact the result.)
+///
+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
{
dtAssert(m_nav);
dtAssert(m_nodePool);
dtAssert(m_openList);
-
+
if (pathCount)
*pathCount = 0;
-
+
// Validate input
- if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
- !startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount)
+ if (numStarts <= 0
+ || !startRefs
+ || !m_nav->isValidPolyRef(endRef)
+ || startPoses == 0
+ || !endPos || !filter || maxPath <= 0 || !path || !pathCount)
return DT_FAILURE | DT_INVALID_PARAM;
- if (startRef == endRef)
+ bool allStartRefsValid = true;
+ for (int i = 0; i < numStarts; ++i)
+ if (!m_nav->isValidPolyRef(startRefs[i]))
+ allStartRefsValid = false;
+
+ if (!allStartRefsValid)
+ 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] = endRef;
+ *pathCount = 1;
+ return DT_SUCCESS;
+ }
+
}
-
+
+
m_nodePool->clear();
m_openList->clear();
-
- 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;
-
+ for (int i = 0; i < numStarts; ++i)
+ {
+ dtNode* startNode = m_nodePool->getNode(startRefs[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* lastBestNode = m_openList->top();
+
bool outOfNodes = false;
-
+ float lastBestNodeCost = lastBestNode->total - lastBestNode->cost;
while (!m_openList->empty())
{
// Remove node from open list and put it in closed list.
dtNode* bestNode = m_openList->pop();
bestNode->flags &= ~DT_NODE_OPEN;
bestNode->flags |= DT_NODE_CLOSED;
-
- // Reached the goal, stop searching.
+
if (bestNode->id == endRef)
{
lastBestNode = bestNode;
+ lastBestNodeCost = bestNode->total - bestNode->cost;
break;
}
-
+
+
// Get current poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef bestRef = bestNode->id;
const dtMeshTile* bestTile = 0;
const dtPoly* bestPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
-
+
// Get parent poly and tile.
dtPolyRef parentRef = 0;
const dtMeshTile* parentTile = 0;
@@ -1083,21 +1129,21 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
if (parentRef)
m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
-
+
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
dtPolyRef neighbourRef = bestTile->links[i].ref;
-
+
// 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;
const dtPoly* neighbourPoly = 0;
- m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+ m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
@@ -1113,7 +1159,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
outOfNodes = true;
continue;
}
-
+
// If the node is visited the first time, calculate node position.
if (neighbourNode->flags == 0)
{
@@ -1125,7 +1171,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
// Calculate cost and heuristic.
float cost = 0;
float heuristic = 0;
-
+
// Special case for last node.
if (neighbourRef == endRef)
{
@@ -1138,7 +1184,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly,
0, 0, 0);
-
+
cost = bestNode->cost + curCost + endCost;
heuristic = 0;
}
@@ -1154,21 +1200,21 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
}
const float total = cost + heuristic;
-
+
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
// The node is already visited and process, and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
continue;
-
+
// Add or update the node.
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->id = neighbourRef;
neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->cost = cost;
neighbourNode->total = total;
-
+
if (neighbourNode->flags & DT_NODE_OPEN)
{
// Already in open, update node location.
@@ -1180,7 +1226,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
neighbourNode->flags |= DT_NODE_OPEN;
m_openList->push(neighbourNode);
}
-
+
// Update nearest node to target so far.
if (heuristic < lastBestNodeCost)
{
@@ -1195,9 +1241,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
if (lastBestNode->id != endRef)
status |= DT_PARTIAL_RESULT;
+
if (outOfNodes)
status |= DT_OUT_OF_NODES;
-
+
return status;
}
@@ -1244,7 +1291,7 @@ dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pa
/// @par
///
-/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath()
+/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath()
/// or finalizeSlicedFindPathPartial() may result in corrupted data!
///
/// The @p filter pointer is stored and used for the duration of the sliced
@@ -1268,10 +1315,10 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
m_query.filter = filter;
m_query.options = options;
m_query.raycastLimitSqr = FLT_MAX;
-
+
if (!startRef || !endRef)
return DT_FAILURE | DT_INVALID_PARAM;
-
+
// Validate input
if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef))
return DT_FAILURE | DT_INVALID_PARAM;
@@ -1279,7 +1326,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
// trade quality with performance?
if (options & DT_FINDPATH_ANY_ANGLE)
{
- // limiting to several times the character radius yields nice results. It is not sensitive
+ // limiting to several times the character radius yields nice results. It is not sensitive
// so it is enough to compute it from the first tile.
const dtMeshTile* tile = m_nav->getTileByRef(startRef);
float agentRadius = tile->header->walkableRadius;
@@ -1291,10 +1338,10 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
m_query.status = DT_SUCCESS;
return DT_SUCCESS;
}
-
+
m_nodePool->clear();
m_openList->clear();
-
+
dtNode* startNode = m_nodePool->getNode(startRef);
dtVcopy(startNode->pos, startPos);
startNode->pidx = 0;
@@ -1303,14 +1350,14 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
startNode->id = startRef;
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
-
+
m_query.status = DT_IN_PROGRESS;
m_query.lastBestNode = startNode;
m_query.lastBestNodeCost = startNode->total;
-
+
return m_query.status;
}
-
+
dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
{
if (!dtStatusInProgress(m_query.status))
@@ -1325,17 +1372,17 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
dtRaycastHit rayHit;
rayHit.maxPath = 0;
-
+
int iter = 0;
while (iter < maxIter && !m_openList->empty())
{
iter++;
-
+
// Remove node from open list and put it in closed list.
dtNode* bestNode = m_openList->pop();
bestNode->flags &= ~DT_NODE_OPEN;
bestNode->flags |= DT_NODE_CLOSED;
-
+
// Reached the goal, stop searching.
if (bestNode->id == m_query.endRef)
{
@@ -1346,7 +1393,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
*doneIters = iter;
return m_query.status;
}
-
+
// Get current poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef bestRef = bestNode->id;
@@ -1360,7 +1407,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
*doneIters = iter;
return m_query.status;
}
-
+
// Get parent and grand parent poly and tile.
dtPolyRef parentRef = 0, grandpaRef = 0;
const dtMeshTile* parentTile = 0;
@@ -1393,24 +1440,24 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr))
tryLOS = true;
}
-
+
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
dtPolyRef neighbourRef = bestTile->links[i].ref;
-
+
// 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;
const dtPoly* neighbourPoly = 0;
- m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+ m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+
if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
-
+
// get the neighbor node
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0);
if (!neighbourNode)
@@ -1418,7 +1465,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
m_query.status |= DT_OUT_OF_NODES;
continue;
}
-
+
// do not expand to nodes that were already visited from the same parent
if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx)
continue;
@@ -1430,11 +1477,11 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
neighbourRef, neighbourPoly, neighbourTile,
neighbourNode->pos);
}
-
+
// Calculate cost and heuristic.
float cost = 0;
float heuristic = 0;
-
+
// raycast parent
bool foundShortCut = false;
rayHit.pathCost = rayHit.t = 0;
@@ -1467,7 +1514,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly,
0, 0, 0);
-
+
cost = cost + endCost;
heuristic = 0;
}
@@ -1475,16 +1522,16 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
{
heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
}
-
+
const float total = cost + heuristic;
-
+
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
// The node is already visited and process, and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
continue;
-
+
// Add or update the node.
neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode);
neighbourNode->id = neighbourRef;
@@ -1493,7 +1540,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
neighbourNode->total = total;
if (foundShortCut)
neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED);
-
+
if (neighbourNode->flags & DT_NODE_OPEN)
{
// Already in open, update node location.
@@ -1505,7 +1552,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
neighbourNode->flags |= DT_NODE_OPEN;
m_openList->push(neighbourNode);
}
-
+
// Update nearest node to target so far.
if (heuristic < m_query.lastBestNodeCost)
{
@@ -1514,7 +1561,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
}
}
}
-
+
// Exhausted all nodes, but could not find path.
if (m_openList->empty())
{
@@ -1531,7 +1578,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath)
{
*pathCount = 0;
-
+
if (dtStatusFailed(m_query.status))
{
// Reset query.
@@ -1550,10 +1597,10 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
{
// Reverse the path.
dtAssert(m_query.lastBestNode);
-
+
if (m_query.lastBestNode->id != m_query.endRef)
m_query.status |= DT_PARTIAL_RESULT;
-
+
dtNode* prev = 0;
dtNode* node = m_query.lastBestNode;
int prevRay = 0;
@@ -1568,7 +1615,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
node = next;
}
while (node);
-
+
// Store path
node = prev;
do
@@ -1601,14 +1648,14 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
}
while (node);
}
-
+
const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
// Reset query.
memset(&m_query, 0, sizeof(dtQueryData));
-
+
*pathCount = n;
-
+
return DT_SUCCESS | details;
}
@@ -1616,21 +1663,21 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
dtPolyRef* path, int* pathCount, const int maxPath)
{
*pathCount = 0;
-
+
if (existingSize == 0)
{
return DT_FAILURE;
}
-
+
if (dtStatusFailed(m_query.status))
{
// Reset query.
memset(&m_query, 0, sizeof(dtQueryData));
return DT_FAILURE;
}
-
+
int n = 0;
-
+
if (m_query.startRef == m_query.endRef)
{
// Special case: the search starts and ends at same poly.
@@ -1647,14 +1694,14 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
if (node)
break;
}
-
+
if (!node)
{
m_query.status |= DT_PARTIAL_RESULT;
dtAssert(m_query.lastBestNode);
node = m_query.lastBestNode;
}
-
+
// Reverse the path.
int prevRay = 0;
do
@@ -1668,7 +1715,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
node = next;
}
while (node);
-
+
// Store path
node = prev;
do
@@ -1701,14 +1748,14 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
}
while (node);
}
-
+
const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
// Reset query.
memset(&m_query, 0, sizeof(dtQueryData));
-
+
*pathCount = n;
-
+
return DT_SUCCESS | details;
}
@@ -1765,24 +1812,24 @@ dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, con
const dtPoly* fromPoly = 0;
if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
const dtPolyRef to = path[i+1];
const dtMeshTile* toTile = 0;
const dtPoly* toPoly = 0;
if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
float left[3], right[3];
if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
break;
-
+
if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
{
// Skip intersection if only area crossings are requested.
if (fromPoly->getArea() == toPoly->getArea())
continue;
}
-
+
// Append intersection
float s,t;
if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
@@ -1801,20 +1848,20 @@ dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, con
}
/// @par
-///
+///
/// This method peforms what is often called 'string pulling'.
///
-/// The start position is clamped to the first polygon in the path, and the
-/// end position is clamped to the last. So the start and end positions should
+/// The start position is clamped to the first polygon in the path, and the
+/// end position is clamped to the last. So the start and end positions should
/// normally be within or very near the first and last polygons respectively.
///
-/// The returned polygon references represent the reference id of the polygon
-/// that is entered at the associated path position. The reference id associated
-/// with the end point will always be zero. This allows, for example, matching
+/// The returned polygon references represent the reference id of the polygon
+/// that is entered at the associated path position. The reference id associated
+/// with the end point will always be zero. This allows, for example, matching
/// off-mesh link points to their representative polygons.
///
-/// If the provided result buffers are too small for the entire result set,
-/// they will be filled as far as possible from the start toward the end
+/// If the provided result buffers are too small for the entire result set,
+/// they will be filled as far as possible from the start toward the end
/// position.
///
dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
@@ -1823,17 +1870,17 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
int* straightPathCount, const int maxStraightPath, const int options) const
{
dtAssert(m_nav);
-
+
*straightPathCount = 0;
-
+
if (!maxStraightPath)
return DT_FAILURE | DT_INVALID_PARAM;
-
+
if (!path[0])
return DT_FAILURE | DT_INVALID_PARAM;
-
+
dtStatus stat = 0;
-
+
// TODO: Should this be callers responsibility?
float closestStartPos[3];
if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
@@ -1842,14 +1889,14 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
float closestEndPos[3];
if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
// Add start point.
stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath);
if (stat != DT_IN_PROGRESS)
return stat;
-
+
if (pathSize > 1)
{
float portalApex[3], portalLeft[3], portalRight[3];
@@ -1859,18 +1906,18 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
int apexIndex = 0;
int leftIndex = 0;
int rightIndex = 0;
-
+
unsigned char leftPolyType = 0;
unsigned char rightPolyType = 0;
-
+
dtPolyRef leftPolyRef = path[0];
dtPolyRef rightPolyRef = path[0];
-
+
for (int i = 0; i < pathSize; ++i)
{
float left[3], right[3];
unsigned char toType;
-
+
if (i+1 < pathSize)
{
unsigned char fromType; // fromType is ignored.
@@ -1880,7 +1927,7 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
{
// Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
// Clamp the end point to path[i], and return the path so far.
-
+
if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
{
// This should only happen when the first polygon is invalid.
@@ -1900,10 +1947,10 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
appendVertex(closestEndPos, 0, path[i],
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath);
-
+
return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
}
-
+
// If starting really close the portal, advance.
if (i == 0)
{
@@ -1917,10 +1964,10 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
// End of the path.
dtVcopy(left, closestEndPos);
dtVcopy(right, closestEndPos);
-
+
toType = DT_POLYTYPE_GROUND;
}
-
+
// Right vertex.
if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
{
@@ -1940,38 +1987,38 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath, options);
if (stat != DT_IN_PROGRESS)
- return stat;
+ return stat;
}
-
+
dtVcopy(portalApex, portalLeft);
apexIndex = leftIndex;
-
+
unsigned char flags = 0;
if (!leftPolyRef)
flags = DT_STRAIGHTPATH_END;
else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
dtPolyRef ref = leftPolyRef;
-
+
// Append or update vertex
stat = appendVertex(portalApex, flags, ref,
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath);
if (stat != DT_IN_PROGRESS)
return stat;
-
+
dtVcopy(portalLeft, portalApex);
dtVcopy(portalRight, portalApex);
leftIndex = apexIndex;
rightIndex = apexIndex;
-
+
// Restart
i = apexIndex;
-
+
continue;
}
}
-
+
// Left vertex.
if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
{
@@ -1996,7 +2043,7 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
dtVcopy(portalApex, portalRight);
apexIndex = rightIndex;
-
+
unsigned char flags = 0;
if (!rightPolyRef)
flags = DT_STRAIGHTPATH_END;
@@ -2010,15 +2057,15 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
straightPathCount, maxStraightPath);
if (stat != DT_IN_PROGRESS)
return stat;
-
+
dtVcopy(portalLeft, portalApex);
dtVcopy(portalRight, portalApex);
leftIndex = apexIndex;
rightIndex = apexIndex;
-
+
// Restart
i = apexIndex;
-
+
continue;
}
}
@@ -2039,28 +2086,28 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
straightPath, straightPathFlags, straightPathRefs,
straightPathCount, maxStraightPath);
-
+
return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
}
/// @par
///
-/// This method is optimized for small delta movement and a small number of
-/// polygons. If used for too great a distance, the result set will form an
+/// This method is optimized for small delta movement and a small number of
+/// polygons. If used for too great a distance, the result set will form an
/// incomplete path.
///
-/// @p resultPos will equal the @p endPos if the end is reached.
+/// @p resultPos will equal the @p endPos if the end is reached.
/// Otherwise the closest reachable position will be returned.
-///
-/// @p resultPos is not projected onto the surface of the navigation
+///
+/// @p resultPos is not projected onto the surface of the navigation
/// mesh. Use #getPolyHeight if this is needed.
///
-/// This method treats the end position in the same manner as
-/// the #raycast method. (As a 2D point.) See that method's documentation
+/// This method treats the end position in the same manner as
+/// the #raycast method. (As a 2D point.) See that method's documentation
/// for details.
-///
-/// If the @p visited array is too small to hold the entire result set, it will
-/// be filled as far as possible from the start position toward the end
+///
+/// If the @p visited array is too small to hold the entire result set, it will
+/// be filled as far as possible from the start position toward the end
/// position.
///
dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
@@ -2071,21 +2118,21 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
dtAssert(m_tinyNodePool);
*visitedCount = 0;
-
+
// Validate input
if (!startRef)
return DT_FAILURE | DT_INVALID_PARAM;
if (!m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
dtStatus status = DT_SUCCESS;
-
+
static const int MAX_STACK = 48;
dtNode* stack[MAX_STACK];
int nstack = 0;
-
+
m_tinyNodePool->clear();
-
+
dtNode* startNode = m_tinyNodePool->getNode(startRef);
startNode->pidx = 0;
startNode->cost = 0;
@@ -2093,19 +2140,19 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
startNode->id = startRef;
startNode->flags = DT_NODE_CLOSED;
stack[nstack++] = startNode;
-
+
float bestPos[3];
float bestDist = FLT_MAX;
dtNode* bestNode = 0;
dtVcopy(bestPos, startPos);
-
+
// Search constraints
float searchPos[3], searchRadSqr;
dtVlerp(searchPos, startPos, endPos, 0.5f);
searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);
-
+
float verts[DT_VERTS_PER_POLYGON*3];
-
+
while (nstack)
{
// Pop front.
@@ -2113,19 +2160,19 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
for (int i = 0; i < nstack-1; ++i)
stack[i] = stack[i+1];
nstack--;
-
+
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef curRef = curNode->id;
const dtMeshTile* curTile = 0;
const dtPoly* curPoly = 0;
- m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
-
+ m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
+
// Collect vertices.
const int nverts = curPoly->vertCount;
for (int i = 0; i < nverts; ++i)
dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]);
-
+
// If target is inside the poly, stop search.
if (dtPointInPolygon(endPos, verts, nverts))
{
@@ -2133,7 +2180,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
dtVcopy(bestPos, endPos);
break;
}
-
+
// Find wall edges and find nearest point inside the walls.
for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++)
{
@@ -2141,7 +2188,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
static const int MAX_NEIS = 8;
int nneis = 0;
dtPolyRef neis[MAX_NEIS];
-
+
if (curPoly->neis[j] & DT_EXT_LINK)
{
// Tile border.
@@ -2174,7 +2221,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
neis[nneis++] = ref;
}
}
-
+
if (!nneis)
{
// Wall edge, calc distance.
@@ -2201,7 +2248,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
// Skip if already visited.
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
-
+
// Skip the link if it is too far from search constraint.
// TODO: Maybe should use getPortalPoints(), but this one is way faster.
const float* vj = &verts[j*3];
@@ -2210,7 +2257,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg);
if (distSqr > searchRadSqr)
continue;
-
+
// Mark as the node as visited and push to queue.
if (nstack < MAX_STACK)
{
@@ -2222,7 +2269,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
}
}
}
-
+
int n = 0;
if (bestNode)
{
@@ -2237,7 +2284,7 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
node = next;
}
while (node);
-
+
// Store result
node = prev;
do
@@ -2252,11 +2299,11 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
}
while (node);
}
-
+
dtVcopy(resultPos, bestPos);
-
+
*visitedCount = n;
-
+
return status;
}
@@ -2265,7 +2312,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* le
unsigned char& fromType, unsigned char& toType) const
{
dtAssert(m_nav);
-
+
const dtMeshTile* fromTile = 0;
const dtPoly* fromPoly = 0;
if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
@@ -2277,7 +2324,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* le
if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
return DT_FAILURE | DT_INVALID_PARAM;
toType = toPoly->getType();
-
+
return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
}
@@ -2298,7 +2345,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly,
}
if (!link)
return DT_FAILURE | DT_INVALID_PARAM;
-
+
// Handle off-mesh connections.
if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
@@ -2315,7 +2362,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly,
}
return DT_FAILURE | DT_INVALID_PARAM;
}
-
+
if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
@@ -2330,13 +2377,13 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly,
}
return DT_FAILURE | DT_INVALID_PARAM;
}
-
+
// Find portal vertices.
const int v0 = fromPoly->verts[link->edge];
const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
dtVcopy(left, &fromTile->verts[v0*3]);
dtVcopy(right, &fromTile->verts[v1*3]);
-
+
// If the link is at tile boundary, dtClamp the vertices to
// the link width.
if (link->side != 0xff)
@@ -2351,7 +2398,7 @@ dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly,
dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax);
}
}
-
+
return DT_SUCCESS;
}
@@ -2387,16 +2434,16 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
///
/// This method is meant to be used for quick, short distance checks.
///
-/// If the path array is too small to hold the result, it will be filled as
+/// If the path array is too small to hold the result, it will be filled as
/// far as possible from the start postion toward the end position.
///
/// Using the Hit Parameter (t)
-///
-/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
-/// the end position. In this case the path represents a valid corridor to the
+///
+/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
+/// the end position. In this case the path represents a valid corridor to the
/// end position and the value of @p hitNormal is undefined.
///
-/// If the hit parameter is zero, then the start position is on the wall that
+/// If the hit parameter is zero, then the start position is on the wall that
/// was hit and the value of @p hitNormal is undefined.
///
/// If 0 < t < 1.0 then the following applies:
@@ -2408,15 +2455,15 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
///
/// Use Case Restriction
///
-/// The raycast ignores the y-value of the end position. (2D check.) This
+/// The raycast ignores the y-value of the end position. (2D check.) This
/// places significant limits on how it can be used. For example:
///
-/// Consider a scene where there is a main floor with a second floor balcony
-/// that hangs over the main floor. So the first floor mesh extends below the
-/// balcony mesh. The start position is somewhere on the first floor. The end
+/// Consider a scene where there is a main floor with a second floor balcony
+/// that hangs over the main floor. So the first floor mesh extends below the
+/// balcony mesh. The start position is somewhere on the first floor. The end
/// position is on the balcony.
///
-/// The raycast will search toward the end position along the first floor mesh.
+/// The raycast will search toward the end position along the first floor mesh.
/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
/// (no wall hit), meaning it reached the end position. This is one example of why
/// this method is meant for short distance checks.
@@ -2430,7 +2477,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
hit.maxPath = maxPath;
dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit);
-
+
*t = hit.t;
if (hitNormal)
dtVcopy(hitNormal, hit.hitNormal);
@@ -2445,16 +2492,16 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
///
/// This method is meant to be used for quick, short distance checks.
///
-/// If the path array is too small to hold the result, it will be filled as
+/// If the path array is too small to hold the result, it will be filled as
/// far as possible from the start postion toward the end position.
///
/// Using the Hit Parameter t of RaycastHit
-///
-/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
-/// the end position. In this case the path represents a valid corridor to the
+///
+/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
+/// the end position. In this case the path represents a valid corridor to the
/// end position and the value of @p hitNormal is undefined.
///
-/// If the hit parameter is zero, then the start position is on the wall that
+/// If the hit parameter is zero, then the start position is on the wall that
/// was hit and the value of @p hitNormal is undefined.
///
/// If 0 < t < 1.0 then the following applies:
@@ -2466,15 +2513,15 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
///
/// Use Case Restriction
///
-/// The raycast ignores the y-value of the end position. (2D check.) This
+/// The raycast ignores the y-value of the end position. (2D check.) This
/// places significant limits on how it can be used. For example:
///
-/// Consider a scene where there is a main floor with a second floor balcony
-/// that hangs over the main floor. So the first floor mesh extends below the
-/// balcony mesh. The start position is somewhere on the first floor. The end
+/// Consider a scene where there is a main floor with a second floor balcony
+/// that hangs over the main floor. So the first floor mesh extends below the
+/// balcony mesh. The start position is somewhere on the first floor. The end
/// position is on the balcony.
///
-/// The raycast will search toward the end position along the first floor mesh.
+/// The raycast will search toward the end position along the first floor mesh.
/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
/// (no wall hit), meaning it reached the end position. This is one example of why
/// this method is meant for short distance checks.
@@ -2484,7 +2531,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
dtRaycastHit* hit, dtPolyRef prevRef) const
{
dtAssert(m_nav);
-
+
hit->t = 0;
hit->pathCount = 0;
hit->pathCost = 0;
@@ -2494,9 +2541,9 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
return DT_FAILURE | DT_INVALID_PARAM;
if (prevRef && !m_nav->isValidPolyRef(prevRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
float dir[3], curPos[3], lastPos[3];
- float verts[DT_VERTS_PER_POLYGON*3+3];
+ float verts[DT_VERTS_PER_POLYGON*3+3];
int n = 0;
dtVcopy(curPos, startPos);
@@ -2522,7 +2569,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
while (curRef)
{
// Cast ray against current polygon.
-
+
// Collect vertices.
int nv = 0;
for (int i = 0; i < (int)poly->vertCount; ++i)
@@ -2530,7 +2577,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
nv++;
}
-
+
float tmin, tmax;
int segMin, segMax;
if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
@@ -2545,7 +2592,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
// Keep track of furthest t so far.
if (tmax > hit->t)
hit->t = tmax;
-
+
// Store visited polygons.
if (n < hit->maxPath)
hit->path[n++] = curRef;
@@ -2557,7 +2604,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
{
hit->t = FLT_MAX;
hit->pathCount = n;
-
+
// add the cost
if (options & DT_RAYCAST_USE_COSTS)
hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
@@ -2566,50 +2613,50 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
// Follow neighbours.
dtPolyRef nextRef = 0;
-
+
for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
{
const dtLink* link = &tile->links[i];
-
+
// Find link which contains this edge.
if ((int)link->edge != segMax)
continue;
-
+
// Get pointer to the next polygon.
nextTile = 0;
nextPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
-
+
// Skip off-mesh connections.
if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
-
+
// Skip links based on filter.
if (!filter->passFilter(link->ref, nextTile, nextPoly))
continue;
-
+
// If the link is internal, just return the ref.
if (link->side == 0xff)
{
nextRef = link->ref;
break;
}
-
+
// If the link is at tile boundary,
-
+
// Check if the link spans the whole edge, and accept.
if (link->bmin == 0 && link->bmax == 255)
{
nextRef = link->ref;
break;
}
-
+
// Check for partial edge links.
const int v0 = poly->verts[link->edge];
const int v1 = poly->verts[(link->edge+1) % poly->vertCount];
const float* left = &tile->verts[v0*3];
const float* right = &tile->verts[v1*3];
-
+
// Check that the intersection lies inside the link portal.
if (link->side == 0 || link->side == 4)
{
@@ -2618,7 +2665,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
float lmin = left[2] + (right[2] - left[2])*(link->bmin*s);
float lmax = left[2] + (right[2] - left[2])*(link->bmax*s);
if (lmin > lmax) dtSwap(lmin, lmax);
-
+
// Find Z intersection.
float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
if (z >= lmin && z <= lmax)
@@ -2634,7 +2681,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
float lmin = left[0] + (right[0] - left[0])*(link->bmin*s);
float lmax = left[0] + (right[0] - left[0])*(link->bmax*s);
if (lmin > lmax) dtSwap(lmin, lmax);
-
+
// Find X intersection.
float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
if (x >= lmin && x <= lmax)
@@ -2644,7 +2691,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
}
}
}
-
+
// add the cost
if (options & DT_RAYCAST_USE_COSTS)
{
@@ -2666,7 +2713,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
if (!nextRef)
{
// No neighbour, we hit a wall.
-
+
// Calculate hit normal.
const int a = segMax;
const int b = segMax+1 < nv ? segMax+1 : 0;
@@ -2678,7 +2725,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
hit->hitNormal[1] = 0;
hit->hitNormal[2] = -dx;
dtVnormalize(hit->hitNormal);
-
+
hit->pathCount = n;
return status;
}
@@ -2691,9 +2738,9 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
prevPoly = poly;
poly = nextPoly;
}
-
+
hit->pathCount = n;
-
+
return status;
}
@@ -2703,29 +2750,29 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
///
/// The order of the result set is from least to highest cost to reach the polygon.
///
-/// A common use case for this method is to perform Dijkstra searches.
+/// A common use case for this method is to perform Dijkstra searches.
/// Candidate polygons are found by searching the graph beginning at the start polygon.
///
-/// If a polygon is not found via the graph search, even if it intersects the
+/// If a polygon is not found via the graph search, even if it intersects the
/// search circle, it will not be included in the result set. For example:
///
/// polyA is the start polygon.
/// polyB shares an edge with polyA. (Is adjacent.)
/// polyC shares an edge with polyB, but not with polyA
-/// Even if the search circle overlaps polyC, it will not be included in the
+/// Even if the search circle overlaps polyC, it will not be included in the
/// result set unless polyB is also in the set.
-///
-/// The value of the center point is used as the start position for cost
-/// calculations. It is not projected onto the surface of the mesh, so its
+///
+/// The value of the center point is used as the start position for cost
+/// calculations. It is not projected onto the surface of the mesh, so its
/// y-value will effect the costs.
///
-/// Intersection tests occur in 2D. All polygons and the search circle are
-/// projected onto the xz-plane. So the y-value of the center point does not
+/// Intersection tests occur in 2D. All polygons and the search circle are
+/// projected onto the xz-plane. So the y-value of the center point does not
/// effect intersection tests.
///
-/// If the result arrays are to small to hold the entire result set, they will be
+/// If the result arrays are to small to hold the entire result set, they will be
/// filled to capacity.
-///
+///
dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
const dtQueryFilter* filter,
dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
@@ -2736,14 +2783,14 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
dtAssert(m_openList);
*resultCount = 0;
-
+
// Validate input
if (!startRef || !m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
m_nodePool->clear();
m_openList->clear();
-
+
dtNode* startNode = m_nodePool->getNode(startRef);
dtVcopy(startNode->pos, centerPos);
startNode->pidx = 0;
@@ -2752,26 +2799,26 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
startNode->id = startRef;
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
-
+
dtStatus status = DT_SUCCESS;
-
+
int n = 0;
-
+
const float radiusSqr = dtSqr(radius);
-
+
while (!m_openList->empty())
{
dtNode* bestNode = m_openList->pop();
bestNode->flags &= ~DT_NODE_OPEN;
bestNode->flags |= DT_NODE_CLOSED;
-
+
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef bestRef = bestNode->id;
const dtMeshTile* bestTile = 0;
const dtPoly* bestPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
-
+
// Get parent poly and tile.
dtPolyRef parentRef = 0;
const dtMeshTile* parentTile = 0;
@@ -2795,7 +2842,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
{
status |= DT_BUFFER_TOO_SMALL;
}
-
+
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
const dtLink* link = &bestTile->links[i];
@@ -2803,41 +2850,41 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
// Skip invalid neighbours and do not follow back to parent.
if (!neighbourRef || neighbourRef == parentRef)
continue;
-
+
// Expand to neighbour
const dtMeshTile* neighbourTile = 0;
const dtPoly* neighbourPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+
// Do not advance if the polygon is excluded by the filter.
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
-
+
// Find edge and calc distance to the edge.
float va[3], vb[3];
if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
continue;
-
+
// If the circle is not touching the next polygon, skip it.
float tseg;
float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
if (distSqr > radiusSqr)
continue;
-
+
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
{
status |= DT_OUT_OF_NODES;
continue;
}
-
+
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
-
+
// Cost
if (neighbourNode->flags == 0)
dtVlerp(neighbourNode->pos, va, vb, 0.5f);
-
+
float cost = filter->getCost(
bestNode->pos, neighbourNode->pos,
parentRef, parentTile, parentPoly,
@@ -2845,15 +2892,15 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
neighbourRef, neighbourTile, neighbourPoly);
const float total = bestNode->total + cost;
-
+
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
-
+
neighbourNode->id = neighbourRef;
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
-
+
if (neighbourNode->flags & DT_NODE_OPEN)
{
m_openList->modify(neighbourNode);
@@ -2865,32 +2912,32 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
}
}
}
-
+
*resultCount = n;
-
+
return status;
}
/// @par
///
/// The order of the result set is from least to highest cost.
-///
+///
/// At least one result array must be provided.
///
-/// A common use case for this method is to perform Dijkstra searches.
-/// Candidate polygons are found by searching the graph beginning at the start
+/// A common use case for this method is to perform Dijkstra searches.
+/// Candidate polygons are found by searching the graph beginning at the start
/// polygon.
-///
+///
/// The same intersection test restrictions that apply to findPolysAroundCircle()
/// method apply to this method.
-///
-/// The 3D centroid of the search polygon is used as the start position for cost
+///
+/// The 3D centroid of the search polygon is used as the start position for cost
/// calculations.
-///
-/// Intersection tests occur in 2D. All polygons are projected onto the
+///
+/// Intersection tests occur in 2D. All polygons are projected onto the
/// xz-plane. So the y-values of the vertices do not effect intersection tests.
-///
-/// If the result arrays are is too small to hold the entire result set, they will
+///
+/// If the result arrays are is too small to hold the entire result set, they will
/// be filled to capacity.
///
dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
@@ -2901,16 +2948,16 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
dtAssert(m_nav);
dtAssert(m_nodePool);
dtAssert(m_openList);
-
+
*resultCount = 0;
-
+
// Validate input
if (!startRef || !m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
m_nodePool->clear();
m_openList->clear();
-
+
float centerPos[3] = {0,0,0};
for (int i = 0; i < nverts; ++i)
dtVadd(centerPos,centerPos,&verts[i*3]);
@@ -2924,24 +2971,24 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
startNode->id = startRef;
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
-
+
dtStatus status = DT_SUCCESS;
int n = 0;
-
+
while (!m_openList->empty())
{
dtNode* bestNode = m_openList->pop();
bestNode->flags &= ~DT_NODE_OPEN;
bestNode->flags |= DT_NODE_CLOSED;
-
+
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef bestRef = bestNode->id;
const dtMeshTile* bestTile = 0;
const dtPoly* bestPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
-
+
// Get parent poly and tile.
dtPolyRef parentRef = 0;
const dtMeshTile* parentTile = 0;
@@ -2966,7 +3013,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
{
status |= DT_BUFFER_TOO_SMALL;
}
-
+
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
const dtLink* link = &bestTile->links[i];
@@ -2974,21 +3021,21 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
// Skip invalid neighbours and do not follow back to parent.
if (!neighbourRef || neighbourRef == parentRef)
continue;
-
+
// Expand to neighbour
const dtMeshTile* neighbourTile = 0;
const dtPoly* neighbourPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+
// Do not advance if the polygon is excluded by the filter.
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
-
+
// Find edge and calc distance to the edge.
float va[3], vb[3];
if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
continue;
-
+
// If the poly is not touching the edge to the next polygon, skip the connection it.
float tmin, tmax;
int segMin, segMax;
@@ -2996,21 +3043,21 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
continue;
if (tmin > 1.0f || tmax < 0.0f)
continue;
-
+
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
{
status |= DT_OUT_OF_NODES;
continue;
}
-
+
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
-
+
// Cost
if (neighbourNode->flags == 0)
dtVlerp(neighbourNode->pos, va, vb, 0.5f);
-
+
float cost = filter->getCost(
bestNode->pos, neighbourNode->pos,
parentRef, parentTile, parentPoly,
@@ -3018,15 +3065,15 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
neighbourRef, neighbourTile, neighbourPoly);
const float total = bestNode->total + cost;
-
+
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
-
+
neighbourNode->id = neighbourRef;
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
-
+
if (neighbourNode->flags & DT_NODE_OPEN)
{
m_openList->modify(neighbourNode);
@@ -3038,9 +3085,9 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
}
}
}
-
+
*resultCount = n;
-
+
return status;
}
@@ -3051,36 +3098,40 @@ dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef*
*pathCount = 0;
+
dtNode* endNode;
- if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 ||
- (endNode->flags & DT_NODE_CLOSED) == 0)
- return DT_FAILURE | DT_INVALID_PARAM;
+ dtStatus status;
+ if (m_nodePool->findNodes(endRef, &endNode, 1) != 1
+ || (endNode->flags & DT_NODE_CLOSED) == 0)
+ status = DT_FAILURE | DT_INVALID_PARAM;
+ else
+ status = getPathToNode(endNode, path, pathCount, maxPath);
- return getPathToNode(endNode, path, pathCount, maxPath);
+ return status;
}
/// @par
///
-/// This method is optimized for a small search radius and small number of result
+/// This method is optimized for a small search radius and small number of result
/// polygons.
///
-/// Candidate polygons are found by searching the navigation graph beginning at
+/// Candidate polygons are found by searching the navigation graph beginning at
/// the start polygon.
///
-/// The same intersection test restrictions that apply to the findPolysAroundCircle
+/// The same intersection test restrictions that apply to the findPolysAroundCircle
/// mehtod applies to this method.
///
-/// The value of the center point is used as the start point for cost calculations.
-/// It is not projected onto the surface of the mesh, so its y-value will effect
+/// The value of the center point is used as the start point for cost calculations.
+/// It is not projected onto the surface of the mesh, so its y-value will effect
/// the costs.
-///
-/// Intersection tests occur in 2D. All polygons and the search circle are
-/// projected onto the xz-plane. So the y-value of the center point does not
+///
+/// Intersection tests occur in 2D. All polygons and the search circle are
+/// projected onto the xz-plane. So the y-value of the center point does not
/// effect intersection tests.
-///
-/// If the result arrays are is too small to hold the entire result set, they will
+///
+/// If the result arrays are is too small to hold the entire result set, they will
/// be filled to capacity.
-///
+///
dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
const dtQueryFilter* filter,
dtPolyRef* resultRef, dtPolyRef* resultParent,
@@ -3088,32 +3139,32 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
{
dtAssert(m_nav);
dtAssert(m_tinyNodePool);
-
+
*resultCount = 0;
// Validate input
if (!startRef || !m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
static const int MAX_STACK = 48;
dtNode* stack[MAX_STACK];
int nstack = 0;
-
+
m_tinyNodePool->clear();
-
+
dtNode* startNode = m_tinyNodePool->getNode(startRef);
startNode->pidx = 0;
startNode->id = startRef;
startNode->flags = DT_NODE_CLOSED;
stack[nstack++] = startNode;
-
+
const float radiusSqr = dtSqr(radius);
-
+
float pa[DT_VERTS_PER_POLYGON*3];
float pb[DT_VERTS_PER_POLYGON*3];
-
+
dtStatus status = DT_SUCCESS;
-
+
int n = 0;
if (n < maxResult)
{
@@ -3126,7 +3177,7 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
{
status |= DT_BUFFER_TOO_SMALL;
}
-
+
while (nstack)
{
// Pop front.
@@ -3134,14 +3185,14 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
for (int i = 0; i < nstack-1; ++i)
stack[i] = stack[i+1];
nstack--;
-
+
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef curRef = curNode->id;
const dtMeshTile* curTile = 0;
const dtPoly* curPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
-
+
for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
{
const dtLink* link = &curTile->links[i];
@@ -3149,7 +3200,7 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
// Skip invalid neighbours.
if (!neighbourRef)
continue;
-
+
// Skip if cannot alloca more nodes.
dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
if (!neighbourNode)
@@ -3157,48 +3208,48 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
// Skip visited.
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
-
+
// Expand to neighbour
const dtMeshTile* neighbourTile = 0;
const dtPoly* neighbourPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+
// Skip off-mesh connections.
if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
-
+
// Do not advance if the polygon is excluded by the filter.
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
-
+
// Find edge and calc distance to the edge.
float va[3], vb[3];
if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
continue;
-
+
// If the circle is not touching the next polygon, skip it.
float tseg;
float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
if (distSqr > radiusSqr)
continue;
-
+
// Mark node visited, this is done before the overlap test so that
// we will not visit the poly again if the test fails.
neighbourNode->flags |= DT_NODE_CLOSED;
neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
-
+
// Check that the polygon does not collide with existing polygons.
-
+
// Collect vertices of the neighbour poly.
const int npa = neighbourPoly->vertCount;
for (int k = 0; k < npa; ++k)
dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]);
-
+
bool overlap = false;
for (int j = 0; j < n; ++j)
{
dtPolyRef pastRef = resultRef[j];
-
+
// Connected polys do not overlap.
bool connected = false;
for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
@@ -3211,17 +3262,17 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
}
if (connected)
continue;
-
+
// Potentially overlapping.
const dtMeshTile* pastTile = 0;
const dtPoly* pastPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);
-
+
// Get vertices and test overlap
const int npb = pastPoly->vertCount;
for (int k = 0; k < npb; ++k)
dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]);
-
+
if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
{
overlap = true;
@@ -3230,7 +3281,7 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
}
if (overlap)
continue;
-
+
// This poly is fine, store and advance to the poly.
if (n < maxResult)
{
@@ -3243,16 +3294,16 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
{
status |= DT_BUFFER_TOO_SMALL;
}
-
+
if (nstack < MAX_STACK)
{
stack[nstack++] = neighbourNode;
}
}
}
-
+
*resultCount = n;
-
+
return status;
}
@@ -3287,37 +3338,37 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
/// @par
///
-/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned.
+/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned.
/// Otherwise only the wall segments are returned.
-///
-/// A segment that is normally a portal will be included in the result set as a
+///
+/// A segment that is normally a portal will be included in the result set as a
/// wall if the @p filter results in the neighbor polygon becoomming impassable.
-///
-/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the
+///
+/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the
/// maximum segments per polygon of the source navigation mesh.
-///
+///
dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
const int maxSegments) const
{
dtAssert(m_nav);
-
+
*segmentCount = 0;
-
+
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
int n = 0;
static const int MAX_INTERVAL = 16;
dtSegInterval ints[MAX_INTERVAL];
int nints;
-
+
const bool storePortals = segmentRefs != 0;
-
+
dtStatus status = DT_SUCCESS;
-
+
for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
{
// Skip non-solid edges.
@@ -3358,7 +3409,7 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
// If the edge leads to another polygon and portals are not stored, skip.
if (neiRef != 0 && !storePortals)
continue;
-
+
if (n < maxSegments)
{
const float* vj = &tile->verts[poly->verts[j]*3];
@@ -3374,14 +3425,14 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
{
status |= DT_BUFFER_TOO_SMALL;
}
-
+
continue;
}
-
+
// Add sentinels
insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
-
+
// Store segments.
const float* vj = &tile->verts[poly->verts[j]*3];
const float* vi = &tile->verts[poly->verts[i]*3];
@@ -3390,8 +3441,8 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
// Portal segment.
if (storePortals && ints[k].ref)
{
- const float tmin = ints[k].tmin/255.0f;
- const float tmax = ints[k].tmax/255.0f;
+ const float tmin = ints[k].tmin/255.0f;
+ const float tmax = ints[k].tmax/255.0f;
if (n < maxSegments)
{
float* seg = &segmentVerts[n*6];
@@ -3412,8 +3463,8 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
const int imax = ints[k].tmin;
if (imin != imax)
{
- const float tmin = imin/255.0f;
- const float tmax = imax/255.0f;
+ const float tmin = imin/255.0f;
+ const float tmax = imax/255.0f;
if (n < maxSegments)
{
float* seg = &segmentVerts[n*6];
@@ -3430,9 +3481,9 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
}
}
}
-
+
*segmentCount = n;
-
+
return status;
}
@@ -3440,7 +3491,7 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
///
/// @p hitPos is not adjusted using the height detail data.
///
-/// @p hitDist will equal the search radius if there is no wall within the
+/// @p hitDist will equal the search radius if there is no wall within the
/// radius. In this case the values of @p hitPos and @p hitNormal are
/// undefined.
///
@@ -3453,14 +3504,14 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
dtAssert(m_nav);
dtAssert(m_nodePool);
dtAssert(m_openList);
-
+
// Validate input
if (!startRef || !m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
-
+
m_nodePool->clear();
m_openList->clear();
-
+
dtNode* startNode = m_nodePool->getNode(startRef);
dtVcopy(startNode->pos, centerPos);
startNode->pidx = 0;
@@ -3469,24 +3520,24 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
startNode->id = startRef;
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
-
+
float radiusSqr = dtSqr(maxRadius);
-
+
dtStatus status = DT_SUCCESS;
-
+
while (!m_openList->empty())
{
dtNode* bestNode = m_openList->pop();
bestNode->flags &= ~DT_NODE_OPEN;
bestNode->flags |= DT_NODE_CLOSED;
-
+
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
const dtPolyRef bestRef = bestNode->id;
const dtMeshTile* bestTile = 0;
const dtPoly* bestPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
-
+
// Get parent poly and tile.
dtPolyRef parentRef = 0;
const dtMeshTile* parentTile = 0;
@@ -3495,7 +3546,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
if (parentRef)
m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
-
+
// Hit test walls.
for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++)
{
@@ -3530,17 +3581,17 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
if (filter->passFilter(ref, bestTile, &bestTile->polys[idx]))
continue;
}
-
+
// Calc distance to the edge.
const float* vj = &bestTile->verts[bestPoly->verts[j]*3];
const float* vi = &bestTile->verts[bestPoly->verts[i]*3];
float tseg;
float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);
-
+
// Edge is too far, skip.
if (distSqr > radiusSqr)
continue;
-
+
// Hit wall, update radius.
radiusSqr = distSqr;
// Calculate hit pos.
@@ -3548,7 +3599,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
}
-
+
for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
{
const dtLink* link = &bestTile->links[i];
@@ -3556,26 +3607,26 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
// Skip invalid neighbours and do not follow back to parent.
if (!neighbourRef || neighbourRef == parentRef)
continue;
-
+
// Expand to neighbour.
const dtMeshTile* neighbourTile = 0;
const dtPoly* neighbourPoly = 0;
m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-
+
// Skip off-mesh connections.
if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
-
+
// Calc distance to the edge.
const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3];
const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3];
float tseg;
float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
-
+
// If the circle is not touching the next polygon, skip it.
if (distSqr > radiusSqr)
continue;
-
+
if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
@@ -3585,28 +3636,28 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
status |= DT_OUT_OF_NODES;
continue;
}
-
+
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
-
+
// Cost
if (neighbourNode->flags == 0)
{
getEdgeMidPoint(bestRef, bestPoly, bestTile,
neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
}
-
+
const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
-
+
// The node is already in open list and the new result is worse, skip.
if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
continue;
-
+
neighbourNode->id = neighbourRef;
neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
-
+
if (neighbourNode->flags & DT_NODE_OPEN)
{
m_openList->modify(neighbourNode);
@@ -3618,13 +3669,13 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
}
}
}
-
+
// Calc hit normal.
dtVsub(hitNormal, centerPos, hitPos);
dtVnormalize(hitNormal);
-
+
*hitDist = dtMathSqrtf(radiusSqr);
-
+
return status;
}
@@ -3644,13 +3695,13 @@ bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter)
/// @par
///
-/// The closed list is the list of polygons that were fully evaluated during
+/// The closed list is the list of polygons that were fully evaluated during
/// the last navigation graph search. (A* or Dijkstra)
-///
+///
bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
{
if (!m_nodePool) return false;
-
+
dtNode* nodes[DT_MAX_STATES_PER_NODE];
int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE);
@@ -3658,7 +3709,7 @@ bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
{
if (nodes[i]->flags & DT_NODE_CLOSED)
return true;
- }
+ }
return false;
}