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; }