text: /*
   * ***** 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 *****
   */
  
  /** \file blender/blenkernel/intern/navmesh_conversion.c
   *  \ingroup bke
   */
  
  #include <math.h>
  #include <stdlib.h>
  
  #include "MEM_guardedalloc.h"
  
  #include "DNA_meshdata_types.h"
  
  #include "BLI_utildefines.h"
  #include "BLI_math.h"
  #include "BLI_sort.h"
  
  #include "BKE_navmesh_conversion.h"
  #include "BKE_cdderivedmesh.h"
  
  #include "recast-capi.h"
  
  BLI_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]);
  }
  
  BLI_INLINE int 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 i, nv = 0;
  	for (i = 0; i < vertsPerPoly; i++) {
  		if (p[i] == 0xffff)
  			break;
  		nv++;
  	}
  	return nv;
  }
  
  int polyIsConvex(const unsigned short *p, const int vertsPerPoly, const float *verts)
  {
  	int j, nv = polyNumVerts(p, vertsPerPoly);
  	if (nv < 3)
  		return 0;
  	for (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 0;
  
  	}
  	return 1;
  }
  
  /* XXX, could replace with #dist_to_line_segment_v3(), or add a squared version */
  float distPointToSegmentSq(const float point[3], const float a[3], const float b[3])
  {
  	float abx[3], dx[3];
  	float d, t;
  
  	sub_v3_v3v3(abx, b, a);
  	sub_v3_v3v3(dx, point, a);
  
  	d = abx[0] * abx[0] + abx[2] * abx[2];
  	t = abx[0] * dx[0] + abx[2] * dx[2];
  
  	if (d > 0.0f)
  		t /= d;
  	if (t < 0.0f)
  		t = 0.0f;
  	else if (t > 1.0f)
  		t = 1.0f;
  	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];
  }
  
  int buildRawVertIndicesData(DerivedMesh *dm, int *nverts_r, float **verts_r,
                              int *ntris_r, unsigned short **tris_r, int **trisToFacesMap_r,
                              int **recastData)
  {
  	int vi, fi, triIdx;
  	int nverts, ntris;
  	int *trisToFacesMap;
  	float *verts;
  	unsigned short *tris, *tri;
  	int nfaces;
  	MFace *faces;
  
  	nverts = dm->getNumVerts(dm);
  	if (nverts >= 0xffff) {
  		printf("Converting navmesh: Error! Too many vertices. Max number of vertices %d\n", 0xffff);
  		return 0;
  	}
  	if (nverts == 0) {
  		printf("Converting navmesh: Error! There are no vertices!\n");
  		return 0;
  	}
  
  	verts = MEM_mallocN(sizeof(float[3]) * nverts, "buildRawVertIndicesData verts");
  	dm->getVertCos(dm, (float(*)[3])verts);
  
  	/* flip coordinates */
  	for (vi = 0; vi < nverts; vi++) {
  		SWAP(float, verts[3 * vi + 1], verts[3 * vi + 2]);
  	}
  
  	/* calculate number of tris */
  	dm->recalcTessellation(dm);
  	nfaces = dm->getNumTessFaces(dm);
  	if (nfaces == 0) {
  		printf("Converting navmesh: Error! There are %i vertices, but no faces!\n", nverts);
  		return 0;
  	}
  
  	faces = dm->getTessFaceArray(dm);
  	ntris = nfaces;
  	for (fi = 0; fi < nfaces; fi++) {
  		MFace *face = &faces[fi];
  		if (face->v4)
  			ntris++;
  	}
  
  	/* copy and transform to triangles (reorder on the run) */
  	trisToFacesMap = MEM_callocN(sizeof(int) * ntris, "buildRawVertIndicesData trisToFacesMap");
  	tris = MEM_callocN(sizeof(unsigned short) * 3 * ntris, "buildRawVertIndicesData tris");
  	tri = tris;
  	triIdx = 0;
  	for (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->polyData, CD_RECAST);
  
  	*nverts_r = nverts;
  	*verts_r = verts;
  	*ntris_r = ntris;
  	*tris_r = tris;
  	*trisToFacesMap_r = trisToFacesMap;
  
  	return 1;
  }
  
  int 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 polyidx;
  	int capacity = vertsPerPoly;
  	unsigned short *newPoly = MEM_callocN(sizeof(unsigned short) * capacity, "buildPolygonsByDetailedMeshes newPoly");
  	memset(newPoly, 0xff, sizeof(unsigned short) * capacity);
  
  	for (polyidx = 0; polyidx < npolys; polyidx++) {
  		size_t i;
  		int j, k;
  		int nv = 0;
  		/* search border */
  		int tri, btri = -1;
  		int edge, bedge = -1;
  		int dtrisNum = dmeshes[polyidx * 4 + 3];
  		int dtrisBase = dmeshes[polyidx * 4 + 2];
  		unsigned char *traversedTris = MEM_callocN(sizeof(unsigned char) * dtrisNum, "buildPolygonsByDetailedMeshes traversedTris");
  		unsigned short *adjustedPoly;
  		int adjustedNv;
  		int allBorderTraversed;
  
  		for (j = 0; j < dtrisNum && btri == -1; j++) {
  			int curpolytri = dtrisBase + j;
  			for (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 */
  			MEM_freeN(traversedTris);
  			MEM_freeN(newPoly);
  
  			return 0;
  		}
  
  		newPoly[nv++] = dtris[btri * 3 * 2 + bedge];
  		tri = btri;
  		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) {
  					unsigned short *newPolyBig;
  					capacity += vertsPerPoly;
  					newPolyBig = MEM_callocN(sizeof(unsigned short) * capacity, "buildPolygonsByDetailedMeshes newPolyBig");
  					memset(newPolyBig, 0xff, sizeof(unsigned short) * capacity);
  					memcpy(newPolyBig, newPoly, sizeof(unsigned short) * nv);
  					MEM_freeN(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 (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");
  					MEM_freeN(traversedTris);
  					goto returnLabel;
  				}
  				tri = neighbortri;
  				edge = (twinedge + 1) % 3;
  				traversedTris[tri - dtrisBase] = 1;
  			}
  		}
  
  		adjustedPoly = MEM_callocN(sizeof(unsigned short) * nv, "buildPolygonsByDetailedMeshes adjustedPoly");
  		adjustedNv = 0;
  		for (i = 0; i < 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));
  		MEM_freeN(adjustedPoly);
  		nv = adjustedNv;
  
  		allBorderTraversed = 1;
  		for (i = 0; i < dtrisNum; i++) {
  			if (traversedTris[i] == 0) {
  				/* check whether it has border edges */
  				int curpolytri = dtrisBase + i;
  				for (k = 0; k < 3; k++) {
  					unsigned short neighbortri = dtris[curpolytri * 3 * 2 + 3 + k];
  					if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
  						allBorderTraversed = 0;
  						break;
  					}
  				}
  			}
  		}
  
  		if (nv <= vertsPerPoly && allBorderTraversed) {
  			for (i = 0; i < nv; i++) {
  				polys[polyidx * vertsPerPoly * 2 + i] = newPoly[i];
  			}
  		}
  
  		MEM_freeN(traversedTris);
  	}
  
  returnLabel:
  	MEM_freeN(newPoly);
  
  	return 1;
  }
  
  struct SortContext {
  	const int *recastData;
  	const int *trisToFacesMap;
  };
  
  static int compareByData(const void *a, const void *b, void *ctx)
  {
  	return (((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int *)a]] -
  	        ((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int *)b]]);
  }
  
  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;
  	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;
  	}
  
  	trisMapping = MEM_callocN(sizeof(int) * ntris, "buildNavMeshData trisMapping");
  
  	/* sort the triangles by polygon idx */
  	for (i = 0; i < ntris; i++)
  		trisMapping[i] = i;
  	context.recastData = recastData;
  	context.trisToFacesMap = trisToFacesMap;
  	BLI_qsort_r(trisMapping, ntris, sizeof(int), compareByData, &context);
  
  	/* 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, 0xff, 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 */
  	if (!recast_buildMeshAdjacency(dtris, ndtris, nverts, 3)) {
  		printf("Converting navmesh: Error! Unable to build mesh adjacency information\n");
  		MEM_freeN(trisMapping);
  		MEM_freeN(dtrisToPolysMap);
  		return 0;
  	}
  
  	/* 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");
  				goto fail;
  			}
  			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);
  
  	if (!buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap)) {
  		printf("Converting navmesh: Error! Unable to build polygons from detailed mesh\n");
  		goto fail;
  	}
  
  	*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;
  
  fail:
  	MEM_freeN(dmeshes);
  	MEM_freeN(dtrisToPolysMap);
  	MEM_freeN(dtrisToTrisMap);
  	return 0;
  }
  
  
  int 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)
  {
  	int res;
  	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 raw 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 build navmesh data from mesh\n");
  		goto exit;
  	}
  
  exit:
  	if (tris)
  		MEM_freeN(tris);
  
  	return res;
  }
  
  int polyFindVertex(const unsigned short *p, const int vertsPerPoly, unsigned short vertexIdx)
  {
  	int i, res = -1;
  	for (i = 0; i < vertsPerPoly; i++) {
  		if (p[i] == 0xffff)
  			break;
  		if (p[i] == vertexIdx) {
  			res = i;
  			break;
  		}
  	}
  	return res;
  }
  