From 4787a8d2231f771266488b5bf70c179e3f033815 Mon Sep 17 00:00:00 2001 From: Mikko Mononen Date: Thu, 9 Jul 2009 08:32:30 +0000 Subject: [PATCH] Added ugly, but functional version of TiledNavMesh and a builder for the same. Fixed a cost bug in the dtStatNavMesh::findPolysAround. Added debug draw functions for tiled navmesh. --- Detour/Include/DetourDebugDraw.h | 4 + Detour/Include/DetourStatNavMesh.h | 4 +- Detour/Include/DetourTiledNavMesh.h | 209 +++ Detour/Include/DetourTiledNavMeshBuilder.h | 9 + Detour/Source/DetourDebugDraw.cpp | 241 +++ Detour/Source/DetourStatNavMesh.cpp | 34 +- Detour/Source/DetourTiledNavMesh.cpp | 1772 +++++++++++++++++++ Detour/Source/DetourTiledNavMeshBuilder.cpp | 190 ++ 8 files changed, 2431 insertions(+), 32 deletions(-) create mode 100644 Detour/Include/DetourTiledNavMesh.h create mode 100644 Detour/Include/DetourTiledNavMeshBuilder.h create mode 100644 Detour/Source/DetourTiledNavMesh.cpp create mode 100644 Detour/Source/DetourTiledNavMeshBuilder.cpp diff --git a/Detour/Include/DetourDebugDraw.h b/Detour/Include/DetourDebugDraw.h index 66586fe..81a851f 100755 --- a/Detour/Include/DetourDebugDraw.h +++ b/Detour/Include/DetourDebugDraw.h @@ -20,9 +20,13 @@ #define DETOURDEBUGDRAW_H #include "DetourStatNavMesh.h" +#include "DetourTiledNavMesh.h" void dtDebugDrawStatNavMeshPoly(const dtStatNavMesh* mesh, dtPolyRef ref, const float* col); void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh); void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh); +void dtDebugDrawTiledNavMesh(const dtTiledNavMesh* mesh); +void dtDebugDrawTiledNavMeshPoly(const dtTiledNavMesh* mesh, dtTilePolyRef ref, const float* col); + #endif // DETOURDEBUGDRAW_H \ No newline at end of file diff --git a/Detour/Include/DetourStatNavMesh.h b/Detour/Include/DetourStatNavMesh.h index 062551f..830f506 100755 --- a/Detour/Include/DetourStatNavMesh.h +++ b/Detour/Include/DetourStatNavMesh.h @@ -84,7 +84,7 @@ public: // 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); + dtPolyRef* polys, const int maxPolys); // Finds path from start polygon to end polygon. // If target polygon canno be reached through the navigation graph, @@ -184,8 +184,6 @@ private: float getCost(dtPolyRef prev, dtPolyRef from, dtPolyRef to) const; float getHeuristic(dtPolyRef from, dtPolyRef to) const; - bool getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const; - // Copies the locations of vertices of a polygon to an array. int getPolyVerts(dtPolyRef ref, float* verts) const; // Returns portal points between two polygons. diff --git a/Detour/Include/DetourTiledNavMesh.h b/Detour/Include/DetourTiledNavMesh.h new file mode 100644 index 0000000..45272c0 --- /dev/null +++ b/Detour/Include/DetourTiledNavMesh.h @@ -0,0 +1,209 @@ +// +// 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 DETOURTILEDNAVMESH_H +#define DETOURTILEDNAVMESH_H + +// Reference to navigation polygon. +typedef unsigned int dtTilePolyRef; + +static const int DT_TILE_REF_SALT_BITS = 12; +static const int DT_TILE_REF_SALT_MASK = (1<> (DT_TILE_REF_POLY_BITS+DT_TILE_REF_TILE_BITS)) & DT_TILE_REF_SALT_MASK; + it = ((ref >> DT_TILE_REF_POLY_BITS) & DT_TILE_REF_TILE_MASK) - 1; + ip = ref & DT_TILE_REF_POLY_MASK; +} + +static const int DT_TILE_LOOKUP_SIZE = DT_MAX_TILES/4; + +class dtTiledNavMesh +{ +public: + dtTiledNavMesh(); + ~dtTiledNavMesh(); + + bool init(const float* orig, float tileSize, float portalHeight); + bool addTile(int x, int y, unsigned char* data, int dataSize); + bool removeTile(int x, int y); + dtTile* getTile(int x, int y); + const dtTile* getTile(int i) const { return &m_tiles[i]; } + + // 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. + // Returns: Reference identifier for the polygon, or 0 if no polygons found. + dtTilePolyRef 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, + dtTilePolyRef* polys, const int maxPolys); + + bool closestPointToPoly(dtTilePolyRef ref, const float* pos, float* closest) const; + + int getPolyNeighbours(dtTilePolyRef ref, dtTilePolyRef* nei, int maxNei) const; + + int findPath(dtTilePolyRef startRef, dtTilePolyRef endRef, + dtTilePolyRef* path, const int maxPathSize); + + int findStraightPath(const float* startPos, const float* endPos, + const dtTilePolyRef* path, const int pathSize, + float* straightPath, const int maxStraightPathSize); + + int raycast(dtTilePolyRef startRef, const float* startPos, const float* endPos, + float& t, dtTilePolyRef* path, const int pathSize); + + int findPolysAround(dtTilePolyRef centerRef, const float* centerPos, float radius, + dtTilePolyRef* resultRef, dtTilePolyRef* resultParent, + float* resultCost, unsigned short* resultDepth, + const int maxResult); + + float findDistanceToWall(dtTilePolyRef centerRef, const float* centerPos, float maxRadius, + float* hitPos, float* hitNormal); + + inline const dtTilePoly* getPolyByRef(dtTilePolyRef ref) const + { + unsigned int salt, it, ip; + decodeId(ref, salt, it, ip); + if (it >= DT_MAX_TILES) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + if (ip >= m_tiles[it].header->npolys) return 0; + return &m_tiles[it].header->polys[ip]; + } + + inline const float* getPolyVertsByRef(dtTilePolyRef ref) const + { + unsigned int salt, it, ip; + decodeId(ref, salt, it, ip); + if (it >= DT_MAX_TILES) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + if (ip >= m_tiles[it].header->npolys) return 0; + return m_tiles[it].header->verts; + } + +private: + + dtTilePolyRef getTileId(dtTile* tile); + dtTile* getNeighbourTile(int x, int y, int side); + + void buildIntLinks(dtTile* tile); + void buildExtLinks(dtTile* tile, dtTile* target, int side); + void removeExtLinks(dtTile* tile, int side); + + void createConnections(dtTile* tilea, dtTile* tileb, int sidea); + void removeConnections(dtTile* tile, int side); + int queryTilePolygons(dtTile* tile, const float* qmin, const float* qmax, + dtTilePolyRef* polys, const int maxPolys); + int findConnectingPolys(const float* va, const float* vb, + dtTile* tile, int side, + dtTilePolyRef* con, float* conarea, int maxcon); + + float getCost(dtTilePolyRef prev, dtTilePolyRef from, dtTilePolyRef to) const; + float getHeuristic(dtTilePolyRef from, dtTilePolyRef to) const; + + // Returns portal points between two polygons. + bool getPortalPoints(dtTilePolyRef from, dtTilePolyRef to, float* left, float* right) const; + + float m_orig[3]; + float m_tileSize; + float m_portalHeight; + + dtTile* m_posLookup[DT_TILE_LOOKUP_SIZE]; + dtTile* m_nextFree; + dtTile m_tiles[DT_MAX_TILES]; + + dtTileLink* m_tmpLinks; + int m_ntmpLinks; + + class dtTileNodePool* m_nodePool; + class dtTileNodeQueue* m_openList; +}; + +#endif // DETOURTILEDNAVMESH_H diff --git a/Detour/Include/DetourTiledNavMeshBuilder.h b/Detour/Include/DetourTiledNavMeshBuilder.h new file mode 100644 index 0000000..49b4ef4 --- /dev/null +++ b/Detour/Include/DetourTiledNavMeshBuilder.h @@ -0,0 +1,9 @@ +#ifndef DETOURTILEDNAVMESHBUILDER_H +#define DETOURTILEDNAVMESHBUILDER_H + +bool dtCreateNavMeshTileData(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, int tileSize, int walkableClimb, + unsigned char** outData, int* outDataSize); + +#endif // DETOURTILEDNAVMESHBUILDER_H \ No newline at end of file diff --git a/Detour/Source/DetourDebugDraw.cpp b/Detour/Source/DetourDebugDraw.cpp index 59ee322..d7f932b 100755 --- a/Detour/Source/DetourDebugDraw.cpp +++ b/Detour/Source/DetourDebugDraw.cpp @@ -186,3 +186,244 @@ void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh) glEnd(); glPointSize(1.0f); } + + +static void drawTile(const dtTileHeader* header) +{ + const float col[4] = {0,0,0,0.25f}; + glBegin(GL_LINES); + drawBoxWire(header->bmin[0],header->bmin[1],header->bmin[2], + header->bmax[0],header->bmax[1],header->bmax[2], col); + glEnd(); + + glBegin(GL_TRIANGLES); + for (int i = 0; i < header->npolys; ++i) + { + const dtTilePoly* p = &header->polys[i]; + + glColor4ub(0,196,255,64); + + 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 = &header->verts[vi[k]*3]; + glVertex3f(v[0], v[1]+0.2f, v[2]); + } + } + } + glEnd(); + + // Draw tri boundaries + glLineWidth(1.5f); + glBegin(GL_LINES); + for (int i = 0; i < header->npolys; ++i) + { + const dtTilePoly* p = &header->polys[i]; + for (int j = 0, nj = (int)p->nv; j < nj; ++j) + { + if (p->n[j] == 0) continue; + if (p->n[j] & 0x8000) + { + bool con = false; + for (int k = 0; k < p->nlinks; ++k) + { + if (header->links[p->links+k].e == j) + { + con = true; + break; + } + } + if (con) + glColor4ub(255,255,255,128); + else + glColor4ub(0,0,0,128); + } + else + glColor4ub(0,48,64,32); + /* + { + // Portal + int side = (p->n[j] >> 13) & 3; + int i = p->n[j] & 0x1fff; + if (!header->portals[side][i].ncon) 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 = &header->verts[vi[k]*3]; + glVertex3f(v[0], v[1]+0.21f, v[2]); + } + } + } + glEnd(); + + // Draw boundaries + glLineWidth(2.5f); + glColor4ub(0,48,64,220); + glBegin(GL_LINES); + for (int i = 0; i < header->npolys; ++i) + { + const dtTilePoly* p = &header->polys[i]; + for (int j = 0, nj = (int)p->nv; j < nj; ++j) + { + if (p->n[j] != 0) + { +/* if (p->n[j] & 0x8000) + { + // Portal + int side = (p->n[j] >> 13) & 3; + int i = p->n[j] & 0x1fff; + if (header->portals[side][i].ncon) continue; + } + else*/ + 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 = &header->verts[vi[k]*3]; + glVertex3f(v[0], v[1]+0.21f, v[2]); + } + } + } + glEnd(); + glLineWidth(1.0f); + + glPointSize(3.0f); + glColor4ub(0,0,0,196); + glBegin(GL_POINTS); + for (int i = 0; i < header->nverts; ++i) + { + const float* v = &header->verts[i*3]; + glVertex3f(v[0], v[1]+0.21f, v[2]); + } + glEnd(); + glPointSize(1.0f); + + // Draw portals +/* glBegin(GL_LINES); + + for (int i = 0; i < header->nportals[0]; ++i) + { + const dtTilePortal* p = &header->portals[0][i]; + if (p->ncon) + glColor4ub(255,255,255,192); + else + glColor4ub(255,0,0,64); + glVertex3f(header->bmax[0]-0.1f, p->bmin[1], p->bmin[0]); + glVertex3f(header->bmax[0]-0.1f, p->bmax[1], p->bmin[0]); + + glVertex3f(header->bmax[0]-0.1f, p->bmax[1], p->bmin[0]); + glVertex3f(header->bmax[0]-0.1f, p->bmax[1], p->bmax[0]); + + glVertex3f(header->bmax[0]-0.1f, p->bmax[1], p->bmax[0]); + glVertex3f(header->bmax[0]-0.1f, p->bmin[1], p->bmax[0]); + + glVertex3f(header->bmax[0]-0.1f, p->bmin[1], p->bmax[0]); + glVertex3f(header->bmax[0]-0.1f, p->bmin[1], p->bmin[0]); + } + for (int i = 0; i < header->nportals[1]; ++i) + { + const dtTilePortal* p = &header->portals[1][i]; + if (p->ncon) + glColor4ub(255,255,255,192); + else + glColor4ub(255,0,0,64); + glVertex3f(p->bmin[0], p->bmin[1], header->bmax[2]-0.1f); + glVertex3f(p->bmin[0], p->bmax[1], header->bmax[2]-0.1f); + + glVertex3f(p->bmin[0], p->bmax[1], header->bmax[2]-0.1f); + glVertex3f(p->bmax[0], p->bmax[1], header->bmax[2]-0.1f); + + glVertex3f(p->bmax[0], p->bmax[1], header->bmax[2]-0.1f); + glVertex3f(p->bmax[0], p->bmin[1], header->bmax[2]-0.1f); + + glVertex3f(p->bmax[0], p->bmin[1], header->bmax[2]-0.1f); + glVertex3f(p->bmin[0], p->bmin[1], header->bmax[2]-0.1f); + } + for (int i = 0; i < header->nportals[2]; ++i) + { + const dtTilePortal* p = &header->portals[2][i]; + if (p->ncon) + glColor4ub(255,255,255,192); + else + glColor4ub(255,0,0,64); + glVertex3f(header->bmin[0]+0.1f, p->bmin[1], p->bmin[0]); + glVertex3f(header->bmin[0]+0.1f, p->bmax[1], p->bmin[0]); + + glVertex3f(header->bmin[0]+0.1f, p->bmax[1], p->bmin[0]); + glVertex3f(header->bmin[0]+0.1f, p->bmax[1], p->bmax[0]); + + glVertex3f(header->bmin[0]+0.1f, p->bmax[1], p->bmax[0]); + glVertex3f(header->bmin[0]+0.1f, p->bmin[1], p->bmax[0]); + + glVertex3f(header->bmin[0]+0.1f, p->bmin[1], p->bmax[0]); + glVertex3f(header->bmin[0]+0.1f, p->bmin[1], p->bmin[0]); + } + for (int i = 0; i < header->nportals[3]; ++i) + { + const dtTilePortal* p = &header->portals[3][i]; + if (p->ncon) + glColor4ub(255,255,255,192); + else + glColor4ub(255,0,0,64); + glVertex3f(p->bmin[0], p->bmin[1], header->bmin[2]+0.1f); + glVertex3f(p->bmin[0], p->bmax[1], header->bmin[2]+0.1f); + + glVertex3f(p->bmin[0], p->bmax[1], header->bmin[2]+0.1f); + glVertex3f(p->bmax[0], p->bmax[1], header->bmin[2]+0.1f); + + glVertex3f(p->bmax[0], p->bmax[1], header->bmin[2]+0.1f); + glVertex3f(p->bmax[0], p->bmin[1], header->bmin[2]+0.1f); + + glVertex3f(p->bmax[0], p->bmin[1], header->bmin[2]+0.1f); + glVertex3f(p->bmin[0], p->bmin[1], header->bmin[2]+0.1f); + } + glEnd();*/ +} + +void dtDebugDrawTiledNavMesh(const dtTiledNavMesh* mesh) +{ + if (!mesh) return; + + for (int i = 0; i < DT_MAX_TILES; ++i) + { + const dtTile* tile = mesh->getTile(i); + if (!tile->header) continue; + + drawTile(tile->header); + } +} + +void dtDebugDrawTiledNavMeshPoly(const dtTiledNavMesh* mesh, dtTilePolyRef ref, const float* col) +{ + const dtTilePoly* p = mesh->getPolyByRef(ref); + if (!p) return; + const float* verts = mesh->getPolyVertsByRef(ref); + 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 = &verts[vi[k]*3]; + glVertex3f(v[0], v[1]+0.2f, v[2]); + } + } + glEnd(); +} + diff --git a/Detour/Source/DetourStatNavMesh.cpp b/Detour/Source/DetourStatNavMesh.cpp index 5d1e18c..9fe438a 100755 --- a/Detour/Source/DetourStatNavMesh.cpp +++ b/Detour/Source/DetourStatNavMesh.cpp @@ -1204,7 +1204,7 @@ int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, if (resultCost) resultCost[n] = actualNode->total; if (resultDepth) - resultDepth[n] = (unsigned short)actualNode->cost; + resultDepth[n] = actualNode->cost; ++n; } actualNode->flags = dtNode::OPEN; @@ -1220,7 +1220,7 @@ int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, // 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) + dtPolyRef* polys, const int maxIds) { if (!m_header) return 0; @@ -1256,7 +1256,7 @@ int dtStatNavMesh::queryPolygons(const float* center, const float* extents, { if (n < maxIds) { - ids[n] = (unsigned short)node->i; + polys[n] = (dtPolyRef)node->i; n++; } } @@ -1278,7 +1278,7 @@ dtPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* exten if (!m_header) return 0; // Get nearby polygons from proximity grid. - unsigned short polys[128]; + dtPolyRef polys[128]; int npolys = queryPolygons(center, extents, polys, 128); // Find nearest polygon amongst the nearby polygons. @@ -1286,7 +1286,7 @@ dtPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* exten float nearestDistanceSqr = FLT_MAX; for (int i = 0; i < npolys; ++i) { - dtPolyRef ref = (dtPolyRef)polys[i]; + dtPolyRef ref = polys[i]; float closest[3]; if (!closestPointToPoly(ref, center, closest)) continue; @@ -1301,30 +1301,6 @@ dtPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* exten return nearest; } -bool dtStatNavMesh::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const -{ - 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) - { - const float* left = getVertex(fromPoly->v[j]); - const float* right = getVertex(fromPoly->v[i]); - 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; - } - } - - return false; -} - bool dtStatNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right) const { const dtPoly* fromPoly = getPolyByRef(from); diff --git a/Detour/Source/DetourTiledNavMesh.cpp b/Detour/Source/DetourTiledNavMesh.cpp new file mode 100644 index 0000000..d905b35 --- /dev/null +++ b/Detour/Source/DetourTiledNavMesh.cpp @@ -0,0 +1,1772 @@ +#include "DetourTiledNavMesh.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 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 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 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 void calcPolyCenter(float* tc, const dtTilePoly* 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; +} + +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; +} + +inline bool overlapRects(const float* amin, const float* amax, + const float* bmin, const float* bmax) +{ + 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; + return overlap; +} + +inline bool overlapBounds(const float* amin, const float* amax, + const float* bmin, const float* bmax) +{ + 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; +} + +inline int opposite(int side) { return (side+2) & 0x3; } + +static void calcBounds(const float* va, const float* vb, + float* bmin, float* bmax, + int side, float padx, float pady) +{ + if ((side&1) == 0) + { + bmin[0] = min(va[2],vb[2]) + padx; + bmin[1] = min(va[1],vb[1]) - pady; + bmax[0] = max(va[2],vb[2]) - padx; + bmax[1] = max(va[1],vb[1]) + pady; + } + else + { + bmin[0] = min(va[0],vb[0]) + padx; + bmin[1] = min(va[1],vb[1]) - pady; + bmax[0] = max(va[0],vb[0]) - padx; + bmax[1] = max(va[1],vb[1]) + pady; + } +} + + +////////////////////////////////////////////////////////////////////////////////////////// +struct dtTileNode +{ + enum dtTileNodeFlags + { + OPEN = 0x01, + CLOSED = 0x02, + }; + dtTileNode* parent; + float cost; + float total; + unsigned int id; + unsigned char flags; // TODO: merge to id or parent? +}; + +class dtTileNodePool +{ +public: + dtTileNodePool(int maxNodes, int hashSize); + ~dtTileNodePool(); + inline void operator=(const dtTileNodePool&) {} + void clear(); + dtTileNode* getNode(unsigned short id); + const dtTileNode* findNode(unsigned short id) const; + + inline int getMemUsed() const + { + return sizeof(*this) + + sizeof(dtTileNode)*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; + } + + dtTileNode* m_nodes; + unsigned short* m_first; + unsigned short* m_next; + const int m_maxNodes; + const int m_hashSize; + int m_nodeCount; +}; + +dtTileNodePool::dtTileNodePool(int maxNodes, int hashSize) : + m_maxNodes(maxNodes), + m_hashSize(hashSize), + m_nodes(0), + m_first(0), + m_next(0) +{ + m_nodes = new dtTileNode[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); +} + +dtTileNodePool::~dtTileNodePool() +{ + delete [] m_nodes; + delete [] m_next; + delete [] m_first; +} + +void dtTileNodePool::clear() +{ + memset(m_first, 0xff, sizeof(unsigned short)*m_hashSize); + m_nodeCount = 0; +} + +const dtTileNode* dtTileNodePool::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; +} + +dtTileNode* dtTileNodePool::getNode(unsigned short id) +{ + unsigned int bucket = hashint((unsigned int)id) & (m_hashSize-1); + unsigned short i = m_first[bucket]; + dtTileNode* 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 dtTileNodeQueue +{ +public: + dtTileNodeQueue(int n); + ~dtTileNodeQueue(); + inline void operator=(dtTileNodeQueue&) {} + + inline void clear() + { + m_size = 0; + } + + inline dtTileNode* top() + { + return m_heap[0]; + } + + inline dtTileNode* pop() + { + dtTileNode* result = m_heap[0]; + m_size--; + trickleDown(0, m_heap[m_size]); + return result; + } + + inline void push(dtTileNode* node) + { + m_size++; + bubbleUp(m_size-1, node); + } + + inline void modify(dtTileNode* node) + { + for (int 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(dtTileNode*)*(m_capacity+1); + } + + +private: + void bubbleUp(int i, dtTileNode* node); + void trickleDown(int i, dtTileNode* node); + + dtTileNode** m_heap; + const int m_capacity; + int m_size; +}; + +dtTileNodeQueue::dtTileNodeQueue(int n) : + m_capacity(n), + m_size(0), + m_heap(0) +{ + m_heap = new dtTileNode*[m_capacity+1]; +} + +dtTileNodeQueue::~dtTileNodeQueue() +{ + delete [] m_heap; +} + +void dtTileNodeQueue::bubbleUp(int i, dtTileNode* 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 dtTileNodeQueue::trickleDown(int i, dtTileNode* 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); +} + + +////////////////////////////////////////////////////////////////////////////////////////// +dtTiledNavMesh::dtTiledNavMesh() : + m_tileSize(0), + m_portalHeight(0), + m_nextFree(0), + m_tmpLinks(0), + m_ntmpLinks(0), + m_nodePool(0), + m_openList(0) +{ +} + +dtTiledNavMesh::~dtTiledNavMesh() +{ + // TODO! the mesh should not handle the tile memory! + for (int i = 0; i < DT_MAX_TILES; ++i) + { + if (m_tiles[i].header) + { + delete [] (unsigned char*)m_tiles[i].header; + m_tiles[i].header = 0; + } + } + + delete [] m_tmpLinks; + + delete m_nodePool; + delete m_openList; +} + +bool dtTiledNavMesh::init(const float* orig, float tileSize, float portalHeight) +{ + vcopy(m_orig, orig); + m_tileSize = tileSize; + m_portalHeight = portalHeight; + + // Init tiles + memset(m_tiles, 0, sizeof(dtTile)*DT_MAX_TILES); + memset(m_posLookup, 0, sizeof(dtTile*)*DT_TILE_LOOKUP_SIZE); + m_nextFree = 0; + for (int i = DT_MAX_TILES-1; i >= 0; --i) + { + m_tiles[i].next = m_nextFree; + m_nextFree = &m_tiles[i]; + } + + m_nodePool = new dtTileNodePool(2048, 256); + if (!m_nodePool) + return false; + + m_openList = new dtTileNodeQueue(2048); + if (!m_openList) + return false; + + return true; +} + +int dtTiledNavMesh::getPolyNeighbours(dtTilePolyRef ref, dtTilePolyRef* nei, int maxNei) const +{ + unsigned int salt, it, ip; + decodeId(ref, salt, it, ip); + if (it >= DT_MAX_TILES) return 0; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0; + const dtTileHeader* h = m_tiles[it].header; + + if (ip >= h->npolys) return 0; + const dtTilePoly* poly = &h->polys[ip]; + + int n = 0; + for (int i = 0; i < poly->nlinks; ++i) + if (n < maxNei) nei[n++] = h->links[poly->links+i].ref; + + return n; +} + +int dtTiledNavMesh::findConnectingPolys(const float* va, const float* vb, + dtTile* tile, int side, + dtTilePolyRef* con, float* conarea, int maxcon) +{ + if (!tile) return 0; + dtTileHeader* h = tile->header; + + float amin[2], amax[2]; + calcBounds(va,vb, amin,amax, side, 0.01f, m_portalHeight); + + // Remove links pointing to 'side' and compact the links array. + float bmin[2], bmax[2]; + unsigned short m = 0x8000 | (unsigned short)side; + int n = 0; + + dtTilePolyRef base = getTileId(tile); + + for (int i = 0; i < h->npolys; ++i) + { + dtTilePoly* poly = &h->polys[i]; + for (int j = 0; j < poly->nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->n[j] != m) continue; + // Check if the segments touch. + const float* vc = &h->verts[poly->v[j]*3]; + const float* vd = &h->verts[poly->v[(j+1) % (int)poly->nv]*3]; + calcBounds(vc,vd, bmin,bmax, side, 0.01f, m_portalHeight); + if (!overlapRects(amin,amax, bmin,bmax)) continue; + // Add return value. + if (n < maxcon) + { + conarea[n*2+0] = max(amin[0], bmin[0]); + conarea[n*2+1] = min(amax[0], bmax[0]); + con[n] = base | (unsigned int)i; + n++; + } + break; + } + } + return n; +} + +void dtTiledNavMesh::removeExtLinks(dtTile* tile, int side) +{ + if (!tile) return; + dtTileHeader* h = tile->header; + + // Remove links pointing to 'side' and compact the links array. + dtTileLink* pool = m_tmpLinks; + int nlinks = 0; + for (int i = 0; i < h->npolys; ++i) + { + dtTilePoly* poly = &h->polys[i]; + int plinks = nlinks; + int nplinks = 0; + for (int j = 0; j < poly->nlinks; ++j) + { + dtTileLink* link = &h->links[poly->links+j]; + if ((int)link->side != side) + { + if (nlinks < h->maxlinks) + { + dtTileLink* dst = &pool[nlinks++]; + memcpy(dst, link, sizeof(dtTileLink)); + nplinks++; + } + } + } + poly->links = plinks; + poly->nlinks = nplinks; + } + h->nlinks = nlinks; + if (h->nlinks) + memcpy(h->links, m_tmpLinks, sizeof(dtTileLink)*nlinks); +} + +void dtTiledNavMesh::buildExtLinks(dtTile* tile, dtTile* target, int side) +{ + if (!tile) return; + dtTileHeader* h = tile->header; + + // Remove links pointing to 'side' and compact the links array. + dtTileLink* pool = m_tmpLinks; + int nlinks = 0; + for (int i = 0; i < h->npolys; ++i) + { + dtTilePoly* poly = &h->polys[i]; + int plinks = nlinks; + int nplinks = 0; + // Copy internal and other external links. + for (int j = 0; j < poly->nlinks; ++j) + { + dtTileLink* link = &h->links[poly->links+j]; + if ((int)link->side != side) + { + if (nlinks < h->maxlinks) + { + dtTileLink* dst = &pool[nlinks++]; + memcpy(dst, link, sizeof(dtTileLink)); + nplinks++; + } + } + } + // Create new links. + unsigned short m = 0x8000 | (unsigned short)side; + for (int j = 0; j < poly->nv; ++j) + { + // Skip edges which do not point to the right side. + if (poly->n[j] != m) continue; + + // Create new links + const float* va = &h->verts[poly->v[j]*3]; + const float* vb = &h->verts[poly->v[(j+1)%(int)poly->nv]*3]; + dtTilePolyRef nei[4]; + float neia[4*2]; + int nnei = findConnectingPolys(va,vb, target, opposite(side), nei,neia,4); + for (int k = 0; k < nnei; ++k) + { + if (nlinks < h->maxlinks) + { + dtTileLink* link = &pool[nlinks++]; + link->ref = nei[k]; + link->p = (unsigned short)i; + link->e = (unsigned char)j; + link->side = (unsigned char)side; + link->bmin = neia[k*2+0]; + link->bmax = neia[k*2+1]; + nplinks++; + } + } + } + + poly->links = plinks; + poly->nlinks = nplinks; + } + h->nlinks = nlinks; + if (h->nlinks) + memcpy(h->links, m_tmpLinks, sizeof(dtTileLink)*nlinks); +} + +void dtTiledNavMesh::buildIntLinks(dtTile* tile) +{ + if (!tile) return; + dtTileHeader* h = tile->header; + + dtTilePolyRef base = getTileId(tile); + dtTileLink* pool = h->links; + int nlinks = 0; + for (int i = 0; i < h->npolys; ++i) + { + dtTilePoly* poly = &h->polys[i]; + poly->links = nlinks; + poly->nlinks = 0; + for (int j = 0; j < poly->nv; ++j) + { + // Skip hard and non-internal edges. + if (poly->n[j] == 0 || (poly->n[j] & 0x8000)) continue; + + if (nlinks < h->maxlinks) + { + dtTileLink* link = &pool[nlinks++]; + link->ref = base | (unsigned int)(poly->n[j]-1); + link->p = (unsigned short)i; + link->e = (unsigned char)j; + link->side = 0xff; + link->bmin = link->bmax = 0; + poly->nlinks++; + } + } + } + h->nlinks = nlinks; +} + +inline int computeTileHash(int x, int y) +{ + const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; + const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes + unsigned int n = h1 * x + h2 * y; + return (int)(n & (DT_TILE_LOOKUP_SIZE-1)); +} + +bool dtTiledNavMesh::addTile(int x, int y, unsigned char* data, int dataSize) +{ + // Remove any old tile at this location. + removeTile(x,y); + // Make sure there is enough space for new tile. + if (!m_nextFree) + return false; + // Make sure the data is in right format. + dtTileHeader* header = (dtTileHeader*)data; + if (header->magic != DT_TILE_NAVMESH_MAGIC) + return false; + if (header->version != DT_TILE_NAVMESH_VERSION) + return false; + + // Make sure the tmp link array is large enough. + if (header->maxlinks > m_ntmpLinks) + { + m_ntmpLinks = header->maxlinks; + delete [] m_tmpLinks; + m_tmpLinks = 0; + m_tmpLinks = new dtTileLink[m_ntmpLinks]; + } + if (!m_tmpLinks) + return false; + + // Allocate a tile. + dtTile* tile = m_nextFree; + m_nextFree = tile->next; + tile->next = 0; + + // Insert tile into the position lut. + int h = computeTileHash(x,y); + tile->next = m_posLookup[h]; + m_posLookup[h] = tile; + + // Patch header pointers. + const int headerSize = sizeof(dtTileHeader); + const int vertsSize = sizeof(float)*3*header->nverts; + const int polysSize = sizeof(dtTilePoly)*header->npolys; + header->verts = (float*)(data + headerSize); + header->polys = (dtTilePoly*)(data + headerSize + vertsSize); + header->links = (dtTileLink*)(data + headerSize + vertsSize + polysSize); + + // Init tile. + tile->header = header; + tile->x = x; + tile->y = y; + + buildIntLinks(tile); + + // Create connections connections. + for (int i = 0; i < 4; ++i) + { + dtTile* nei = getNeighbourTile(x,y,i); + tile->header->nei[i] = nei; + if (tile->header->nei[i]) + { + nei->header->nei[opposite(i)] = tile; + buildExtLinks(tile, nei, i); + buildExtLinks(nei, tile, opposite(i)); + } + } + + return true; +} + +dtTile* dtTiledNavMesh::getTile(int x, int y) +{ + // Find tile based on hash. + int h = computeTileHash(x,y); + dtTile* tile = m_posLookup[h]; + while (tile) + { + if (tile->x == x && tile->y == y) + return tile; + tile = tile->next; + } + return 0; +} + +dtTile* dtTiledNavMesh::getNeighbourTile(int x, int y, int side) +{ + switch (side) + { + case 0: x++; break; + case 1: y++; break; + case 2: x--; break; + case 3: y--; break; + }; + return getTile(x,y); +} + +bool dtTiledNavMesh::removeTile(int x, int y) +{ + dtTile* tile = getTile(x, y); + if (!tile) + return false; + + // Remove connections to neighbour tiles. + for (int i = 0; i < 4; ++i) + { + dtTile* nei = getNeighbourTile(x,y,i); + if (!nei) continue; + nei->header->nei[opposite(i)] = 0; + removeExtLinks(nei, opposite(i)); + } + + // Reset tile. + unsigned char* data = (unsigned char*)tile->header; + tile->header = 0; + tile->x = -1; + tile->y = -1; + tile->salt++; + + // TODO! the mesh should not handle the tile memory! + delete [] data; + + return true; +} + + + + + +bool dtTiledNavMesh::closestPointToPoly(dtTilePolyRef ref, const float* pos, float* closest) const +{ + unsigned int salt, it, ip; + decodeId(ref, salt, it, ip); + if (it >= DT_MAX_TILES) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + const dtTileHeader* h = m_tiles[it].header; + + if (ip >= h->npolys) return false; + const dtTilePoly* poly = &h->polys[ip]; + + float closestDistSqr = FLT_MAX; + + for (int i = 2; i < (int)poly->nv; ++i) + { + const float* v0 = &h->verts[poly->v[0]*3]; + const float* v1 = &h->verts[poly->v[i-1]*3]; + const float* v2 = &h->verts[poly->v[i]*3]; + + float pt[3]; + closestPtPointTriangle(pt, pos, v0, v1, v2); + float d = vdistSqr(pos, pt); + if (d < closestDistSqr) + { + vcopy(closest, pt); + closestDistSqr = d; + } + } + + return true; +} + +dtTilePolyRef dtTiledNavMesh::findNearestPoly(const float* center, const float* extents) +{ + // Get nearby polygons from proximity grid. + dtTilePolyRef polys[128]; + int npolys = queryPolygons(center, extents, polys, 128); + + // Find nearest polygon amongst the nearby polygons. + dtTilePolyRef nearest = 0; + float nearestDistanceSqr = FLT_MAX; + for (int i = 0; i < npolys; ++i) + { + dtTilePolyRef 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; +} + +dtTilePolyRef dtTiledNavMesh::getTileId(dtTile* tile) +{ + if (!tile) return 0; + const unsigned int it = tile - m_tiles; + return encodeId(tile->salt, it, 0); +} + +int dtTiledNavMesh::queryTilePolygons(dtTile* tile, + const float* qmin, const float* qmax, + dtTilePolyRef* polys, const int maxPolys) +{ + float bmin[3], bmax[3]; + const dtTileHeader* header = tile->header; + int n = 0; + dtTilePolyRef base = getTileId(tile); + for (int i = 0; i < header->npolys; ++i) + { + // Calc polygon bounds. + dtTilePoly* p = &header->polys[i]; + const float* v = &header->verts[p->v[0]*3]; + vcopy(bmin, v); + vcopy(bmax, v); + for (int j = 1; j < p->nv; ++j) + { + v = &header->verts[p->v[j]*3]; + vmin(bmin, v); + vmax(bmax, v); + } + if (overlapBounds(qmin,qmax, bmin,bmax)) + { + if (n < maxPolys) + polys[n++] = base | (dtTilePolyRef)i; + } + } + return n; +} + +int dtTiledNavMesh::queryPolygons(const float* center, const float* extents, + dtTilePolyRef* polys, const int maxPolys) +{ + float bmin[3], bmax[3]; + bmin[0] = center[0] - extents[0]; + bmin[1] = center[1] - extents[1]; + bmin[2] = center[2] - extents[2]; + + bmax[0] = center[0] + extents[0]; + bmax[1] = center[1] + extents[1]; + bmax[2] = center[2] + extents[2]; + + // Find tiles the query touches. + const int minx = (int)floorf((bmin[0]-m_orig[0]) / m_tileSize); + const int maxx = (int)ceilf((bmax[0]-m_orig[0]) / m_tileSize); + + const int miny = (int)floorf((bmin[2]-m_orig[2]) / m_tileSize); + const int maxy = (int)ceilf((bmax[2]-m_orig[2]) / m_tileSize); + + int n = 0; + for (int y = miny; y < maxy; ++y) + { + for (int x = minx; x < maxx; ++x) + { + dtTile* tile = getTile(x,y); + if (!tile) continue; + n += queryTilePolygons(tile, bmin, bmax, polys+n, maxPolys-n); + if (n >= maxPolys) return n; + } + } + + return n; +} + +float dtTiledNavMesh::getCost(dtTilePolyRef prev, dtTilePolyRef from, dtTilePolyRef to) const +{ + unsigned int salt, it, ip; + if (prev) from = prev; + // The API input has been cheked already, skip checking internal data. + decodeId(from, salt, it, ip); + const dtTileHeader* fromHeader = m_tiles[it].header; + const dtTilePoly* fromPoly = &fromHeader->polys[ip]; + decodeId(to, salt, it, ip); + const dtTileHeader* toHeader = m_tiles[it].header; + const dtTilePoly* toPoly = &toHeader->polys[ip]; + + float fromPc[3], toPc[3]; + calcPolyCenter(fromPc, fromPoly, fromHeader->verts); + calcPolyCenter(toPc, toPoly, toHeader->verts); + + float dx = fromPc[0]-toPc[0]; + float dy = fromPc[1]-toPc[1]; + float dz = fromPc[2]-toPc[2]; + + return sqrtf(dx*dx + dy*dy + dz*dz); +} + +float dtTiledNavMesh::getHeuristic(dtTilePolyRef from, dtTilePolyRef to) const +{ + unsigned int salt, it, ip; + // The API input has been cheked already, skip checking internal data. + decodeId(from, salt, it, ip); + const dtTileHeader* fromHeader = m_tiles[it].header; + const dtTilePoly* fromPoly = &fromHeader->polys[ip]; + decodeId(to, salt, it, ip); + const dtTileHeader* toHeader = m_tiles[it].header; + const dtTilePoly* toPoly = &toHeader->polys[ip]; + + float fromPc[3], toPc[3]; + calcPolyCenter(fromPc, fromPoly, fromHeader->verts); + calcPolyCenter(toPc, toPoly, toHeader->verts); + + float dx = fromPc[0]-toPc[0]; + float dy = fromPc[1]-toPc[1]; + float dz = fromPc[2]-toPc[2]; + + return sqrtf(dx*dx + dy*dy + dz*dz) * 2.0f; +} + +int dtTiledNavMesh::findPath(dtTilePolyRef startRef, dtTilePolyRef endRef, + dtTilePolyRef* path, const int maxPathSize) +{ + if (!startRef || !endRef) + return 0; + + if (!maxPathSize) + return 0; + + if (!getPolyByRef(startRef) || !getPolyByRef(endRef)) + return 0; + + if (startRef == endRef) + { + path[0] = startRef; + return 1; + } + + if (!m_nodePool || !m_openList) + return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtTileNode* startNode = m_nodePool->getNode(startRef); + startNode->parent = 0; + startNode->cost = 0; + startNode->total = getHeuristic(startRef, endRef); + startNode->id = startRef; + startNode->flags = dtTileNode::OPEN; + m_openList->push(startNode); + + dtTileNode* lastBestNode = startNode; + unsigned short lastBestNodeCost = startNode->total; + while (!m_openList->empty()) + { + dtTileNode* bestNode = m_openList->pop(); + + if (bestNode->id == endRef) + { + lastBestNode = bestNode; + break; + } + + // Get poly and tile. + unsigned int salt, it, ip; + decodeId(bestNode->id, salt, it, ip); + // The API input has been cheked already, skip checking internal data. + const dtTileHeader* h = m_tiles[it].header; + const dtTilePoly* poly = &h->polys[ip]; + + for (int i = 0; i < poly->nlinks; ++i) + { + dtTilePolyRef neighbour = h->links[poly->links+i].ref; + if (neighbour) + { + // Skip parent node. + if (bestNode->parent && bestNode->parent->id == neighbour) + continue; + + dtTileNode newNode; + newNode.parent = bestNode; + newNode.id = neighbour; + newNode.cost = bestNode->cost + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id); + float h = getHeuristic(newNode.id, endRef); + newNode.total = newNode.cost + h; + + dtTileNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & dtTileNode::OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & dtTileNode::CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= dtTileNode::CLOSED; + actualNode->parent = newNode.parent; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (h < lastBestNodeCost) + { + lastBestNodeCost = h; + lastBestNode = actualNode; + } + + if (actualNode->flags & dtTileNode::OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags = dtTileNode::OPEN; + m_openList->push(actualNode); + } + } + } + } + } + + // Reverse the path. + dtTileNode* prev = 0; + dtTileNode* node = lastBestNode; + do + { + dtTileNode* 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; +} + +int dtTiledNavMesh::findStraightPath(const float* startPos, const float* endPos, + const dtTilePolyRef* 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. + if (!getPortalPoints(path[i], path[i+1], left, right)) + { + if (!closestPointToPoly(path[i], endPos, closestEndPos)) + return 0; + vcopy(&straightPath[straightPathSize*3], closestEndPos); + straightPathSize++; + return straightPathSize; + } + } + 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; +} + +// Returns portal points between two polygons. +bool dtTiledNavMesh::getPortalPoints(dtTilePolyRef from, dtTilePolyRef to, float* left, float* right) const +{ + unsigned int salt, it, ip; + decodeId(from, salt, it, ip); + if (it >= DT_MAX_TILES) return false; + if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false; + if (ip >= m_tiles[it].header->npolys) return false; + const dtTileHeader* fromHeader = m_tiles[it].header; + const dtTilePoly* fromPoly = &fromHeader->polys[ip]; + + for (int i = 0; i < fromPoly->nlinks; ++i) + { + const dtTileLink* link = &fromHeader->links[fromPoly->links+i]; + if (link->ref == to) + { + // Find portal vertices. + const int v0 = fromPoly->v[link->e]; + const int v1 = fromPoly->v[(link->e+1) % fromPoly->nv]; + vcopy(left, &fromHeader->verts[v0*3]); + vcopy(right, &fromHeader->verts[v1*3]); + // If the link is at tile boundary, clamp the vertices to + // the link width. + if (link->side == 0 || link->side == 2) + { + left[2] = max(left[2],link->bmin); + left[2] = min(left[2],link->bmax); + right[2] = max(right[2],link->bmin); + right[2] = min(right[2],link->bmax); + } + else if (link->side == 1 || link->side == 3) + { + left[0] = max(left[0],link->bmin); + left[0] = min(left[0],link->bmax); + right[0] = max(right[0],link->bmin); + right[0] = min(right[0],link->bmax); + } + return true; + } + } + return false; +} + +int dtTiledNavMesh::raycast(dtTilePolyRef centerRef, const float* startPos, const float* endPos, + float& t, dtTilePolyRef* path, const int pathSize) +{ + t = 0; + + if (!centerRef || !getPolyByRef(centerRef)) + return 0; + + dtTilePolyRef curRef = centerRef; + float verts[DT_TILE_VERTS_PER_POLYGON*3]; + int n = 0; + + while (curRef) + { + // Cast ray against current polygon. + + // The API input has been cheked already, skip checking internal data. + unsigned int salt, it, ip; + decodeId(curRef, salt, it, ip); + const dtTileHeader* h = m_tiles[it].header; + const dtTilePoly* poly = &h->polys[ip]; + + // Collect vertices. + int nv = 0; + for (int i = 0; i < (int)poly->nv; ++i) + { + vcopy(&verts[nv*3], &h->verts[poly->v[i]*3]); + nv++; + } + 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 hit the 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; + + // Follow neighbours. + dtTilePolyRef nextRef = 0; + for (int i = 0; i < poly->nlinks; ++i) + { + const dtTileLink* link = &h->links[poly->links+i]; + if ((int)link->e == segMax) + { + // 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 that the intersection lies inside the portal. + if (link->side == 0 || link->side == 2) + { + // Find Z intersection. + float z = startPos[2] + (endPos[2]-startPos[2])*tmax; + if (z >= link->bmin && z <= link->bmax) + { + nextRef = link->ref; + break; + } + } + else if (link->side == 1 || link->side == 3) + { + // Find X intersection. + float x = startPos[0] + (endPos[0]-startPos[0])*tmax; + if (x >= link->bmin && x <= link->bmax) + { + nextRef = link->ref; + break; + } + } + } + } + + if (!nextRef) + { + // No neighbour, we hit a wall. + return n; + } + + // No hit, advance to neighbour polygon. + curRef = nextRef; + } + + return n; +} + +int dtTiledNavMesh::findPolysAround(dtTilePolyRef centerRef, const float* centerPos, float radius, + dtTilePolyRef* resultRef, dtTilePolyRef* resultParent, + float* resultCost, unsigned short* resultDepth, + const int maxResult) +{ + if (!centerRef) return 0; + if (!getPolyByRef(centerRef)) return 0; + if (!m_nodePool || !m_openList) return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtTileNode* startNode = m_nodePool->getNode(centerRef); + startNode->parent = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = dtTileNode::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; + if (resultDepth) + resultDepth[n] = 0; + ++n; + } + + const float radiusSqr = sqr(radius); + + while (!m_openList->empty()) + { + dtTileNode* bestNode = m_openList->pop(); + + // Get poly and tile. + unsigned int salt, it, ip; + decodeId(bestNode->id, salt, it, ip); + // The API input has been cheked already, skip checking internal data. + const dtTileHeader* h = m_tiles[it].header; + const dtTilePoly* poly = &h->polys[ip]; + + for (int i = 0; i < poly->nlinks; ++i) + { + const dtTileLink* link = &h->links[poly->links+i]; + dtTilePolyRef neighbour = link->ref; + if (neighbour) + { + // Skip parent node. + if (bestNode->parent && bestNode->parent->id == neighbour) + continue; + + // Calc distance to the edge. + const float* va = &h->verts[poly->v[link->e]*3]; + const float* vb = &h->verts[poly->v[(link->e+1)%poly->nv]*3]; + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, va, vb, tseg); + + // If the circle is not touching the next polygon, skip it. + if (distSqr > radiusSqr) + continue; + + dtTileNode newNode; + newNode.parent = bestNode; + newNode.id = neighbour; + newNode.cost = bestNode->cost + 1; // Depth + newNode.total = bestNode->total + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id); + + dtTileNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & dtTileNode::OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & dtTileNode::CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~dtTileNode::CLOSED; + actualNode->parent = newNode.parent; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (actualNode->flags & dtTileNode::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 = dtTileNode::OPEN; + m_openList->push(actualNode); + } + } + } + } + } + + return n; +} + +float dtTiledNavMesh::findDistanceToWall(dtTilePolyRef centerRef, const float* centerPos, float maxRadius, + float* hitPos, float* hitNormal) +{ + if (!centerRef) return 0; + if (!getPolyByRef(centerRef)) return 0; + if (!m_nodePool || !m_openList) return 0; + + m_nodePool->clear(); + m_openList->clear(); + + dtTileNode* startNode = m_nodePool->getNode(centerRef); + startNode->parent = 0; + startNode->cost = 0; + startNode->total = 0; + startNode->id = centerRef; + startNode->flags = dtTileNode::OPEN; + m_openList->push(startNode); + + float radiusSqr = sqr(maxRadius); + + while (!m_openList->empty()) + { + dtTileNode* bestNode = m_openList->pop(); + + // Get poly and tile. + unsigned int salt, it, ip; + decodeId(bestNode->id, salt, it, ip); + // The API input has been cheked already, skip checking internal data. + const dtTileHeader* h = m_tiles[it].header; + const dtTilePoly* poly = &h->polys[ip]; + + // 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] & 0x8000) + { + // Tile border. + bool solid = true; + for (int i = 0; i < poly->nlinks; ++i) + { + const dtTileLink* link = &h->links[poly->links+i]; + if (link->e == j && link->ref != 0) + { + solid = false; + break; + } + } + if (!solid) continue; + } + else if (poly->n[j]) + { + // Internal edge + continue; + } + + // Calc distance to the edge. + const float* vj = &h->verts[poly->v[j]*3]; + const float* vi = &h->verts[poly->v[i]*3]; + 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; + } + + for (int i = 0; i < poly->nlinks; ++i) + { + const dtTileLink* link = &h->links[poly->links+i]; + dtTilePolyRef neighbour = link->ref; + if (neighbour) + { + // Skip parent node. + if (bestNode->parent && bestNode->parent->id == neighbour) + continue; + + // Calc distance to the edge. + const float* va = &h->verts[poly->v[link->e]*3]; + const float* vb = &h->verts[poly->v[(link->e+1)%poly->nv]*3]; + float tseg; + float distSqr = distancePtSegSqr2D(centerPos, va, vb, tseg); + + // If the circle is not touching the next polygon, skip it. + if (distSqr > radiusSqr) + continue; + + dtTileNode newNode; + newNode.parent = bestNode; + newNode.id = neighbour; + newNode.cost = bestNode->cost + 1; // Depth + newNode.total = bestNode->total + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id); + + dtTileNode* actualNode = m_nodePool->getNode(newNode.id); + if (!actualNode) + continue; + + if (!((actualNode->flags & dtTileNode::OPEN) && newNode.total > actualNode->total) && + !((actualNode->flags & dtTileNode::CLOSED) && newNode.total > actualNode->total)) + { + actualNode->flags &= ~dtTileNode::CLOSED; + actualNode->parent = newNode.parent; + actualNode->cost = newNode.cost; + actualNode->total = newNode.total; + + if (actualNode->flags & dtTileNode::OPEN) + { + m_openList->modify(actualNode); + } + else + { + actualNode->flags = dtTileNode::OPEN; + m_openList->push(actualNode); + } + } + } + } + } + + // Calc hit normal. + vsub(hitNormal, centerPos, hitPos); + vnormalize(hitNormal); + + return sqrtf(radiusSqr); +} + diff --git a/Detour/Source/DetourTiledNavMeshBuilder.cpp b/Detour/Source/DetourTiledNavMeshBuilder.cpp new file mode 100644 index 0000000..7298991 --- /dev/null +++ b/Detour/Source/DetourTiledNavMeshBuilder.cpp @@ -0,0 +1,190 @@ +// +// 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 +#include "DetourTiledNavMesh.h" + +template inline T abs(T a) { return a < 0 ? -a : a; } +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; } + +bool dtCreateNavMeshTileData(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, int tileSize, int walkableClimb, + unsigned char** outData, int* outDataSize) +{ + if (nvp != DT_TILE_VERTS_PER_POLYGON) + return false; + if (nverts >= 0xffff) + return false; + + if (!nverts) + return false; + if (!npolys) + return false; + +/* int nportals[4] = {0,0,0,0}; + + // Find portals. + for (int i = 0; i < npolys; ++i) + { + const unsigned short* p = &polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == 0xffff) break; + int nj = j+1; + if (nj >= nvp || p[nj] == 0xffff) nj = 0; + const unsigned short* va = &verts[p[j]*3]; + const unsigned short* vb = &verts[p[nj]*3]; + + if (va[0] == tileSize && vb[0] == tileSize) + nportals[0]++; // x+ + else if (va[2] == tileSize && vb[2] == tileSize) + nportals[1]++; // z+ + else if (va[0] == 0 && vb[0] == 0) + nportals[2]++; // x- + else if (va[2] == 0 && vb[2] == 0) + nportals[3]++; // z- + } + } +*/ + + // Find portals. + int nedges = 0; + int nportals = 0; + for (int i = 0; i < npolys; ++i) + { + const unsigned short* p = &polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == 0xffff) break; + int nj = j+1; + if (nj >= nvp || p[nj] == 0xffff) nj = 0; + const unsigned short* va = &verts[p[j]*3]; + const unsigned short* vb = &verts[p[nj]*3]; + + nedges++; + + if (va[0] == tileSize && vb[0] == tileSize) + nportals++; // x+ + else if (va[2] == tileSize && vb[2] == tileSize) + nportals++; // z+ + else if (va[0] == 0 && vb[0] == 0) + nportals++; // x- + else if (va[2] == 0 && vb[2] == 0) + nportals++; // z- + } + } + + // Calculate data size + const int headerSize = sizeof(dtTileHeader); + const int vertsSize = sizeof(float)*3*nverts; + const int polysSize = sizeof(dtTilePoly)*npolys; +// const int portalsSize = sizeof(dtTilePortal)*(nportals[0]+nportals[1]+nportals[2]+nportals[3]); +// const int dataSize = headerSize + vertsSize + polysSize + portalsSize; + const int linksSize = sizeof(dtTileLink)*(nedges + nportals*2); + const int dataSize = headerSize + vertsSize + polysSize + linksSize; + unsigned char* data = new unsigned char[dataSize]; + if (!data) + return false; + memset(data, 0, dataSize); + + dtTileHeader* header = (dtTileHeader*)(data); + float* navVerts = (float*)(data + headerSize); + dtTilePoly* navPolys = (dtTilePoly*)(data + headerSize + vertsSize); +/* dtTilePortal* portals[4]; + portals[0] = (dtTilePortal*)(data + headerSize + vertsSize + polysSize); + portals[1] = portals[0] + nportals[0]; + portals[2] = portals[1] + nportals[1]; + portals[3] = portals[2] + nportals[2];*/ + + // Store header + header->magic = DT_TILE_NAVMESH_MAGIC; + header->version = DT_TILE_NAVMESH_VERSION; + header->npolys = npolys; + header->nverts = nverts; + header->maxlinks = nedges + nportals*2; +/* header->nportals[0] = nportals[0]; + header->nportals[1] = nportals[1]; + header->nportals[2] = nportals[2]; + header->nportals[3] = nportals[3];*/ + 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) + { + dtTilePoly* 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) & 0xffff; + p->nv++; + } + src += nvp*2; + } + + // Store portals. + for (int i = 0; i < npolys; ++i) + { + dtTilePoly* poly = &navPolys[i]; + for (int j = 0; j < poly->nv; ++j) + { + int nj = j+1; + if (nj >= poly->nv) nj = 0; + + const unsigned short* va = &verts[poly->v[j]*3]; + const unsigned short* vb = &verts[poly->v[nj]*3]; + + if (va[0] == tileSize && vb[0] == tileSize) // x+ + poly->n[j] = 0x8000 | 0; + else if (va[2] == tileSize && vb[2] == tileSize) // z+ + poly->n[j] = 0x8000 | 1; + else if (va[0] == 0 && vb[0] == 0) // x- + poly->n[j] = 0x8000 | 2; + else if (va[2] == 0 && vb[2] == 0) // z- + poly->n[j] = 0x8000 | 3; + } + } + + *outData = data; + *outDataSize = dataSize; + + return true; +}