From d2306cacdd681905053424440eb452f02155dabe Mon Sep 17 00:00:00 2001 From: axelrodR Date: Thu, 30 Jan 2014 11:57:13 +0200 Subject: [PATCH 01/17] improving path aesthetics at tile crossing: in the old code, when a new node is found a single 3D position is associated with it. This position is used regardless of arriving from a new tile. A minor issue on a normal navmesh, this produces many problems when crossing tiles. Points might be located on different borders altogether depending on where one arrived from first. The new code assigns several (consecutive) dtNodes for polygons that lie on tile boundary. Each node gets its own point. since the nodes are packed together consecutively, it is easy to find the right node by reference even though there are several. --- Detour/Include/DetourNode.h | 8 +++++- Detour/Source/DetourNavMeshQuery.cpp | 38 +++++++++++++++++++++++++--- Detour/Source/DetourNode.cpp | 38 +++++++++++++++++----------- 3 files changed, 65 insertions(+), 19 deletions(-) diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index b68c922..811135f 100644 --- a/Detour/Include/DetourNode.h +++ b/Detour/Include/DetourNode.h @@ -48,7 +48,12 @@ public: ~dtNodePool(); inline void operator=(const dtNodePool&) {} void clear(); - dtNode* getNode(dtPolyRef id); + + /* Get a dtNode by ref. If there is none then - allocate + * There can be more than one node for the same polyRef, in which case they are seqential + * nb [in] the number of extra nodes to allocate if needed + */ + dtNode* getNode(dtPolyRef id, int nExtra=0); dtNode* findNode(dtPolyRef id); inline unsigned int getNodeIdx(const dtNode* node) const @@ -82,6 +87,7 @@ public: inline int getHashSize() const { return m_hashSize; } inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; } inline dtNodeIndex getNext(int i) const { return m_next[i]; } + inline int getNodeCount() const { return m_nodeCount;} private: diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index bdbee00..9856094 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1047,12 +1047,22 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + // deal explicitly with crossing tile boundaries + int crossSide = 0, extraNodes = 0; + if (bestTile->links[i].side != 0xff) + { + extraNodes = 3; + crossSide = bestTile->links[i].side >> 1; + } + + // get the node + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, extraNodes); if (!neighbourNode) { status |= DT_OUT_OF_NODES; continue; } + neighbourNode += crossSide; // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -1304,12 +1314,21 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) continue; - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + // deal explicitly with crossing tile boundaries + int crossSide = 0, extraNodes = 0; + if (bestTile->links[i].side != 0xff) + { + extraNodes = 3; + crossSide = bestTile->links[i].side >> 1; + } + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, extraNodes); if (!neighbourNode) { m_query.status |= DT_OUT_OF_NODES; continue; } + neighbourNode += crossSide; // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -3370,5 +3389,18 @@ bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const { if (!m_nodePool) return false; const dtNode* node = m_nodePool->findNode(ref); - return node && node->flags & DT_NODE_CLOSED; + if (!node) + return false; + + const dtNode* last=m_nodePool->getNodeAtIdx(m_nodePool->getNodeCount() -1); + + do // several nodes might be allocated for the same ref + { + if (node->flags & DT_NODE_CLOSED) + return true; + + node++; + } while (nodeid == ref); + + return false; } diff --git a/Detour/Source/DetourNode.cpp b/Detour/Source/DetourNode.cpp index 57cb206..f1e121d 100644 --- a/Detour/Source/DetourNode.cpp +++ b/Detour/Source/DetourNode.cpp @@ -97,7 +97,7 @@ dtNode* dtNodePool::findNode(dtPolyRef id) return 0; } -dtNode* dtNodePool::getNode(dtPolyRef id) +dtNode* dtNodePool::getNode(dtPolyRef id, int nExtra) { unsigned int bucket = dtHashRef(id) & (m_hashSize-1); dtNodeIndex i = m_first[bucket]; @@ -108,25 +108,33 @@ dtNode* dtNodePool::getNode(dtPolyRef id) return &m_nodes[i]; i = m_next[i]; } - - if (m_nodeCount >= m_maxNodes) + + if (m_nodeCount + nExtra >= m_maxNodes) return 0; - + + // add to hash table i = (dtNodeIndex)m_nodeCount; - m_nodeCount++; - - // Init node - node = &m_nodes[i]; - node->pidx = 0; - node->cost = 0; - node->total = 0; - node->id = id; - node->flags = 0; - m_next[i] = m_first[bucket]; m_first[bucket] = i; + + dtNode* ret = &m_nodes[i]; + + for ( ; nExtra >= 0; --nExtra) + { + i = (dtNodeIndex)m_nodeCount; + m_nodeCount++; - return node; + // Init node + node = &m_nodes[i]; + node->pidx = 0; + node->cost = 0; + node->total = 0; + node->id = id; + node->flags = 0; + } + + + return ret; } From 9b574984063404820ee75df09d42c1bf4ee7b780 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Fri, 31 Jan 2014 00:08:23 +0200 Subject: [PATCH 02/17] fix: multiple node allocation had a bug --- Detour/Source/DetourNode.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/Detour/Source/DetourNode.cpp b/Detour/Source/DetourNode.cpp index f1e121d..7892706 100644 --- a/Detour/Source/DetourNode.cpp +++ b/Detour/Source/DetourNode.cpp @@ -113,17 +113,15 @@ dtNode* dtNodePool::getNode(dtPolyRef id, int nExtra) return 0; // add to hash table - i = (dtNodeIndex)m_nodeCount; - m_next[i] = m_first[bucket]; - m_first[bucket] = i; + int n = (dtNodeIndex)m_nodeCount; + dtNode* ret = &m_nodes[n]; - dtNode* ret = &m_nodes[i]; - - for ( ; nExtra >= 0; --nExtra) + // add the nodes from last to first + for (int j=n+nExtra ; j>=n; --j) { - i = (dtNodeIndex)m_nodeCount; + i = (dtNodeIndex)j; m_nodeCount++; - + // Init node node = &m_nodes[i]; node->pidx = 0; @@ -131,8 +129,10 @@ dtNode* dtNodePool::getNode(dtPolyRef id, int nExtra) node->total = 0; node->id = id; node->flags = 0; - } + m_next[i] = m_first[bucket]; + m_first[bucket] = i; + } return ret; } From 72b7123e729225872d5c56814319e24501f56902 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Fri, 31 Jan 2014 11:26:12 +0200 Subject: [PATCH 03/17] moving from creating several nodes at once for the same ref to creating as needed based on ref and state (where state is currently used only for border poly to keep the arrival side). --- Detour/Include/DetourNode.h | 14 +++--- Detour/Source/DetourNavMeshQuery.cpp | 34 +++++--------- Detour/Source/DetourNode.cpp | 70 ++++++++++++++++------------ 3 files changed, 59 insertions(+), 59 deletions(-) diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index 811135f..868724f 100644 --- a/Detour/Include/DetourNode.h +++ b/Detour/Include/DetourNode.h @@ -35,7 +35,8 @@ struct dtNode float pos[3]; ///< Position of the node. float cost; ///< Cost from previous node to current node. float total; ///< Cost up to the node. - unsigned int pidx : 30; ///< Index to parent node. + unsigned int pidx : 28; ///< Index to parent node. + unsigned int state : 2; ///< extra state information. A polyRef can have multiple nodes with different extra info. unsigned int flags : 2; ///< Node flags 0/open/closed. dtPolyRef id; ///< Polygon ref the node corresponds to. }; @@ -49,12 +50,11 @@ public: inline void operator=(const dtNodePool&) {} void clear(); - /* Get a dtNode by ref. If there is none then - allocate - * There can be more than one node for the same polyRef, in which case they are seqential - * nb [in] the number of extra nodes to allocate if needed - */ - dtNode* getNode(dtPolyRef id, int nExtra=0); - dtNode* findNode(dtPolyRef id); + // Get a dtNode by ref and extra state information. If there is none then - allocate + // 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); inline unsigned int getNodeIdx(const dtNode* node) const { diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 9856094..1beb065 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1048,21 +1048,17 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, continue; // deal explicitly with crossing tile boundaries - int crossSide = 0, extraNodes = 0; + unsigned char crossSide = 0; if (bestTile->links[i].side != 0xff) - { - extraNodes = 3; crossSide = bestTile->links[i].side >> 1; - } // get the node - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, extraNodes); + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); if (!neighbourNode) { status |= DT_OUT_OF_NODES; continue; } - neighbourNode += crossSide; // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -1315,20 +1311,16 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) continue; // deal explicitly with crossing tile boundaries - int crossSide = 0, extraNodes = 0; + unsigned char crossSide = 0; if (bestTile->links[i].side != 0xff) - { - extraNodes = 3; crossSide = bestTile->links[i].side >> 1; - } - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, extraNodes); + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); if (!neighbourNode) { m_query.status |= DT_OUT_OF_NODES; continue; } - neighbourNode += crossSide; // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) @@ -1512,7 +1504,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing dtNode* node = 0; for (int i = existingSize-1; i >= 0; --i) { - node = m_nodePool->findNode(existing[i]); + m_nodePool->findNodes(existing[i], 1, &node); if (node) break; } @@ -3388,19 +3380,15 @@ bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const { if (!m_nodePool) return false; - const dtNode* node = m_nodePool->findNode(ref); - if (!node) - return false; + + dtNode* nodes[4]; + int n= m_nodePool->findNodes(ref, 4, nodes); - const dtNode* last=m_nodePool->getNodeAtIdx(m_nodePool->getNodeCount() -1); - - do // several nodes might be allocated for the same ref + for (int i=0; iflags & DT_NODE_CLOSED) + if (nodes[i]->flags & DT_NODE_CLOSED) return true; - - node++; - } while (nodeid == ref); + } return false; } diff --git a/Detour/Source/DetourNode.cpp b/Detour/Source/DetourNode.cpp index 7892706..2849c67 100644 --- a/Detour/Source/DetourNode.cpp +++ b/Detour/Source/DetourNode.cpp @@ -84,57 +84,69 @@ void dtNodePool::clear() m_nodeCount = 0; } -dtNode* dtNodePool::findNode(dtPolyRef id) +unsigned int dtNodePool::findNodes(dtPolyRef id, int bufSize, dtNode** buf) { + int n = 0; unsigned int bucket = dtHashRef(id) & (m_hashSize-1); dtNodeIndex i = m_first[bucket]; while (i != DT_NULL_IDX) { if (m_nodes[i].id == id) + { + if (n >= bufSize) + return n; + buf[n++] = &m_nodes[i]; + } + i = m_next[i]; + } + + return n; +} + +dtNode* dtNodePool::findNode(dtPolyRef id, unsigned char state) +{ + unsigned int bucket = dtHashRef(id) & (m_hashSize-1); + dtNodeIndex i = m_first[bucket]; + while (i != DT_NULL_IDX) + { + if (m_nodes[i].id == id && m_nodes[i].state == state) return &m_nodes[i]; i = m_next[i]; } return 0; } -dtNode* dtNodePool::getNode(dtPolyRef id, int nExtra) +dtNode* dtNodePool::getNode(dtPolyRef id, unsigned char state) { unsigned int bucket = dtHashRef(id) & (m_hashSize-1); dtNodeIndex i = m_first[bucket]; dtNode* node = 0; while (i != DT_NULL_IDX) { - if (m_nodes[i].id == id) + if (m_nodes[i].id == id && m_nodes[i].state == state) return &m_nodes[i]; i = m_next[i]; } - - if (m_nodeCount + nExtra >= m_maxNodes) + + if (m_nodeCount >= m_maxNodes) return 0; - - // add to hash table - int n = (dtNodeIndex)m_nodeCount; - dtNode* ret = &m_nodes[n]; - - // add the nodes from last to first - for (int j=n+nExtra ; j>=n; --j) - { - i = (dtNodeIndex)j; - m_nodeCount++; - - // Init node - node = &m_nodes[i]; - node->pidx = 0; - node->cost = 0; - node->total = 0; - node->id = id; - node->flags = 0; - - m_next[i] = m_first[bucket]; - m_first[bucket] = i; - } - - return ret; + + i = (dtNodeIndex)m_nodeCount; + m_nodeCount++; + + // Init node + node = &m_nodes[i]; + node->pidx = 0; + node->cost = 0; + node->total = 0; + node->id = id; + node->state = state; + node->flags = 0; + + m_next[i] = m_first[bucket]; + m_first[bucket] = i; + + return node; } From e2722403bebf119d6f8246ab746c56073e91d461 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Mon, 10 Feb 2014 00:30:46 +0200 Subject: [PATCH 04/17] bugfix: raycasts sometimes fail when the origin is near a boundary of a triangle with a slope. The fix of considers polygons to have a thickness for the purpose of finding closest nodes. --- Detour/Include/DetourNavMesh.h | 10 +++- Detour/Include/DetourNavMeshQuery.h | 8 +-- Detour/Source/DetourNavMesh.cpp | 34 ++++++++++--- Detour/Source/DetourNavMeshQuery.cpp | 75 +++++++++++++++++++++++----- 4 files changed, 103 insertions(+), 24 deletions(-) diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index d87fee8..a90c1e2 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -94,6 +94,10 @@ static const unsigned int DT_OFFMESH_CON_BIDIR = 1; /// @ingroup detour static const int DT_MAX_AREAS = 64; +/// Polygon thickness relative to agent height (for correct scaling) +/// We define the polygon to have a slight thickness so that things slightly above and below will be considered on the polygon. +static const float SCALE_POLY_THICK = 1.f / 6.f; + /// Tile flags used for various functions and fields. /// For an example, see dtNavMesh::addTile(). enum dtTileFlags @@ -612,8 +616,10 @@ private: /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const; - /// Returns closest point on polygon. - void closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, + + /// find the closest point on polygon. + /// @return whether the point is straight above the closest polygon + bool closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, const float* pos, float* closest) const; dtNavMeshParams m_params; ///< Current initialization params. TODO: do not store this info twice. diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 6edf5bf..8f6589e 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -378,8 +378,9 @@ public: /// @param[in] ref The reference id of the polygon. /// @param[in] pos The position to check. [(x, y, z)] /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] inside2D true if the closest point is above/below the polygon /// @returns The status flags for the query. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const; + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* inside2D=0) const; /// Returns a point on the boundary closest to the source point if the source point is outside the /// polygon's xz-bounds. @@ -431,8 +432,9 @@ private: /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, const dtQueryFilter* filter, float* nearestPt) const; - /// Returns closest point on polygon. - void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; + /// finds the closest point on a polygon. + /// returns whether the point is inside the polygon (in 2D) + bool closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; /// Returns portal points between two polygons. dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index 3bc2b73..407611b 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -617,7 +617,7 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) } } -void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, +bool dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, const float* pos, float* closest) const { const dtPoly* poly = &tile->polys[ip]; @@ -630,7 +630,7 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip const float d1 = dtVdist(pos, v1); const float u = d0 / (d0+d1); dtVlerp(closest, v0, v1, u); - return; + return false; } const dtPolyDetail* pd = &tile->detailMeshes[ip]; @@ -644,7 +644,8 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); + if (!inside) { // Point is outside the polygon, dtClamp to nearest edge. float dmin = FLT_MAX; @@ -681,6 +682,8 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip break; } } + + return inside; } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, @@ -694,6 +697,9 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, // Get nearby polygons from proximity grid. dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); + + // tile thickness is scaled relative to an agent height + const float polyThick = tile->header->walkableHeight * SCALE_POLY_THICK; // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; @@ -701,15 +707,31 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly); - float d = dtVdistSqr(center, closestPtPoly); + float d, closestPtPoly[3], offset[3]; + bool inside2D = closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly); + + dtVsub(offset, center, closestPtPoly); + + if (inside2D) + { + // right above the polygon is considered on the polygon + d = dtAbs(offset[1]) - polyThick; + d = d > 0 ? d*d : 0; + } + else + d = dtVlenSqr(offset); + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; + if (d == 0) + { + nearestDistanceSqr = 0; + break; + } } } diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 1beb065..052bbf6 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -501,7 +501,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f /// /// See closestPointOnPolyBoundary() for a limited but faster option. /// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const +dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* inside2D) const { dtAssert(m_nav); const dtMeshTile* tile = 0; @@ -511,12 +511,14 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo if (!tile) return DT_FAILURE | DT_INVALID_PARAM; - closestPointOnPolyInTile(tile, poly, pos, closest); + bool in2D = closestPointOnPolyInTile(tile, poly, pos, closest); + if (inside2D) + *inside2D = in2D; return DT_SUCCESS; } -void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, +bool dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const { // Off-mesh connections don't have detail polygons. @@ -528,7 +530,7 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo const float d1 = dtVdist(pos, v1); const float u = d0 / (d0+d1); dtVlerp(closest, v0, v1, u); - return; + return false; } const unsigned int ip = (unsigned int)(poly - tile->polys); @@ -543,7 +545,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); + if (!inside) { // Point is outside the polygon, dtClamp to nearest edge. float dmin = FLT_MAX; @@ -581,6 +584,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo } } + return inside; + /* float closestDistSqr = FLT_MAX; for (int j = 0; j < pd->triCount; ++j) { @@ -739,6 +744,15 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten int polyCount = 0; if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, 128))) return DT_FAILURE | DT_INVALID_PARAM; + + if (polyCount == 0) + return DT_SUCCESS; + + // tile thickness is scaled relative to an agent height + const dtMeshTile* tile0; + const dtPoly* poly0; + m_nav->getTileAndPolyByRefUnsafe(polys[0], &tile0, &poly0); + const float polyThick = tile0->header->walkableHeight * SCALE_POLY_THICK; // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; @@ -746,15 +760,32 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - closestPointOnPoly(ref, center, closestPtPoly); - float d = dtVdistSqr(center, closestPtPoly); + float d, closestPtPoly[3], offset[3]; + bool inside2d; + closestPointOnPoly(ref, center, closestPtPoly, &inside2d); + + dtVsub(offset, center, closestPtPoly); + + if (inside2d) + { + // right above the polygon is considered on the polygon + d = dtAbs(offset[1]) - polyThick; + d = d > 0 ? d*d : 0; + } + else + d = dtVlenSqr(offset); + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; + if (d == 0) + { + nearestDistanceSqr = 0; + break; + } } } @@ -768,7 +799,7 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl const dtQueryFilter* filter, float* nearestPt) const { dtAssert(m_nav); - + float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); @@ -777,6 +808,9 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128); + // tile thickness is scaled relative to an agent height + const float polyThick = tile->header->walkableHeight * SCALE_POLY_THICK; + // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; @@ -784,16 +818,31 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl { dtPolyRef ref = polys[i]; const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - closestPointOnPolyInTile(tile, poly, center, closestPtPoly); - - float d = dtVdistSqr(center, closestPtPoly); + float d, closestPtPoly[3], offset[3]; + bool inside2d = closestPointOnPolyInTile(tile, poly, center, closestPtPoly); + + dtVsub(offset, center, closestPtPoly); + + if (inside2d) + { + // right above the polygon is considered on the polygon + d = dtAbs(offset[1]) - polyThick; + d = d > 0 ? d*d : 0; + } + else + d = dtVlenSqr(offset); + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; + if (d == 0) + { + nearestDistanceSqr = 0; + break; + } } } From 120de6256f3e03b68b28cf2b5263d52813a61142 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Sun, 16 Feb 2014 16:51:29 +0200 Subject: [PATCH 05/17] changed path planner to Theta*, changed the raycast function to emit a cost based on the actual traversal cost --- Detour/Include/DetourNavMeshQuery.h | 5 +- Detour/Include/DetourNode.h | 6 +- Detour/Source/DetourNavMeshQuery.cpp | 270 ++++++++++++++++++++------- 3 files changed, 207 insertions(+), 74 deletions(-) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 6edf5bf..955e2a0 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -318,10 +318,13 @@ 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) const; + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath, + float* pathCost=0, 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. diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index 868724f..4a42bb8 100644 --- a/Detour/Include/DetourNode.h +++ b/Detour/Include/DetourNode.h @@ -25,6 +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. }; typedef unsigned short dtNodeIndex; @@ -35,13 +36,14 @@ struct dtNode float pos[3]; ///< Position of the node. float cost; ///< Cost from previous node to current node. float total; ///< Cost up to the node. - unsigned int pidx : 28; ///< Index to parent node. + unsigned int pidx : 24; ///< Index to parent node. unsigned int state : 2; ///< extra state information. A polyRef can have multiple nodes with different extra info. - unsigned int flags : 2; ///< Node flags 0/open/closed. + unsigned int flags : 3; ///< Node flags 0/open/closed. dtPolyRef id; ///< Polygon ref the node corresponds to. }; + class dtNodePool { public: diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 1beb065..a118c3e 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1016,19 +1016,29 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Get current poly and tile. // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; + 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; + // Get parent and grand parent poly and tile. + dtPolyRef parentRef = 0, grandpaRef = 0; const dtMeshTile* parentTile = 0; const dtPoly* parentPoly = 0; + dtNode* parentNode = 0; if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + { + parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); + parentRef = parentNode->id; + if (parentNode->pidx) + grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; + } if (parentRef) m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + // decide whether to test raycast to previous nodes + const float rayCutDist = 30.0f; // TODO: maybe this needs to be on the filter? + bool tryLOS = (parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < dtSqr(rayCutDist)); for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { @@ -1060,6 +1070,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, continue; } + // do not expand to nodes that were already visited from the same parent + if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) + continue; + // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) { @@ -1071,31 +1085,38 @@ 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) + + // raycast parent + float t = 0; + if (tryLOS) { - // Cost - const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - const float endCost = filter->getCost(neighbourNode->pos, endPos, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly, - 0, 0, 0); - - cost = bestNode->cost + curCost + endCost; - heuristic = 0; + float normal[3], curCost; + raycast(parentRef, parentNode->pos, neighbourNode->pos, filter, &t, normal, 0, 0, 0, &curCost, grandpaRef); + cost = parentNode->cost + curCost; } - else + + if (t < 1.0f) // hit { - // Cost const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); cost = bestNode->cost + curCost; + } + + // Special case for last node. + if (neighbourRef == endRef) + { + const float endCost = filter->getCost(neighbourNode->pos, endPos, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly, + 0, 0, 0); + + cost = cost + endCost; + heuristic = 0; + } + else + { heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE; } @@ -1109,11 +1130,13 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, continue; // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->pidx = t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; neighbourNode->id = neighbourRef; - neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); neighbourNode->cost = cost; neighbourNode->total = total; + if (t >= 1.0f) + neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); if (neighbourNode->flags & DT_NODE_OPEN) { @@ -1142,10 +1165,14 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Reverse the path. dtNode* prev = 0; dtNode* node = lastBestNode; + int prevRay = 0; do { 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; + prevRay = nextRay; prev = node; node = next; } @@ -1156,13 +1183,29 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, int n = 0; do { - path[n++] = node->id; - if (n >= maxPath) + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status2 = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) { - status |= DT_BUFFER_TOO_SMALL; + float t, normal[3]; + int m; + status2 = raycast(node->id, node->pos, next->pos, filter, &t, normal, path+n, &m, maxPath-n); + n += m; + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status2 = DT_BUFFER_TOO_SMALL; + } + + if (status2 & DT_STATUS_DETAIL_MASK) + { + status |= status2 & DT_STATUS_DETAIL_MASK; break; } - node = m_nodePool->getNodeAtIdx(node->pidx); + + node = next; } while (node); @@ -1275,15 +1318,22 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) return m_query.status; } - // Get parent poly and tile. - dtPolyRef parentRef = 0; + // Get parent and grand parent poly and tile. + dtPolyRef parentRef = 0, grandpaRef = 0; const dtMeshTile* parentTile = 0; const dtPoly* parentPoly = 0; + dtNode* parentNode = 0; if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + { + parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); + parentRef = parentNode->id; + if (parentNode->pidx) + grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; + } if (parentRef) { - if (dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly))) + bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); + if (invalidParent || (grandpaRef && dtStatusFailed(m_nav->isValidPolyRef(grandpaRef))) ) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; @@ -1292,6 +1342,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) return m_query.status; } } + + // decide whether to test raycast to previous nodes + const float rayCutDist = 30.0f; // TODO: maybe this needs to be on the filter? + bool tryLOS = (parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < dtSqr(rayCutDist)); for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { @@ -1322,6 +1376,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) continue; } + // do not expand to nodes that were already visited from the same parent + if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) + continue; + // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) { @@ -1334,30 +1392,37 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) float cost = 0; float heuristic = 0; + // raycast parent + 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; + } + + if (t < 1.0f) // hit + { + const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + cost = bestNode->cost + curCost; + } + // Special case for last node. if (neighbourRef == m_query.endRef) { - // Cost - const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0, 0, 0); - cost = bestNode->cost + curCost + endCost; + cost = cost + endCost; heuristic = 0; } else { - // Cost - const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, - parentRef, parentTile, parentPoly, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly); - cost = bestNode->cost + curCost; heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE; } @@ -1371,11 +1436,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) continue; // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->pidx = t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; neighbourNode->id = neighbourRef; - neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); neighbourNode->cost = cost; neighbourNode->total = total; + if (t >= 1.0f) + neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); if (neighbourNode->flags & DT_NODE_OPEN) { @@ -1439,11 +1506,15 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, dtNode* prev = 0; dtNode* node = m_query.lastBestNode; + int prevRay = 0; do { 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; + prevRay = nextRay; node = next; } while (node); @@ -1452,13 +1523,28 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, node = prev; do { - path[n++] = node->id; - if (n >= maxPath) + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) { - m_query.status |= DT_BUFFER_TOO_SMALL; + float t, normal[3]; + int m; + status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); + n += m; + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status = DT_BUFFER_TOO_SMALL; + } + + if (status & DT_STATUS_DETAIL_MASK) + { + m_query.status |= status & DT_STATUS_DETAIL_MASK; break; } - node = m_nodePool->getNodeAtIdx(node->pidx); + node = next; } while (node); } @@ -2248,44 +2334,59 @@ 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) const + float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath, + float* pathCost, dtPolyRef prevRef) const { dtAssert(m_nav); *t = 0; if (pathCount) *pathCount = 0; - + if (pathCost) + *pathCost = 0; + // Validate input if (!startRef || !m_nav->isValidPolyRef(startRef)) return DT_FAILURE | DT_INVALID_PARAM; + if (prevRef && !m_nav->isValidPolyRef(prevRef)) + return DT_FAILURE | DT_INVALID_PARAM; - dtPolyRef curRef = startRef; - float verts[DT_VERTS_PER_POLYGON*3]; + float dir[3], curPos[3], lastPos[3]; + float verts[DT_VERTS_PER_POLYGON*3+3]; int n = 0; - - hitNormal[0] = 0; - hitNormal[1] = 0; - hitNormal[2] = 0; - + + dtVcopy(curPos, startPos); + dtVsub(dir, endPos, startPos); + dtVset(hitNormal, 0, 0, 0); + dtStatus status = DT_SUCCESS; - + + const dtMeshTile* prevTile, *tile, *nextTile; + const dtPoly* prevPoly, *poly, *nextPoly; + dtPolyRef curRef, nextRef; + + // The API input has been checked already, skip checking internal data. + nextRef = curRef = startRef; + tile = 0; + poly = 0; + m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); + nextTile = prevTile = tile; + nextPoly = prevPoly = poly; + if (prevRef) + m_nav->getTileAndPolyByRefUnsafe(prevRef, &tile, &poly); + while (curRef) { // Cast ray against current polygon. - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); - // Collect vertices. int nv = 0; for (int i = 0; i < (int)poly->vertCount; ++i) { dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); nv++; - } + } + dtVcopy(&verts[nv*3], &tile->verts[poly->verts[0]*3]); float tmin, tmax; int segMin, segMax; @@ -2305,18 +2406,22 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons 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; + + // add the cost + if (pathCost) + *pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); return status; } - + // Follow neighbours. - dtPolyRef nextRef = 0; + nextRef = 0; for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) { @@ -2327,8 +2432,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons continue; // Get pointer to the next polygon. - const dtMeshTile* nextTile = 0; - const dtPoly* nextPoly = 0; + nextTile = 0; + nextPoly = 0; m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly); // Skip off-mesh connections. @@ -2396,6 +2501,24 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons } } + // add the cost + if (pathCost) + { + // 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); + 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]; + dtVsub(eDir, e2, e1); + dtVsub(diff, curPos, e1); + 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); + } + if (!nextRef) { // No neighbour, we hit a wall. @@ -2416,9 +2539,14 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons *pathCount = n; return status; } - + // No hit, advance to neighbour polygon. + prevRef = curRef; curRef = nextRef; + prevTile = tile; + tile = nextTile; + prevPoly = poly; + poly = nextPoly; } if (pathCount) From 1b7b918641ef0d6a4a41fac4ee24a3aca08ab529 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Tue, 25 Feb 2014 11:33:06 +0200 Subject: [PATCH 06/17] following mikko's comments on push #17. Interfaces changes, the most notable is adding a new raycast function and converted the existing to a thin wrapper. --- Detour/Include/DetourNavMesh.h | 7 ++ Detour/Include/DetourNavMeshQuery.h | 50 +++++++++- Detour/Include/DetourNode.h | 4 +- Detour/Source/DetourNavMeshQuery.cpp | 138 +++++++++++++++++++-------- Detour/Source/DetourNode.cpp | 6 +- 5 files changed, 154 insertions(+), 51 deletions(-) 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]; } From 1613a5565f0d490f351c06f0957d81025b28a892 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Wed, 26 Feb 2014 21:29:13 +0200 Subject: [PATCH 07/17] following mikko's comments on push #17 (part 2). Interfaces changes, the most notable is adding option flags to initSlicedFindPath that can cancel the any-angle search --- Detour/Include/DetourNavMesh.h | 11 +++ Detour/Include/DetourNavMeshQuery.h | 5 +- Detour/Source/DetourNavMeshQuery.cpp | 118 ++++++++++----------------- 3 files changed, 60 insertions(+), 74 deletions(-) diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index 1f80f6d..19c2208 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -118,12 +118,23 @@ enum dtStraightPathOptions }; +/// Options for dtNavMeshQuery::findPath +enum dtFindPathOptions +{ + DT_FINDPATH_LOW_QUALITY_FAR = 0x01, ///< [provisional] trade quality for performance far from the origin. The idea is that by then a new query will be issued + DT_FINDPATH_ANY_ANGLE = 0x02, ///< use raycasts during pathfind to "shortcut" (raycast still consider costs) +}; + /// Options for dtNavMeshQuery::raycast enum dtRaycastOptions { DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost }; + +/// used when #DT_FINDPATH_LIMIT_RAYCASTS is set to limit raycasting to a multiple of the character radius +static const float RAY_CAST_LIMIT_PROPORTIONS = 20.0f; + /// Flags representing the type of a navigation mesh polygon. enum dtPolyTypes { diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index c5b0303..8b09702 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -207,10 +207,11 @@ public: /// @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[in] options query options (see: #dtFindPathOptions) /// @returns The status flags for the query. dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter); + const dtQueryFilter* filter, const unsigned int options=0); /// Updates an in-progress sliced path query. /// @param[in] maxIter The maximum number of iterations to perform. @@ -508,6 +509,8 @@ private: dtPolyRef startRef, endRef; float startPos[3], endPos[3]; const dtQueryFilter* filter; + unsigned int options; + float raycastLimitSqr; }; dtQueryData m_query; ///< Sliced query state. diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 6618b83..6e12bb3 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -961,7 +961,6 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, dtNode* lastBestNode = startNode; float lastBestNodeCost = startNode->total; - RaycastHit rayHit; dtStatus status = DT_SUCCESS; while (!m_openList->empty()) @@ -980,29 +979,19 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Get current poly and tile. // The API input has been cheked already, skip checking internal data. - dtPolyRef bestRef = bestNode->id; + const dtPolyRef bestRef = bestNode->id; const dtMeshTile* bestTile = 0; const dtPoly* bestPoly = 0; m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - // Get parent and grand parent poly and tile. - dtPolyRef parentRef = 0, grandpaRef = 0; + // Get parent poly and tile. + dtPolyRef parentRef = 0; const dtMeshTile* parentTile = 0; const dtPoly* parentPoly = 0; - dtNode* parentNode = 0; if (bestNode->pidx) - { - parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); - parentRef = parentNode->id; - if (parentNode->pidx) - grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; - } + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; if (parentRef) m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - // decide whether to test raycast to previous nodes - const float rayCutDist = 30.0f; // TODO: maybe this needs to be on the filter? - bool tryLOS = (parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < dtSqr(rayCutDist)); for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { @@ -1034,10 +1023,6 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, continue; } - // do not expand to nodes that were already visited from the same parent - if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) - continue; - // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) { @@ -1049,37 +1034,31 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Calculate cost and heuristic. float cost = 0; float heuristic = 0; - - // raycast parent - float t = 0; - if (tryLOS) - { - raycast(parentRef, parentNode->pos, neighbourNode->pos, filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef); - cost = parentNode->cost + rayHit.pathCost; - } - - if (t < 1.0f) // hit + + // Special case for last node. + if (neighbourRef == endRef) { + // Cost const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); - cost = bestNode->cost + curCost; - } - - // Special case for last node. - if (neighbourRef == endRef) - { const float endCost = filter->getCost(neighbourNode->pos, endPos, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0, 0, 0); - cost = cost + endCost; + cost = bestNode->cost + curCost + endCost; heuristic = 0; } else { + // Cost + const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos, + parentRef, parentTile, parentPoly, + bestRef, bestTile, bestPoly, + neighbourRef, neighbourTile, neighbourPoly); + cost = bestNode->cost + curCost; heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE; } @@ -1093,13 +1072,11 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, continue; // Add or update the node. - neighbourNode->pidx = t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; - neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); neighbourNode->cost = cost; neighbourNode->total = total; - if (t >= 1.0f) - neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); if (neighbourNode->flags & DT_NODE_OPEN) { @@ -1128,14 +1105,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, // Reverse the path. dtNode* prev = 0; dtNode* node = lastBestNode; - int prevRay = 0; do { dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); node->pidx = m_nodePool->getNodeIdx(prev); - 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; } @@ -1146,29 +1119,13 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, int n = 0; do { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - dtStatus status2 = 0; - if (node->flags & DT_NODE_PARENT_DETACHED) + path[n++] = node->id; + if (n >= maxPath) { - float t, normal[3]; - int m; - status2 = raycast(node->id, node->pos, next->pos, filter, &t, normal, path+n, &m, maxPath-n); - n += m; - } - else - { - path[n++] = node->id; - if (n >= maxPath) - status2 = DT_BUFFER_TOO_SMALL; - } - - if (status2 & DT_STATUS_DETAIL_MASK) - { - status |= status2 & DT_STATUS_DETAIL_MASK; + status |= DT_BUFFER_TOO_SMALL; break; } - - node = next; + node = m_nodePool->getNodeAtIdx(node->pidx); } while (node); @@ -1187,7 +1144,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, /// dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter) + const dtQueryFilter* filter, const unsigned int options) { dtAssert(m_nav); dtAssert(m_nodePool); @@ -1201,6 +1158,8 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef dtVcopy(m_query.startPos, startPos); dtVcopy(m_query.endPos, endPos); m_query.filter = filter; + m_query.options = options; + m_query.raycastLimitSqr = 1E37; if (!startRef || !endRef) return DT_FAILURE | DT_INVALID_PARAM; @@ -1209,6 +1168,16 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) return DT_FAILURE | DT_INVALID_PARAM; + // trade quality with performance? + if (options & DT_FINDPATH_ANY_ANGLE) + { + // 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; + m_query.raycastLimitSqr = dtSqr(agentRadius * RAY_CAST_LIMIT_PROPORTIONS); + } + if (startRef == endRef) { m_query.status = DT_SUCCESS; @@ -1309,8 +1278,12 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) } // decide whether to test raycast to previous nodes - const float rayCutDist = 30.0f; // TODO: maybe this needs to be on the filter? - bool tryLOS = (parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < dtSqr(rayCutDist)); + bool tryLOS = false; + if (m_query.options & DT_FINDPATH_ANY_ANGLE) + { + 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) { @@ -1358,14 +1331,14 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) float heuristic = 0; // raycast parent - float t = 0; + rayHit.t = 0; if (tryLOS) { 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 + if (rayHit.t < 1.0f) // hit { const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, @@ -1400,12 +1373,12 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) continue; // Add or update the node. - neighbourNode->pidx = t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; + neighbourNode->pidx = rayHit.t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; neighbourNode->id = neighbourRef; neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); neighbourNode->cost = cost; neighbourNode->total = total; - if (t >= 1.0f) + if (rayHit.t >= 1.0f) neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); if (neighbourNode->flags & DT_NODE_OPEN) @@ -2407,7 +2380,6 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); nv++; } - dtVcopy(&verts[nv*3], &tile->verts[poly->verts[0]*3]); float tmin, tmax; int segMin, segMax; @@ -2528,7 +2500,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons dtVcopy(lastPos, curPos); 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* e2 = &verts[((segMax+1)%nv)*3]; float eDir[3], diff[3]; dtVsub(eDir, e2, e1); dtVsub(diff, curPos, e1); From 5686f4a83c0462c3c07d5d2254a186f3865919e0 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Wed, 26 Feb 2014 21:30:24 +0200 Subject: [PATCH 08/17] changed the NavMeshTesterTool so that pathFind and straightPath use the default options (i.e. not any-angle) while sliced path uses the any angle search. --- RecastDemo/Source/NavMeshTesterTool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/RecastDemo/Source/NavMeshTesterTool.cpp b/RecastDemo/Source/NavMeshTesterTool.cpp index 9839d2f..5f6b92f 100644 --- a/RecastDemo/Source/NavMeshTesterTool.cpp +++ b/RecastDemo/Source/NavMeshTesterTool.cpp @@ -880,7 +880,7 @@ void NavMeshTesterTool::recalc() m_npolys = 0; m_nstraightPath = 0; - m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter); + m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, DT_FINDPATH_ANY_ANGLE); } else { From ffbff5dcc96ad7388deed04e7d0c461d3c367566 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Wed, 26 Feb 2014 22:48:27 +0200 Subject: [PATCH 09/17] changed the remarks and value of RAY_CAST_LIMIT_PROPORTIONS --- Detour/Include/DetourNavMesh.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index 19c2208..ae8cec9 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -132,8 +132,9 @@ enum dtRaycastOptions }; -/// used when #DT_FINDPATH_LIMIT_RAYCASTS is set to limit raycasting to a multiple of the character radius -static const float RAY_CAST_LIMIT_PROPORTIONS = 20.0f; +/// Limit raycasting during any angle pahfinding +/// The limit is given as a multiple of the character radius +static const float RAY_CAST_LIMIT_PROPORTIONS = 50.0f; /// Flags representing the type of a navigation mesh polygon. enum dtPolyTypes From 3a540dbc865fed2f1eee4ed95207e343bab26bc0 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Thu, 27 Feb 2014 01:32:12 +0200 Subject: [PATCH 10/17] bug fix: When building the detailed mesh, if getHeightData() can't find seeds using the region - fallback to the old code of finding seeds based on the vertices --- Recast/Source/RecastMeshDetail.cpp | 146 +++++++++++++++++++++++++++++ 1 file changed, 146 insertions(+) diff --git a/Recast/Source/RecastMeshDetail.cpp b/Recast/Source/RecastMeshDetail.cpp index 69ed147..927a414 100644 --- a/Recast/Source/RecastMeshDetail.cpp +++ b/Recast/Source/RecastMeshDetail.cpp @@ -741,6 +741,145 @@ static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, return true; } + +static void getHeightDataSeedsFromVertices(const rcCompactHeightfield& chf, + const unsigned short* poly, const int npoly, + const unsigned short* verts, const int bs, + rcHeightPatch& hp, rcIntArray& stack) +{ + // Floodfill the heightfield to get 2D height data, + // starting at vertex locations as seeds. + + // Note: Reads to the compact heightfield are offset by border size (bs) + // since border size offset is already removed from the polymesh vertices. + + memset(hp.data, 0, sizeof(unsigned short)*hp.width*hp.height); + + stack.resize(0); + + static const int offset[9*2] = + { + 0,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1, -1,0, + }; + + // Use poly vertices as seed points for the flood fill. + for (int j = 0; j < npoly; ++j) + { + int cx = 0, cz = 0, ci =-1; + int dmin = RC_UNSET_HEIGHT; + for (int k = 0; k < 9; ++k) + { + const int ax = (int)verts[poly[j]*3+0] + offset[k*2+0]; + const int ay = (int)verts[poly[j]*3+1]; + const int az = (int)verts[poly[j]*3+2] + offset[k*2+1]; + if (ax < hp.xmin || ax >= hp.xmin+hp.width || + az < hp.ymin || az >= hp.ymin+hp.height) + continue; + + const rcCompactCell& c = chf.cells[(ax+bs)+(az+bs)*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int d = rcAbs(ay - (int)s.y); + if (d < dmin) + { + cx = ax; + cz = az; + ci = i; + dmin = d; + } + } + } + if (ci != -1) + { + stack.push(cx); + stack.push(cz); + stack.push(ci); + } + } + + // Find center of the polygon using flood fill. + int pcx = 0, pcz = 0; + for (int j = 0; j < npoly; ++j) + { + pcx += (int)verts[poly[j]*3+0]; + pcz += (int)verts[poly[j]*3+2]; + } + pcx /= npoly; + pcz /= npoly; + + for (int i = 0; i < stack.size(); i += 3) + { + int cx = stack[i+0]; + int cy = stack[i+1]; + int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width; + hp.data[idx] = 1; + } + + while (stack.size() > 0) + { + int ci = stack.pop(); + int cy = stack.pop(); + int cx = stack.pop(); + + // Check if close to center of the polygon. + if (rcAbs(cx-pcx) <= 1 && rcAbs(cy-pcz) <= 1) + { + stack.resize(0); + stack.push(cx); + stack.push(cy); + stack.push(ci); + break; + } + + const rcCompactSpan& cs = chf.spans[ci]; + + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue; + + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + + if (ax < hp.xmin || ax >= (hp.xmin+hp.width) || + ay < hp.ymin || ay >= (hp.ymin+hp.height)) + continue; + + if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0) + continue; + + const int ai = (int)chf.cells[(ax+bs)+(ay+bs)*chf.width].index + rcGetCon(cs, dir); + + int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width; + hp.data[idx] = 1; + + stack.push(ax); + stack.push(ay); + stack.push(ai); + } + } + + memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + + // Mark start locations. + for (int i = 0; i < stack.size(); i += 3) + { + int cx = stack[i+0]; + int cy = stack[i+1]; + int ci = stack[i+2]; + int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width; + const rcCompactSpan& cs = chf.spans[ci]; + hp.data[idx] = cs.y; + + // getHeightData seeds are given in coordinates with borders + stack[i+0] += bs; + stack[i+1] += bs; + } + +} + + + static void getHeightData(const rcCompactHeightfield& chf, const unsigned short* poly, const int npoly, const unsigned short* verts, const int bs, @@ -752,6 +891,8 @@ static void getHeightData(const rcCompactHeightfield& chf, stack.resize(0); memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + + bool empty = true; // Copy the height from the same region, and mark region borders // as seed points to fill the rest. @@ -769,6 +910,7 @@ static void getHeightData(const rcCompactHeightfield& chf, { // Store height hp.data[hx + hy*hp.width] = s.y; + empty = false; // If any of the neighbours is not in same region, // add the current location as flood fill start @@ -800,6 +942,10 @@ static void getHeightData(const rcCompactHeightfield& chf, } } + // if the polygon does not contian any points from the current region (rare, but happens) + // then use the cells closest to the polygon vertices as seeds to fill the height field + if (empty) + getHeightDataSeedsFromVertices(chf, poly, npoly, verts, bs, hp, stack); static const int RETRACT_SIZE = 256; int head = 0; From c2b2b03b8f2a12d1486ce19781529fb0221ab38d Mon Sep 17 00:00:00 2001 From: axelrodR Date: Thu, 27 Feb 2014 01:47:21 +0200 Subject: [PATCH 11/17] bug fix: returned path had duplicates of nodes --- Detour/Include/DetourNavMeshQuery.h | 2 +- Detour/Source/DetourNavMeshQuery.cpp | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 8b09702..a01ee8d 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -211,7 +211,7 @@ public: /// @returns The status flags for the query. dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options=0); + const dtQueryFilter* filter, const unsigned int options=DT_FINDPATH_ANY_ANGLE); /// Updates an in-progress sliced path query. /// @param[in] maxIter The maximum number of iterations to perform. diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 6e12bb3..594e83c 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1216,6 +1216,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) } RaycastHit rayHit; + rayHit.maxPath = 0; int iter = 0; while (iter < maxIter && !m_openList->empty()) @@ -1468,6 +1469,9 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, int m; status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); n += m; + // raycast ends on poly boundary and the path might include the next poly boundary. + if (path[n-1] == next->id) + n--; // remove to avoid duplicates } else { From 4b43c33ae1fdedf81836640b024f2168d62ed29d Mon Sep 17 00:00:00 2001 From: axelrodR Date: Thu, 27 Feb 2014 09:27:32 +0200 Subject: [PATCH 12/17] bug fix: finalizeSlicedFindPathPartial() did not handle the case of DT_NODE_PARENT_DETACHED at all. --- Detour/Source/DetourNavMeshQuery.cpp | 30 ++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 594e83c..3e01c62 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1544,11 +1544,15 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing } // Reverse the path. + int prevRay = 0; do { dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); node->pidx = m_nodePool->getNodeIdx(prev); prev = node; + 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; } while (node); @@ -1557,13 +1561,31 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing node = prev; do { - path[n++] = node->id; - if (n >= maxPath) + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + dtStatus status = 0; + if (node->flags & DT_NODE_PARENT_DETACHED) { - m_query.status |= DT_BUFFER_TOO_SMALL; + float t, normal[3]; + int m; + status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); + n += m; + // raycast ends on poly boundary and the path might include the next poly boundary. + if (path[n-1] == next->id) + n--; // remove to avoid duplicates + } + else + { + path[n++] = node->id; + if (n >= maxPath) + status = DT_BUFFER_TOO_SMALL; + } + + if (status & DT_STATUS_DETAIL_MASK) + { + m_query.status |= status & DT_STATUS_DETAIL_MASK; break; } - node = m_nodePool->getNodeAtIdx(node->pidx); + node = next; } while (node); } From 232187b9d631caa1de09e3edec6a073bb0a0aaf1 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Thu, 27 Feb 2014 14:21:09 +0200 Subject: [PATCH 13/17] changed the default options of the pathfinder to 0 (i.e. no raycasts), changed the the tester tool to display all points along the sliced path (a better default for non flat terrain). --- Detour/Include/DetourNavMeshQuery.h | 2 +- RecastDemo/Source/NavMeshTesterTool.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index a01ee8d..427e4a9 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -211,7 +211,7 @@ public: /// @returns The status flags for the query. dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options=DT_FINDPATH_ANY_ANGLE); + const dtQueryFilter* filter, const unsigned int options=0/*DT_FINDPATH_ANY_ANGLE*/); /// Updates an in-progress sliced path query. /// @param[in] maxIter The maximum number of iterations to perform. diff --git a/RecastDemo/Source/NavMeshTesterTool.cpp b/RecastDemo/Source/NavMeshTesterTool.cpp index 5f6b92f..5bb363d 100644 --- a/RecastDemo/Source/NavMeshTesterTool.cpp +++ b/RecastDemo/Source/NavMeshTesterTool.cpp @@ -654,7 +654,7 @@ void NavMeshTesterTool::handleUpdate(const float /*dt*/) m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys, m_straightPath, m_straightPathFlags, - m_straightPathPolys, &m_nstraightPath, MAX_POLYS); + m_straightPathPolys, &m_nstraightPath, MAX_POLYS, DT_STRAIGHTPATH_ALL_CROSSINGS); } m_pathFindStatus = DT_FAILURE; From a7f1d5e6a4c8010db6fd251750a156cc2a34f904 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Wed, 23 Apr 2014 14:52:54 +0300 Subject: [PATCH 14/17] spelling mistakes --- Detour/Include/DetourNode.h | 2 +- DetourCrowd/Source/DetourCrowd.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index 9094ff2..17abf67 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/adjacent. Found using raycast. + DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is nmnot connected/adjacent. Found using raycast. }; typedef unsigned short dtNodeIndex; diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp index cf7a027..6d15022 100644 --- a/DetourCrowd/Source/DetourCrowd.cpp +++ b/DetourCrowd/Source/DetourCrowd.cpp @@ -689,7 +689,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/) dtPolyRef reqPath[MAX_RES]; // The path to the request location int reqPathCount = 0; - // Quick seach towards the goal. + // Quick search towards the goal. static const int MAX_ITER = 20; m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter); m_navquery->updateSlicedFindPath(MAX_ITER, 0); From 27ffa5b4d377f1f730c00a7d553213fe0c73590a Mon Sep 17 00:00:00 2001 From: axelrodR Date: Wed, 23 Apr 2014 16:24:36 +0300 Subject: [PATCH 15/17] fixes following mikko's comments from 11/4/14 --- Detour/Include/DetourNavMesh.h | 2 +- Detour/Include/DetourNavMeshQuery.h | 10 ++++----- Detour/Include/DetourNode.h | 9 +++++--- Detour/Source/DetourNavMeshQuery.cpp | 32 +++++++++++++++++----------- 4 files changed, 32 insertions(+), 21 deletions(-) diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index ae8cec9..1060845 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -134,7 +134,7 @@ enum dtRaycastOptions /// Limit raycasting during any angle pahfinding /// The limit is given as a multiple of the character radius -static const float RAY_CAST_LIMIT_PROPORTIONS = 50.0f; +static const float DT_RAY_CAST_LIMIT_PROPORTIONS = 50.0f; /// Flags representing the type of a navigation mesh polygon. enum dtPolyTypes diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 427e4a9..af8489a 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -120,7 +120,7 @@ public: /// Provides information about raycast hit /// filled by dtNavMeshQuery::raycast /// @ingroup detour -struct RaycastHit +struct dtRaycastHit { /// The hit parameter. (FLT_MAX if no wall hit.) float t; @@ -128,7 +128,7 @@ struct RaycastHit /// hitNormal The normal of the nearest wall hit. [(x, y, z)] float hitNormal[3]; - /// The reference ids of the visited polygons. [opt] + /// Pointer to an array of reference ids of the visited polygons. [opt] dtPolyRef* path; /// The number of visited polygons. [opt] @@ -211,7 +211,7 @@ public: /// @returns The status flags for the query. dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options=0/*DT_FINDPATH_ANY_ANGLE*/); + const dtQueryFilter* filter, const unsigned int options=0); /// Updates an in-progress sliced path query. /// @param[in] maxIter The maximum number of iterations to perform. @@ -361,12 +361,12 @@ public: /// @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[out] hit Pointer to a raycast hit structure which will be filled by the results. /// @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; + dtRaycastHit* hit, dtPolyRef prevRef=0) const; /// Finds the distance from the specified position to the nearest polygon wall. diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index 17abf67..9054748 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 nmnot connected/adjacent. Found using raycast. + DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast. }; typedef unsigned short dtNodeIndex; @@ -37,12 +37,15 @@ struct dtNode float cost; ///< Cost from previous node to current node. float total; ///< Cost up to the node. unsigned int pidx : 24; ///< Index to parent node. - unsigned int state : 2; ///< extra state information. A polyRef can have multiple nodes with different extra info. - unsigned int flags : 3; ///< Node flags 0/open/closed. + unsigned int state : 2; ///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE + unsigned int flags : 3; ///< Node flags. A combination of dtNodeFlags. dtPolyRef id; ///< Polygon ref the node corresponds to. }; +static const int DT_MAX_STATES_PER_NODE = 4; // number of extra states per node. See dtNode::state + + class dtNodePool { diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index cbec28a..73fdf67 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1159,7 +1159,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef dtVcopy(m_query.endPos, endPos); m_query.filter = filter; m_query.options = options; - m_query.raycastLimitSqr = 1E37; + m_query.raycastLimitSqr = FLT_MAX; if (!startRef || !endRef) return DT_FAILURE | DT_INVALID_PARAM; @@ -1175,7 +1175,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef // so it is enough to compute it from the first tile. const dtMeshTile* tile = m_nav->getTileByRef(startRef); float agentRadius = tile->header->walkableRadius; - m_query.raycastLimitSqr = dtSqr(agentRadius * RAY_CAST_LIMIT_PROPORTIONS); + m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS); } if (startRef == endRef) @@ -1215,7 +1215,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) return DT_FAILURE; } - RaycastHit rayHit; + dtRaycastHit rayHit; rayHit.maxPath = 0; int iter = 0; @@ -1332,15 +1332,23 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) float heuristic = 0; // raycast parent - rayHit.t = 0; + bool foundShortCut = false; + rayHit.pathCost = rayHit.t = 0; if (tryLOS) { raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef); - cost = parentNode->cost + rayHit.pathCost; + foundShortCut = rayHit.t >= 1.0f; } - if (rayHit.t < 1.0f) // hit + // update move cost + if (foundShortCut) { + // shortcut found using raycast. Using shorter cost instead + cost = parentNode->cost + rayHit.pathCost; + } + else + { + // No shortcut found. const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, @@ -1374,12 +1382,12 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) continue; // Add or update the node. - neighbourNode->pidx = rayHit.t < 1.0f ? m_nodePool->getNodeIdx(bestNode) : bestNode->pidx; + neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); neighbourNode->cost = cost; neighbourNode->total = total; - if (rayHit.t >= 1.0f) + if (foundShortCut) neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); if (neighbourNode->flags & DT_NODE_OPEN) @@ -2301,7 +2309,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons const dtQueryFilter* filter, float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const { - RaycastHit hit; + dtRaycastHit hit; hit.path = path; hit.maxPath = maxPath; @@ -2357,7 +2365,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons /// dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, const unsigned int options, - RaycastHit* hit, dtPolyRef prevRef) const + dtRaycastHit* hit, dtPolyRef prevRef) const { dtAssert(m_nav); @@ -3524,8 +3532,8 @@ bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const { if (!m_nodePool) return false; - dtNode* nodes[4]; - int n= m_nodePool->findNodes(ref, nodes, 4); + dtNode* nodes[DT_MAX_STATES_PER_NODE]; + int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); for (int i=0; i Date: Wed, 28 May 2014 00:18:01 +0300 Subject: [PATCH 16/17] addressed Mikko's comment about isValidPolyRef returning a boolean and not dtStatus. --- Detour/Source/DetourNavMeshQuery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 73fdf67..64e8383 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -1268,7 +1268,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters) if (parentRef) { bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); - if (invalidParent || (grandpaRef && dtStatusFailed(m_nav->isValidPolyRef(grandpaRef))) ) + if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) ) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; From 90a3c7a8aefa230972c43f5a9e2ce7c7d16b5121 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Thu, 19 Jun 2014 17:36:37 +0300 Subject: [PATCH 17/17] code style correction following rafzi's comments --- Detour/Include/DetourNavMeshQuery.h | 4 ++-- Detour/Include/DetourNode.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 453ce14..c7b360d 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -215,7 +215,7 @@ public: /// @returns The status flags for the query. dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, const unsigned int options=0); + const dtQueryFilter* filter, const unsigned int options = 0); /// Updates an in-progress sliced path query. /// @param[in] maxIter The maximum number of iterations to perform. @@ -370,7 +370,7 @@ public: /// @returns The status flags for the query. dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, const unsigned int options, - dtRaycastHit* hit, dtPolyRef prevRef=0) const; + dtRaycastHit* hit, dtPolyRef prevRef = 0) const; /// Finds the distance from the specified position to the nearest polygon wall. diff --git a/Detour/Include/DetourNode.h b/Detour/Include/DetourNode.h index 9054748..6fefdc8 100644 --- a/Detour/Include/DetourNode.h +++ b/Detour/Include/DetourNode.h @@ -92,7 +92,7 @@ public: inline int getHashSize() const { return m_hashSize; } inline dtNodeIndex getFirst(int bucket) const { return m_first[bucket]; } inline dtNodeIndex getNext(int i) const { return m_next[i]; } - inline int getNodeCount() const { return m_nodeCount;} + inline int getNodeCount() const { return m_nodeCount; } private: