Sergey Sharybin
41e5040e2f
Should be safe until modifier stack is not threaded. Solves issues with mingw and older glibc version (like used in release environment).
459 lines
12 KiB
C++
459 lines
12 KiB
C++
/**
|
|
* $Id$
|
|
*
|
|
* ***** BEGIN GPL LICENSE BLOCK *****
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation; either version 2
|
|
* of the License, or (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software Foundation,
|
|
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
*
|
|
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
|
|
* All rights reserved.
|
|
*
|
|
* The Original Code is: all of this file.
|
|
*
|
|
* Contributor(s): none yet.
|
|
*
|
|
* ***** END GPL LICENSE BLOCK *****
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include "Recast.h"
|
|
|
|
extern "C"{
|
|
#include "BKE_navmesh_conversion.h"
|
|
|
|
#include "DNA_meshdata_types.h"
|
|
#include "BKE_cdderivedmesh.h"
|
|
#include "BLI_math.h"
|
|
}
|
|
|
|
inline float area2(const float* a, const float* b, const float* c)
|
|
{
|
|
return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
|
|
}
|
|
|
|
inline bool left(const float* a, const float* b, const float* c)
|
|
{
|
|
return area2(a, b, c) < 0;
|
|
}
|
|
|
|
int polyNumVerts(const unsigned short* p, const int vertsPerPoly)
|
|
{
|
|
int nv = 0;
|
|
for (int i=0; i<vertsPerPoly; i++)
|
|
{
|
|
if (p[i]==0xffff)
|
|
break;
|
|
nv++;
|
|
}
|
|
return nv;
|
|
}
|
|
|
|
bool polyIsConvex(const unsigned short* p, const int vertsPerPoly, const float* verts)
|
|
{
|
|
int nv = polyNumVerts(p, vertsPerPoly);
|
|
if (nv<3)
|
|
return false;
|
|
for (int j=0; j<nv; j++)
|
|
{
|
|
const float* v = &verts[3*p[j]];
|
|
const float* v_next = &verts[3*p[(j+1)%nv]];
|
|
const float* v_prev = &verts[3*p[(nv+j-1)%nv]];
|
|
if (!left(v_prev, v, v_next))
|
|
return false;
|
|
|
|
}
|
|
return true;
|
|
}
|
|
|
|
float distPointToSegmentSq(const float* point, const float* a, const float* b)
|
|
{
|
|
float abx[3], dx[3];
|
|
vsub(abx, b,a);
|
|
vsub(dx, point,a);
|
|
float d = abx[0]*abx[0]+abx[2]*abx[2];
|
|
float t = abx[0]*dx[0]+abx[2]*dx[2];
|
|
if (d > 0)
|
|
t /= d;
|
|
if (t < 0)
|
|
t = 0;
|
|
else if (t > 1)
|
|
t = 1;
|
|
dx[0] = a[0] + t*abx[0] - point[0];
|
|
dx[2] = a[2] + t*abx[2] - point[2];
|
|
return dx[0]*dx[0] + dx[2]*dx[2];
|
|
}
|
|
|
|
bool buildRawVertIndicesData(DerivedMesh* dm, int &nverts, float *&verts,
|
|
int &ntris, unsigned short *&tris, int *&trisToFacesMap,
|
|
int *&recastData)
|
|
{
|
|
nverts = dm->getNumVerts(dm);
|
|
if (nverts>=0xffff)
|
|
{
|
|
printf("Converting navmesh: Error! Too many vertices. Max number of vertices %d\n", 0xffff);
|
|
return false;
|
|
}
|
|
verts = new float[3*nverts];
|
|
dm->getVertCos(dm, (float(*)[3])verts);
|
|
|
|
//flip coordinates
|
|
for (int vi=0; vi<nverts; vi++)
|
|
{
|
|
SWAP(float, verts[3*vi+1], verts[3*vi+2]);
|
|
}
|
|
|
|
//calculate number of tris
|
|
int nfaces = dm->getNumFaces(dm);
|
|
MFace *faces = dm->getFaceArray(dm);
|
|
ntris = nfaces;
|
|
for (int fi=0; fi<nfaces; fi++)
|
|
{
|
|
MFace* face = &faces[fi];
|
|
if (face->v4)
|
|
ntris++;
|
|
}
|
|
|
|
//copy and transform to triangles (reorder on the run)
|
|
trisToFacesMap = new int[ntris];
|
|
tris = new unsigned short[3*ntris];
|
|
unsigned short* tri = tris;
|
|
int triIdx = 0;
|
|
for (int fi=0; fi<nfaces; fi++)
|
|
{
|
|
MFace* face = &faces[fi];
|
|
tri[3*triIdx+0] = (unsigned short) face->v1;
|
|
tri[3*triIdx+1] = (unsigned short) face->v3;
|
|
tri[3*triIdx+2] = (unsigned short) face->v2;
|
|
trisToFacesMap[triIdx++]=fi;
|
|
if (face->v4)
|
|
{
|
|
tri[3*triIdx+0] = (unsigned short) face->v1;
|
|
tri[3*triIdx+1] = (unsigned short) face->v4;
|
|
tri[3*triIdx+2] = (unsigned short) face->v3;
|
|
trisToFacesMap[triIdx++]=fi;
|
|
}
|
|
}
|
|
|
|
//carefully, recast data is just reference to data in derived mesh
|
|
recastData = (int*)CustomData_get_layer(&dm->faceData, CD_RECAST);
|
|
return true;
|
|
}
|
|
|
|
bool buildPolygonsByDetailedMeshes(const int vertsPerPoly, const int npolys,
|
|
unsigned short* polys, const unsigned short* dmeshes,
|
|
const float* verts, const unsigned short* dtris,
|
|
const int* dtrisToPolysMap)
|
|
{
|
|
int capacity = vertsPerPoly;
|
|
unsigned short* newPoly = new unsigned short[capacity];
|
|
memset(newPoly, 0xff, sizeof(unsigned short)*capacity);
|
|
for (int polyidx=0; polyidx<npolys; polyidx++)
|
|
{
|
|
int nv = 0;
|
|
//search border
|
|
int btri = -1;
|
|
int bedge = -1;
|
|
int dtrisNum = dmeshes[polyidx*4+3];
|
|
int dtrisBase = dmeshes[polyidx*4+2];
|
|
unsigned char *traversedTris = new unsigned char[dtrisNum];
|
|
memset(traversedTris, 0, dtrisNum*sizeof(unsigned char));
|
|
for (int j=0; j<dtrisNum && btri==-1;j++)
|
|
{
|
|
int curpolytri = dtrisBase+j;
|
|
for (int k=0; k<3; k++)
|
|
{
|
|
unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
|
|
if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
|
|
{
|
|
btri = curpolytri;
|
|
bedge = k;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (btri==-1 || bedge==-1)
|
|
{
|
|
//can't find triangle with border edge
|
|
return false;
|
|
}
|
|
|
|
newPoly[nv++] = dtris[btri*3*2+bedge];
|
|
int tri = btri;
|
|
int edge = (bedge+1)%3;
|
|
traversedTris[tri-dtrisBase] = 1;
|
|
while (tri!=btri || edge!=bedge)
|
|
{
|
|
int neighbortri = dtris[tri*3*2+3+edge];
|
|
if (neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
|
|
{
|
|
if (nv==capacity)
|
|
{
|
|
capacity += vertsPerPoly;
|
|
unsigned short* newPolyBig = new unsigned short[capacity];
|
|
memset(newPolyBig, 0xff, sizeof(unsigned short)*capacity);
|
|
memcpy(newPolyBig, newPoly, sizeof(unsigned short)*nv);
|
|
delete newPoly;
|
|
newPoly = newPolyBig;
|
|
}
|
|
newPoly[nv++] = dtris[tri*3*2+edge];
|
|
//move to next edge
|
|
edge = (edge+1)%3;
|
|
}
|
|
else
|
|
{
|
|
//move to next tri
|
|
int twinedge = -1;
|
|
for (int k=0; k<3; k++)
|
|
{
|
|
if (dtris[neighbortri*3*2+3+k] == tri)
|
|
{
|
|
twinedge = k;
|
|
break;
|
|
}
|
|
}
|
|
if (twinedge==-1)
|
|
{
|
|
printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
|
|
goto returnLabel;
|
|
}
|
|
tri = neighbortri;
|
|
edge = (twinedge+1)%3;
|
|
traversedTris[tri-dtrisBase] = 1;
|
|
}
|
|
}
|
|
|
|
unsigned short* adjustedPoly = new unsigned short[nv];
|
|
int adjustedNv = 0;
|
|
for (size_t i=0; i<(size_t)nv; i++)
|
|
{
|
|
unsigned short prev = newPoly[(nv+i-1)%nv];
|
|
unsigned short cur = newPoly[i];
|
|
unsigned short next = newPoly[(i+1)%nv];
|
|
float distSq = distPointToSegmentSq(&verts[3*cur], &verts[3*prev], &verts[3*next]);
|
|
static const float tolerance = 0.001f;
|
|
if (distSq>tolerance)
|
|
adjustedPoly[adjustedNv++] = cur;
|
|
}
|
|
memcpy(newPoly, adjustedPoly, adjustedNv*sizeof(unsigned short));
|
|
delete adjustedPoly;
|
|
nv = adjustedNv;
|
|
|
|
bool allBorderTraversed = true;
|
|
for (size_t i=0; i<(size_t)dtrisNum; i++)
|
|
{
|
|
if (traversedTris[i]==0)
|
|
{
|
|
//check whether it has border edges
|
|
int curpolytri = dtrisBase+i;
|
|
for (int k=0; k<3; k++)
|
|
{
|
|
unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
|
|
if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
|
|
{
|
|
allBorderTraversed = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (nv<=vertsPerPoly && allBorderTraversed)
|
|
{
|
|
for (int i=0; i<nv; i++)
|
|
{
|
|
polys[polyidx*vertsPerPoly*2+i] = newPoly[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
returnLabel:
|
|
delete newPoly;
|
|
return true;
|
|
}
|
|
|
|
struct SortContext
|
|
{
|
|
const int* recastData;
|
|
const int* trisToFacesMap;
|
|
};
|
|
|
|
/* XXX: not thread-safe, but it's called only from modifiers stack
|
|
which isn't threaded. Anyway, better to avoid this in the future */
|
|
static SortContext *_qsort_context;
|
|
|
|
static int compareByData(const void * a, const void * b)
|
|
{
|
|
return ( _qsort_context->recastData[_qsort_context->trisToFacesMap[*(int*)a]] -
|
|
_qsort_context->recastData[_qsort_context->trisToFacesMap[*(int*)b]] );
|
|
}
|
|
|
|
bool buildNavMeshData(const int nverts, const float* verts,
|
|
const int ntris, const unsigned short *tris,
|
|
const int* recastData, const int* trisToFacesMap,
|
|
int &ndtris, unsigned short *&dtris,
|
|
int &npolys, unsigned short *&dmeshes, unsigned short *&polys,
|
|
int &vertsPerPoly, int *&dtrisToPolysMap, int *&dtrisToTrisMap)
|
|
|
|
{
|
|
if (!recastData)
|
|
{
|
|
printf("Converting navmesh: Error! Can't find recast custom data\n");
|
|
return false;
|
|
}
|
|
|
|
//sort the triangles by polygon idx
|
|
int* trisMapping = new int[ntris];
|
|
for (int i=0; i<ntris; i++)
|
|
trisMapping[i]=i;
|
|
SortContext context;
|
|
context.recastData = recastData;
|
|
context.trisToFacesMap = trisToFacesMap;
|
|
_qsort_context = &context;
|
|
qsort(trisMapping, ntris, sizeof(int), compareByData);
|
|
//search first valid triangle - triangle of convex polygon
|
|
int validTriStart = -1;
|
|
for (int i=0; i< ntris; i++)
|
|
{
|
|
if (recastData[trisToFacesMap[trisMapping[i]]]>0)
|
|
{
|
|
validTriStart = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (validTriStart<0)
|
|
{
|
|
printf("Converting navmesh: Error! No valid polygons in mesh\n");
|
|
delete trisMapping;
|
|
return false;
|
|
}
|
|
|
|
ndtris = ntris-validTriStart;
|
|
//fill dtris to faces mapping
|
|
dtrisToTrisMap = new int[ndtris];
|
|
memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
|
|
delete trisMapping; trisMapping=NULL;
|
|
|
|
//create detailed mesh triangles - copy only valid triangles
|
|
//and reserve memory for adjacency info
|
|
dtris = new unsigned short[3*2*ndtris];
|
|
memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
|
|
for (int i=0; i<ndtris; i++)
|
|
{
|
|
memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
|
|
}
|
|
//create new recast data corresponded to dtris and renumber for continuous indices
|
|
int prevPolyIdx=-1, curPolyIdx, newPolyIdx=0;
|
|
dtrisToPolysMap = new int[ndtris];
|
|
for (int i=0; i<ndtris; i++)
|
|
{
|
|
curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
|
|
if (curPolyIdx!=prevPolyIdx)
|
|
{
|
|
newPolyIdx++;
|
|
prevPolyIdx=curPolyIdx;
|
|
}
|
|
dtrisToPolysMap[i] = newPolyIdx;
|
|
}
|
|
|
|
|
|
//build adjacency info for detailed mesh triangles
|
|
buildMeshAdjacency(dtris, ndtris, nverts, 3);
|
|
|
|
//create detailed mesh description for each navigation polygon
|
|
npolys = dtrisToPolysMap[ndtris-1];
|
|
dmeshes = new unsigned short[npolys*4];
|
|
memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
|
|
unsigned short *dmesh = NULL;
|
|
int prevpolyidx = 0;
|
|
for (int i=0; i<ndtris; i++)
|
|
{
|
|
int curpolyidx = dtrisToPolysMap[i];
|
|
if (curpolyidx!=prevpolyidx)
|
|
{
|
|
if (curpolyidx!=prevpolyidx+1)
|
|
{
|
|
printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
|
|
return false;
|
|
}
|
|
dmesh = dmesh==NULL ? dmeshes : dmesh+4;
|
|
dmesh[2] = (unsigned short)i; //tbase
|
|
dmesh[3] = 0; //tnum
|
|
prevpolyidx = curpolyidx;
|
|
}
|
|
dmesh[3]++;
|
|
}
|
|
|
|
//create navigation polygons
|
|
vertsPerPoly = 6;
|
|
polys = new unsigned short[npolys*vertsPerPoly*2];
|
|
memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
|
|
|
|
buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int& vertsPerPoly,
|
|
int &nverts, float *&verts,
|
|
int &ndtris, unsigned short *&dtris,
|
|
int& npolys, unsigned short *&dmeshes,
|
|
unsigned short*& polys, int *&dtrisToPolysMap,
|
|
int *&dtrisToTrisMap, int *&trisToFacesMap)
|
|
{
|
|
bool res = true;
|
|
int ntris =0, *recastData=NULL;
|
|
unsigned short *tris=NULL;
|
|
res = buildRawVertIndicesData(dm, nverts, verts, ntris, tris, trisToFacesMap, recastData);
|
|
if (!res)
|
|
{
|
|
printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
|
|
goto exit;
|
|
}
|
|
|
|
res = buildNavMeshData(nverts, verts, ntris, tris, recastData, trisToFacesMap,
|
|
ndtris, dtris, npolys, dmeshes,polys, vertsPerPoly,
|
|
dtrisToPolysMap, dtrisToTrisMap);
|
|
if (!res)
|
|
{
|
|
printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
|
|
goto exit;
|
|
}
|
|
|
|
exit:
|
|
if (tris)
|
|
delete tris;
|
|
|
|
return res;
|
|
}
|
|
|
|
int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx)
|
|
{
|
|
int res = -1;
|
|
for(int i=0; i<vertsPerPoly; i++)
|
|
{
|
|
if (p[i]==0xffff)
|
|
break;
|
|
if (p[i]==vertexIdx)
|
|
{
|
|
res = i;
|
|
break;
|
|
}
|
|
}
|
|
return res;
|
|
}
|