void recast_qsort(void *a, size_t n, size_t es, void *thunk, cmp_t *cmp)
{
char *pa, *pb, *pc, *pd, *pl, *pm, *pn;
int d, r, swaptype, swap_cnt;
loop:
SWAPINIT(a, es);
swap_cnt = 0;
if (n < 7) {
for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es)
for (pl = pm;
pl > (char *)a && CMP(thunk, pl - es, pl) > 0;
pl -= es)
swap(pl, pl - es);
return;
}
pm = (char *)a + (n / 2) * es;
if (n > 7) {
pl = (char *)a;
pn = (char *)a + (n - 1) * es;
if (n > 40) {
d = (n / 8) * es;
pl = med3(pl, pl + d, pl + 2 * d, cmp, thunk);
pm = med3(pm - d, pm, pm + d, cmp, thunk);
pn = med3(pn - 2 * d, pn - d, pn, cmp, thunk);
}
pm = med3(pl, pm, pn, cmp, thunk);
}
swap((char *)a, pm);
pa = pb = (char *)a + es;
pc = pd = (char *)a + (n - 1) * es;
for (;;) {
while (pb <= pc && (r = CMP(thunk, pb, a)) <= 0) {
if (r == 0) {
swap_cnt = 1;
swap(pa, pb);
pa += es;
}
pb += es;
}
while (pb <= pc && (r = CMP(thunk, pc, a)) >= 0) {
if (r == 0) {
swap_cnt = 1;
swap(pc, pd);
pd -= es;
}
pc -= es;
}
if (pb > pc)
break;
swap(pb, pc);
swap_cnt = 1;
pb += es;
pc -= es;
}
if (swap_cnt == 0) { /* Switch to insertion sort */
for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es)
for (pl = pm;
pl > (char *)a && CMP(thunk, pl - es, pl) > 0;
pl -= es)
swap(pl, pl - es);
return;
}
pn = (char *)a + n * es;
r = min(pa - (char *)a, pb - pa);
vecswap((char *)a, pb - r, r);
r = min(pd - pc, pn - pd - es);
vecswap(pb, pn - r, r);
if ((r = pb - pa) > es)
recast_qsort(a, r / es, es, thunk, cmp);
if ((r = pd - pc) > es) {
/* Iterate rather than recurse to save stack space */
a = pn - r;
n = r / es;
goto loop;
}
}
int buildNavMeshData(const int nverts, const float* verts,
const int ntris, const unsigned short *tris,
const int* recastData, const int* trisToFacesMap,
int *ndtris_r, unsigned short **dtris_r,
int *npolys_r, unsigned short **dmeshes_r, unsigned short **polys_r,
int *vertsPerPoly_r, int **dtrisToPolysMap_r, int **dtrisToTrisMap_r)
{
int *trisMapping = MEM_callocN(sizeof(int)*ntris, "buildNavMeshData trisMapping");
int i;
struct SortContext context;
int validTriStart, prevPolyIdx, curPolyIdx, newPolyIdx, prevpolyidx;
unsigned short *dmesh;
int ndtris, npolys, vertsPerPoly;
unsigned short *dtris, *dmeshes, *polys;
int *dtrisToPolysMap, *dtrisToTrisMap;
if (!recastData)
{
printf("Converting navmesh: Error! Can't find recast custom data\n");
return 0;
}
//sort the triangles by polygon idx
for (i=0; i<ntris; i++)
trisMapping[i]=i;
context.recastData = recastData;
context.trisToFacesMap = trisToFacesMap;
recast_qsort(trisMapping, ntris, sizeof(int), &context, compareByData);
//search first valid triangle - triangle of convex polygon
validTriStart = -1;
for (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");
MEM_freeN(trisMapping);
return 0;
}
ndtris = ntris-validTriStart;
//fill dtris to faces mapping
dtrisToTrisMap = MEM_callocN(sizeof(int)*ndtris, "buildNavMeshData dtrisToTrisMap");
memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
MEM_freeN(trisMapping);
//create detailed mesh triangles - copy only valid triangles
//and reserve memory for adjacency info
dtris = MEM_callocN(sizeof(unsigned short)*3*2*ndtris, "buildNavMeshData dtris");
memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
for (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
prevPolyIdx = -1;
newPolyIdx = 0;
dtrisToPolysMap = MEM_callocN(sizeof(int)*ndtris, "buildNavMeshData dtrisToPolysMap");
for (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
recast_buildMeshAdjacency(dtris, ndtris, nverts, 3);
//create detailed mesh description for each navigation polygon
npolys = dtrisToPolysMap[ndtris-1];
dmeshes = MEM_callocN(sizeof(unsigned short)*npolys*4, "buildNavMeshData dmeshes");
memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
dmesh = NULL;
prevpolyidx = 0;
for (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 0;
}
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 = MEM_callocN(sizeof(unsigned short)*npolys*vertsPerPoly*2, "buildNavMeshData polys");
memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
*ndtris_r = ndtris;
*npolys_r = npolys;
*vertsPerPoly_r = vertsPerPoly;
*dtris_r = dtris;
*dmeshes_r = dmeshes;
*polys_r = polys;
*dtrisToPolysMap_r = dtrisToPolysMap;
*dtrisToTrisMap_r = dtrisToTrisMap;
return 1;
}