diff options
Diffstat (limited to 'deps/recastnavigation/Detour/DetourNavMeshQuery.cpp')
| -rw-r--r-- | deps/recastnavigation/Detour/DetourNavMeshQuery.cpp | 2578 |
1 files changed, 0 insertions, 2578 deletions
diff --git a/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp b/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp deleted file mode 100644 index 33ee87fb95..0000000000 --- a/deps/recastnavigation/Detour/DetourNavMeshQuery.cpp +++ /dev/null @@ -1,2578 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#include <math.h> -#include <float.h> -#include <string.h> -#include "DetourNavMeshQuery.h" -#include "DetourNavMesh.h" -#include "DetourNode.h" -#include "DetourCommon.h" -#include "DetourAlloc.h" -#include "DetourAssert.h" -#include <new> - - -dtQueryFilter::dtQueryFilter() : - m_includeFlags(0xffff), - m_excludeFlags(0) -{ - for (int i = 0; i < DT_MAX_AREAS; ++i) - m_areaCost[i] = 1.0f; -} - -#ifdef DT_VIRTUAL_QUERYFILTER -bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, - const dtMeshTile* /*tile*/, - const dtPoly* poly) const -{ - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; -} - -float dtQueryFilter::getCost(const float* pa, const float* pb, - const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, - const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, - const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const -{ - return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; -} -#else -inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/, - const dtMeshTile* /*tile*/, - const dtPoly* poly) const -{ - return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0; -} - -inline float dtQueryFilter::getCost(const float* pa, const float* pb, - const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/, - const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly, - const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const -{ - return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()]; -} -#endif - -static const float H_SCALE = 2.0f; // Search heuristic scale. - - -dtNavMeshQuery* dtAllocNavMeshQuery() -{ - void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM); - if (!mem) return 0; - return new(mem) dtNavMeshQuery; -} - -void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh) -{ - if (!navmesh) return; - navmesh->~dtNavMeshQuery(); - dtFree(navmesh); -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtNavMeshQuery::dtNavMeshQuery() : - m_nav(0), - m_tinyNodePool(0), - m_nodePool(0), - m_openList(0) -{ - memset(&m_query, 0, sizeof(dtQueryData)); -} - -dtNavMeshQuery::~dtNavMeshQuery() -{ - if (m_tinyNodePool) - m_tinyNodePool->~dtNodePool(); - if (m_nodePool) - m_nodePool->~dtNodePool(); - if (m_openList) - m_openList->~dtNodeQueue(); - dtFree(m_tinyNodePool); - dtFree(m_nodePool); - dtFree(m_openList); -} - -dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) -{ - m_nav = nav; - - if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes) - { - if (m_nodePool) - { - m_nodePool->~dtNodePool(); - dtFree(m_nodePool); - m_nodePool = 0; - } - m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4)); - if (!m_nodePool) - return DT_FAILURE_OUT_OF_MEMORY; - } - else - { - m_nodePool->clear(); - } - - if (!m_tinyNodePool) - { - m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32); - if (!m_tinyNodePool) - return DT_FAILURE_OUT_OF_MEMORY; - } - else - { - m_tinyNodePool->clear(); - } - - // TODO: check the open list size too. - if (!m_openList || m_openList->getCapacity() < maxNodes) - { - if (m_openList) - { - m_openList->~dtNodeQueue(); - dtFree(m_openList); - m_openList = 0; - } - m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes); - if (!m_openList) - return DT_FAILURE_OUT_OF_MEMORY; - } - else - { - m_openList->clear(); - } - - return DT_SUCCESS; -} - -////////////////////////////////////////////////////////////////////////////////////////// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const -{ - dtAssert(m_nav); - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - if (!tile) return DT_FAILURE; - - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - return DT_FAILURE; - - if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS) - return DT_FAILURE; - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, - const float* pos, float* closest) const -{ - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - - // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh. - // Test and enable. -/* - // Clamp point to be inside the polygon. - float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; - const int nv = poly->vertCount; - for (int i = 0; i < nv; ++i) - dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); - - dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = FLT_MAX; - int imin = -1; - for (int i = 0; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - } - - // Find height at the location. - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - closest[1] = h; - break; - } - } -*/ - float closestDistSqr = FLT_MAX; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - - if (d < closestDistSqr) - { - dtVcopy(closest, pt); - closestDistSqr = d; - } - } - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const -{ - dtAssert(m_nav); - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - - // Collect vertices. - float verts[DT_VERTS_PER_POLYGON*3]; - float edged[DT_VERTS_PER_POLYGON]; - float edget[DT_VERTS_PER_POLYGON]; - int nv = 0; - for (int i = 0; i < (int)poly->vertCount; ++i) - { - dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]); - nv++; - } - - bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); - if (inside) - { - // Point is inside the polygon, return the point. - dtVcopy(closest, pos); - } - else - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = FLT_MAX; - int imin = -1; - for (int i = 0; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - const float* va = &verts[imin*3]; - const float* vb = &verts[((imin+1)%nv)*3]; - dtVlerp(closest, va, vb, edget[imin]); - } - - return DT_SUCCESS; -} - - -dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const -{ - dtAssert(m_nav); - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - - if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - const float* v0 = &tile->verts[poly->verts[0]*3]; - const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist(pos, v0); - const float d1 = dtVdist(pos, v1); - const float u = d0 / (d0+d1); - if (height) - *height = v0[1] + (v1[1] - v0[1]) * u; - return DT_SUCCESS; - } - else - { - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - float h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - if (height) - *height = h; - return DT_SUCCESS; - } - } - } - - return DT_FAILURE; -} - -dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* nearestRef, float* nearestPt) const -{ - dtAssert(m_nav); - - *nearestRef = 0; - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = 0; - if (queryPolygons(center, extents, filter, polys, &polyCount, 128) != DT_SUCCESS) - return DT_FAILURE; - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS) - continue; - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - if (nearestRef) - *nearestRef = nearest; - - return DT_SUCCESS; -} - -dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const -{ - dtAssert(m_nav); - - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Get nearby polygons from proximity grid. - dtPolyRef polys[128]; - int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128); - - // Find nearest polygon amongst the nearby polygons. - dtPolyRef nearest = 0; - float nearestDistanceSqr = FLT_MAX; - for (int i = 0; i < polyCount; ++i) - { - dtPolyRef ref = polys[i]; - const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS) - continue; - - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - -int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, - const dtQueryFilter* filter, - dtPolyRef* polys, const int maxPolys) const -{ - dtAssert(m_nav); - - if (tile->bvTree) - { - const dtBVNode* node = &tile->bvTree[0]; - const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; - const float* tbmin = tile->header->bmin; - const float* tbmax = tile->header->bmax; - const float qfac = tile->header->bvQuantFactor; - - // Calculate quantized box - unsigned short bmin[3], bmax[3]; - // dtClamp query box to world box. - float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; - float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; - float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; - float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; - float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; - float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; - // Quantize - bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; - bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; - bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; - bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; - bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; - bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; - - // Traverse tree - const dtPolyRef base = m_nav->getPolyRefBase(tile); - int n = 0; - while (node < end) - { - const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); - const bool isLeafNode = node->i >= 0; - - if (isLeafNode && overlap) - { - dtPolyRef ref = base | (dtPolyRef)node->i; - if (filter->passFilter(ref, tile, &tile->polys[node->i])) - { - if (n < maxPolys) - polys[n++] = ref; - } - } - - if (overlap || isLeafNode) - node++; - else - { - const int escapeIndex = -node->i; - node += escapeIndex; - } - } - - return n; - } - else - { - float bmin[3], bmax[3]; - int n = 0; - const dtPolyRef base = m_nav->getPolyRefBase(tile); - for (int i = 0; i < tile->header->polyCount; ++i) - { - // Calc polygon bounds. - dtPoly* p = &tile->polys[i]; - // Do not return off-mesh connection polygons. - if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - const float* v = &tile->verts[p->verts[0]*3]; - dtVcopy(bmin, v); - dtVcopy(bmax, v); - for (int j = 1; j < p->vertCount; ++j) - { - v = &tile->verts[p->verts[j]*3]; - dtVmin(bmin, v); - dtVmax(bmax, v); - } - if (dtOverlapBounds(qmin,qmax, bmin,bmax)) - { - const dtPolyRef ref = base | (dtPolyRef)i; - if (filter->passFilter(ref, tile, p)) - { - if (n < maxPolys) - polys[n++] = ref; - } - } - } - return n; - } -} - -dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents, - const dtQueryFilter* filter, - dtPolyRef* polys, int* polyCount, const int maxPolys) const -{ - dtAssert(m_nav); - - float bmin[3], bmax[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); - - // Find tiles the query touches. - int minx, miny, maxx, maxy; - m_nav->calcTileLoc(bmin, &minx, &miny); - m_nav->calcTileLoc(bmax, &maxx, &maxy); - - int n = 0; - - /// pussywizard: additional checks as in PathGenerator::HaveTile - if (minx < 0) minx = 0; if (miny < 0) miny = 0; // min can be negative because we subtract extents (few lines above) - if (maxx < 0 || maxy < 0 || maxx-minx >= 64 /*MAX_NUMBER_OF_GRIDS*/ || maxy-miny >= 64) // max should never be negative - { - *polyCount = n; - return DT_SUCCESS; - } - - for (int y = miny; y <= maxy; ++y) - { - for (int x = minx; x <= maxx; ++x) - { - const dtMeshTile* tile = m_nav->getTileAt(x,y); - if (!tile) continue; - n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n); - if (n >= maxPolys) - { - *polyCount = n; - return DT_SUCCESS; - } - } - } - *polyCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter, - dtPolyRef* path, int* pathCount, const int maxPath) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - *pathCount = 0; - - if (!startRef || !endRef) - return DT_FAILURE; - - if (!maxPath) - return DT_FAILURE; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) - return DT_FAILURE; - - if (startRef == endRef) - { - path[0] = startRef; - *pathCount = 1; - return DT_SUCCESS; - } - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, startPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = dtVdist(startPos, endPos) * H_SCALE; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - dtNode* lastBestNode = startNode; - float lastBestNodeCost = startNode->total; - - while (!m_openList->empty()) - { - // Remove node from open list and put it in closed list. - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Reached the goal, stop searching. - if (bestNode->id == endRef) - { - lastBestNode = bestNode; - break; - } - - // Get current poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - dtPolyRef neighbourRef = bestTile->links[i].ref; - - // Skip invalid ids and do not expand back to where we came from. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Get neighbour poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - // If the node is visited the first time, calculate node position. - if (neighbourNode->flags == 0) - { - getEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, - neighbourNode->pos); - } - - // Calculate cost and heuristic. - float cost = 0; - float heuristic = 0; - - // 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); - const float endCost = filter->getCost(neighbourNode->pos, endPos, - bestRef, bestTile, bestPoly, - neighbourRef, neighbourTile, neighbourPoly, - 0, 0, 0); - - 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; - } - - const float total = cost + heuristic; - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - // The node is already visited and process, and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total) - continue; - - // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->cost = cost; - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - // Already in open, update node location. - m_openList->modify(neighbourNode); - } - else - { - // Put the node in open list. - neighbourNode->flags |= DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - - // Update nearest node to target so far. - if (heuristic < lastBestNodeCost) - { - lastBestNodeCost = heuristic; - lastBestNode = neighbourNode; - } - } - } - - // Reverse the path. - dtNode* prev = 0; - dtNode* node = lastBestNode; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - int n = 0; - do - { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxPath); - - *pathCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, - const float* startPos, const float* endPos, - const dtQueryFilter* filter) -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - // Init path state. - memset(&m_query, 0, sizeof(dtQueryData)); - m_query.status = DT_FAILURE; - m_query.startRef = startRef; - m_query.endRef = endRef; - dtVcopy(m_query.startPos, startPos); - dtVcopy(m_query.endPos, endPos); - m_query.filter = filter; - - if (!startRef || !endRef) - return DT_FAILURE; - - // Validate input - if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef)) - return DT_FAILURE; - - if (startRef == endRef) - { - m_query.status = DT_SUCCESS; - return DT_SUCCESS; - } - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, startPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = dtVdist(startPos, endPos) * H_SCALE; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - m_query.status = DT_IN_PROGRESS; - m_query.lastBestNode = startNode; - m_query.lastBestNodeCost = startNode->total; - - return m_query.status; -} - -dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter) -{ - if (m_query.status!= DT_IN_PROGRESS) - return m_query.status; - - // Make sure the request is still valid. - if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef)) - { - m_query.status = DT_FAILURE; - return DT_FAILURE; - } - - int iter = 0; - while (iter < maxIter && !m_openList->empty()) - { - iter++; - - // Remove node from open list and put it in closed list. - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Reached the goal, stop searching. - if (bestNode->id == m_query.endRef) - { - m_query.lastBestNode = bestNode; - m_query.status = DT_SUCCESS; - return m_query.status; - } - - // Get current poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - if (m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly) != DT_SUCCESS) - { - // The polygon has disappeared during the sliced query, fail. - m_query.status = DT_FAILURE; - return m_query.status; - } - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - { - if (m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly) != DT_SUCCESS) - { - // The polygon has disappeared during the sliced query, fail. - m_query.status = DT_FAILURE; - return m_query.status; - } - } - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - dtPolyRef neighbourRef = bestTile->links[i].ref; - - // Skip invalid ids and do not expand back to where we came from. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Get neighbour poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - // If the node is visited the first time, calculate node position. - if (neighbourNode->flags == 0) - { - getEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, - neighbourNode->pos); - } - - // Calculate cost and heuristic. - float cost = 0; - float heuristic = 0; - - // 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; - 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; - } - - const float total = cost + heuristic; - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - // The node is already visited and process, and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total) - continue; - - // Add or update the node. - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->cost = cost; - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - // Already in open, update node location. - m_openList->modify(neighbourNode); - } - else - { - // Put the node in open list. - neighbourNode->flags |= DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - - // Update nearest node to target so far. - if (heuristic < m_query.lastBestNodeCost) - { - m_query.lastBestNodeCost = heuristic; - m_query.lastBestNode = neighbourNode; - } - } - } - - // Exhausted all nodes, but could not find path. - if (m_openList->empty()) - m_query.status = DT_SUCCESS; - - return m_query.status; -} - -dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath) -{ - *pathCount = 0; - - if (m_query.status != DT_SUCCESS) - { - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - return DT_FAILURE; - } - - int n = 0; - - if (m_query.startRef == m_query.endRef) - { - // Special case: the search starts and ends at same poly. - path[n++] = m_query.startRef; - } - else - { - // Reverse the path. - dtAssert(m_query.lastBestNode); - dtNode* prev = 0; - dtNode* node = m_query.lastBestNode; - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - do - { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxPath); - } - - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - - *pathCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, - dtPolyRef* path, int* pathCount, const int maxPath) -{ - *pathCount = 0; - - if (existingSize == 0) - { - return DT_FAILURE; - } - - if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS) - { - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - return DT_FAILURE; - } - - int n = 0; - - if (m_query.startRef == m_query.endRef) - { - // Special case: the search starts and ends at same poly. - path[n++] = m_query.startRef; - } - else - { - // Find furthest existing node that was visited. - dtNode* prev = 0; - dtNode* node = 0; - for (int i = existingSize-1; i >= 0; --i) - { - node = m_nodePool->findNode(existing[i]); - if (node) - break; - } - - if (!node) - { - return DT_FAILURE; - } - - // Reverse the path. - do - { - dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); - node->pidx = m_nodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store path - node = prev; - do - { - path[n++] = node->id; - node = m_nodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxPath); - } - - // Reset query. - memset(&m_query, 0, sizeof(dtQueryData)); - - *pathCount = n; - - return DT_SUCCESS; -} - - -dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos, - const dtPolyRef* path, const int pathSize, - float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, - int* straightPathCount, const int maxStraightPath) const -{ - dtAssert(m_nav); - - *straightPathCount = 0; - - if (!maxStraightPath) - return DT_FAILURE; - - if (!path[0]) - return DT_FAILURE; - - int n = 0; - - // TODO: Should this be callers responsibility? - float closestStartPos[3]; - if (closestPointOnPolyBoundary(path[0], startPos, closestStartPos) != DT_SUCCESS) - return DT_FAILURE; - - // Add start point. - dtVcopy(&straightPath[n*3], closestStartPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_START; - if (straightPathRefs) - straightPathRefs[n] = path[0]; - n++; - if (n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - - float closestEndPos[3]; - if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; - - if (pathSize > 1) - { - float portalApex[3], portalLeft[3], portalRight[3]; - dtVcopy(portalApex, closestStartPos); - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - int apexIndex = 0; - int leftIndex = 0; - int rightIndex = 0; - - unsigned char leftPolyType = 0; - unsigned char rightPolyType = 0; - - dtPolyRef leftPolyRef = path[0]; - dtPolyRef rightPolyRef = path[0]; - - for (int i = 0; i < pathSize; ++i) - { - float left[3], right[3]; - unsigned char fromType, toType; - - if (i+1 < pathSize) - { - // Next portal. - if (getPortalPoints(path[i], path[i+1], left, right, fromType, toType) != DT_SUCCESS) - { - if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS) - return DT_FAILURE; - - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = 0; - if (straightPathRefs) - straightPathRefs[n] = path[i]; - n++; - - return DT_SUCCESS; - } - - // If starting really close the portal, advance. - if (i == 0) - { - float t; - if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f)) - continue; - } - } - else - { - // End of the path. - dtVcopy(left, closestEndPos); - dtVcopy(right, closestEndPos); - - fromType = toType = DT_POLYTYPE_GROUND; - } - - // Right vertex. - if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f) - { - if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f) - { - dtVcopy(portalRight, right); - rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0; - rightPolyType = toType; - rightIndex = i; - } - else - { - dtVcopy(portalApex, portalLeft); - apexIndex = leftIndex; - - unsigned char flags = 0; - if (!leftPolyRef) - flags = DT_STRAIGHTPATH_END; - else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) - flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; - dtPolyRef ref = leftPolyRef; - - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } - - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - - // Left vertex. - if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f) - { - if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f) - { - dtVcopy(portalLeft, left); - leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0; - leftPolyType = toType; - leftIndex = i; - } - else - { - dtVcopy(portalApex, portalRight); - apexIndex = rightIndex; - - unsigned char flags = 0; - if (!rightPolyRef) - flags = DT_STRAIGHTPATH_END; - else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION) - flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION; - dtPolyRef ref = rightPolyRef; - - if (!dtVequal(&straightPath[(n-1)*3], portalApex)) - { - // Append new vertex. - dtVcopy(&straightPath[n*3], portalApex); - if (straightPathFlags) - straightPathFlags[n] = flags; - if (straightPathRefs) - straightPathRefs[n] = ref; - n++; - // If reached end of path or there is no space to append more vertices, return. - if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath) - { - *straightPathCount = n; - return DT_SUCCESS; - } - } - else - { - // The vertices are equal, update flags and poly. - if (straightPathFlags) - straightPathFlags[n-1] = flags; - if (straightPathRefs) - straightPathRefs[n-1] = ref; - } - - dtVcopy(portalLeft, portalApex); - dtVcopy(portalRight, portalApex); - leftIndex = apexIndex; - rightIndex = apexIndex; - - // Restart - i = apexIndex; - - continue; - } - } - } - } - - // If the point already exists, remove it and add reappend the actual end location. - if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos)) - n--; - - // Add end point. - if (n < maxStraightPath) - { - dtVcopy(&straightPath[n*3], closestEndPos); - if (straightPathFlags) - straightPathFlags[n] = DT_STRAIGHTPATH_END; - if (straightPathRefs) - straightPathRefs[n] = 0; - n++; - } - - *straightPathCount = n; - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos, - const dtQueryFilter* filter, - float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const -{ - dtAssert(m_nav); - dtAssert(m_tinyNodePool); - - *visitedCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - static const int MAX_STACK = 48; - dtNode* stack[MAX_STACK]; - int nstack = 0; - - m_tinyNodePool->clear(); - - dtNode* startNode = m_tinyNodePool->getNode(startRef); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_CLOSED; - stack[nstack++] = startNode; - - float bestPos[3]; - float bestDist = FLT_MAX; - dtNode* bestNode = 0; - dtVcopy(bestPos, startPos); - - // Search constraints - float searchPos[3], searchRadSqr; - dtVlerp(searchPos, startPos, endPos, 0.5f); - searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f); - - float verts[DT_VERTS_PER_POLYGON*3]; - - while (nstack) - { - // Pop front. - dtNode* curNode = stack[0]; - for (int i = 0; i < nstack-1; ++i) - stack[i] = stack[i+1]; - nstack--; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef curRef = curNode->id; - const dtMeshTile* curTile = 0; - const dtPoly* curPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly); - - // Collect vertices. - const int nverts = curPoly->vertCount; - for (int i = 0; i < nverts; ++i) - dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]); - - // If target is inside the poly, stop search. - if (dtPointInPolygon(endPos, verts, nverts)) - { - bestNode = curNode; - dtVcopy(bestPos, endPos); - break; - } - - // Find wall edges and find nearest point inside the walls. - for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++) - { - // Find links to neighbours. - static const int MAX_NEIS = 8; - int nneis = 0; - dtPolyRef neis[MAX_NEIS]; - - if (curPoly->neis[j] & DT_EXT_LINK) - { - // Tile border. - for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next) - { - const dtLink* link = &curTile->links[k]; - if (link->edge == j) - { - if (link->ref != 0) - { - const dtMeshTile* neiTile = 0; - const dtPoly* neiPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); - if (filter->passFilter(link->ref, neiTile, neiPoly)) - { - if (nneis < MAX_NEIS) - neis[nneis++] = link->ref; - } - } - } - } - } - else if (curPoly->neis[j]) - { - const unsigned int idx = (unsigned int)(curPoly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx; - if (filter->passFilter(ref, curTile, &curTile->polys[idx])) - { - // Internal edge, encode id. - neis[nneis++] = ref; - } - } - - if (!nneis) - { - // Wall edge, calc distance. - const float* vj = &verts[j*3]; - const float* vi = &verts[i*3]; - float tseg; - const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg); - if (distSqr < bestDist) - { - // Update nearest distance. - dtVlerp(bestPos, vj,vi, tseg); - bestDist = distSqr; - bestNode = curNode; - } - } - else - { - for (int k = 0; k < nneis; ++k) - { - // Skip if no node can be allocated. - dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]); - if (!neighbourNode) - continue; - // Skip if already visited. - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Skip the link if it is too far from search constraint. - // TODO: Maybe should use getPortalPoints(), but this one is way faster. - const float* vj = &verts[j*3]; - const float* vi = &verts[i*3]; - float tseg; - float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg); - if (distSqr > searchRadSqr) - continue; - - // Mark as the node as visited and push to queue. - if (nstack < MAX_STACK) - { - neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode); - neighbourNode->flags |= DT_NODE_CLOSED; - stack[nstack++] = neighbourNode; - } - } - } - } - } - - int n = 0; - if (bestNode) - { - // Reverse the path. - dtNode* prev = 0; - dtNode* node = bestNode; - do - { - dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx); - node->pidx = m_tinyNodePool->getNodeIdx(prev); - prev = node; - node = next; - } - while (node); - - // Store result - node = prev; - do - { - visited[n++] = node->id; - node = m_tinyNodePool->getNodeAtIdx(node->pidx); - } - while (node && n < maxVisitedSize); - } - - dtVcopy(resultPos, bestPos); - - *visitedCount = n; - - return DT_SUCCESS; -} - - -dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, - unsigned char& fromType, unsigned char& toType) const -{ - dtAssert(m_nav); - - const dtMeshTile* fromTile = 0; - const dtPoly* fromPoly = 0; - if (m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly) != DT_SUCCESS) - return DT_FAILURE; - fromType = fromPoly->getType(); - - const dtMeshTile* toTile = 0; - const dtPoly* toPoly = 0; - if (m_nav->getTileAndPolyByRef(to, &toTile, &toPoly) != DT_SUCCESS) - return DT_FAILURE; - toType = toPoly->getType(); - - return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right); -} - -// Returns portal points between two polygons. -dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* left, float* right) const -{ - // Find the link that points to the 'to' polygon. - const dtLink* link = 0; - for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) - { - if (fromTile->links[i].ref == to) - { - link = &fromTile->links[i]; - break; - } - } - if (!link) - return DT_FAILURE; - - // Handle off-mesh connections. - if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - // Find link that points to first vertex. - for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next) - { - if (fromTile->links[i].ref == to) - { - const int v = fromTile->links[i].edge; - dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]); - dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]); - return DT_SUCCESS; - } - } - return DT_FAILURE; - } - - if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - { - for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next) - { - if (toTile->links[i].ref == from) - { - const int v = toTile->links[i].edge; - dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]); - dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]); - return DT_SUCCESS; - } - } - return DT_FAILURE; - } - - // Find portal vertices. - const int v0 = fromPoly->verts[link->edge]; - const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount]; - dtVcopy(left, &fromTile->verts[v0*3]); - dtVcopy(right, &fromTile->verts[v1*3]); - - // If the link is at tile boundary, dtClamp the vertices to - // the link width. - if (link->side != 0xff) - { - // Unpack portal limits. - if (link->bmin != 0 || link->bmax != 255) - { - const float s = 1.0f/255.0f; - const float tmin = link->bmin*s; - const float tmax = link->bmax*s; - dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin); - dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax); - } - } - - return DT_SUCCESS; -} - -// Returns edge mid point between two polygons. -dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const -{ - float left[3], right[3]; - unsigned char fromType, toType; - if (!getPortalPoints(from, to, left,right, fromType, toType)) return DT_FAILURE; - mid[0] = (left[0]+right[0])*0.5f; - mid[1] = (left[1]+right[1])*0.5f; - mid[2] = (left[2]+right[2])*0.5f; - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile, - dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile, - float* mid) const -{ - float left[3], right[3]; - if (getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right) != DT_SUCCESS) - return DT_FAILURE; - mid[0] = (left[0]+right[0])*0.5f; - mid[1] = (left[1]+right[1])*0.5f; - mid[2] = (left[2]+right[2])*0.5f; - return DT_SUCCESS; -} - -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 -{ - dtAssert(m_nav); - - *t = 0; - if (pathCount) - *pathCount = 0; - - // Validate input - if (!startRef || !m_nav->isValidPolyRef(startRef)) - return DT_FAILURE; - - dtPolyRef curRef = startRef; - float verts[DT_VERTS_PER_POLYGON*3]; - int n = 0; - - hitNormal[0] = 0; - hitNormal[1] = 0; - hitNormal[2] = 0; - - 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++; - } - - float tmin, tmax; - int segMin, segMax; - 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; - return DT_SUCCESS; - } - // Keep track of furthest t so far. - if (tmax > *t) - *t = tmax; - - // Store visited polygons. - if (n < maxPath) - path[n++] = curRef; - - // Ray end is completely inside the polygon. - if (segMax == -1) - { - *t = FLT_MAX; - if (pathCount) - *pathCount = n; - return DT_SUCCESS; - } - - // Follow neighbours. - dtPolyRef nextRef = 0; - - for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next) - { - const dtLink* link = &tile->links[i]; - - // Find link which contains this edge. - if ((int)link->edge != segMax) - continue; - - // Get pointer to the next polygon. - const dtMeshTile* nextTile = 0; - const dtPoly* nextPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly); - - // Skip off-mesh connections. - if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Skip links based on filter. - if (!filter->passFilter(link->ref, nextTile, nextPoly)) - continue; - - // If the link is internal, just return the ref. - if (link->side == 0xff) - { - nextRef = link->ref; - break; - } - - // If the link is at tile boundary, - - // Check if the link spans the whole edge, and accept. - if (link->bmin == 0 && link->bmax == 255) - { - nextRef = link->ref; - break; - } - - // Check for partial edge links. - const int v0 = poly->verts[link->edge]; - const int v1 = poly->verts[(link->edge+1) % poly->vertCount]; - const float* left = &tile->verts[v0*3]; - const float* right = &tile->verts[v1*3]; - - // Check that the intersection lies inside the link portal. - if (link->side == 0 || link->side == 4) - { - // Calculate link size. - const float s = 1.0f/255.0f; - float lmin = left[2] + (right[2] - left[2])*(link->bmin*s); - float lmax = left[2] + (right[2] - left[2])*(link->bmax*s); - if (lmin > lmax) dtSwap(lmin, lmax); - - // Find Z intersection. - float z = startPos[2] + (endPos[2]-startPos[2])*tmax; - if (z >= lmin && z <= lmax) - { - nextRef = link->ref; - break; - } - } - else if (link->side == 2 || link->side == 6) - { - // Calculate link size. - const float s = 1.0f/255.0f; - float lmin = left[0] + (right[0] - left[0])*(link->bmin*s); - float lmax = left[0] + (right[0] - left[0])*(link->bmax*s); - if (lmin > lmax) dtSwap(lmin, lmax); - - // Find X intersection. - float x = startPos[0] + (endPos[0]-startPos[0])*tmax; - if (x >= lmin && x <= lmax) - { - nextRef = link->ref; - break; - } - } - } - - if (!nextRef) - { - // No neighbour, we hit a wall. - - // Calculate hit normal. - const int a = segMax; - const int b = segMax+1 < nv ? segMax+1 : 0; - const float* va = &verts[a*3]; - 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); - - if (pathCount) - *pathCount = n; - return DT_SUCCESS; - } - - // No hit, advance to neighbour polygon. - curRef = nextRef; - } - - if (pathCount) - *pathCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - *resultCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, centerPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - int n = 0; - if (n < maxResult) - { - if (resultRef) - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - if (resultCost) - resultCost[n] = 0; - ++n; - } - - const float radiusSqr = dtSqr(radius); - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - const dtLink* link = &bestTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours and do not follow back to parent. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Expand to neighbour - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Do not advance if the polygon is excluded by the filter. - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - // Find edge and calc distance to the edge. - float va[3], vb[3]; - if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) - continue; - - // If the circle is not touching the next polygon, skip it. - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); - if (distSqr > radiusSqr) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - dtVlerp(neighbourNode->pos, va, vb, 0.5f); - - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - m_openList->modify(neighbourNode); - } - else - { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = neighbourNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id; - if (resultCost) - resultCost[n] = neighbourNode->total; - ++n; - } - neighbourNode->flags = DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - } - } - - *resultCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost, - int* resultCount, const int maxResult) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - *resultCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - m_nodePool->clear(); - m_openList->clear(); - - float centerPos[3] = {0,0,0}; - for (int i = 0; i < nverts; ++i) - dtVadd(centerPos,centerPos,&verts[i*3]); - dtVscale(centerPos,centerPos,1.0f/nverts); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, centerPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - int n = 0; - if (n < maxResult) - { - if (resultRef) - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - if (resultCost) - resultCost[n] = 0; - ++n; - } - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - const dtLink* link = &bestTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours and do not follow back to parent. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Expand to neighbour - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Do not advance if the polygon is excluded by the filter. - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - // Find edge and calc distance to the edge. - float va[3], vb[3]; - if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) - continue; - - // If the poly is not touching the edge to the next polygon, skip the connection it. - float tmin, tmax; - int segMin, segMax; - if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax)) - continue; - if (tmin > 1.0f || tmax < 0.0f) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - dtVlerp(neighbourNode->pos, va, vb, 0.5f); - - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - m_openList->modify(neighbourNode); - } - else - { - if (n < maxResult) - { - if (resultRef) - resultRef[n] = neighbourNode->id; - if (resultParent) - resultParent[n] = m_nodePool->getNodeAtIdx(neighbourNode->pidx)->id; - if (resultCost) - resultCost[n] = neighbourNode->total; - ++n; - } - neighbourNode->flags = DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - } - } - - *resultCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius, - const dtQueryFilter* filter, - dtPolyRef* resultRef, dtPolyRef* resultParent, - int* resultCount, const int maxResult) const -{ - dtAssert(m_nav); - dtAssert(m_tinyNodePool); - - *resultCount = 0; - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - static const int MAX_STACK = 48; - dtNode* stack[MAX_STACK]; - int nstack = 0; - - m_tinyNodePool->clear(); - - dtNode* startNode = m_tinyNodePool->getNode(startRef); - startNode->pidx = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_CLOSED; - stack[nstack++] = startNode; - - const float radiusSqr = dtSqr(radius); - - float pa[DT_VERTS_PER_POLYGON*3]; - float pb[DT_VERTS_PER_POLYGON*3]; - - int n = 0; - if (n < maxResult) - { - resultRef[n] = startNode->id; - if (resultParent) - resultParent[n] = 0; - ++n; - } - - while (nstack) - { - // Pop front. - dtNode* curNode = stack[0]; - for (int i = 0; i < nstack-1; ++i) - stack[i] = stack[i+1]; - nstack--; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef curRef = curNode->id; - const dtMeshTile* curTile = 0; - const dtPoly* curPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly); - - for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next) - { - const dtLink* link = &curTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours. - if (!neighbourRef) - continue; - - // Skip if cannot alloca more nodes. - dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - // Skip visited. - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Expand to neighbour - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Skip off-mesh connections. - if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Do not advance if the polygon is excluded by the filter. - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - // Find edge and calc distance to the edge. - float va[3], vb[3]; - if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) - continue; - - // If the circle is not touching the next polygon, skip it. - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); - if (distSqr > radiusSqr) - continue; - - // Mark node visited, this is done before the overlap test so that - // we will not visit the poly again if the test fails. - neighbourNode->flags |= DT_NODE_CLOSED; - neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode); - - // Check that the polygon does not collide with existing polygons. - - // Collect vertices of the neighbour poly. - const int npa = neighbourPoly->vertCount; - for (int k = 0; k < npa; ++k) - dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]); - - bool overlap = false; - for (int j = 0; j < n; ++j) - { - dtPolyRef pastRef = resultRef[j]; - - // Connected polys do not overlap. - bool connected = false; - for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next) - { - if (curTile->links[k].ref == pastRef) - { - connected = true; - break; - } - } - if (connected) - continue; - - // Potentially overlapping. - const dtMeshTile* pastTile = 0; - const dtPoly* pastPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly); - - // Get vertices and test overlap - const int npb = pastPoly->vertCount; - for (int k = 0; k < npb; ++k) - dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]); - - if (dtOverlapPolyPoly2D(pa,npa, pb,npb)) - { - overlap = true; - break; - } - } - if (overlap) - continue; - - // This poly is fine, store and advance to the poly. - if (n < maxResult) - { - resultRef[n] = neighbourRef; - if (resultParent) - resultParent[n] = curRef; - ++n; - } - - if (nstack < MAX_STACK) - { - stack[nstack++] = neighbourNode; - } - } - } - - *resultCount = n; - - return DT_SUCCESS; -} - - -struct dtSegInterval -{ - short tmin, tmax; -}; - -static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, - const short tmin, const short tmax) -{ - if (nints+1 > maxInts) return; - // Find insertion point. - int idx = 0; - while (idx < nints) - { - if (tmax <= ints[idx].tmin) - break; - idx++; - } - // Move current results. - if (nints-idx) - memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx)); - // Store - ints[idx].tmin = tmin; - ints[idx].tmax = tmax; - nints++; -} - -dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, - float* segments, int* segmentCount, const int maxSegments) const -{ - dtAssert(m_nav); - - *segmentCount = 0; - - const dtMeshTile* tile = 0; - const dtPoly* poly = 0; - if (m_nav->getTileAndPolyByRef(ref, &tile, &poly) != DT_SUCCESS) - return DT_FAILURE; - - int n = 0; - static const int MAX_INTERVAL = 16; - dtSegInterval ints[MAX_INTERVAL]; - int nints; - - for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++) - { - // Skip non-solid edges. - nints = 0; - if (poly->neis[j] & DT_EXT_LINK) - { - // Tile border. - for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next) - { - const dtLink* link = &tile->links[k]; - if (link->edge == j) - { - if (link->ref != 0) - { - const dtMeshTile* neiTile = 0; - const dtPoly* neiPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); - if (filter->passFilter(link->ref, neiTile, neiPoly)) - { - insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax); - } - } - } - } - } - else if (poly->neis[j]) - { - // Internal edge - const unsigned int idx = (unsigned int)(poly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx; - if (filter->passFilter(ref, tile, &tile->polys[idx])) - continue; - } - - // Add sentinels - insertInterval(ints, nints, MAX_INTERVAL, -1, 0); - insertInterval(ints, nints, MAX_INTERVAL, 255, 256); - - // Store segment. - const float* vj = &tile->verts[poly->verts[j]*3]; - const float* vi = &tile->verts[poly->verts[i]*3]; - for (int k = 1; k < nints; ++k) - { - // Find the space inbetween the opening areas. - const int imin = ints[k-1].tmax; - const int imax = ints[k].tmin; - if (imin == imax) continue; - if (imin == 0 && imax == 255) - { - if (n < maxSegments) - { - float* seg = &segments[n*6]; - n++; - dtVcopy(seg+0, vj); - dtVcopy(seg+3, vi); - } - } - else - { - const float tmin = imin/255.0f; - const float tmax = imax/255.0f; - if (n < maxSegments) - { - float* seg = &segments[n*6]; - n++; - dtVlerp(seg+0, vj,vi, tmin); - dtVlerp(seg+3, vj,vi, tmax); - } - } - } - } - - *segmentCount = n; - - return DT_SUCCESS; -} - -dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius, - const dtQueryFilter* filter, - float* hitDist, float* hitPos, float* hitNormal) const -{ - dtAssert(m_nav); - dtAssert(m_nodePool); - dtAssert(m_openList); - - // Validate input - if (!startRef) return DT_FAILURE; - if (!m_nav->isValidPolyRef(startRef)) return DT_FAILURE; - - m_nodePool->clear(); - m_openList->clear(); - - dtNode* startNode = m_nodePool->getNode(startRef); - dtVcopy(startNode->pos, centerPos); - startNode->pidx = 0; - startNode->cost = 0; - startNode->total = 0; - startNode->id = startRef; - startNode->flags = DT_NODE_OPEN; - m_openList->push(startNode); - - float radiusSqr = dtSqr(maxRadius); - - while (!m_openList->empty()) - { - dtNode* bestNode = m_openList->pop(); - bestNode->flags &= ~DT_NODE_OPEN; - bestNode->flags |= DT_NODE_CLOSED; - - // Get poly and tile. - // The API input has been cheked already, skip checking internal data. - const dtPolyRef bestRef = bestNode->id; - const dtMeshTile* bestTile = 0; - const dtPoly* bestPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); - - // Get parent poly and tile. - dtPolyRef parentRef = 0; - const dtMeshTile* parentTile = 0; - const dtPoly* parentPoly = 0; - if (bestNode->pidx) - parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; - if (parentRef) - m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); - - // Hit test walls. - for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++) - { - // Skip non-solid edges. - if (bestPoly->neis[j] & DT_EXT_LINK) - { - // Tile border. - bool solid = true; - for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next) - { - const dtLink* link = &bestTile->links[k]; - if (link->edge == j) - { - if (link->ref != 0) - { - const dtMeshTile* neiTile = 0; - const dtPoly* neiPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly); - if (filter->passFilter(link->ref, neiTile, neiPoly)) - solid = false; - } - break; - } - } - if (!solid) continue; - } - else if (bestPoly->neis[j]) - { - // Internal edge - const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1); - const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx; - if (filter->passFilter(ref, bestTile, &bestTile->polys[idx])) - continue; - } - - // Calc distance to the edge. - const float* vj = &bestTile->verts[bestPoly->verts[j]*3]; - const float* vi = &bestTile->verts[bestPoly->verts[i]*3]; - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg); - - // Edge is too far, skip. - if (distSqr > radiusSqr) - continue; - - // Hit wall, update radius. - radiusSqr = distSqr; - // Calculate hit pos. - hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg; - hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg; - hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg; - } - - for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) - { - const dtLink* link = &bestTile->links[i]; - dtPolyRef neighbourRef = link->ref; - // Skip invalid neighbours and do not follow back to parent. - if (!neighbourRef || neighbourRef == parentRef) - continue; - - // Expand to neighbour. - const dtMeshTile* neighbourTile = 0; - const dtPoly* neighbourPoly = 0; - m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly); - - // Skip off-mesh connections. - if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) - continue; - - // Calc distance to the edge. - const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3]; - const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3]; - float tseg; - float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); - - // If the circle is not touching the next polygon, skip it. - if (distSqr > radiusSqr) - continue; - - if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly)) - continue; - - dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); - if (!neighbourNode) - continue; - - if (neighbourNode->flags & DT_NODE_CLOSED) - continue; - - // Cost - if (neighbourNode->flags == 0) - { - getEdgeMidPoint(bestRef, bestPoly, bestTile, - neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos); - } - - const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos); - - // The node is already in open list and the new result is worse, skip. - if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total) - continue; - - neighbourNode->id = neighbourRef; - neighbourNode->flags &= ~DT_NODE_CLOSED; - neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); - neighbourNode->total = total; - - if (neighbourNode->flags & DT_NODE_OPEN) - { - m_openList->modify(neighbourNode); - } - else - { - neighbourNode->flags |= DT_NODE_OPEN; - m_openList->push(neighbourNode); - } - } - } - - // Calc hit normal. - dtVsub(hitNormal, centerPos, hitPos); - dtVnormalize(hitNormal); - - *hitDist = sqrtf(radiusSqr); - - return DT_SUCCESS; -} - -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; -} |