diff options
Diffstat (limited to 'dep/recastnavigation/Detour/DetourNavMeshQuery.cpp')
| -rw-r--r-- | dep/recastnavigation/Detour/DetourNavMeshQuery.cpp | 2564 |
1 files changed, 2564 insertions, 0 deletions
diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp new file mode 100644 index 00000000000..6a6eb94b6d4 --- /dev/null +++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp @@ -0,0 +1,2564 @@ +// +// 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->area]; +} +#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_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]; + 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; + 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; +} |