530 lines
15 KiB
C++
530 lines
15 KiB
C++
//
|
|
// Copyright (c) 2009 Mikko Mononen memon@inside.org
|
|
//
|
|
// This software is provided 'as-is', without any express or implied
|
|
// warranty. In no event will the authors be held liable for any damages
|
|
// arising from the use of this software.
|
|
// Permission is granted to anyone to use this software for any purpose,
|
|
// including commercial applications, and to alter it and redistribute it
|
|
// freely, subject to the following restrictions:
|
|
// 1. The origin of this software must not be misrepresented; you must not
|
|
// claim that you wrote the original software. If you use this software
|
|
// in a product, an acknowledgment in the product documentation would be
|
|
// appreciated but is not required.
|
|
// 2. Altered source versions must be plainly marked as such, and must not be
|
|
// misrepresented as being the original software.
|
|
// 3. This notice may not be removed or altered from any source distribution.
|
|
//
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include "DetourNavMesh.h"
|
|
#include "DetourCommon.h"
|
|
#include "DetourNavMeshBuilder.h"
|
|
|
|
static unsigned short MESH_NULL_IDX = 0xffff;
|
|
|
|
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;
|
|
}
|
|
|
|
static 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 (int 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;
|
|
}
|
|
|
|
static 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;
|
|
}
|
|
}
|
|
|
|
static 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;
|
|
// 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] == MESH_NULL_IDX) 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;
|
|
}
|
|
|
|
static unsigned char classifyOffMeshPoint(const float* pt, const float* bmin, const float* bmax)
|
|
{
|
|
static const unsigned char XP = 1<<0;
|
|
static const unsigned char ZP = 1<<1;
|
|
static const unsigned char XM = 1<<2;
|
|
static const unsigned char ZM = 1<<3;
|
|
|
|
unsigned char outcode = 0;
|
|
outcode |= (pt[0] >= bmax[0]) ? XP : 0;
|
|
outcode |= (pt[2] >= bmax[2]) ? ZP : 0;
|
|
outcode |= (pt[0] < bmin[0]) ? XM : 0;
|
|
outcode |= (pt[2] < bmin[2]) ? ZM : 0;
|
|
|
|
switch (outcode)
|
|
{
|
|
case XP: return 0;
|
|
case XP|ZP: return 1;
|
|
case ZP: return 2;
|
|
case XM|ZP: return 3;
|
|
case XM: return 4;
|
|
case XM|ZM: return 5;
|
|
case ZM: return 6;
|
|
case XP|ZM: return 7;
|
|
};
|
|
return 0xff;
|
|
}
|
|
|
|
// TODO: Better error handling.
|
|
|
|
bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize)
|
|
{
|
|
if (params->nvp > DT_VERTS_PER_POLYGON)
|
|
return false;
|
|
if (params->vertCount >= 0xffff)
|
|
return false;
|
|
if (!params->vertCount || !params->verts)
|
|
return false;
|
|
if (!params->polyCount || !params->polys)
|
|
return false;
|
|
if (!params->detailMeshes || !params->detailVerts || !params->detailTris)
|
|
return false;
|
|
|
|
const int nvp = params->nvp;
|
|
|
|
// Classify off-mesh connection points. We store only the connections
|
|
// whose start point is inside the tile.
|
|
unsigned char* offMeshConFlags = new unsigned char [params->offMeshConCount*2];
|
|
if (!offMeshConFlags)
|
|
return false;
|
|
|
|
int storedOffMeshConCount = 0;
|
|
int offMeshConLinkCount = 0;
|
|
|
|
printf("classify\n");
|
|
for (int i = 0; i < params->offMeshConCount; ++i)
|
|
{
|
|
offMeshConFlags[i*2+0] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax);
|
|
offMeshConFlags[i*2+1] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax);
|
|
|
|
printf(" %d, %d\n", (int)offMeshConFlags[i*2+0], (int)offMeshConFlags[i*2+1]);
|
|
|
|
// Cound how many links should be allocated for off-mesh connections.
|
|
if (offMeshConFlags[i*2+0] == 0xff)
|
|
offMeshConLinkCount++;
|
|
if (offMeshConFlags[i*2+1] == 0xff)
|
|
offMeshConLinkCount++;
|
|
|
|
if (offMeshConFlags[i*2+0] == 0xff)
|
|
storedOffMeshConCount++;
|
|
}
|
|
|
|
// Off-mesh connectionss are stored as polygons, adjust values.
|
|
const int totPolyCount = params->polyCount + storedOffMeshConCount;
|
|
const int totVertCount = params->vertCount + storedOffMeshConCount*2;
|
|
|
|
// Find portal edges which are at tile borders.
|
|
int edgeCount = 0;
|
|
int portalCount = 0;
|
|
for (int i = 0; i < params->polyCount; ++i)
|
|
{
|
|
const unsigned short* p = ¶ms->polys[i*2*nvp];
|
|
for (int j = 0; j < nvp; ++j)
|
|
{
|
|
if (p[j] == MESH_NULL_IDX) break;
|
|
int nj = j+1;
|
|
if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0;
|
|
const unsigned short* va = ¶ms->verts[p[j]*3];
|
|
const unsigned short* vb = ¶ms->verts[p[nj]*3];
|
|
|
|
edgeCount++;
|
|
|
|
if (params->tileSize > 0)
|
|
{
|
|
if (va[0] == params->tileSize && vb[0] == params->tileSize)
|
|
portalCount++; // x+
|
|
else if (va[2] == params->tileSize && vb[2] == params->tileSize)
|
|
portalCount++; // z+
|
|
else if (va[0] == 0 && vb[0] == 0)
|
|
portalCount++; // x-
|
|
else if (va[2] == 0 && vb[2] == 0)
|
|
portalCount++; // z-
|
|
}
|
|
}
|
|
}
|
|
|
|
const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2;
|
|
|
|
// Find unique detail vertices.
|
|
int uniqueDetailVertCount = 0;
|
|
for (int i = 0; i < params->polyCount; ++i)
|
|
{
|
|
const unsigned short* p = ¶ms->polys[i*nvp*2];
|
|
int ndv = params->detailMeshes[i*4+1];
|
|
int nv = 0;
|
|
for (int j = 0; j < nvp; ++j)
|
|
{
|
|
if (p[j] == MESH_NULL_IDX) break;
|
|
nv++;
|
|
}
|
|
ndv -= nv;
|
|
uniqueDetailVertCount += ndv;
|
|
}
|
|
|
|
// Calculate data size
|
|
const int headerSize = align4(sizeof(dtMeshHeader));
|
|
const int vertsSize = align4(sizeof(float)*3*totVertCount);
|
|
const int polysSize = align4(sizeof(dtPoly)*totPolyCount);
|
|
const int linksSize = align4(sizeof(dtLink)*maxLinkCount);
|
|
const int detailMeshesSize = align4(sizeof(dtPolyDetail)*params->polyCount);
|
|
const int detailVertsSize = align4(sizeof(float)*3*uniqueDetailVertCount);
|
|
const int detailTrisSize = align4(sizeof(unsigned char)*4*params->detailTriCount);
|
|
const int bvTreeSize = align4(sizeof(dtBVNode)*params->polyCount*2);
|
|
const int offMeshConsSize = align4(sizeof(dtOffMeshConnection)*storedOffMeshConCount);
|
|
|
|
const int dataSize = headerSize + vertsSize + polysSize + linksSize +
|
|
detailMeshesSize + detailVertsSize + detailTrisSize +
|
|
bvTreeSize + offMeshConsSize;
|
|
|
|
unsigned char* data = new unsigned char[dataSize];
|
|
if (!data)
|
|
return false;
|
|
memset(data, 0, dataSize);
|
|
|
|
unsigned char* d = data;
|
|
dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize;
|
|
float* navVerts = (float*)d; d += vertsSize;
|
|
dtPoly* navPolys = (dtPoly*)d; d += polysSize;
|
|
d += linksSize;
|
|
dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
|
|
float* navDVerts = (float*)d; d += detailVertsSize;
|
|
unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize;
|
|
dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize;
|
|
dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize;
|
|
|
|
|
|
// Store header
|
|
header->magic = DT_NAVMESH_MAGIC;
|
|
header->version = DT_NAVMESH_VERSION;
|
|
header->polyCount = totPolyCount;
|
|
header->vertCount = totVertCount;
|
|
header->maxLinkCount = maxLinkCount;
|
|
vcopy(header->bmin, params->bmin);
|
|
vcopy(header->bmax, params->bmax);
|
|
header->detailMeshCount = params->polyCount;
|
|
header->detailVertCount = uniqueDetailVertCount;
|
|
header->detailTriCount = params->detailTriCount;
|
|
header->bvQuantFactor = 1.0f / params->cs;
|
|
header->offMeshBase = params->polyCount;
|
|
header->walkableHeight = params->walkableHeight;
|
|
header->walkableRadius = params->walkableRadius;
|
|
header->walkableClimb = params->walkableClimb;
|
|
header->offMeshConCount = storedOffMeshConCount;
|
|
header->bvNodeCount = params->polyCount*2;
|
|
|
|
const int offMeshVertsBase = params->vertCount;
|
|
const int offMeshPolyBase = params->polyCount;
|
|
|
|
// Store vertices
|
|
// Mesh vertices
|
|
for (int i = 0; i < params->vertCount; ++i)
|
|
{
|
|
const unsigned short* iv = ¶ms->verts[i*3];
|
|
float* v = &navVerts[i*3];
|
|
v[0] = params->bmin[0] + iv[0] * params->cs;
|
|
v[1] = params->bmin[1] + iv[1] * params->ch;
|
|
v[2] = params->bmin[2] + iv[2] * params->cs;
|
|
}
|
|
// Off-mesh link vertices.
|
|
int n = 0;
|
|
for (int i = 0; i < params->offMeshConCount; ++i)
|
|
{
|
|
// Only store connections which start from this tile.
|
|
if (offMeshConFlags[i*2+0] == 0xff)
|
|
{
|
|
const float* linkv = ¶ms->offMeshConVerts[i*2*3];
|
|
float* v = &navVerts[(offMeshVertsBase + n*2)*3];
|
|
vcopy(&v[0], &linkv[0]);
|
|
vcopy(&v[3], &linkv[3]);
|
|
n++;
|
|
}
|
|
}
|
|
|
|
// Store polygons
|
|
// Mesh polys
|
|
const unsigned short* src = params->polys;
|
|
for (int i = 0; i < params->polyCount; ++i)
|
|
{
|
|
dtPoly* p = &navPolys[i];
|
|
p->vertCount = 0;
|
|
p->flags = DT_POLY_GROUND;
|
|
for (int j = 0; j < nvp; ++j)
|
|
{
|
|
if (src[j] == MESH_NULL_IDX) break;
|
|
p->verts[j] = src[j];
|
|
p->neis[j] = (src[nvp+j]+1) & 0xffff;
|
|
p->vertCount++;
|
|
}
|
|
src += nvp*2;
|
|
}
|
|
// Off-mesh connection vertices.
|
|
n = 0;
|
|
for (int i = 0; i < params->offMeshConCount; ++i)
|
|
{
|
|
// Only store connections which start from this tile.
|
|
if (offMeshConFlags[i*2+0] == 0xff)
|
|
{
|
|
dtPoly* p = &navPolys[offMeshPolyBase+n];
|
|
p->vertCount = 2;
|
|
p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0);
|
|
p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1);
|
|
p->flags = DT_POLY_OFFMESH_CONNECTION; // Off-mesh link poly.
|
|
n++;
|
|
}
|
|
}
|
|
|
|
// Store portal edges.
|
|
if (params->tileSize > 0)
|
|
{
|
|
for (int i = 0; i < params->polyCount; ++i)
|
|
{
|
|
dtPoly* poly = &navPolys[i];
|
|
for (int j = 0; j < poly->vertCount; ++j)
|
|
{
|
|
int nj = j+1;
|
|
if (nj >= poly->vertCount) nj = 0;
|
|
|
|
const unsigned short* va = ¶ms->verts[poly->verts[j]*3];
|
|
const unsigned short* vb = ¶ms->verts[poly->verts[nj]*3];
|
|
|
|
if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+
|
|
poly->neis[j] = DT_EXT_LINK | 0;
|
|
else if (va[2] == params->tileSize && vb[2] == params->tileSize) // z+
|
|
poly->neis[j] = DT_EXT_LINK | 2;
|
|
else if (va[0] == 0 && vb[0] == 0) // x-
|
|
poly->neis[j] = DT_EXT_LINK | 4;
|
|
else if (va[2] == 0 && vb[2] == 0) // z-
|
|
poly->neis[j] = DT_EXT_LINK | 6;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Store detail meshes and vertices.
|
|
// The nav polygon vertices are stored as the first vertices on each mesh.
|
|
// We compress the mesh data by skipping them and using the navmesh coordinates.
|
|
unsigned short vbase = 0;
|
|
for (int i = 0; i < params->polyCount; ++i)
|
|
{
|
|
dtPolyDetail& dtl = navDMeshes[i];
|
|
const int vb = params->detailMeshes[i*4+0];
|
|
const int ndv = params->detailMeshes[i*4+1];
|
|
const int nv = navPolys[i].vertCount;
|
|
dtl.vertBase = vbase;
|
|
dtl.vertCount = ndv-nv;
|
|
dtl.triBase = params->detailMeshes[i*4+2];
|
|
dtl.triCount = params->detailMeshes[i*4+3];
|
|
// Copy vertices except the first 'nv' verts which are equal to nav poly verts.
|
|
if (ndv-nv)
|
|
{
|
|
memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv));
|
|
vbase += ndv-nv;
|
|
}
|
|
}
|
|
// Store triangles.
|
|
memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount);
|
|
|
|
// Store and create BVtree.
|
|
// TODO: take detail mesh into account! use byte per bbox extent?
|
|
createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
|
|
nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
|
|
|
|
// Store Off-Mesh connections.
|
|
n = 0;
|
|
for (int i = 0; i < params->offMeshConCount; ++i)
|
|
{
|
|
// Only store connections which start from this tile.
|
|
if (offMeshConFlags[i*2+0] == 0xff)
|
|
{
|
|
dtOffMeshConnection* con = &offMeshCons[n];
|
|
con->poly = offMeshPolyBase + n;
|
|
// Copy connection end-points.
|
|
const float* endPts = ¶ms->offMeshConVerts[i*2*3];
|
|
vcopy(&con->pos[0], &endPts[0]);
|
|
vcopy(&con->pos[3], &endPts[3]);
|
|
con->rad = params->offMeshConRad[i];
|
|
con->flags = params->offMeshConDir[i];
|
|
con->side = offMeshConFlags[i*2+1];
|
|
n++;
|
|
}
|
|
}
|
|
|
|
delete [] offMeshConFlags;
|
|
|
|
*outData = data;
|
|
*outDataSize = dataSize;
|
|
|
|
return true;
|
|
}
|