diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h
index 95a63e4..1f80f6d 100644
--- a/Detour/Include/DetourNavMesh.h
+++ b/Detour/Include/DetourNavMesh.h
@@ -117,6 +117,13 @@ enum dtStraightPathOptions
DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, ///< Add a vertex at every polygon edge crossing.
};
+
+/// Options for dtNavMeshQuery::raycast
+enum dtRaycastOptions
+{
+ DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost
+};
+
/// Flags representing the type of a navigation mesh polygon.
enum dtPolyTypes
{
diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h
index 6bb316b..c5b0303 100644
--- a/Detour/Include/DetourNavMeshQuery.h
+++ b/Detour/Include/DetourNavMeshQuery.h
@@ -115,6 +115,34 @@ public:
};
+
+
+/// Provides information about raycast hit
+/// filled by dtNavMeshQuery::raycast
+/// @ingroup detour
+struct RaycastHit
+{
+ /// The hit parameter. (FLT_MAX if no wall hit.)
+ float t;
+
+ /// hitNormal The normal of the nearest wall hit. [(x, y, z)]
+ float hitNormal[3];
+
+ /// The reference ids of the visited polygons. [opt]
+ dtPolyRef* path;
+
+ /// The number of visited polygons. [opt]
+ int pathCount;
+
+ /// The maximum number of polygons the @p path array can hold.
+ int maxPath;
+
+ /// The cost of the path until hit.
+ float pathCost;
+};
+
+
+
/// Provides the ability to perform pathfinding related queries against
/// a navigation mesh.
/// @ingroup detour
@@ -308,6 +336,7 @@ public:
/// 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
/// the start of the ray. [(x, y, z)]
@@ -318,14 +347,27 @@ public:
/// @param[out] path The reference ids of the visited polygons. [opt]
/// @param[out] pathCount The number of visited polygons. [opt]
/// @param[in] maxPath The maximum number of polygons the @p path array can hold.
- /// @param[out] pathCost The cost of the path until hit.
- /// @param[in] prevRef [optional]: cost calculation allow for an additional parent ref. Used during pathfinding
/// @returns The status flags for the query.
dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
const dtQueryFilter* filter,
- float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath,
- float* pathCost=0, dtPolyRef prevRef=0) const;
+ float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const;
+ /// 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
+ /// 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.
+ /// @param[in] flags govern how the raycast behaves. See dtRaycastOptions
+ /// @param[out] hit The raycast hit structure.
+ /// @param[in] prevRef parent of start ref. Used during for cost calculation [opt]
+ /// @returns The status flags for the query.
+ dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+ const dtQueryFilter* filter, const unsigned int options,
+ RaycastHit* hit, dtPolyRef prevRef=0) const;
+
+
/// Finds the distance from the specified position to the nearest polygon wall.
/// @param[in] startRef The reference id of the polygon containing @p centerPos.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h
index 4a42bb8..9094ff2 100644
--- a/Detour/Include/DetourNode.h
+++ b/Detour/Include/DetourNode.h
@@ -25,7 +25,7 @@ enum dtNodeFlags
{
DT_NODE_OPEN = 0x01,
DT_NODE_CLOSED = 0x02,
- DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not connected. Found using raycast.
+ DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not connected/adjacent. Found using raycast.
};
typedef unsigned short dtNodeIndex;
@@ -56,7 +56,7 @@ public:
// There can be more than one node for the same polyRef but with different extra state information
dtNode* getNode(dtPolyRef id, unsigned char state=0);
dtNode* findNode(dtPolyRef id, unsigned char state);
- unsigned int findNodes(dtPolyRef id, int bufsize, dtNode** buf);
+ unsigned int findNodes(dtPolyRef id, dtNode** nodes, const int maxNodes);
inline unsigned int getNodeIdx(const dtNode* node) const
{
diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp
index c0c33fb..6618b83 100644
--- a/Detour/Source/DetourNavMeshQuery.cpp
+++ b/Detour/Source/DetourNavMeshQuery.cpp
@@ -961,6 +961,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode* lastBestNode = startNode;
float lastBestNodeCost = startNode->total;
+ RaycastHit rayHit;
dtStatus status = DT_SUCCESS;
while (!m_openList->empty())
@@ -1053,9 +1054,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
float t = 0;
if (tryLOS)
{
- float normal[3], curCost;
- raycast(parentRef, parentNode->pos, neighbourNode->pos, filter, &t, normal, 0, 0, 0, &curCost, grandpaRef);
- cost = parentNode->cost + curCost;
+ raycast(parentRef, parentNode->pos, neighbourNode->pos, filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
+ cost = parentNode->cost + rayHit.pathCost;
}
if (t < 1.0f) // hit
@@ -1133,8 +1133,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
{
dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
node->pidx = m_nodePool->getNodeIdx(prev);
- int nextRay = node->flags & DT_NODE_PARENT_DETACHED;
- node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay;
+ int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
+ node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and sotre it in the reversed path's node
prevRay = nextRay;
prev = node;
node = next;
@@ -1245,6 +1245,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
m_query.status = DT_FAILURE;
return DT_FAILURE;
}
+
+ RaycastHit rayHit;
int iter = 0;
while (iter < maxIter && !m_openList->empty())
@@ -1359,9 +1361,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
float t = 0;
if (tryLOS)
{
- float normal[3], curCost;
- raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, &t, normal, 0, 0, 0, &curCost, grandpaRef);
- cost = parentNode->cost + curCost;
+ raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
+ cost = parentNode->cost + rayHit.pathCost;
}
if (t < 1.0f) // hit
@@ -1475,8 +1476,8 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
node->pidx = m_nodePool->getNodeIdx(prev);
prev = node;
- int nextRay = node->flags & DT_NODE_PARENT_DETACHED;
- node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay;
+ int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
+ node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
prevRay = nextRay;
node = next;
}
@@ -1553,7 +1554,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
dtNode* node = 0;
for (int i = existingSize-1; i >= 0; --i)
{
- m_nodePool->findNodes(existing[i], 1, &node);
+ m_nodePool->findNodes(existing[i], &node, 1);
if (node)
break;
}
@@ -2257,6 +2258,8 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
return DT_SUCCESS;
}
+
+
/// @par
///
/// This method is meant to be used for quick, short distance checks.
@@ -2297,16 +2300,71 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly,
///
dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
const dtQueryFilter* filter,
- float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath,
- float* pathCost, dtPolyRef prevRef) const
+ float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const
+{
+ RaycastHit hit;
+ hit.path = path;
+ hit.maxPath = maxPath;
+
+ dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit);
+
+ *t = hit.t;
+ if (hitNormal)
+ dtVcopy(hitNormal, hit.hitNormal);
+ if (pathCount)
+ *pathCount = hit.pathCount;
+
+ return status;
+}
+
+
+/// @par
+///
+/// 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
+/// 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
+/// 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
+/// was hit and the value of @p hitNormal is undefined.
+///
+/// If 0 < t < 1.0 then the following applies:
+///
+/// @code
+/// distanceToHitBorder = distanceToEndPosition * t
+/// hitPoint = startPos + (endPos - startPos) * t
+/// @endcode
+///
+/// Use Case Restriction
+///
+/// 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
+/// position is on the balcony.
+///
+/// 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.
+///
+dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
+ const dtQueryFilter* filter, const unsigned int options,
+ RaycastHit* hit, dtPolyRef prevRef) const
{
dtAssert(m_nav);
- *t = 0;
- if (pathCount)
- *pathCount = 0;
- if (pathCost)
- *pathCost = 0;
+ hit->t = 0;
+ hit->pathCount = 0;
+ hit->pathCost = 0;
// Validate input
if (!startRef || !m_nav->isValidPolyRef(startRef))
@@ -2320,7 +2378,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
dtVcopy(curPos, startPos);
dtVsub(dir, endPos, startPos);
- dtVset(hitNormal, 0, 0, 0);
+ dtVset(hit->hitNormal, 0, 0, 0);
dtStatus status = DT_SUCCESS;
@@ -2356,30 +2414,28 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
{
// Could not hit the polygon, keep the old t and report hit.
- if (pathCount)
- *pathCount = n;
+ hit->pathCount = n;
return status;
}
// Keep track of furthest t so far.
- if (tmax > *t)
- *t = tmax;
+ if (tmax > hit->t)
+ hit->t = tmax;
// Store visited polygons.
- if (n < maxPath)
- path[n++] = curRef;
+ if (n < hit->maxPath)
+ hit->path[n++] = curRef;
else
status |= DT_BUFFER_TOO_SMALL;
// Ray end is completely inside the polygon.
if (segMax == -1)
{
- *t = FLT_MAX;
- if (pathCount)
- *pathCount = n;
+ hit->t = FLT_MAX;
+ hit->pathCount = n;
// add the cost
- if (pathCost)
- *pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
+ if (options & DT_RAYCAST_USE_COSTS)
+ hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
return status;
}
@@ -2465,12 +2521,12 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
}
// add the cost
- if (pathCost)
+ if (options & DT_RAYCAST_USE_COSTS)
{
// compute the intersection point at the furthest end of the polygon
// and correct the height (since the raycast moves in 2d)
dtVcopy(lastPos, curPos);
- dtVmad(curPos, startPos, dir, *t);
+ dtVmad(curPos, startPos, dir, hit->t);
float* e1 = &verts[segMax*3];
float* e2 = &verts[(segMax+1)*3]; // no need to modulu nv, seg[nv+1] was added earlier
float eDir[3], diff[3];
@@ -2479,7 +2535,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
curPos[1] = e1[1] + eDir[1] * s;
- *pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
+ hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
}
if (!nextRef)
@@ -2493,13 +2549,12 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
const float* vb = &verts[b*3];
const float dx = vb[0] - va[0];
const float dz = vb[2] - va[2];
- hitNormal[0] = dz;
- hitNormal[1] = 0;
- hitNormal[2] = -dx;
- dtVnormalize(hitNormal);
+ hit->hitNormal[0] = dz;
+ hit->hitNormal[1] = 0;
+ hit->hitNormal[2] = -dx;
+ dtVnormalize(hit->hitNormal);
- if (pathCount)
- *pathCount = n;
+ hit->pathCount = n;
return status;
}
@@ -2512,8 +2567,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
poly = nextPoly;
}
- if (pathCount)
- *pathCount = n;
+ hit->pathCount = n;
return status;
}
@@ -3473,7 +3527,7 @@ bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
if (!m_nodePool) return false;
dtNode* nodes[4];
- int n= m_nodePool->findNodes(ref, 4, nodes);
+ int n= m_nodePool->findNodes(ref, nodes, 4);
for (int i=0; i= bufSize)
+ if (n >= maxNodes)
return n;
- buf[n++] = &m_nodes[i];
+ nodes[n++] = &m_nodes[i];
}
i = m_next[i];
}