// // 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 <float.h> #include <string.h> #include "DetourNavMeshQuery.h" #include "DetourNavMesh.h" #include "DetourNode.h" #include "DetourCommon.h" #include "DetourMath.h" #include "DetourAlloc.h" #include "DetourAssert.h" #include <new> /// @class dtQueryFilter /// /// <b>The Default Implementation</b> /// /// At construction: All area costs default to 1.0. All flags are included /// and none are excluded. /// /// If a polygon has both an include and an exclude flag, it will be excluded. /// /// The way filtering works, a navigation mesh polygon must have at least one flag /// set to ever be considered by a query. So a polygon with no flags will never /// be considered. /// /// Setting the include flags to 0 will result in all polygons being excluded. /// /// <b>Custom Implementations</b> /// /// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class. /// /// Implement a custom query filter by overriding the virtual passFilter() /// and getCost() functions. If this is done, both functions should be as /// fast as possible. Use cached local copies of data rather than accessing /// your own objects where possible. /// /// Custom implementations do not need to adhere to the flags or cost logic /// used by the default implementation. /// /// In order for A* searches to work properly, the cost should be proportional to /// the travel distance. Implementing a cost modifier less than 1.0 is likely /// to lead to problems during pathfinding. /// /// @see dtNavMeshQuery 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 = 0.999f; // 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); } ////////////////////////////////////////////////////////////////////////////////////////// /// @class dtNavMeshQuery /// /// For methods that support undersized buffers, if the buffer is too small /// to hold the entire result set the return status of the method will include /// the #DT_BUFFER_TOO_SMALL flag. /// /// Constant member functions can be used by multiple clients without side /// effects. (E.g. No change to the closed list. No impact on an in-progress /// sliced path query. Etc.) /// /// Walls and portals: A @e wall is a polygon segment that is /// considered impassable. A @e portal is a passable segment between polygons. /// A portal may be treated as a wall based on the dtQueryFilter used for a query. /// /// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery() 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); } /// @par /// /// Must be the first function called after construction, before other /// functions are used. /// /// This function can be used multiple times. dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) { if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1) return DT_FAILURE | DT_INVALID_PARAM; 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 | DT_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 | DT_OUT_OF_MEMORY; } else { m_tinyNodePool->clear(); } 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 | DT_OUT_OF_MEMORY; } else { m_openList->clear(); } return DT_SUCCESS; } dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), dtPolyRef* randomRef, float* randomPt) const { dtAssert(m_nav); if (!filter || !frand || !randomRef || !randomPt) return DT_FAILURE | DT_INVALID_PARAM; // Randomly pick one tile. Assume that all tiles cover roughly the same area. const dtMeshTile* tile = 0; float tsum = 0.0f; for (int i = 0; i < m_nav->getMaxTiles(); i++) { const dtMeshTile* t = m_nav->getTile(i); if (!t || !t->header) continue; // Choose random tile using reservoi sampling. const float area = 1.0f; // Could be tile area too. tsum += area; const float u = frand(); if (u*tsum <= area) tile = t; } if (!tile) return DT_FAILURE; // Randomly pick one polygon weighted by polygon area. const dtPoly* poly = 0; dtPolyRef polyRef = 0; const dtPolyRef base = m_nav->getPolyRefBase(tile); float areaSum = 0.0f; for (int i = 0; i < tile->header->polyCount; ++i) { const dtPoly* p = &tile->polys[i]; // Do not return off-mesh connection polygons. if (p->getType() != DT_POLYTYPE_GROUND) continue; // Must pass filter const dtPolyRef ref = base | (dtPolyRef)i; if (!filter->passFilter(ref, tile, p)) continue; // Calc area of the polygon. float polyArea = 0.0f; for (int j = 2; j < p->vertCount; ++j) { const float* va = &tile->verts[p->verts[0]*3]; const float* vb = &tile->verts[p->verts[j-1]*3]; const float* vc = &tile->verts[p->verts[j]*3]; polyArea += dtTriArea2D(va,vb,vc); } // Choose random polygon weighted by area, using reservoi sampling. areaSum += polyArea; const float u = frand(); if (u*areaSum <= polyArea) { poly = p; polyRef = ref; } } if (!poly) return DT_FAILURE; // Randomly pick point on polygon. const float* v = &tile->verts[poly->verts[0]*3]; float verts[3*DT_VERTS_PER_POLYGON]; float areas[DT_VERTS_PER_POLYGON]; dtVcopy(&verts[0*3],v); for (int j = 1; j < poly->vertCount; ++j) { v = &tile->verts[poly->verts[j]*3]; dtVcopy(&verts[j*3],v); } const float s = frand(); const float t = frand(); float pt[3]; dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); float h = 0.0f; dtStatus status = getPolyHeight(polyRef, pt, &h); if (dtStatusFailed(status)) return status; pt[1] = h; dtVcopy(randomPt, pt); *randomRef = polyRef; return DT_SUCCESS; } dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, const dtQueryFilter* filter, float (*frand)(), dtPolyRef* randomRef, float* randomPt) const { dtAssert(m_nav); dtAssert(m_nodePool); dtAssert(m_openList); // Validate input if (!m_nav->isValidPolyRef(startRef) || !centerPos || !dtVisfinite(centerPos) || maxRadius < 0 || !dtMathIsfinite(maxRadius) || !filter || !frand || !randomRef || !randomPt) { return DT_FAILURE | DT_INVALID_PARAM; } const dtMeshTile* startTile = 0; const dtPoly* startPoly = 0; m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly); if (!filter->passFilter(startRef, startTile, startPoly)) return DT_FAILURE | DT_INVALID_PARAM; 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); dtStatus status = DT_SUCCESS; const float radiusSqr = dtSqr(maxRadius); float areaSum = 0.0f; const dtMeshTile* randomTile = 0; const dtPoly* randomPoly = 0; dtPolyRef randomPolyRef = 0; 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); // Place random locations on on ground. if (bestPoly->getType() == DT_POLYTYPE_GROUND) { // Calc area of the polygon. float polyArea = 0.0f; for (int j = 2; j < bestPoly->vertCount; ++j) { const float* va = &bestTile->verts[bestPoly->verts[0]*3]; const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3]; const float* vc = &bestTile->verts[bestPoly->verts[j]*3]; polyArea += dtTriArea2D(va,vb,vc); } // Choose random polygon weighted by area, using reservoi sampling. areaSum += polyArea; const float u = frand(); if (u*areaSum <= polyArea) { randomTile = bestTile; randomPoly = bestPoly; randomPolyRef = bestRef; } } // 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) { status |= DT_OUT_OF_NODES; 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 = (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); } } } if (!randomPoly) return DT_FAILURE; // Randomly pick point on polygon. const float* v = &randomTile->verts[randomPoly->verts[0]*3]; float verts[3*DT_VERTS_PER_POLYGON]; float areas[DT_VERTS_PER_POLYGON]; dtVcopy(&verts[0*3],v); for (int j = 1; j < randomPoly->vertCount; ++j) { v = &randomTile->verts[randomPoly->verts[j]*3]; dtVcopy(&verts[j*3],v); } const float s = frand(); const float t = frand(); float pt[3]; dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); float h = 0.0f; dtStatus stat = getPolyHeight(randomPolyRef, pt, &h); if (dtStatusFailed(status)) return stat; pt[1] = h; dtVcopy(randomPt, pt); *randomRef = randomPolyRef; return DT_SUCCESS; } ////////////////////////////////////////////////////////////////////////////////////////// /// @par /// /// Uses the detail polygons to find the surface height. (Most accurate.) /// /// @p pos does not have to be within the bounds of the polygon or navigation mesh. /// /// See closestPointOnPolyBoundary() for a limited but faster option. /// dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { dtAssert(m_nav); if (!m_nav->isValidPolyRef(ref) || !pos || !dtVisfinite(pos) || !closest) { return DT_FAILURE | DT_INVALID_PARAM; } m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly); return DT_SUCCESS; } /// @par /// /// Much faster than closestPointOnPoly(). /// /// If the provided position lies within the polygon's xz-bounds (above or below), /// then @p pos and @p closest will be equal. /// /// The height of @p closest will be the polygon boundary. The height detail is not used. /// /// @p pos does not have to be within the bounds of the polybon or the navigation mesh. /// dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const { dtAssert(m_nav); const dtMeshTile* tile = 0; const dtPoly* poly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) return DT_FAILURE | DT_INVALID_PARAM; if (!pos || !dtVisfinite(pos) || !closest) return DT_FAILURE | DT_INVALID_PARAM; // 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 = edged[0]; int imin = 0; for (int i = 1; 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; } /// @par /// /// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds /// of the polygon. /// dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const { dtAssert(m_nav); const dtMeshTile* tile = 0; const dtPoly* poly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) return DT_FAILURE | DT_INVALID_PARAM; if (!pos || !dtVisfinite2D(pos)) return DT_FAILURE | DT_INVALID_PARAM; // We used to return success for offmesh connections, but the // getPolyHeight in DetourNavMesh does not do this, so special // case it here. 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]; float t; dtDistancePtSegSqr2D(pos, v0, v1, t); if (height) *height = v0[1] + (v1[1] - v0[1])*t; return DT_SUCCESS; } return m_nav->getPolyHeight(tile, poly, pos, height) ? DT_SUCCESS : DT_FAILURE | DT_INVALID_PARAM; } class dtFindNearestPolyQuery : public dtPolyQuery { const dtNavMeshQuery* m_query; const float* m_center; float m_nearestDistanceSqr; dtPolyRef m_nearestRef; float m_nearestPoint[3]; public: dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center) : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint() { } dtPolyRef nearestRef() const { return m_nearestRef; } const float* nearestPoint() const { return m_nearestPoint; } void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) { dtIgnoreUnused(polys); for (int i = 0; i < count; ++i) { dtPolyRef ref = refs[i]; float closestPtPoly[3]; float diff[3]; bool posOverPoly = false; float d; m_query->closestPointOnPoly(ref, m_center, closestPtPoly, &posOverPoly); // If a point is directly over a polygon and closer than // climb height, favor that instead of straight line nearest point. dtVsub(diff, m_center, closestPtPoly); if (posOverPoly) { d = dtAbs(diff[1]) - tile->header->walkableClimb; d = d > 0 ? d*d : 0; } else { d = dtVlenSqr(diff); } if (d < m_nearestDistanceSqr) { dtVcopy(m_nearestPoint, closestPtPoly); m_nearestDistanceSqr = d; m_nearestRef = ref; } } } }; /// @par /// /// @note If the search box does not intersect any polygons the search will /// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check /// @p nearestRef before using @p nearestPt. /// dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* nearestRef, float* nearestPt) const { dtAssert(m_nav); if (!nearestRef) return DT_FAILURE | DT_INVALID_PARAM; // queryPolygons below will check rest of params dtFindNearestPolyQuery query(this, center); dtStatus status = queryPolygons(center, halfExtents, filter, &query); if (dtStatusFailed(status)) return status; *nearestRef = query.nearestRef(); // Only override nearestPt if we actually found a poly so the nearest point // is valid. if (nearestPt && *nearestRef) dtVcopy(nearestPt, query.nearestPoint()); return DT_SUCCESS; } void dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, dtPolyQuery* query) const { dtAssert(m_nav); static const int batchSize = 32; dtPolyRef polyRefs[batchSize]; dtPoly* polys[batchSize]; int n = 0; 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); 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])) { polyRefs[n] = ref; polys[n] = &tile->polys[node->i]; if (n == batchSize - 1) { query->process(tile, polys, polyRefs, batchSize); n = 0; } else { n++; } } } if (overlap || isLeafNode) node++; else { const int escapeIndex = -node->i; node += escapeIndex; } } } else { float bmin[3], bmax[3]; const dtPolyRef base = m_nav->getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { dtPoly* p = &tile->polys[i]; // Do not return off-mesh connection polygons. if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; // Must pass filter const dtPolyRef ref = base | (dtPolyRef)i; if (!filter->passFilter(ref, tile, p)) continue; // Calc polygon bounds. 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)) { polyRefs[n] = ref; polys[n] = p; if (n == batchSize - 1) { query->process(tile, polys, polyRefs, batchSize); n = 0; } else { n++; } } } } // Process the last polygons that didn't make a full batch. if (n > 0) query->process(tile, polys, polyRefs, n); } class dtCollectPolysQuery : public dtPolyQuery { dtPolyRef* m_polys; const int m_maxPolys; int m_numCollected; bool m_overflow; public: dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys) : m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false) { } int numCollected() const { return m_numCollected; } bool overflowed() const { return m_overflow; } void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count) { dtIgnoreUnused(tile); dtIgnoreUnused(polys); int numLeft = m_maxPolys - m_numCollected; int toCopy = count; if (toCopy > numLeft) { m_overflow = true; toCopy = numLeft; } memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef)); m_numCollected += toCopy; } }; /// @par /// /// If no polygons are found, the function will return #DT_SUCCESS with a /// @p polyCount of zero. /// /// If @p polys is too small to hold the entire result set, then the array will /// be filled to capacity. The method of choosing which polygons from the /// full set are included in the partial result set is undefined. /// dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyRef* polys, int* polyCount, const int maxPolys) const { if (!polys || !polyCount || maxPolys < 0) return DT_FAILURE | DT_INVALID_PARAM; dtCollectPolysQuery collector(polys, maxPolys); dtStatus status = queryPolygons(center, halfExtents, filter, &collector); if (dtStatusFailed(status)) return status; *polyCount = collector.numCollected(); return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS; } /// @par /// /// The query will be invoked with batches of polygons. Polygons passed /// to the query have bounding boxes that overlap with the center and halfExtents /// passed to this function. The dtPolyQuery::process function is invoked multiple /// times until all overlapping polygons have been processed. /// dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents, const dtQueryFilter* filter, dtPolyQuery* query) const { dtAssert(m_nav); if (!center || !dtVisfinite(center) || !halfExtents || !dtVisfinite(halfExtents) || !filter || !query) { return DT_FAILURE | DT_INVALID_PARAM; } float bmin[3], bmax[3]; dtVsub(bmin, center, halfExtents); dtVadd(bmax, center, halfExtents); // Find tiles the query touches. int minx, miny, maxx, maxy; m_nav->calcTileLoc(bmin, &minx, &miny); m_nav->calcTileLoc(bmax, &maxx, &maxy); static const int MAX_NEIS = 32; const dtMeshTile* neis[MAX_NEIS]; for (int y = miny; y <= maxy; ++y) { for (int x = minx; x <= maxx; ++x) { const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS); for (int j = 0; j < nneis; ++j) { queryPolygonsInTile(neis[j], bmin, bmax, filter, query); } } } return DT_SUCCESS; } /// @par /// /// If the end polygon cannot be reached through the navigation graph, /// the last polygon in the path will be the nearest the end polygon. /// /// If the path array is to small to hold the full result, it will be filled as /// far as possible from the start polygon toward the end polygon. /// /// The start and end positions are used to calculate traversal costs. /// (The y-values impact the result.) /// 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); if (!pathCount) return DT_FAILURE | DT_INVALID_PARAM; *pathCount = 0; // Validate input if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || !startPos || !dtVisfinite(startPos) || !endPos || !dtVisfinite(endPos) || !filter || !path || maxPath <= 0) { return DT_FAILURE | DT_INVALID_PARAM; } 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; bool outOfNodes = false; 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; // deal explicitly with crossing tile boundaries unsigned char crossSide = 0; if (bestTile->links[i].side != 0xff) crossSide = bestTile->links[i].side >> 1; // get the node dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide); if (!neighbourNode) { outOfNodes = true; 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 = (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; } } } dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath); if (lastBestNode->id != endRef) status |= DT_PARTIAL_RESULT; if (outOfNodes) status |= DT_OUT_OF_NODES; return status; } dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const { // Find the length of the entire path. dtNode* curNode = endNode; int length = 0; do { length++; curNode = m_nodePool->getNodeAtIdx(curNode->pidx); } while (curNode); // If the path cannot be fully stored then advance to the last node we will be able to store. curNode = endNode; int writeCount; for (writeCount = length; writeCount > maxPath; writeCount--) { dtAssert(curNode); curNode = m_nodePool->getNodeAtIdx(curNode->pidx); } // Write path for (int i = writeCount - 1; i >= 0; i--) { dtAssert(curNode); path[i] = curNode->id; curNode = m_nodePool->getNodeAtIdx(curNode->pidx); } dtAssert(!curNode); *pathCount = dtMin(length, maxPath); if (length > maxPath) return DT_SUCCESS | DT_BUFFER_TOO_SMALL; return DT_SUCCESS; } /// @par /// /// @warning Calling any non-slice methods before calling finalizeSlicedFindPath() /// or finalizeSlicedFindPathPartial() may result in corrupted data! /// /// The @p filter pointer is stored and used for the duration of the sliced /// path query. /// dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, const unsigned int options) { 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; if (startPos) dtVcopy(m_query.startPos, startPos); if (endPos) dtVcopy(m_query.endPos, endPos); m_query.filter = filter; m_query.options = options; m_query.raycastLimitSqr = FLT_MAX; // Validate input if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) || !startPos || !dtVisfinite(startPos) || !endPos || !dtVisfinite(endPos) || !filter) { return DT_FAILURE | DT_INVALID_PARAM; } // trade quality with performance? if (options & DT_FINDPATH_ANY_ANGLE) { // limiting to several times the character radius yields nice results. It is not sensitive // so it is enough to compute it from the first tile. const dtMeshTile* tile = m_nav->getTileByRef(startRef); float agentRadius = tile->header->walkableRadius; m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS); } 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, int* doneIters) { if (!dtStatusInProgress(m_query.status)) 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; } dtRaycastHit rayHit; rayHit.maxPath = 0; 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; const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; m_query.status = DT_SUCCESS | details; if (doneIters) *doneIters = iter; 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 (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly))) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; if (doneIters) *doneIters = iter; return m_query.status; } // Get parent and grand parent poly and tile. dtPolyRef parentRef = 0, grandpaRef = 0; const dtMeshTile* parentTile = 0; const dtPoly* parentPoly = 0; dtNode* parentNode = 0; if (bestNode->pidx) { parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx); parentRef = parentNode->id; if (parentNode->pidx) grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id; } if (parentRef) { bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly)); if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) ) { // The polygon has disappeared during the sliced query, fail. m_query.status = DT_FAILURE; if (doneIters) *doneIters = iter; return m_query.status; } } // decide whether to test raycast to previous nodes bool tryLOS = false; if (m_query.options & DT_FINDPATH_ANY_ANGLE) { if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr)) tryLOS = true; } for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) { 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; // get the neighbor node dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0); if (!neighbourNode) { m_query.status |= DT_OUT_OF_NODES; continue; } // do not expand to nodes that were already visited from the same parent if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx) continue; // If the node is visited the first time, calculate node position. if (neighbourNode->flags == 0) { getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos); } // Calculate cost and heuristic. float cost = 0; float heuristic = 0; // raycast parent bool foundShortCut = false; rayHit.pathCost = rayHit.t = 0; if (tryLOS) { raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef); foundShortCut = rayHit.t >= 1.0f; } // update move cost if (foundShortCut) { // shortcut found using raycast. Using shorter cost instead cost = parentNode->cost + rayHit.pathCost; } else { // No shortcut found. const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); cost = bestNode->cost + curCost; } // Special case for last node. if (neighbourRef == m_query.endRef) { const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0, 0, 0); cost = cost + endCost; heuristic = 0; } else { 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 = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode); neighbourNode->id = neighbourRef; neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED)); neighbourNode->cost = cost; neighbourNode->total = total; if (foundShortCut) neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED); 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()) { const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; m_query.status = DT_SUCCESS | details; } if (doneIters) *doneIters = iter; return m_query.status; } dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath) { if (!pathCount) return DT_FAILURE | DT_INVALID_PARAM; *pathCount = 0; if (!path || maxPath <= 0) return DT_FAILURE | DT_INVALID_PARAM; if (dtStatusFailed(m_query.status)) { // 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); if (m_query.lastBestNode->id != m_query.endRef) m_query.status |= DT_PARTIAL_RESULT; dtNode* prev = 0; dtNode* node = m_query.lastBestNode; int prevRay = 0; do { dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); node->pidx = m_nodePool->getNodeIdx(prev); prev = node; int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node prevRay = nextRay; node = next; } while (node); // Store path node = prev; do { dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); dtStatus status = 0; if (node->flags & DT_NODE_PARENT_DETACHED) { float t, normal[3]; int m; status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); n += m; // raycast ends on poly boundary and the path might include the next poly boundary. if (path[n-1] == next->id) n--; // remove to avoid duplicates } else { path[n++] = node->id; if (n >= maxPath) status = DT_BUFFER_TOO_SMALL; } if (status & DT_STATUS_DETAIL_MASK) { m_query.status |= status & DT_STATUS_DETAIL_MASK; break; } node = next; } while (node); } const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); *pathCount = n; return DT_SUCCESS | details; } dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize, dtPolyRef* path, int* pathCount, const int maxPath) { if (!pathCount) return DT_FAILURE | DT_INVALID_PARAM; *pathCount = 0; if (!existing || existingSize <= 0 || !path || !pathCount || maxPath <= 0) return DT_FAILURE | DT_INVALID_PARAM; if (dtStatusFailed(m_query.status)) { // 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) { m_nodePool->findNodes(existing[i], &node, 1); if (node) break; } if (!node) { m_query.status |= DT_PARTIAL_RESULT; dtAssert(m_query.lastBestNode); node = m_query.lastBestNode; } // Reverse the path. int prevRay = 0; do { dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); node->pidx = m_nodePool->getNodeIdx(prev); prev = node; int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut) node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node prevRay = nextRay; node = next; } while (node); // Store path node = prev; do { dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); dtStatus status = 0; if (node->flags & DT_NODE_PARENT_DETACHED) { float t, normal[3]; int m; status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n); n += m; // raycast ends on poly boundary and the path might include the next poly boundary. if (path[n-1] == next->id) n--; // remove to avoid duplicates } else { path[n++] = node->id; if (n >= maxPath) status = DT_BUFFER_TOO_SMALL; } if (status & DT_STATUS_DETAIL_MASK) { m_query.status |= status & DT_STATUS_DETAIL_MASK; break; } node = next; } while (node); } const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK; // Reset query. memset(&m_query, 0, sizeof(dtQueryData)); *pathCount = n; return DT_SUCCESS | details; } dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref, float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, int* straightPathCount, const int maxStraightPath) const { if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos)) { // The vertices are equal, update flags and poly. if (straightPathFlags) straightPathFlags[(*straightPathCount)-1] = flags; if (straightPathRefs) straightPathRefs[(*straightPathCount)-1] = ref; } else { // Append new vertex. dtVcopy(&straightPath[(*straightPathCount)*3], pos); if (straightPathFlags) straightPathFlags[(*straightPathCount)] = flags; if (straightPathRefs) straightPathRefs[(*straightPathCount)] = ref; (*straightPathCount)++; // If there is no space to append more vertices, return. if ((*straightPathCount) >= maxStraightPath) { return DT_SUCCESS | DT_BUFFER_TOO_SMALL; } // If reached end of path, return. if (flags == DT_STRAIGHTPATH_END) { return DT_SUCCESS; } } return DT_IN_PROGRESS; } dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path, float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs, int* straightPathCount, const int maxStraightPath, const int options) const { const float* startPos = &straightPath[(*straightPathCount-1)*3]; // Append or update last vertex dtStatus stat = 0; for (int i = startIdx; i < endIdx; i++) { // Calculate portal const dtPolyRef from = path[i]; const dtMeshTile* fromTile = 0; const dtPoly* fromPoly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) return DT_FAILURE | DT_INVALID_PARAM; const dtPolyRef to = path[i+1]; const dtMeshTile* toTile = 0; const dtPoly* toPoly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) return DT_FAILURE | DT_INVALID_PARAM; float left[3], right[3]; if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) break; if (options & DT_STRAIGHTPATH_AREA_CROSSINGS) { // Skip intersection if only area crossings are requested. if (fromPoly->getArea() == toPoly->getArea()) continue; } // Append intersection float s,t; if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t)) { float pt[3]; dtVlerp(pt, left,right, t); stat = appendVertex(pt, 0, path[i+1], straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); if (stat != DT_IN_PROGRESS) return stat; } } return DT_IN_PROGRESS; } /// @par /// /// This method peforms what is often called 'string pulling'. /// /// The start position is clamped to the first polygon in the path, and the /// end position is clamped to the last. So the start and end positions should /// normally be within or very near the first and last polygons respectively. /// /// The returned polygon references represent the reference id of the polygon /// that is entered at the associated path position. The reference id associated /// with the end point will always be zero. This allows, for example, matching /// off-mesh link points to their representative polygons. /// /// If the provided result buffers are too small for the entire result set, /// they will be filled as far as possible from the start toward the end /// position. /// 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 int options) const { dtAssert(m_nav); if (!straightPathCount) return DT_FAILURE | DT_INVALID_PARAM; *straightPathCount = 0; if (!startPos || !dtVisfinite(startPos) || !endPos || !dtVisfinite(endPos) || !path || pathSize <= 0 || !path[0] || maxStraightPath <= 0) { return DT_FAILURE | DT_INVALID_PARAM; } dtStatus stat = 0; // TODO: Should this be callers responsibility? float closestStartPos[3]; if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos))) return DT_FAILURE | DT_INVALID_PARAM; float closestEndPos[3]; if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos))) return DT_FAILURE | DT_INVALID_PARAM; // Add start point. stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); if (stat != DT_IN_PROGRESS) return stat; 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 toType; if (i+1 < pathSize) { unsigned char fromType; // fromType is ignored. // Next portal. if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType))) { // Failed to get portal points, in practice this means that path[i+1] is invalid polygon. // Clamp the end point to path[i], and return the path so far. if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos))) { // This should only happen when the first polygon is invalid. return DT_FAILURE | DT_INVALID_PARAM; } // Apeend portals along the current straight path segment. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) { // Ignore status return value as we're just about to return anyway. appendPortals(apexIndex, i, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath, options); } // Ignore status return value as we're just about to return anyway. appendVertex(closestEndPos, 0, path[i], straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); } // 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); 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 { // Append portals along the current straight path segment. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) { stat = appendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath, options); if (stat != DT_IN_PROGRESS) return stat; } 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; // Append or update vertex stat = appendVertex(portalApex, flags, ref, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); if (stat != DT_IN_PROGRESS) return stat; 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 { // Append portals along the current straight path segment. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) { stat = appendPortals(apexIndex, rightIndex, portalRight, path, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath, options); if (stat != DT_IN_PROGRESS) return stat; } 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; // Append or update vertex stat = appendVertex(portalApex, flags, ref, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); if (stat != DT_IN_PROGRESS) return stat; dtVcopy(portalLeft, portalApex); dtVcopy(portalRight, portalApex); leftIndex = apexIndex; rightIndex = apexIndex; // Restart i = apexIndex; continue; } } } // Append portals along the current straight path segment. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) { stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath, options); if (stat != DT_IN_PROGRESS) return stat; } } // Ignore status return value as we're just about to return anyway. appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, straightPath, straightPathFlags, straightPathRefs, straightPathCount, maxStraightPath); return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); } /// @par /// /// This method is optimized for small delta movement and a small number of /// polygons. If used for too great a distance, the result set will form an /// incomplete path. /// /// @p resultPos will equal the @p endPos if the end is reached. /// Otherwise the closest reachable position will be returned. /// /// @p resultPos is not projected onto the surface of the navigation /// mesh. Use #getPolyHeight if this is needed. /// /// This method treats the end position in the same manner as /// the #raycast method. (As a 2D point.) See that method's documentation /// for details. /// /// If the @p visited array is too small to hold the entire result set, it will /// be filled as far as possible from the start position toward the end /// position. /// 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); if (!visitedCount) return DT_FAILURE | DT_INVALID_PARAM; *visitedCount = 0; if (!m_nav->isValidPolyRef(startRef) || !startPos || !dtVisfinite(startPos) || !endPos || !dtVisfinite(endPos) || !filter || !resultPos || !visited || maxVisitedSize <= 0) { return DT_FAILURE | DT_INVALID_PARAM; } dtStatus status = DT_SUCCESS; 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; if (n >= maxVisitedSize) { status |= DT_BUFFER_TOO_SMALL; break; } node = m_tinyNodePool->getNodeAtIdx(node->pidx); } while (node); } dtVcopy(resultPos, bestPos); *visitedCount = n; return status; } 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 (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly))) return DT_FAILURE | DT_INVALID_PARAM; fromType = fromPoly->getType(); const dtMeshTile* toTile = 0; const dtPoly* toPoly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly))) return DT_FAILURE | DT_INVALID_PARAM; 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 | DT_INVALID_PARAM; // 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 | DT_INVALID_PARAM; } 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 | DT_INVALID_PARAM; } // 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 (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType))) return DT_FAILURE | DT_INVALID_PARAM; 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 (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right))) return DT_FAILURE | DT_INVALID_PARAM; 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; } /// @par /// /// This method is meant to be used for quick, short distance checks. /// /// If the path array is too small to hold the result, it will be filled as /// far as possible from the start postion toward the end position. /// /// <b>Using the Hit Parameter (t)</b> /// /// If the hit parameter is a very high value (FLT_MAX), then the ray has hit /// the end position. In this case the path represents a valid corridor to the /// end position and the value of @p hitNormal is undefined. /// /// If the hit parameter is zero, then the start position is on the wall that /// was hit and the value of @p hitNormal is undefined. /// /// If 0 < t < 1.0 then the following applies: /// /// @code /// distanceToHitBorder = distanceToEndPosition * t /// hitPoint = startPos + (endPos - startPos) * t /// @endcode /// /// <b>Use Case Restriction</b> /// /// The raycast ignores the y-value of the end position. (2D check.) This /// places significant limits on how it can be used. For example: /// /// Consider a scene where there is a main floor with a second floor balcony /// that hangs over the main floor. So the first floor mesh extends below the /// balcony mesh. The start position is somewhere on the first floor. The end /// position is on the balcony. /// /// The raycast will search toward the end position along the first floor mesh. /// If it reaches the end position's xz-coordinates it will indicate FLT_MAX /// (no wall hit), meaning it reached the end position. This is one example of why /// this method is meant for short distance checks. /// dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const { dtRaycastHit hit; hit.path = path; hit.maxPath = maxPath; dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit); *t = hit.t; if (hitNormal) dtVcopy(hitNormal, hit.hitNormal); if (pathCount) *pathCount = hit.pathCount; return status; } /// @par /// /// This method is meant to be used for quick, short distance checks. /// /// If the path array is too small to hold the result, it will be filled as /// far as possible from the start postion toward the end position. /// /// <b>Using the Hit Parameter t of RaycastHit</b> /// /// If the hit parameter is a very high value (FLT_MAX), then the ray has hit /// the end position. In this case the path represents a valid corridor to the /// end position and the value of @p hitNormal is undefined. /// /// If the hit parameter is zero, then the start position is on the wall that /// was hit and the value of @p hitNormal is undefined. /// /// If 0 < t < 1.0 then the following applies: /// /// @code /// distanceToHitBorder = distanceToEndPosition * t /// hitPoint = startPos + (endPos - startPos) * t /// @endcode /// /// <b>Use Case Restriction</b> /// /// The raycast ignores the y-value of the end position. (2D check.) This /// places significant limits on how it can be used. For example: /// /// Consider a scene where there is a main floor with a second floor balcony /// that hangs over the main floor. So the first floor mesh extends below the /// balcony mesh. The start position is somewhere on the first floor. The end /// position is on the balcony. /// /// The raycast will search toward the end position along the first floor mesh. /// If it reaches the end position's xz-coordinates it will indicate FLT_MAX /// (no wall hit), meaning it reached the end position. This is one example of why /// this method is meant for short distance checks. /// dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos, const dtQueryFilter* filter, const unsigned int options, dtRaycastHit* hit, dtPolyRef prevRef) const { dtAssert(m_nav); if (!hit) return DT_FAILURE | DT_INVALID_PARAM; hit->t = 0; hit->pathCount = 0; hit->pathCost = 0; // Validate input if (!m_nav->isValidPolyRef(startRef) || !startPos || !dtVisfinite(startPos) || !endPos || !dtVisfinite(endPos) || !filter || (prevRef && !m_nav->isValidPolyRef(prevRef))) { return DT_FAILURE | DT_INVALID_PARAM; } float dir[3], curPos[3], lastPos[3]; float verts[DT_VERTS_PER_POLYGON*3+3]; int n = 0; dtVcopy(curPos, startPos); dtVsub(dir, endPos, startPos); dtVset(hit->hitNormal, 0, 0, 0); dtStatus status = DT_SUCCESS; const dtMeshTile* prevTile, *tile, *nextTile; const dtPoly* prevPoly, *poly, *nextPoly; dtPolyRef curRef; // The API input has been checked already, skip checking internal data. curRef = startRef; tile = 0; poly = 0; m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly); nextTile = prevTile = tile; nextPoly = prevPoly = poly; if (prevRef) m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly); while (curRef) { // Cast ray against current polygon. // 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. hit->pathCount = n; return status; } hit->hitEdgeIndex = segMax; // Keep track of furthest t so far. if (tmax > hit->t) hit->t = tmax; // Store visited polygons. if (n < hit->maxPath) hit->path[n++] = curRef; else status |= DT_BUFFER_TOO_SMALL; // Ray end is completely inside the polygon. if (segMax == -1) { hit->t = FLT_MAX; hit->pathCount = n; // add the cost if (options & DT_RAYCAST_USE_COSTS) hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly); return status; } // 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. nextTile = 0; 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; } } } // add the cost if (options & DT_RAYCAST_USE_COSTS) { // compute the intersection point at the furthest end of the polygon // and correct the height (since the raycast moves in 2d) dtVcopy(lastPos, curPos); dtVmad(curPos, startPos, dir, hit->t); float* e1 = &verts[segMax*3]; float* e2 = &verts[((segMax+1)%nv)*3]; float eDir[3], diff[3]; dtVsub(eDir, e2, e1); dtVsub(diff, curPos, e1); float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2]; curPos[1] = e1[1] + eDir[1] * s; hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly); } 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]; hit->hitNormal[0] = dz; hit->hitNormal[1] = 0; hit->hitNormal[2] = -dx; dtVnormalize(hit->hitNormal); hit->pathCount = n; return status; } // No hit, advance to neighbour polygon. prevRef = curRef; curRef = nextRef; prevTile = tile; tile = nextTile; prevPoly = poly; poly = nextPoly; } hit->pathCount = n; return status; } /// @par /// /// At least one result array must be provided. /// /// The order of the result set is from least to highest cost to reach the polygon. /// /// A common use case for this method is to perform Dijkstra searches. /// Candidate polygons are found by searching the graph beginning at the start polygon. /// /// If a polygon is not found via the graph search, even if it intersects the /// search circle, it will not be included in the result set. For example: /// /// polyA is the start polygon. /// polyB shares an edge with polyA. (Is adjacent.) /// polyC shares an edge with polyB, but not with polyA /// Even if the search circle overlaps polyC, it will not be included in the /// result set unless polyB is also in the set. /// /// The value of the center point is used as the start position for cost /// calculations. It is not projected onto the surface of the mesh, so its /// y-value will effect the costs. /// /// Intersection tests occur in 2D. All polygons and the search circle are /// projected onto the xz-plane. So the y-value of the center point does not /// effect intersection tests. /// /// If the result arrays are to small to hold the entire result set, they will be /// filled to capacity. /// 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); if (!resultCount) return DT_FAILURE | DT_INVALID_PARAM; *resultCount = 0; if (!m_nav->isValidPolyRef(startRef) || !centerPos || !dtVisfinite(centerPos) || radius < 0 || !dtMathIsfinite(radius) || !filter || maxResult < 0) { return DT_FAILURE | DT_INVALID_PARAM; } 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); dtStatus status = DT_SUCCESS; int n = 0; 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); if (n < maxResult) { if (resultRef) resultRef[n] = bestRef; if (resultParent) resultParent[n] = parentRef; if (resultCost) resultCost[n] = bestNode->total; ++n; } else { status |= DT_BUFFER_TOO_SMALL; } 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) { status |= DT_OUT_OF_NODES; continue; } if (neighbourNode->flags & DT_NODE_CLOSED) continue; // Cost if (neighbourNode->flags == 0) dtVlerp(neighbourNode->pos, va, vb, 0.5f); float cost = filter->getCost( bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); const float total = bestNode->total + cost; // 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->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); } } } *resultCount = n; return status; } /// @par /// /// The order of the result set is from least to highest cost. /// /// At least one result array must be provided. /// /// A common use case for this method is to perform Dijkstra searches. /// Candidate polygons are found by searching the graph beginning at the start /// polygon. /// /// The same intersection test restrictions that apply to findPolysAroundCircle() /// method apply to this method. /// /// The 3D centroid of the search polygon is used as the start position for cost /// calculations. /// /// Intersection tests occur in 2D. All polygons are projected onto the /// xz-plane. So the y-values of the vertices do not effect intersection tests. /// /// If the result arrays are is too small to hold the entire result set, they will /// be filled to capacity. /// 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); if (!resultCount) return DT_FAILURE | DT_INVALID_PARAM; *resultCount = 0; if (!m_nav->isValidPolyRef(startRef) || !verts || nverts < 3 || !filter || maxResult < 0) { return DT_FAILURE | DT_INVALID_PARAM; } // Validate input if (!startRef || !m_nav->isValidPolyRef(startRef)) return DT_FAILURE | DT_INVALID_PARAM; 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); dtStatus status = DT_SUCCESS; int n = 0; 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); if (n < maxResult) { if (resultRef) resultRef[n] = bestRef; if (resultParent) resultParent[n] = parentRef; if (resultCost) resultCost[n] = bestNode->total; ++n; } else { status |= DT_BUFFER_TOO_SMALL; } 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) { status |= DT_OUT_OF_NODES; continue; } if (neighbourNode->flags & DT_NODE_CLOSED) continue; // Cost if (neighbourNode->flags == 0) dtVlerp(neighbourNode->pos, va, vb, 0.5f); float cost = filter->getCost( bestNode->pos, neighbourNode->pos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly); const float total = bestNode->total + cost; // 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->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); } } } *resultCount = n; return status; } dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const { if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0) return DT_FAILURE | DT_INVALID_PARAM; *pathCount = 0; dtNode* endNode; if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 || (endNode->flags & DT_NODE_CLOSED) == 0) return DT_FAILURE | DT_INVALID_PARAM; return getPathToNode(endNode, path, pathCount, maxPath); } /// @par /// /// This method is optimized for a small search radius and small number of result /// polygons. /// /// Candidate polygons are found by searching the navigation graph beginning at /// the start polygon. /// /// The same intersection test restrictions that apply to the findPolysAroundCircle /// mehtod applies to this method. /// /// The value of the center point is used as the start point for cost calculations. /// It is not projected onto the surface of the mesh, so its y-value will effect /// the costs. /// /// Intersection tests occur in 2D. All polygons and the search circle are /// projected onto the xz-plane. So the y-value of the center point does not /// effect intersection tests. /// /// If the result arrays are is too small to hold the entire result set, they will /// be filled to capacity. /// 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); if (!resultCount) return DT_FAILURE | DT_INVALID_PARAM; *resultCount = 0; if (!m_nav->isValidPolyRef(startRef) || !centerPos || !dtVisfinite(centerPos) || radius < 0 || !dtMathIsfinite(radius) || !filter || maxResult < 0) { return DT_FAILURE | DT_INVALID_PARAM; } 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]; dtStatus status = DT_SUCCESS; int n = 0; if (n < maxResult) { resultRef[n] = startNode->id; if (resultParent) resultParent[n] = 0; ++n; } else { status |= DT_BUFFER_TOO_SMALL; } 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; } else { status |= DT_BUFFER_TOO_SMALL; } if (nstack < MAX_STACK) { stack[nstack++] = neighbourNode; } } } *resultCount = n; return status; } struct dtSegInterval { dtPolyRef ref; short tmin, tmax; }; static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts, const short tmin, const short tmax, const dtPolyRef ref) { 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].ref = ref; ints[idx].tmin = tmin; ints[idx].tmax = tmax; nints++; } /// @par /// /// If the @p segmentRefs parameter is provided, then all polygon segments will be returned. /// Otherwise only the wall segments are returned. /// /// A segment that is normally a portal will be included in the result set as a /// wall if the @p filter results in the neighbor polygon becoomming impassable. /// /// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the /// maximum segments per polygon of the source navigation mesh. /// dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, const int maxSegments) const { dtAssert(m_nav); if (!segmentCount) return DT_FAILURE | DT_INVALID_PARAM; *segmentCount = 0; const dtMeshTile* tile = 0; const dtPoly* poly = 0; if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) return DT_FAILURE | DT_INVALID_PARAM; if (!filter || !segmentVerts || maxSegments < 0) return DT_FAILURE | DT_INVALID_PARAM; int n = 0; static const int MAX_INTERVAL = 16; dtSegInterval ints[MAX_INTERVAL]; int nints; const bool storePortals = segmentRefs != 0; dtStatus status = DT_SUCCESS; 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, link->ref); } } } } } else { // Internal edge dtPolyRef neiRef = 0; if (poly->neis[j]) { const unsigned int idx = (unsigned int)(poly->neis[j]-1); neiRef = m_nav->getPolyRefBase(tile) | idx; if (!filter->passFilter(neiRef, tile, &tile->polys[idx])) neiRef = 0; } // If the edge leads to another polygon and portals are not stored, skip. if (neiRef != 0 && !storePortals) continue; if (n < maxSegments) { const float* vj = &tile->verts[poly->verts[j]*3]; const float* vi = &tile->verts[poly->verts[i]*3]; float* seg = &segmentVerts[n*6]; dtVcopy(seg+0, vj); dtVcopy(seg+3, vi); if (segmentRefs) segmentRefs[n] = neiRef; n++; } else { status |= DT_BUFFER_TOO_SMALL; } continue; } // Add sentinels insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0); insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0); // Store segments. 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) { // Portal segment. if (storePortals && ints[k].ref) { const float tmin = ints[k].tmin/255.0f; const float tmax = ints[k].tmax/255.0f; if (n < maxSegments) { float* seg = &segmentVerts[n*6]; dtVlerp(seg+0, vj,vi, tmin); dtVlerp(seg+3, vj,vi, tmax); if (segmentRefs) segmentRefs[n] = ints[k].ref; n++; } else { status |= DT_BUFFER_TOO_SMALL; } } // Wall segment. const int imin = ints[k-1].tmax; const int imax = ints[k].tmin; if (imin != imax) { const float tmin = imin/255.0f; const float tmax = imax/255.0f; if (n < maxSegments) { float* seg = &segmentVerts[n*6]; dtVlerp(seg+0, vj,vi, tmin); dtVlerp(seg+3, vj,vi, tmax); if (segmentRefs) segmentRefs[n] = 0; n++; } else { status |= DT_BUFFER_TOO_SMALL; } } } } *segmentCount = n; return status; } /// @par /// /// @p hitPos is not adjusted using the height detail data. /// /// @p hitDist will equal the search radius if there is no wall within the /// radius. In this case the values of @p hitPos and @p hitNormal are /// undefined. /// /// The normal will become unpredicable if @p hitDist is a very small number. /// 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 (!m_nav->isValidPolyRef(startRef) || !centerPos || !dtVisfinite(centerPos) || maxRadius < 0 || !dtMathIsfinite(maxRadius) || !filter || !hitDist || !hitPos || !hitNormal) { return DT_FAILURE | DT_INVALID_PARAM; } 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); dtStatus status = DT_SUCCESS; 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) { status |= DT_OUT_OF_NODES; 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 = (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 = dtMathSqrtf(radiusSqr); return status; } bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const { const dtMeshTile* tile = 0; const dtPoly* poly = 0; dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly); // If cannot get polygon, assume it does not exists and boundary is invalid. if (dtStatusFailed(status)) return false; // If cannot pass filter, assume flags has changed and boundary is invalid. if (!filter->passFilter(ref, tile, poly)) return false; return true; } /// @par /// /// The closed list is the list of polygons that were fully evaluated during /// the last navigation graph search. (A* or Dijkstra) /// bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const { if (!m_nodePool) return false; dtNode* nodes[DT_MAX_STATES_PER_NODE]; int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE); for (int i=0; i<n; i++) { if (nodes[i]->flags & DT_NODE_CLOSED) return true; } return false; }