diff options
Diffstat (limited to 'extern/recastnavigation/Detour/Source/DetourStatNavMesh.cpp')
| -rw-r--r-- | extern/recastnavigation/Detour/Source/DetourStatNavMesh.cpp | 876 |
1 files changed, 876 insertions, 0 deletions
diff --git a/extern/recastnavigation/Detour/Source/DetourStatNavMesh.cpp b/extern/recastnavigation/Detour/Source/DetourStatNavMesh.cpp new file mode 100644 index 00000000000..bf59cd89dda --- /dev/null +++ b/extern/recastnavigation/Detour/Source/DetourStatNavMesh.cpp @@ -0,0 +1,876 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include <math.h> +#include <float.h> +#include <string.h> +#include <stdio.h> +#include "DetourStatNavMesh.h" +#include "DetourNode.h" +#include "DetourCommon.h" + + +////////////////////////////////////////////////////////////////////////////////////////// +dtStatNavMesh::dtStatNavMesh() : + m_data(0), + m_dataSize(0), + m_header(0), + m_nodePool(0), + m_openList(0) +{ +} + +dtStatNavMesh::~dtStatNavMesh() +{ + delete m_nodePool; + delete m_openList; + if (m_data) + delete [] m_data; +} + +bool dtStatNavMesh::init(unsigned char* data, int dataSize, bool ownsData) +{ + dtStatNavMeshHeader* header = (dtStatNavMeshHeader*)data; + + if (header->magic != DT_STAT_NAVMESH_MAGIC) + return false; + if (header->version != DT_STAT_NAVMESH_VERSION) + return false; + + const int headerSize = sizeof(dtStatNavMeshHeader); + const int vertsSize = sizeof(float)*3*header->nverts; + const int polysSize = sizeof(dtStatPoly)*header->npolys; + const int nodesSize = sizeof(dtStatBVNode)*header->npolys*2; + const int detailMeshesSize = sizeof(dtStatPolyDetail)*header->ndmeshes; + const int detailVertsSize = sizeof(float)*3*header->ndverts; + const int detailTrisSize = sizeof(unsigned char)*4*header->ndtris; + + + unsigned char* d = data + headerSize; + header->verts = (float*)d; d += vertsSize; + header->polys = (dtStatPoly*)d; d += polysSize; + header->bvtree = (dtStatBVNode*)d; d += nodesSize; + header->dmeshes = (dtStatPolyDetail*)d; d += detailMeshesSize; + header->dverts = (float*)d; d += detailVertsSize; + header->dtris = (unsigned char*)d; d += detailTrisSize; + + m_nodePool = new dtNodePool(2048, 256); + if (!m_nodePool) + return false; + + m_openList = new dtNodeQueue(2048); + if (!m_openList) + return false; + + if (ownsData) + { + m_data = data; + m_dataSize = dataSize; + } + + m_header = header; + + return true; +} + +const dtStatPoly* dtStatNavMesh::getPolyByRef(dtStatPolyRef ref) const +{ + if (!m_header || ref == 0 || (int)ref > m_header->npolys) return 0; + return &m_header->polys[ref-1]; +} + +int dtStatNavMesh::getPolyIndexByRef(dtStatPolyRef ref) const +{ + if (!m_header || ref == 0 || (int)ref > m_header->npolys) return -1; + return (int)ref-1; +} + +int dtStatNavMesh::findPath(dtStatPolyRef startRef, dtStatPolyRef endRef, + const float* startPos, const float* endPos, + dtStatPolyRef* path, const int maxPathSize) +{ + if (!m_header) return 0; + + if (!startRef || !endRef) + return 0; + + if (!maxPathSize) + return 0; + + if (startRef == endRef) + { + path[0] = startRef; + return 1; + } + + m_nodePool->clear(); + m_openList->clear(); + + static const float H_SCALE = 1.1f; // Heuristic scale. + + dtNode* startNode = m_nodePool->getNode(startRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = vdist(startPos, endPos) * H_SCALE; + startNode->id = startRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + dtNode* lastBestNode = startNode; + float lastBestNodeCost = startNode->total; + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + + if (bestNode->id == endRef) + { + lastBestNode = bestNode; + break; + } + + const dtStatPoly* poly = getPoly(bestNode->id-1); + for (int i = 0; i < (int)poly->nv; ++i) + { + dtStatPolyRef neighbour = poly->n[i]; + if (neighbour) + { + // Skip parent node. + if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour) + continue; + + dtNode* parent = bestNode; + dtNode newNode; + newNode.pidx = m_nodePool->getNodeIdx(parent); + newNode.id = neighbour; + + // Calculate cost. + float p0[3], p1[3]; + if (!parent->pidx) + vcopy(p0, startPos); + else + getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0); + getEdgeMidPoint(parent->id, newNode.id, p1); + newNode.cost = parent->cost + vdist(p0,p1); + // Special case for last node. + if (newNode.id == endRef) + newNode.cost += vdist(p1, endPos); + + // Heuristic + const float h = vdist(p1,endPos)*H_SCALE; + newNode.total = newNode.cost + h; + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~DT_NODE_CLOSED; + actualNode->pidx = newNode.pidx; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (h < lastBestNodeCost) + { + lastBestNodeCost = h; + lastBestNode = actualNode; + } + + if (actualNode->flags & DT_NODE_OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags |= DT_NODE_OPEN; + m_openList->push(actualNode); + } + } + } + } + bestNode->flags |= DT_NODE_CLOSED; + } + + // Reverse the path. + dtNode* prev = 0; + dtNode* node = lastBestNode; + do + { + dtNode* next = m_nodePool->getNodeAtIdx(node->pidx); + node->pidx = m_nodePool->getNodeIdx(prev); + prev = node; + node = next; + } + while (node); + + // Store path + node = prev; + int n = 0; + do + { + path[n++] = node->id; + node = m_nodePool->getNodeAtIdx(node->pidx); + } + while (node && n < maxPathSize); + + return n; +} + +bool dtStatNavMesh::closestPointToPoly(dtStatPolyRef ref, const float* pos, float* closest) const +{ + int idx = getPolyIndexByRef(ref); + if (idx == -1) + return false; + + float closestDistSqr = FLT_MAX; + const dtStatPoly* p = getPoly(idx); + const dtStatPolyDetail* pd = getPolyDetail(idx); + + for (int j = 0; j < pd->ntris; ++j) + { + const unsigned char* t = getDetailTri(pd->tbase+j); + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < p->nv) + v[k] = getVertex(p->v[t[k]]); + else + v[k] = getDetailVertex(pd->vbase+(t[k]-p->nv)); + } + float pt[3]; + closestPtPointTriangle(pt, pos, v[0], v[1], v[2]); + float d = vdistSqr(pos, pt); + if (d < closestDistSqr) + { + vcopy(closest, pt); + closestDistSqr = d; + } + } + + return true; +} + +bool dtStatNavMesh::getPolyHeight(dtStatPolyRef ref, const float* pos, float* height) const +{ + int idx = getPolyIndexByRef(ref); + if (idx == -1) + return false; + + const dtStatPoly* p = getPoly(idx); + const dtStatPolyDetail* pd = getPolyDetail(idx); + + for (int i = 0; i < pd->ntris; ++i) + { + const unsigned char* t = getDetailTri(pd->tbase+i); + const float* v[3]; + for (int j = 0; j < 3; ++j) + { + if (t[j] < p->nv) + v[j] = getVertex(p->v[t[j]]); + else + v[j] = getDetailVertex(pd->vbase+(t[j]-p->nv)); + } + float h; + if (closestHeightPointTriangle(pos, v[0], v[1], v[2], h)) + { + if (height) + *height = h; + return true; + } + } + + return false; +} + +int dtStatNavMesh::findStraightPath(const float* startPos, const float* endPos, + const dtStatPolyRef* path, const int pathSize, + float* straightPath, const int maxStraightPathSize) +{ + if (!m_header) return 0; + + if (!maxStraightPathSize) + return 0; + + if (!path[0]) + return 0; + + int straightPathSize = 0; + + float closestStartPos[3]; + if (!closestPointToPoly(path[0], startPos, closestStartPos)) + return 0; + + // Add start point. + vcopy(&straightPath[straightPathSize*3], closestStartPos); + straightPathSize++; + if (straightPathSize >= maxStraightPathSize) + return straightPathSize; + + float closestEndPos[3]; + if (!closestPointToPoly(path[pathSize-1], endPos, closestEndPos)) + return 0; + + float portalApex[3], portalLeft[3], portalRight[3]; + + if (pathSize > 1) + { + vcopy(portalApex, closestStartPos); + vcopy(portalLeft, portalApex); + vcopy(portalRight, portalApex); + int apexIndex = 0; + int leftIndex = 0; + int rightIndex = 0; + + for (int i = 0; i < pathSize; ++i) + { + float left[3], right[3]; + if (i < pathSize-1) + { + // Next portal. + getPortalPoints(path[i], path[i+1], left, right); + } + else + { + // End of the path. + vcopy(left, closestEndPos); + vcopy(right, closestEndPos); + } + + // Right vertex. + if (vequal(portalApex, portalRight)) + { + vcopy(portalRight, right); + rightIndex = i; + } + else + { + if (triArea2D(portalApex, portalRight, right) <= 0.0f) + { + if (triArea2D(portalApex, portalLeft, right) > 0.0f) + { + vcopy(portalRight, right); + rightIndex = i; + } + else + { + vcopy(portalApex, portalLeft); + apexIndex = leftIndex; + + if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex)) + { + vcopy(&straightPath[straightPathSize*3], portalApex); + straightPathSize++; + if (straightPathSize >= maxStraightPathSize) + return straightPathSize; + } + + vcopy(portalLeft, portalApex); + vcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + } + + // Left vertex. + if (vequal(portalApex, portalLeft)) + { + vcopy(portalLeft, left); + leftIndex = i; + } + else + { + if (triArea2D(portalApex, portalLeft, left) >= 0.0f) + { + if (triArea2D(portalApex, portalRight, left) < 0.0f) + { + vcopy(portalLeft, left); + leftIndex = i; + } + else + { + vcopy(portalApex, portalRight); + apexIndex = rightIndex; + + if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex)) + { + vcopy(&straightPath[straightPathSize*3], portalApex); + straightPathSize++; + if (straightPathSize >= maxStraightPathSize) + return straightPathSize; + } + + vcopy(portalLeft, portalApex); + vcopy(portalRight, portalApex); + leftIndex = apexIndex; + rightIndex = apexIndex; + + // Restart + i = apexIndex; + + continue; + } + } + } + } + } + + // Add end point. + vcopy(&straightPath[straightPathSize*3], closestEndPos); + straightPathSize++; + + return straightPathSize; +} + +int dtStatNavMesh::getPolyVerts(dtStatPolyRef ref, float* verts) const +{ + if (!m_header) return 0; + const dtStatPoly* poly = getPolyByRef(ref); + if (!poly) return 0; + float* v = verts; + for (int i = 0; i < (int)poly->nv; ++i) + { + const float* cv = &m_header->verts[poly->v[i]*3]; + *v++ = cv[0]; + *v++ = cv[1]; + *v++ = cv[2]; + } + return (int)poly->nv; +} + +int dtStatNavMesh::raycast(dtStatPolyRef centerRef, const float* startPos, const float* endPos, + float& t, dtStatPolyRef* path, const int pathSize) +{ + if (!m_header) return 0; + if (!centerRef) return 0; + + dtStatPolyRef prevRef = centerRef; + dtStatPolyRef curRef = centerRef; + t = 0; + + float verts[DT_STAT_VERTS_PER_POLYGON*3]; + int n = 0; + + while (curRef) + { + // Cast ray against current polygon. + int nv = getPolyVerts(curRef, verts); + if (nv < 3) + { + // Hit bad polygon, report hit. + return n; + } + + float tmin, tmax; + int segMin, segMax; + if (!intersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax)) + { + // Could not a polygon, keep the old t and report hit. + return n; + } + // Keep track of furthest t so far. + if (tmax > t) + t = tmax; + + if (n < pathSize) + path[n++] = curRef; + + // Check the neighbour of this polygon. + const dtStatPoly* poly = getPolyByRef(curRef); + dtStatPolyRef nextRef = poly->n[segMax]; + if (!nextRef) + { + // No neighbour, we hit a wall. + return n; + } + + // No hit, advance to neighbour polygon. + prevRef = curRef; + curRef = nextRef; + } + + return n; +} + + +float dtStatNavMesh::findDistanceToWall(dtStatPolyRef centerRef, const float* centerPos, float maxRadius, + float* hitPos, float* hitNormal) +{ + if (!m_header) return 0; + if (!centerRef) return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(centerRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + float radiusSqr = sqr(maxRadius); + + hitNormal[0] = 1; + hitNormal[1] = 0; + hitNormal[2] = 0; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + const dtStatPoly* poly = getPoly(bestNode->id-1); + + // Hit test walls. + for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++) + { + // Skip non-solid edges. + if (poly->n[j]) continue; + + // Calc distance to the edge. + const float* vj = getVertex(poly->v[j]); + const float* vi = getVertex(poly->v[i]); + float tseg; + float distSqr = distancePtSegSqr2D(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; + } + + // Check to see if teh circle expands to one of the neighbours and expand. + for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++) + { + // Skip solid edges. + if (!poly->n[j]) continue; + + // Expand to neighbour if not visited yet. + dtStatPolyRef neighbour = poly->n[j]; + + // Skip parent node. + if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour) + continue; + + // Calc distance to the edge. + const float* vj = getVertex(poly->v[j]); + const float* vi = getVertex(poly->v[i]); + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg); + + // Edge is too far, skip. + if (distSqr > radiusSqr) + continue; + + dtNode* parent = bestNode; + dtNode newNode; + newNode.pidx = m_nodePool->getNodeIdx(parent); + newNode.id = neighbour; + + // Cost + float p0[3], p1[3]; + if (!parent->pidx) + vcopy(p0, centerPos); + else + getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0); + getEdgeMidPoint(parent->id, newNode.id, p1); + newNode.total = parent->total + vdist(p0,p1); + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~DT_NODE_CLOSED; + actualNode->pidx = newNode.pidx; + actualNode->total = newNode.total; + + if (actualNode->flags & DT_NODE_OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags |= DT_NODE_OPEN; + m_openList->push(actualNode); + } + } + } + bestNode->flags |= DT_NODE_CLOSED; + } + + // Calc hit normal. + vsub(hitNormal, centerPos, hitPos); + vnormalize(hitNormal); + + return sqrtf(radiusSqr); +} + +int dtStatNavMesh::findPolysAround(dtStatPolyRef centerRef, const float* centerPos, float radius, + dtStatPolyRef* resultRef, dtStatPolyRef* resultParent, float* resultCost, + const int maxResult) +{ + if (!m_header) return 0; + if (!centerRef) return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(centerRef); + startNode->pidx = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = DT_NODE_OPEN; + m_openList->push(startNode); + + int n = 0; + if (n < maxResult) + { + if (resultRef) + resultRef[n] = startNode->id; + if (resultParent) + resultParent[n] = 0; + if (resultCost) + resultCost[n] = 0; + ++n; + } + + const float radiusSqr = sqr(radius); + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + const dtStatPoly* poly = getPoly(bestNode->id-1); + for (unsigned i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j=i++) + { + dtStatPolyRef neighbour = poly->n[j]; + + if (neighbour) + { + // Skip parent node. + if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour) + continue; + + // Calc distance to the edge. + const float* vj = getVertex(poly->v[j]); + const float* vi = getVertex(poly->v[i]); + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg); + + // If the circle is not touching the next polygon, skip it. + if (distSqr > radiusSqr) + continue; + + dtNode* parent = bestNode; + dtNode newNode; + newNode.pidx = m_nodePool->getNodeIdx(parent); + newNode.id = neighbour; + + // Cost + float p0[3], p1[3]; + if (!parent->pidx) + vcopy(p0, centerPos); + else + getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0); + getEdgeMidPoint(parent->id, newNode.id, p1); + newNode.total = parent->total + vdist(p0,p1); + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~DT_NODE_CLOSED; + actualNode->pidx = newNode.pidx; + actualNode->total = newNode.total; + + if (actualNode->flags & DT_NODE_OPEN) + { + m_openList->modify(actualNode); + } + else + { + if (n < maxResult) + { + if (resultRef) + resultRef[n] = actualNode->id; + if (resultParent) + resultParent[n] = m_nodePool->getNodeAtIdx(actualNode->pidx)->id; + if (resultCost) + resultCost[n] = actualNode->total; + ++n; + } + actualNode->flags |= DT_NODE_OPEN; + m_openList->push(actualNode); + } + } + } + } + bestNode->flags |= DT_NODE_CLOSED; + + } + + return n; +} + +// Returns polygons which are withing certain radius from the query location. +int dtStatNavMesh::queryPolygons(const float* center, const float* extents, + dtStatPolyRef* polys, const int maxIds) +{ + if (!m_header) return 0; + + const dtStatBVNode* node = &m_header->bvtree[0]; + const dtStatBVNode* end = &m_header->bvtree[m_header->nnodes]; + + // Calculate quantized box + const float ics = 1.0f / m_header->cs; + unsigned short bmin[3], bmax[3]; + // Clamp query box to world box. + float minx = clamp(center[0] - extents[0], m_header->bmin[0], m_header->bmax[0]) - m_header->bmin[0]; + float miny = clamp(center[1] - extents[1], m_header->bmin[1], m_header->bmax[1]) - m_header->bmin[1]; + float minz = clamp(center[2] - extents[2], m_header->bmin[2], m_header->bmax[2]) - m_header->bmin[2]; + float maxx = clamp(center[0] + extents[0], m_header->bmin[0], m_header->bmax[0]) - m_header->bmin[0]; + float maxy = clamp(center[1] + extents[1], m_header->bmin[1], m_header->bmax[1]) - m_header->bmin[1]; + float maxz = clamp(center[2] + extents[2], m_header->bmin[2], m_header->bmax[2]) - m_header->bmin[2]; + // Quantize + bmin[0] = (unsigned short)(ics * minx) & 0xfffe; + bmin[1] = (unsigned short)(ics * miny) & 0xfffe; + bmin[2] = (unsigned short)(ics * minz) & 0xfffe; + bmax[0] = (unsigned short)(ics * maxx + 1) | 1; + bmax[1] = (unsigned short)(ics * maxy + 1) | 1; + bmax[2] = (unsigned short)(ics * maxz + 1) | 1; + + // Traverse tree + int n = 0; + while (node < end) + { + bool overlap = checkOverlapBox(bmin, bmax, node->bmin, node->bmax); + bool isLeafNode = node->i >= 0; + + if (isLeafNode && overlap) + { + if (n < maxIds) + { + polys[n] = (dtStatPolyRef)node->i; + n++; + } + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + + return n; +} + +dtStatPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* extents) +{ + if (!m_header) return 0; + + // Get nearby polygons from proximity grid. + dtStatPolyRef polys[128]; + int npolys = queryPolygons(center, extents, polys, 128); + + // Find nearest polygon amongst the nearby polygons. + dtStatPolyRef nearest = 0; + float nearestDistanceSqr = FLT_MAX; + for (int i = 0; i < npolys; ++i) + { + dtStatPolyRef ref = polys[i]; + float closest[3]; + if (!closestPointToPoly(ref, center, closest)) + continue; + float d = vdistSqr(center, closest); + if (d < nearestDistanceSqr) + { + nearestDistanceSqr = d; + nearest = ref; + } + } + + return nearest; +} + +bool dtStatNavMesh::getPortalPoints(dtStatPolyRef from, dtStatPolyRef to, float* left, float* right) const +{ + const dtStatPoly* fromPoly = getPolyByRef(from); + if (!fromPoly) + return false; + + // Find common edge between the polygons and returns the segment end points. + for (unsigned i = 0, j = (int)fromPoly->nv - 1; i < (int)fromPoly->nv; j = i++) + { + unsigned short neighbour = fromPoly->n[j]; + if (neighbour == to) + { + vcopy(left, getVertex(fromPoly->v[j])); + vcopy(right, getVertex(fromPoly->v[i])); + return true; + } + } + + return false; +} + +bool dtStatNavMesh::getEdgeMidPoint(dtStatPolyRef from, dtStatPolyRef to, float* mid) const +{ + float left[3], right[3]; + if (!getPortalPoints(from, to, left,right)) return false; + 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 true; +} + +bool dtStatNavMesh::isInClosedList(dtStatPolyRef ref) const +{ + if (!m_nodePool) return false; + const dtNode* node = m_nodePool->findNode(ref); + return node && node->flags & DT_NODE_CLOSED; +} + +int dtStatNavMesh::getMemUsed() const +{ + if (!m_nodePool || ! m_openList) + return 0; + return sizeof(*this) + m_dataSize + + m_nodePool->getMemUsed() + + m_openList->getMemUsed(); +} |