diff --git a/Detour/Include/DetourDebugDraw.h b/Detour/Include/DetourDebugDraw.h new file mode 100755 index 0000000..66586fe --- /dev/null +++ b/Detour/Include/DetourDebugDraw.h @@ -0,0 +1,28 @@ +// +// 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. +// + +#ifndef DETOURDEBUGDRAW_H +#define DETOURDEBUGDRAW_H + +#include "DetourStatNavMesh.h" + +void dtDebugDrawStatNavMeshPoly(const dtStatNavMesh* mesh, dtPolyRef ref, const float* col); +void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh); +void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh); + +#endif // DETOURDEBUGDRAW_H \ No newline at end of file diff --git a/Detour/Include/DetourStatNavMesh.h b/Detour/Include/DetourStatNavMesh.h new file mode 100755 index 0000000..b5adca1 --- /dev/null +++ b/Detour/Include/DetourStatNavMesh.h @@ -0,0 +1,201 @@ +// +// 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. +// + +#ifndef DETOURSTATNAVMESH_H +#define DETOURSTATNAVMESH_H + +// Reference to navigation polygon. +typedef unsigned short dtPolyRef; + +// Maximum number of vertices per navigation polygon. +static const int DT_VERTS_PER_POLYGON = 6; + +// Structure holding the navigation polygon data. +struct dtPoly +{ + unsigned short v[DT_VERTS_PER_POLYGON]; // Indices to vertices of the poly. + dtPolyRef n[DT_VERTS_PER_POLYGON]; // Refs to neighbours of the poly. + unsigned char nv; // Number of vertices. + unsigned char flags; // Flags (not used). + unsigned char pad[2]; +}; + +const int DT_NAVMESH_MAGIC = 'NAVM'; +const int DT_NAVMESH_VERSION = 2; + +struct dtBVNode +{ + unsigned short bmin[3], bmax[3]; + int i; +}; + +struct dtStatNavMeshHeader +{ + int magic; + int version; + int npolys; + int nverts; + int nnodes; + float cs; + float bmin[3], bmax[3]; +}; + +class dtStatNavMesh +{ +public: + + dtStatNavMesh(); + ~dtStatNavMesh(); + + // Initializes the path finder with path data. + // Params: + // data - (in) Pointer to path data. + // dataSize - (in) size of the path data. + // ownsData - (in) Flag indicating if the pathfinder should delete the data. + bool init(unsigned char* data, int dataSize, bool ownsData); + + // Finds the nearest navigation polygon around the center location. + // Params: + // center - (in) The center of the search box. + // extents - (in) The extents of the search box. + dtPolyRef findNearestPoly(const float* center, const float* extents); + + // Returns polygons which touch the query box. + // Params: + // center - (in) the center of the search box. + // extents - (in) the extents of the search box. + // polys - (out) array holding the search result. + // maxPolys - (in) The max number of polygons the polys array can hold. + // Returns: Number of polygons in search result array. + int queryPolygons(const float* center, const float* extents, + unsigned short* polys, const int maxPolys); + + // Finds path from start polygon to end polygon. + // If target polygon canno be reached through the navigation graph, + // the last node on the array is nearest node to the end polygon. + // Params: + // startRef - (in) ref to path start polygon. + // endRef - (in) ref to path end polygon. + // path - (out) array holding the search result. + // maxPathSize - (in) The max number of polygons the path array can hold. + // Returns: Number of polygons in search result array. + int findPath(dtPolyRef startRef, dtPolyRef endRef, + dtPolyRef* path, const int maxPathSize); + + // Finds a straight path from start to end locations within the corridor + // described by the path polygons. + // Start and end locations will be clamped on the corridor. + // Params: + // startPos - (in) Path start location. + // endPos - (in) Path end location. + // path - (in) Array of connected polygons describing the corridor. + // pathSize - (in) Number of polygons in path array. + // straightPath - (out) Points describing the straight path. + // maxStraightPathSize - (in) The max number of points the straight path array can hold. + int findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, const int maxStraightPathSize); + + // Finds intersection againts walls starting from start pos. + // Params: + // startRef - (in) ref to the polygon where the start lies. + // startPos - (in) start position of the query. + // endPos - (in) end position of the query. + // t - (out) hit parameter along the segment, valid only if hit. + // endRef - (out) ref to the last polygon which was processed. + // Returns: True if hit wall. + // TODO: Return the whole corridor!! + bool raycast(dtPolyRef startRef, const float* startPos, const float* endPos, float& t, dtPolyRef& endRef); + + // Returns distance to nearest wall from the specified location. + // Params: + // centerRef - (in) ref to the polygon where the center lies. + // centerPos - (in) center if the query circle. + // maxRadius - (in) max search radius. + // hitPos - (out) location of the nearest hit. + // hitNormal - (out) normal of the nearest hit. + // Returns: Distance to nearest wall from the test location. + float findDistanceToWall(dtPolyRef centerRef, const float* centerPos, float maxRadius, + float* hitPos, float* hitNormal); + + // Finds polygons found along the navigation graph which touch the specified circle. + // Params: + // centerRef - (in) ref to the polygon where the center lies. + // centerPos - (in) center if the query circle + // radius - (in) radius of the query circle + // resultRef - (out, opt) refs to the polygons touched by the circle. + // resultParent - (out, opt) parent of each result polygon. + // resultCost - (out, opt) search cost at each result polygon. + // resultDepth - (out, opt) search depth at each result polygon. + // maxResult - (int) maximum capacity of search results. + // Returns: Number of results. + int findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius, + dtPolyRef* resultRef, dtPolyRef* resultParent, + unsigned short* resultCost, unsigned short* resultDepth, + const int maxResult); + + // Returns closest point on navigation polygon. + // Params: + // ref - (in) ref to the polygon. + // pos - (in) the point to check. + // closest - (out) closest point. + bool closestPointToPoly(dtPolyRef ref, const float* pos, float* closest) const; + + // Returns cost between two polygons. + unsigned short getCost(dtPolyRef from, dtPolyRef to) const; + + // Returns pointer to a polygon based on ref. + const dtPoly* getPolyByRef(dtPolyRef ref) const; + // Returns number of navigation polygons. + inline int getPolyCount() const { return m_header ? m_header->npolys : 0; } + // Rerturns pointer to specified navigation polygon. + inline const dtPoly* getPoly(int i) const { return &m_polys[i]; } + // Returns number of vertices. + inline int getVertexCount() const { return m_header ? m_header->nverts : 0; } + // Returns pointer to specified vertex. + inline const float* getVertex(int i) const { return &m_verts[i*3]; } + + bool isInOpenList(dtPolyRef ref) const; + + int getMemUsed() const; + + inline const dtStatNavMeshHeader* getHeader() const { return m_header; } + + inline const dtBVNode* getBvTreeNodes() const { return m_bvtree; } + inline int getBvTreeNodeCount() const { return m_header->nnodes; } + +private: + + // Copies the locations of vertices of a polygon to an array. + int getPolyVerts(dtPolyRef ref, float* verts); + // Returns portal points between two polygons. + bool getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right); + + unsigned char* m_data; + int m_dataSize; + + dtStatNavMeshHeader* m_header; + dtPoly* m_polys; + float* m_verts; + dtBVNode* m_bvtree; + + class dtNodePool* m_nodePool; + class dtNodeQueue* m_openList; +}; + +#endif // DETOURSTATNAVMESH_H diff --git a/Detour/Include/DetourStatNavMeshBuilder.h b/Detour/Include/DetourStatNavMeshBuilder.h new file mode 100755 index 0000000..34e8dc0 --- /dev/null +++ b/Detour/Include/DetourStatNavMeshBuilder.h @@ -0,0 +1,27 @@ +// +// 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. +// + +#ifndef DETOURSTATNAVMESHBUILDER_H +#define DETOURSTATNAVMESHBUILDER_H + +bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, + const unsigned short* polys, const int npolys, const int nvp, + const float* bmin, const float* bmax, float cs, float ch, + unsigned char** outData, int* outDataSize); + +#endif // DETOURSTATNAVMESHBUILDER_H \ No newline at end of file diff --git a/Detour/Source/DetourDebugDraw.cpp b/Detour/Source/DetourDebugDraw.cpp new file mode 100755 index 0000000..e16fd0c --- /dev/null +++ b/Detour/Source/DetourDebugDraw.cpp @@ -0,0 +1,183 @@ +// +// 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 "DetourDebugDraw.h" +#include "DetourStatNavMesh.h" +#include "SDL.h" +#include "SDL_Opengl.h" + +void dtDebugDrawStatNavMeshPoly(const dtStatNavMesh* mesh, dtPolyRef ref, const float* col) +{ + const dtPoly* p = mesh->getPolyByRef(ref); + if (!p) + return; + glColor4f(col[0],col[1],col[2],0.25f); + glBegin(GL_TRIANGLES); + unsigned short vi[3]; + for (int j = 2; j < (int)p->nv; ++j) + { + vi[0] = p->v[0]; + vi[1] = p->v[j-1]; + vi[2] = p->v[j]; + for (int k = 0; k < 3; ++k) + { + const float* v = mesh->getVertex(vi[k]); + glVertex3f(v[0], v[1]+0.2f, v[2]); + } + } + glEnd(); +} + +static void drawBoxWire(float minx, float miny, float minz, float maxx, float maxy, float maxz, const float* col) +{ + glColor4fv(col); + + // Top + glVertex3f(minx, miny, minz); + glVertex3f(maxx, miny, minz); + glVertex3f(maxx, miny, minz); + glVertex3f(maxx, miny, maxz); + glVertex3f(maxx, miny, maxz); + glVertex3f(minx, miny, maxz); + glVertex3f(minx, miny, maxz); + glVertex3f(minx, miny, minz); + + // bottom + glVertex3f(minx, maxy, minz); + glVertex3f(maxx, maxy, minz); + glVertex3f(maxx, maxy, minz); + glVertex3f(maxx, maxy, maxz); + glVertex3f(maxx, maxy, maxz); + glVertex3f(minx, maxy, maxz); + glVertex3f(minx, maxy, maxz); + glVertex3f(minx, maxy, minz); + + // Sides + glVertex3f(minx, miny, minz); + glVertex3f(minx, maxy, minz); + glVertex3f(maxx, miny, minz); + glVertex3f(maxx, maxy, minz); + glVertex3f(maxx, miny, maxz); + glVertex3f(maxx, maxy, maxz); + glVertex3f(minx, miny, maxz); + glVertex3f(minx, maxy, maxz); +} + +void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh) +{ + const float col[] = { 1,1,1,0.5f }; + const dtStatNavMeshHeader* hdr = mesh->getHeader(); + + const dtBVNode* nodes = mesh->getBvTreeNodes(); + int nnodes = mesh->getBvTreeNodeCount(); + + glBegin(GL_LINES); + + for (int i = 0; i < nnodes; ++i) + { + const dtBVNode* n = &nodes[i]; + if (n->i < 0) // Leaf indices are positive. + continue; + drawBoxWire(hdr->bmin[0] + n->bmin[0]*hdr->cs, + hdr->bmin[1] + n->bmin[1]*hdr->cs, + hdr->bmin[2] + n->bmin[2]*hdr->cs, + hdr->bmin[0] + n->bmax[0]*hdr->cs, + hdr->bmin[1] + n->bmax[1]*hdr->cs, + hdr->bmin[2] + n->bmax[2]*hdr->cs, col); + } + glEnd(); +} + +void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh) +{ + glColor4ub(0,196,255,64); + glBegin(GL_TRIANGLES); + for (int i = 0; i < mesh->getPolyCount(); ++i) + { + const dtPoly* p = mesh->getPoly(i); + unsigned short vi[3]; + for (int j = 2; j < (int)p->nv; ++j) + { + vi[0] = p->v[0]; + vi[1] = p->v[j-1]; + vi[2] = p->v[j]; + for (int k = 0; k < 3; ++k) + { + const float* v = mesh->getVertex(vi[k]); + glVertex3f(v[0], v[1]+0.2f, v[2]); + } + } + } + glEnd(); + + // Draw tri boundaries + glColor4ub(0,0,0,64); + glLineWidth(1.0f); + glBegin(GL_LINES); + for (int i = 0; i < mesh->getPolyCount(); ++i) + { + const dtPoly* p = mesh->getPoly(i); + for (int j = 0, nj = (int)p->nv; j < nj; ++j) + { + if (p->n[j] == 0) continue; + int vi[2]; + vi[0] = p->v[j]; + vi[1] = p->v[(j+1) % nj]; + for (int k = 0; k < 2; ++k) + { + const float* v = mesh->getVertex(vi[k]); + glVertex3f(v[0], v[1]+0.21f, v[2]); + } + } + } + glEnd(); + + // Draw boundaries + glLineWidth(3.0f); + glColor4ub(0,0,0,128); + glBegin(GL_LINES); + for (int i = 0; i < mesh->getPolyCount(); ++i) + { + const dtPoly* p = mesh->getPoly(i); + for (int j = 0, nj = (int)p->nv; j < nj; ++j) + { + if (p->n[j] != 0) continue; + int vi[2]; + vi[0] = p->v[j]; + vi[1] = p->v[(j+1) % nj]; + for (int k = 0; k < 2; ++k) + { + const float* v = mesh->getVertex(vi[k]); + glVertex3f(v[0], v[1]+0.21f, v[2]); + } + } + } + glEnd(); + glLineWidth(1.0f); + + glPointSize(4.0f); + glColor4ub(0,0,0,128); + glBegin(GL_POINTS); + for (int i = 0; i < mesh->getVertexCount(); ++i) + { + const float* v = mesh->getVertex(i); + glVertex3f(v[0], v[1]+0.21f, v[2]); + } + glEnd(); + glPointSize(1.0f); +} diff --git a/Detour/Source/DetourStatNavMesh.cpp b/Detour/Source/DetourStatNavMesh.cpp new file mode 100755 index 0000000..163ac16 --- /dev/null +++ b/Detour/Source/DetourStatNavMesh.cpp @@ -0,0 +1,1314 @@ +// +// 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 "DetourStatNavMesh.h" +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// + +template inline void swap(T& a, T& b) { T t = a; a = b; b = t; } +template inline T min(T a, T b) { return a < b ? a : b; } +template inline T max(T a, T b) { return a > b ? a : b; } +template inline T abs(T a) { return a < 0 ? -a : a; } +template inline T sqr(T a) { return a*a; } +template inline T clamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } + +// Some vector utils +inline void vcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +inline float vdot(const float* v1, const float* v2) +{ + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; +} + +inline void vsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +inline void vmin(float* mn, const float* v) +{ + mn[0] = min(mn[0], v[0]); + mn[1] = min(mn[1], v[1]); + mn[2] = min(mn[2], v[2]); +} + +inline void vmax(float* mx, const float* v) +{ + mx[0] = max(mx[0], v[0]); + mx[1] = max(mx[1], v[1]); + mx[2] = max(mx[2], v[2]); +} + +inline void vcopy(float* dest, const float* a) +{ + dest[0] = a[0]; + dest[1] = a[1]; + dest[2] = a[2]; +} + +inline float vdistSqr(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return dx*dx + dy*dy + dz*dz; +} + +inline void vnormalize(float* v) +{ + float d = 1.0f / sqrtf(sqr(v[0]) + sqr(v[1]) + sqr(v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +inline bool vequal(const float* p0, const float* p1) +{ + static const float thr = sqr(1.0f/16384.0f); + const float d = vdistSqr(p0, p1); + return d < thr; +} + +inline int nextPow2(int v) +{ + v--; + v |= v >> 1; + v |= v >> 2; + v |= v >> 4; + v |= v >> 8; + v |= v >> 16; + v++; + return v; +} + +inline float vdot2D(const float* u, const float* v) +{ + return u[0]*v[0] + u[2]*v[2]; +} +inline float vperp2D(const float* u, const float* v) +{ + return u[2]*v[0] - u[0]*v[2]; +} + +inline float triArea2D(const float* a, const float* b, const float* c) +{ + return ((b[0]*a[2] - a[0]*b[2]) + (c[0]*b[2] - b[0]*c[2]) + (a[0]*c[2] - c[0]*a[2])) * 0.5f; +} + +static void closestPtPointTriangle(float* closest, const float* p, + const float* a, const float* b, const float* c) +{ + // Check if P in vertex region outside A + float ab[3], ac[3], ap[3]; + vsub(ab, b, a); + vsub(ac, c, a); + vsub(ap, p, a); + float d1 = vdot(ab, ap); + float d2 = vdot(ac, ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + // barycentric coordinates (1,0,0) + vcopy(closest, a); + return; + } + + // Check if P in vertex region outside B + float bp[3]; + vsub(bp, p, b); + float d3 = vdot(ab, bp); + float d4 = vdot(ac, bp); + if (d3 >= 0.0f && d4 <= d3) + { + // barycentric coordinates (0,1,0) + vcopy(closest, b); + return; + } + + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + { + // barycentric coordinates (1-v,v,0) + float v = d1 / (d1 - d3); + closest[0] = a[0] + v * ab[0]; + closest[1] = a[1] + v * ab[1]; + closest[2] = a[2] + v * ab[2]; + return; + } + + // Check if P in vertex region outside C + float cp[3]; + vsub(cp, p, c); + float d5 = vdot(ab, cp); + float d6 = vdot(ac, cp); + if (d6 >= 0.0f && d5 <= d6) + { + // barycentric coordinates (0,0,1) + vcopy(closest, c); + return; + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5*d2 - d1*d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) + { + // barycentric coordinates (1-w,0,w) + float w = d2 / (d2 - d6); + closest[0] = a[0] + w * ac[0]; + closest[1] = a[1] + w * ac[1]; + closest[2] = a[2] + w * ac[2]; + return; + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3*d6 - d5*d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) + { + // barycentric coordinates (0,1-w,w) + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + closest[0] = b[0] + w * (c[0] - b[0]); + closest[1] = b[1] + w * (c[1] - b[1]); + closest[2] = b[2] + w * (c[2] - b[2]); + return; + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + closest[0] = a[0] + ab[0] * v + ac[0] * w; + closest[1] = a[1] + ab[1] * v + ac[1] * w; + closest[2] = a[2] + ab[2] * v + ac[2] * w; +} + +static bool intersectSegmentPoly2D(const float* p0, const float* p1, + const float* verts, int nverts, + float& tmin, float& tmax, + int& segMin, int& segMax) +{ + static const float EPS = 0.00000001f; + + tmin = 0; + tmax = 1; + segMin = -1; + segMax = -1; + + float dir[3]; + vsub(dir, p1, p0); + + for (int i = 0, j = nverts-1; i < nverts; j=i++) + { + float edge[3], diff[3]; + vsub(edge, &verts[i*3], &verts[j*3]); + vsub(diff, p0, &verts[j*3]); + float n = vperp2D(edge, diff); + float d = -vperp2D(edge, dir); + if (fabs(d) < EPS) + { + // S is nearly parallel to this edge + if (n < 0) + return false; + else + continue; + } + float t = n / d; + if (d < 0) + { + // segment S is entering across this edge + if (t > tmin) + { + tmin = t; + segMin = j; + // S enters after leaving polygon + if (tmin > tmax) + return false; + } + } + else + { + // segment S is leaving across this edge + if (t < tmax) + { + tmax = t; + segMax = j; + // S leaves before entering polygon + if (tmax < tmin) + return false; + } + } + } + + return true; +} + +static float distancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t) +{ + float pqx = q[0] - p[0]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqz*pqz; + t = pqx*dx + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + + return dx*dx + dz*dz; +} + +static void calcPolyCenter(float* tc, const dtPoly* p, const float* verts) +{ + tc[0] = 0.0f; + tc[1] = 0.0f; + tc[2] = 0.0f; + for (int j = 0; j < (int)p->nv; ++j) + { + const float* v = &verts[p->v[j]*3]; + tc[0] += v[0]; + tc[1] += v[1]; + tc[2] += v[2]; + } + const float s = 1.0f / p->nv; + tc[0] *= s; + tc[1] *= s; + tc[2] *= s; +} + + +////////////////////////////////////////////////////////////////////////////////////////// +struct dtNode +{ + enum dtNodeFlags + { + OPEN = 0x01, + CLOSED = 0x02, + }; + dtNode* parent; + unsigned short cost; + unsigned short total; + unsigned short id; + unsigned short flags; +}; + +class dtNodePool +{ +public: + dtNodePool(int maxNodes, int hashSize); + ~dtNodePool(); + inline void operator=(const dtNodePool&) {} + void clear(); + dtNode* getNode(unsigned short id); + const dtNode* findNode(unsigned short id) const; + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtNode)*m_maxNodes + + sizeof(unsigned short)*m_maxNodes + + sizeof(unsigned short)*m_hashSize; + } + +private: + inline unsigned int hashint(unsigned int a) const + { + a += ~(a<<15); + a ^= (a>>10); + a += (a<<3); + a ^= (a>>6); + a += ~(a<<11); + a ^= (a>>16); + return a; + } + + dtNode* m_nodes; + unsigned short* m_first; + unsigned short* m_next; + const int m_maxNodes; + const int m_hashSize; + int m_nodeCount; +}; + +dtNodePool::dtNodePool(int maxNodes, int hashSize) : + m_maxNodes(maxNodes), + m_hashSize(hashSize), + m_nodes(0), + m_first(0), + m_next(0) +{ + m_nodes = new dtNode[m_maxNodes]; + m_next = new unsigned short[m_maxNodes]; + m_first = new unsigned short[hashSize]; + memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); + memset(m_next, 0xff, sizeof(unsigned short)*m_maxNodes); +} + +dtNodePool::~dtNodePool() +{ + delete [] m_nodes; + delete [] m_next; + delete [] m_first; +} + +void dtNodePool::clear() +{ + memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); + m_nodeCount = 0; +} + +const dtNode* dtNodePool::findNode(unsigned short id) const +{ + unsigned int bucket = hashint((unsigned int)id) & (m_hashSize-1); + unsigned short i = m_first[bucket]; + while (i != 0xffff) + { + if (m_nodes[i].id == id) + return &m_nodes[i]; + i = m_next[i]; + } + return 0; +} + +dtNode* dtNodePool::getNode(unsigned short id) +{ + unsigned int bucket = hashint((unsigned int)id) & (m_hashSize-1); + unsigned short i = m_first[bucket]; + dtNode* node = 0; + while (i != 0xffff) + { + if (m_nodes[i].id == id) + return &m_nodes[i]; + i = m_next[i]; + } + + if (m_nodeCount >= m_maxNodes) + return 0; + + i = (unsigned short)m_nodeCount; + m_nodeCount++; + + // Init node + node = &m_nodes[i]; + node->parent = 0; + node->cost = 0; + node->total = 0; + node->id = id; + node->flags = 0; + + m_next[i] = m_first[bucket]; + m_first[bucket] = i; + + return node; +} + + +////////////////////////////////////////////////////////////////////////////////////////// +class dtNodeQueue +{ +public: + dtNodeQueue(int n); + ~dtNodeQueue(); + inline void operator=(dtNodeQueue&) {} + + inline void clear() + { + m_size = 0; + } + + inline dtNode* top() + { + return m_heap[0]; + } + + inline dtNode* pop() + { + dtNode* result = m_heap[0]; + m_size--; + trickleDown(0, m_heap[m_size]); + return result; + } + + inline void push(dtNode* node) + { + m_size++; + bubbleUp(m_size-1, node); + } + + inline void modify(dtNode* node) + { + for (unsigned i = 0; i < m_size; ++i) + { + if (m_heap[i] == node) + { + bubbleUp(i, node); + return; + } + } + } + + inline bool empty() const { return m_size == 0; } + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtNode*)*(m_capacity+1); + } + + +private: + void bubbleUp(int i, dtNode* node); + void trickleDown(int i, dtNode* node); + + dtNode** m_heap; + const int m_capacity; + int m_size; +}; + +dtNodeQueue::dtNodeQueue(int n) : + m_capacity(n), + m_size(0), + m_heap(0) +{ + m_heap = new dtNode*[m_capacity+1]; +} + +dtNodeQueue::~dtNodeQueue() +{ + delete [] m_heap; +} + +void dtNodeQueue::bubbleUp(int i, dtNode* node) +{ + int parent = (i-1)/2; + // note: (index > 0) means there is a parent + while ((i > 0) && (m_heap[parent]->total > node->total)) + { + m_heap[i] = m_heap[parent]; + i = parent; + parent = (i-1)/2; + } + m_heap[i] = node; +} + +void dtNodeQueue::trickleDown(int i, dtNode* node) +{ + int child = (i*2)+1; + while (child < m_size) + { + if (((child+1) < m_size) && + (m_heap[child]->total > m_heap[child+1]->total)) + { + child++; + } + m_heap[i] = m_heap[child]; + i = child; + child = (i*2)+1; + } + bubbleUp(i, node); +} + + +////////////////////////////////////////////////////////////////////////////////////////// +dtStatNavMesh::dtStatNavMesh() : + m_header(0), + m_polys(0), + m_verts(0), + m_bvtree(0), + m_nodePool(0), + m_openList(0), + m_data(0), + m_dataSize(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) +{ + m_header = (dtStatNavMeshHeader*)data; + if (m_header->magic != DT_NAVMESH_MAGIC) + { + return false; + } + if (m_header->version != DT_NAVMESH_VERSION) + { + return false; + } + + const int headerSize = sizeof(dtStatNavMeshHeader); + const int vertsSize = sizeof(float)*3*m_header->nverts; + const int polysSize = sizeof(dtPoly)*m_header->npolys; + + m_verts = (float*)(data + headerSize); + m_polys = (dtPoly*)(data + headerSize + vertsSize); + m_bvtree = (dtBVNode*)(data + headerSize + vertsSize + polysSize); + + 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; + } + + return true; +} + +unsigned short dtStatNavMesh::getCost(dtPolyRef from, dtPolyRef to) const +{ + const dtPoly* fromPoly = getPoly(from-1); + const dtPoly* toPoly = getPoly(to-1); + float fromPc[3], toPc[3]; + calcPolyCenter(fromPc, fromPoly, m_verts); + calcPolyCenter(toPc, toPoly, m_verts); + int cost = (int)sqrtf(sqr(fromPc[0]-toPc[0]) + sqr(fromPc[2]-toPc[2])); + if (cost < 1) cost = 1; + if (cost > 0xffff) cost = 0xffff; + return cost; +} + +const dtPoly* dtStatNavMesh::getPolyByRef(dtPolyRef ref) const +{ + if (!m_header || ref == 0 || (int)ref > m_header->npolys) return 0; + return &m_polys[ref-1]; +} + +int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef, + dtPolyRef* path, const int maxPathSize) +{ + if (!startRef || !endRef) + return 0; + + if (!maxPathSize) + return 0; + + if (startRef == endRef) + { + path[0] = startRef; + return 1; + } + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(startRef); + startNode->parent = 0; + startNode->cost = 0; + startNode->total = getCost(startRef, endRef); + startNode->id = startRef; + startNode->flags = dtNode::OPEN; + m_openList->push(startNode); + + dtNode* lastBestNode = startNode; + unsigned short lastBestNodeCost = startNode->total; + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + + if (bestNode->id == endRef) + { + lastBestNode = bestNode; + break; + } + + const dtPoly* poly = getPoly(bestNode->id-1); + for (int i = 0; i < (int)poly->nv; ++i) + { + dtPolyRef neighbour = poly->n[i]; + if (neighbour) + { + // Skip parent node. + if (bestNode->parent && bestNode->parent->id == neighbour) + continue; + + dtNode newNode; + newNode.parent = bestNode; + newNode.id = neighbour; + newNode.cost = bestNode->cost + getCost(newNode.parent->id, newNode.id); + unsigned short costToGoal = getCost(newNode.id, endRef); + newNode.total = newNode.cost + costToGoal; + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & dtNode::OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & dtNode::CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~dtNode::CLOSED; + actualNode->parent = newNode.parent; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (costToGoal < lastBestNodeCost) + { + lastBestNodeCost = costToGoal; + lastBestNode = actualNode; + } + + if (actualNode->flags & dtNode::OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags = dtNode::OPEN; + m_openList->push(actualNode); + } + } + } + } + } + + // Reverse the path. + dtNode* prev = 0; + dtNode* node = lastBestNode; + do + { + dtNode* next = node->parent; + node->parent = prev; + prev = node; + node = next; + } + while (node); + + // Store path + node = prev; + int n = 0; + do + { + path[n++] = node->id; + node = node->parent; + } + while (node && n < maxPathSize); + + return n; +} + +bool dtStatNavMesh::closestPointToPoly(dtPolyRef ref, const float* pos, float* closest) const +{ + const dtPoly* poly = getPolyByRef(ref); + if (!poly) + return false; + + float closestDistSqr = FLT_MAX; + + for (int i = 2; i < (int)poly->nv; ++i) + { + const float* v0 = getVertex(poly->v[0]); + const float* v1 = getVertex(poly->v[i-1]); + const float* v2 = getVertex(poly->v[i]); + + float pt[3]; + closestPtPointTriangle(pt, pos, v0, v1, v2); + float d = vdistSqr(pos, pt); + if (d < closestDistSqr) + { + vcopy(closest, pt); + closestDistSqr = d; + } + } + + return true; +} + +int dtStatNavMesh::findStraightPath(const float* startPos, const float* endPos, + const dtPolyRef* path, const int pathSize, + float* straightPath, const int maxStraightPathSize) +{ + 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(dtPolyRef ref, float* verts) +{ + const dtPoly* poly = getPolyByRef(ref); + if (!poly) + return 0; + float* v = verts; + for (int i = 0; i < (int)poly->nv; ++i) + { + const float* cv = &m_verts[poly->v[i]*3]; + *v++ = cv[0]; + *v++ = cv[1]; + *v++ = cv[2]; + } + return (int)poly->nv; +} + +bool dtStatNavMesh::raycast(dtPolyRef centerRef, const float* startPos, const float* endPos, + float& t, dtPolyRef& endRef) +{ + endRef = centerRef; + + if (!centerRef) + return 0; + + dtPolyRef prevRef = centerRef; + dtPolyRef curRef = centerRef; + t = 0; + + float verts[DT_VERTS_PER_POLYGON*3]; + + while (curRef) + { + // Cast ray against current polygon. + int nv = getPolyVerts(curRef, verts); + if (nv < 3) + { + // Hit bad polygon, report hit. + return true; + } + + 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 true; + } + // Keep track of furthest t so far. + if (tmax > t) + t = tmax; + + endRef = curRef; + + // Check the neighbour of this polygon. + const dtPoly* poly = getPolyByRef(curRef); + dtPolyRef nextRef = poly->n[segMax]; + if (!nextRef) + { + // No neighbour, we hit a wall. + return true; + } + + // No hit, advance to neighbour polygon. + prevRef = curRef; + curRef = nextRef; + } + + return 0; +} + + +float dtStatNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* centerPos, float maxRadius, + float* hitPos, float* hitNormal) +{ + if (!centerRef) + return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(centerRef); + startNode->parent = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = dtNode::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 dtPoly* 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. + dtPolyRef neighbour = poly->n[j]; + + // Skip parent node. + if (bestNode->parent && bestNode->parent->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 newNode; + newNode.parent = bestNode; + newNode.id = neighbour; + newNode.cost = bestNode->cost + 1; // Depth + newNode.total = bestNode->total + getCost(newNode.parent->id, newNode.id); + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & dtNode::OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & dtNode::CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~dtNode::CLOSED; + actualNode->parent = newNode.parent; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (actualNode->flags & dtNode::OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags = dtNode::OPEN; + m_openList->push(actualNode); + } + } + } + } + + // Calc hit normal. + vsub(hitNormal, centerPos, hitPos); + vnormalize(hitNormal); + + return sqrtf(radiusSqr); +} + +int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius, + dtPolyRef* resultRef, dtPolyRef* resultParent, + unsigned short* resultCost, unsigned short* resultDepth, + const int maxResult) +{ + if (!centerRef) + return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtNode* startNode = m_nodePool->getNode(centerRef); + startNode->parent = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = dtNode::OPEN; + m_openList->push(startNode); + + unsigned n = 0; + if (n < maxResult) + { + if (resultRef) + resultRef[n] = startNode->id; + if (resultParent) + resultParent[n] = 0; + if (resultCost) + resultCost[n] = 0; + if (resultDepth) + resultDepth[n] = 0; + ++n; + } + + const float radiusSqr = sqr(radius); + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + const dtPoly* poly = getPoly(bestNode->id-1); + for (unsigned i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j=i++) + { + dtPolyRef neighbour = poly->n[j]; + + if (neighbour) + { + // Skip parent node. + if (bestNode->parent && bestNode->parent->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 newNode; + newNode.parent = bestNode; + newNode.id = neighbour; + newNode.cost = bestNode->cost + 1; // Depth + newNode.total = bestNode->total + getCost(newNode.parent->id, newNode.id); + + dtNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & dtNode::OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & dtNode::CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~dtNode::CLOSED; + actualNode->parent = newNode.parent; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (actualNode->flags & dtNode::OPEN) + { + m_openList->modify(actualNode); + } + else + { + if (n < maxResult) + { + if (resultRef) + resultRef[n] = actualNode->id; + if (resultParent) + resultParent[n] = actualNode->parent->id; + if (resultCost) + resultCost[n] = actualNode->total; + if (resultDepth) + resultDepth[n] = actualNode->cost; + ++n; + } + actualNode->flags = dtNode::OPEN; + m_openList->push(actualNode); + } + } + } + } + } + + return n; +} + +inline bool checkOverlapBox(const unsigned short amin[3], const unsigned short amax[3], + const unsigned short bmin[3], const unsigned short bmax[3]) +{ + bool overlap = true; + overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; + overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; + overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; + return overlap; +} + +// Returns polygons which are withing certain radius from the query location. +int dtStatNavMesh::queryPolygons(const float* center, const float* extents, + unsigned short* ids, const int maxIds) +{ + const dtBVNode* node = &m_bvtree[0]; + const dtBVNode* end = &m_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 + unsigned 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) + { + ids[n] = (unsigned short)node->i; + n++; + } + } + + if (overlap || isLeafNode) + node++; + else + { + const int escapeIndex = -node->i; + node += escapeIndex; + } + } + + return n; +} + +dtPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* extents) +{ + // Get nearby polygons from proximity grid. + unsigned short polys[128]; + int npolys = queryPolygons(center, extents, polys, 128); + + // Find nearest polygon amongst the nearby polygons. + dtPolyRef nearest = 0; + float nearestDistanceSqr = FLT_MAX; + for (int i = 0; i < npolys; ++i) + { + dtPolyRef ref = (dtPolyRef)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(dtPolyRef from, dtPolyRef to, float* left, float* right) +{ + const dtPoly* 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::isInOpenList(dtPolyRef ref) const +{ + if (!m_nodePool) return false; + return m_nodePool->findNode(ref) != 0; +} + +int dtStatNavMesh::getMemUsed() const +{ + if (!m_nodePool || ! m_openList) + return 0; + return sizeof(*this) + m_dataSize + + m_nodePool->getMemUsed() + + m_openList->getMemUsed(); +} diff --git a/Detour/Source/DetourStatNavMeshBuilder.cpp b/Detour/Source/DetourStatNavMeshBuilder.cpp new file mode 100755 index 0000000..400e9c2 --- /dev/null +++ b/Detour/Source/DetourStatNavMeshBuilder.cpp @@ -0,0 +1,291 @@ +// +// 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 +#include +#include +#include "DetourStatNavMesh.h" + +struct BVItem +{ + unsigned short bmin[3]; + unsigned short bmax[3]; + int i; +}; + +static int compareItemX(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[0] < b->bmin[0]) + return -1; + if (a->bmin[0] > b->bmin[0]) + return 1; + return 0; +} + +static int compareItemY(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[1] < b->bmin[1]) + return -1; + if (a->bmin[1] > b->bmin[1]) + return 1; + return 0; +} + +static int compareItemZ(const void* va, const void* vb) +{ + const BVItem* a = (const BVItem*)va; + const BVItem* b = (const BVItem*)vb; + if (a->bmin[2] < b->bmin[2]) + return -1; + if (a->bmin[2] > b->bmin[2]) + return 1; + return 0; +} + +void calcExtends(BVItem* items, int nitems, int imin, int imax, + unsigned short* bmin, unsigned short* bmax) +{ + bmin[0] = items[imin].bmin[0]; + bmin[1] = items[imin].bmin[1]; + bmin[2] = items[imin].bmin[2]; + + bmax[0] = items[imin].bmax[0]; + bmax[1] = items[imin].bmax[1]; + bmax[2] = items[imin].bmax[2]; + + for (unsigned i = imin+1; i < imax; ++i) + { + const BVItem& it = items[i]; + if (it.bmin[0] < bmin[0]) bmin[0] = it.bmin[0]; + if (it.bmin[1] < bmin[1]) bmin[1] = it.bmin[1]; + if (it.bmin[2] < bmin[2]) bmin[2] = it.bmin[2]; + + if (it.bmax[0] > bmax[0]) bmax[0] = it.bmax[0]; + if (it.bmax[1] > bmax[1]) bmax[1] = it.bmax[1]; + if (it.bmax[2] > bmax[2]) bmax[2] = it.bmax[2]; + } +} + +inline int longestAxis(unsigned short x, unsigned short y, unsigned short z) +{ + int axis = 0; + unsigned short maxVal = x; + if (y > maxVal) + { + axis = 1; + maxVal = y; + } + if (z > maxVal) + { + axis = 2; + maxVal = z; + } + return axis; +} + +void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNode, dtBVNode* nodes) +{ + int inum = imax - imin; + int icur = curNode; + + dtBVNode& node = nodes[curNode++]; + + if (inum == 1) + { + // Leaf + node.bmin[0] = items[imin].bmin[0]; + node.bmin[1] = items[imin].bmin[1]; + node.bmin[2] = items[imin].bmin[2]; + + node.bmax[0] = items[imin].bmax[0]; + node.bmax[1] = items[imin].bmax[1]; + node.bmax[2] = items[imin].bmax[2]; + + node.i = items[imin].i; + } + else + { + // Split + calcExtends(items, nitems, imin, imax, node.bmin, node.bmax); + + int axis = longestAxis(node.bmax[0] - node.bmin[0], + node.bmax[1] - node.bmin[1], + node.bmax[2] - node.bmin[2]); + + if (axis == 0) + { + // Sort along x-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemX); + } + else if (axis == 1) + { + // Sort along y-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemY); + } + else + { + // Sort along z-axis + qsort(items+imin, inum, sizeof(BVItem), compareItemZ); + } + + int isplit = imin+inum/2; + + // Left + subdivide(items, nitems, imin, isplit, curNode, nodes); + // Right + subdivide(items, nitems, isplit, imax, curNode, nodes); + + int iescape = curNode - icur; + // Negative index means escape. + node.i = -iescape; + } +} + +/*struct rcPolyMesh +{ + inline rcPolyMesh() : verts(0), polys(0), nverts(0), npolys(0), nvp(3) {} + inline ~rcPolyMesh() { delete [] verts; delete [] polys; } + unsigned short* verts; + unsigned short* polys; + int nverts; + int npolys; + int nvp; +};*/ + +int createBVTree(const unsigned short* verts, const int nverts, + const unsigned short* polys, const int npolys, const int nvp, + float cs, float ch, + int nnodes, dtBVNode* nodes) +{ + // Build tree + BVItem* items = new BVItem[npolys]; + for (int i = 0; i < npolys; i++) + { + BVItem& it = items[i]; + it.i = i+1; + // Calc polygon bounds. + const unsigned short* p = &polys[i*nvp*2]; + it.bmin[0] = it.bmax[0] = verts[p[0]*3+0]; + it.bmin[1] = it.bmax[1] = verts[p[0]*3+1]; + it.bmin[2] = it.bmax[2] = verts[p[0]*3+2]; + + for (int j = 1; j < nvp; ++j) + { + if (p[j] == 0xffff) break; + unsigned short x = verts[p[j]*3+0]; + unsigned short y = verts[p[j]*3+1]; + unsigned short z = verts[p[j]*3+2]; + + if (x < it.bmin[0]) it.bmin[0] = x; + if (y < it.bmin[1]) it.bmin[1] = y; + if (z < it.bmin[2]) it.bmin[2] = z; + + if (x > it.bmax[0]) it.bmax[0] = x; + if (y > it.bmax[1]) it.bmax[1] = y; + if (z > it.bmax[2]) it.bmax[2] = z; + } + // Remap y + it.bmin[1] = (unsigned short)floorf((float)it.bmin[1]*ch/cs); + it.bmax[1] = (unsigned short)ceilf((float)it.bmax[1]*ch/cs); + } + + int curNode = 0; + subdivide(items, npolys, 0, npolys, curNode, nodes); + + delete [] items; + + return curNode; +} + + +bool dtCreateNavMeshData(const unsigned short* verts, const int nverts, + const unsigned short* polys, const int npolys, const int nvp, + const float* bmin, const float* bmax, float cs, float ch, + unsigned char** outData, int* outDataSize) +{ + if (nvp != DT_VERTS_PER_POLYGON) + return false; + + // Calculate data size + const int headerSize = sizeof(dtStatNavMeshHeader); + const int vertsSize = sizeof(float)*3*nverts; + const int polysSize = sizeof(dtPoly)*npolys; + const int nodesSize = sizeof(dtBVNode)*npolys*2; + + const int dataSize = headerSize + vertsSize + polysSize + nodesSize; + unsigned char* data = new unsigned char[dataSize]; + if (!data) + return false; + memset(data, 0, dataSize); + + dtStatNavMeshHeader* header = (dtStatNavMeshHeader*)(data); + float* navVerts = (float*)(data + headerSize); + dtPoly* navPolys = (dtPoly*)(data + headerSize + vertsSize); + dtBVNode* nodes = (dtBVNode*)(data + headerSize + vertsSize + polysSize); + + // Store header + header->magic = DT_NAVMESH_MAGIC; + header->version = DT_NAVMESH_VERSION; + header->npolys = npolys; + header->nverts = nverts; + header->cs = cs; + header->bmin[0] = bmin[0]; + header->bmin[1] = bmin[1]; + header->bmin[2] = bmin[2]; + header->bmax[0] = bmax[0]; + header->bmax[1] = bmax[1]; + header->bmax[2] = bmax[2]; + + // Store vertices + for (int i = 0; i < nverts; ++i) + { + const unsigned short* iv = &verts[i*3]; + float* v = &navVerts[i*3]; + v[0] = bmin[0] + iv[0] * cs; + v[1] = bmin[1] + iv[1] * ch; + v[2] = bmin[2] + iv[2] * cs; + } + + // Store polygons + const unsigned short* src = polys; + for (int i = 0; i < npolys; ++i) + { + dtPoly* p = &navPolys[i]; + p->nv = 0; + for (int j = 0; j < nvp; ++j) + { + if (src[j] == 0xffff) break; + p->v[j] = src[j]; + p->n[j] = src[nvp+j]+1; + p->nv++; + } + src += nvp*2; + } + + header->nnodes = createBVTree(verts, nverts, polys, npolys, nvp, + cs, ch, npolys*2, nodes); + + *outData = data; + *outDataSize = dataSize; + + return true; +} diff --git a/Readme.txt b/Readme.txt index 6fdf3d9..0bcce04 100644 --- a/Readme.txt +++ b/Readme.txt @@ -35,6 +35,12 @@ The project files with this distribution can be compiled with Microsoft Visual C Release Notes +---------------- +* Recast 1.1 + Released April 11th, 2009 + +This is the first release of Detour. + ---------------- * Recast 1.0 Released March 29th, 2009 diff --git a/Recast/Source/RecastMesh.cpp b/Recast/Source/RecastMesh.cpp index 243298d..f357410 100644 --- a/Recast/Source/RecastMesh.cpp +++ b/Recast/Source/RecastMesh.cpp @@ -384,10 +384,10 @@ static int countPolyVerts(const unsigned short* p, const int nvp) return nvp; } -inline bool uleftOn(const unsigned short* a, const unsigned short* b, const unsigned short* c) +inline bool uleft(const unsigned short* a, const unsigned short* b, const unsigned short* c) { return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) - - ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) <= 0; + ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) < 0; } static int getPolyMergeValue(unsigned short* pa, unsigned short* pb, @@ -436,13 +436,13 @@ static int getPolyMergeValue(unsigned short* pa, unsigned short* pb, va = pa[(ea+na-1) % na]; vb = pa[ea]; vc = pb[(eb+2) % nb]; - if (!uleftOn(&verts[va*3], &verts[vb*3], &verts[vc*3])) + if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3])) return -1; va = pb[(eb+nb-1) % nb]; vb = pb[eb]; vc = pa[(ea+2) % na]; - if (!uleftOn(&verts[va*3], &verts[vb*3], &verts[vc*3])) + if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3])) return -1; va = pa[ea];