sim: Remove "continue physics" code
[blender.git] / source / blender / blenkernel / intern / navmesh_conversion.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/blenkernel/intern/navmesh_conversion.c
29  *  \ingroup bke
30  */
31
32 #include <math.h>
33 #include <stdlib.h>
34
35 #include "MEM_guardedalloc.h"
36
37 #include "DNA_meshdata_types.h"
38
39 #include "BLI_utildefines.h"
40 #include "BLI_math.h"
41
42 #include "BKE_navmesh_conversion.h"
43 #include "BKE_cdderivedmesh.h"
44
45 #include "recast-capi.h"
46
47 BLI_INLINE float area2(const float *a, const float *b, const float *c)
48 {
49         return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
50 }
51
52 BLI_INLINE int left(const float *a, const float *b, const float *c)
53 {
54         return area2(a, b, c) < 0;
55 }
56
57 int polyNumVerts(const unsigned short *p, const int vertsPerPoly)
58 {
59         int i, nv = 0;
60         for (i = 0; i < vertsPerPoly; i++) {
61                 if (p[i] == 0xffff)
62                         break;
63                 nv++;
64         }
65         return nv;
66 }
67
68 int polyIsConvex(const unsigned short *p, const int vertsPerPoly, const float *verts)
69 {
70         int j, nv = polyNumVerts(p, vertsPerPoly);
71         if (nv < 3)
72                 return 0;
73         for (j = 0; j < nv; j++) {
74                 const float *v = &verts[3 * p[j]];
75                 const float *v_next = &verts[3 * p[(j + 1) % nv]];
76                 const float *v_prev = &verts[3 * p[(nv + j - 1) % nv]];
77                 if (!left(v_prev, v, v_next))
78                         return 0;
79
80         }
81         return 1;
82 }
83
84 /* XXX, could replace with #dist_to_line_segment_v3(), or add a squared version */
85 float distPointToSegmentSq(const float point[3], const float a[3], const float b[3])
86 {
87         float abx[3], dx[3];
88         float d, t;
89
90         sub_v3_v3v3(abx, b, a);
91         sub_v3_v3v3(dx, point, a);
92
93         d = abx[0] * abx[0] + abx[2] * abx[2];
94         t = abx[0] * dx[0] + abx[2] * dx[2];
95
96         if (d > 0.0f)
97                 t /= d;
98         if (t < 0.0f)
99                 t = 0.0f;
100         else if (t > 1.0f)
101                 t = 1.0f;
102         dx[0] = a[0] + t * abx[0] - point[0];
103         dx[2] = a[2] + t * abx[2] - point[2];
104
105         return dx[0] * dx[0] + dx[2] * dx[2];
106 }
107
108 int buildRawVertIndicesData(DerivedMesh *dm, int *nverts_r, float **verts_r,
109                             int *ntris_r, unsigned short **tris_r, int **trisToFacesMap_r,
110                             int **recastData)
111 {
112         int vi, fi, triIdx;
113         int nverts, ntris;
114         int *trisToFacesMap;
115         float *verts;
116         unsigned short *tris, *tri;
117         int nfaces;
118         MFace *faces;
119
120         nverts = dm->getNumVerts(dm);
121         if (nverts >= 0xffff) {
122                 printf("Converting navmesh: Error! Too many vertices. Max number of vertices %d\n", 0xffff);
123                 return 0;
124         }
125         verts = MEM_callocN(sizeof(float) * 3 * nverts, "buildRawVertIndicesData verts");
126         dm->getVertCos(dm, (float(*)[3])verts);
127
128         /* flip coordinates */
129         for (vi = 0; vi < nverts; vi++) {
130                 SWAP(float, verts[3 * vi + 1], verts[3 * vi + 2]);
131         }
132
133         /* calculate number of tris */
134         nfaces = dm->getNumTessFaces(dm);
135         faces = dm->getTessFaceArray(dm);
136         ntris = nfaces;
137         for (fi = 0; fi < nfaces; fi++) {
138                 MFace *face = &faces[fi];
139                 if (face->v4)
140                         ntris++;
141         }
142
143         /* copy and transform to triangles (reorder on the run) */
144         trisToFacesMap = MEM_callocN(sizeof(int) * ntris, "buildRawVertIndicesData trisToFacesMap");
145         tris = MEM_callocN(sizeof(unsigned short) * 3 * ntris, "buildRawVertIndicesData tris");
146         tri = tris;
147         triIdx = 0;
148         for (fi = 0; fi < nfaces; fi++) {
149                 MFace *face = &faces[fi];
150                 tri[3 * triIdx + 0] = (unsigned short) face->v1;
151                 tri[3 * triIdx + 1] = (unsigned short) face->v3;
152                 tri[3 * triIdx + 2] = (unsigned short) face->v2;
153                 trisToFacesMap[triIdx++] = fi;
154                 if (face->v4) {
155                         tri[3 * triIdx + 0] = (unsigned short) face->v1;
156                         tri[3 * triIdx + 1] = (unsigned short) face->v4;
157                         tri[3 * triIdx + 2] = (unsigned short) face->v3;
158                         trisToFacesMap[triIdx++] = fi;
159                 }
160         }
161
162         /* carefully, recast data is just reference to data in derived mesh */
163         *recastData = (int *)CustomData_get_layer(&dm->polyData, CD_RECAST);
164
165         *nverts_r = nverts;
166         *verts_r = verts;
167         *ntris_r = ntris;
168         *tris_r = tris;
169         *trisToFacesMap_r = trisToFacesMap;
170
171         return 1;
172 }
173
174 int buildPolygonsByDetailedMeshes(const int vertsPerPoly, const int npolys,
175                                   unsigned short *polys, const unsigned short *dmeshes,
176                                   const float *verts, const unsigned short *dtris,
177                                   const int *dtrisToPolysMap)
178 {
179         int polyidx;
180         int capacity = vertsPerPoly;
181         unsigned short *newPoly = MEM_callocN(sizeof(unsigned short) * capacity, "buildPolygonsByDetailedMeshes newPoly");
182         memset(newPoly, 0xff, sizeof(unsigned short) * capacity);
183
184         for (polyidx = 0; polyidx < npolys; polyidx++) {
185                 size_t i;
186                 int j, k;
187                 int nv = 0;
188                 /* search border */
189                 int tri, btri = -1;
190                 int edge, bedge = -1;
191                 int dtrisNum = dmeshes[polyidx * 4 + 3];
192                 int dtrisBase = dmeshes[polyidx * 4 + 2];
193                 unsigned char *traversedTris = MEM_callocN(sizeof(unsigned char) * dtrisNum, "buildPolygonsByDetailedMeshes traversedTris");
194                 unsigned short *adjustedPoly;
195                 int adjustedNv;
196                 int allBorderTraversed;
197
198                 for (j = 0; j < dtrisNum && btri == -1; j++) {
199                         int curpolytri = dtrisBase + j;
200                         for (k = 0; k < 3; k++) {
201                                 unsigned short neighbortri = dtris[curpolytri * 3 * 2 + 3 + k];
202                                 if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
203                                         btri = curpolytri;
204                                         bedge = k;
205                                         break;
206                                 }
207                         }
208                 }
209                 if (btri == -1 || bedge == -1) {
210                         /* can't find triangle with border edge */
211                         MEM_freeN(traversedTris);
212                         MEM_freeN(newPoly);
213
214                         return 0;
215                 }
216
217                 newPoly[nv++] = dtris[btri * 3 * 2 + bedge];
218                 tri = btri;
219                 edge = (bedge + 1) % 3;
220                 traversedTris[tri - dtrisBase] = 1;
221                 while (tri != btri || edge != bedge) {
222                         int neighbortri = dtris[tri * 3 * 2 + 3 + edge];
223                         if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
224                                 if (nv == capacity) {
225                                         unsigned short *newPolyBig;
226                                         capacity += vertsPerPoly;
227                                         newPolyBig = MEM_callocN(sizeof(unsigned short) * capacity, "buildPolygonsByDetailedMeshes newPolyBig");
228                                         memset(newPolyBig, 0xff, sizeof(unsigned short) * capacity);
229                                         memcpy(newPolyBig, newPoly, sizeof(unsigned short) * nv);
230                                         MEM_freeN(newPoly);
231                                         newPoly = newPolyBig;
232                                 }
233                                 newPoly[nv++] = dtris[tri * 3 * 2 + edge];
234                                 /* move to next edge */
235                                 edge = (edge + 1) % 3;
236                         }
237                         else {
238                                 /* move to next tri */
239                                 int twinedge = -1;
240                                 for (k = 0; k < 3; k++) {
241                                         if (dtris[neighbortri * 3 * 2 + 3 + k] == tri) {
242                                                 twinedge = k;
243                                                 break;
244                                         }
245                                 }
246                                 if (twinedge == -1) {
247                                         printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
248                                         MEM_freeN(traversedTris);
249                                         goto returnLabel;
250                                 }
251                                 tri = neighbortri;
252                                 edge = (twinedge + 1) % 3;
253                                 traversedTris[tri - dtrisBase] = 1;
254                         }
255                 }
256
257                 adjustedPoly = MEM_callocN(sizeof(unsigned short) * nv, "buildPolygonsByDetailedMeshes adjustedPoly");
258                 adjustedNv = 0;
259                 for (i = 0; i < nv; i++) {
260                         unsigned short prev = newPoly[(nv + i - 1) % nv];
261                         unsigned short cur = newPoly[i];
262                         unsigned short next = newPoly[(i + 1) % nv];
263                         float distSq = distPointToSegmentSq(&verts[3 * cur], &verts[3 * prev], &verts[3 * next]);
264                         static const float tolerance = 0.001f;
265                         if (distSq > tolerance)
266                                 adjustedPoly[adjustedNv++] = cur;
267                 }
268                 memcpy(newPoly, adjustedPoly, adjustedNv * sizeof(unsigned short));
269                 MEM_freeN(adjustedPoly);
270                 nv = adjustedNv;
271
272                 allBorderTraversed = 1;
273                 for (i = 0; i < dtrisNum; i++) {
274                         if (traversedTris[i] == 0) {
275                                 /* check whether it has border edges */
276                                 int curpolytri = dtrisBase + i;
277                                 for (k = 0; k < 3; k++) {
278                                         unsigned short neighbortri = dtris[curpolytri * 3 * 2 + 3 + k];
279                                         if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
280                                                 allBorderTraversed = 0;
281                                                 break;
282                                         }
283                                 }
284                         }
285                 }
286
287                 if (nv <= vertsPerPoly && allBorderTraversed) {
288                         for (i = 0; i < nv; i++) {
289                                 polys[polyidx * vertsPerPoly * 2 + i] = newPoly[i];
290                         }
291                 }
292
293                 MEM_freeN(traversedTris);
294         }
295
296 returnLabel:
297         MEM_freeN(newPoly);
298
299         return 1;
300 }
301
302 struct SortContext {
303         const int *recastData;
304         const int *trisToFacesMap;
305 };
306
307 static int compareByData(void *ctx, const void *a, const void *b)
308 {
309         return (((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int *)a]] -
310                 ((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int *)b]]);
311 }
312
313 int buildNavMeshData(const int nverts, const float *verts,
314                      const int ntris, const unsigned short *tris,
315                      const int *recastData, const int *trisToFacesMap,
316                      int *ndtris_r, unsigned short **dtris_r,
317                      int *npolys_r, unsigned short **dmeshes_r, unsigned short **polys_r,
318                      int *vertsPerPoly_r, int **dtrisToPolysMap_r, int **dtrisToTrisMap_r)
319
320 {
321         int *trisMapping;
322         int i;
323         struct SortContext context;
324         int validTriStart, prevPolyIdx, curPolyIdx, newPolyIdx, prevpolyidx;
325         unsigned short *dmesh;
326
327         int ndtris, npolys, vertsPerPoly;
328         unsigned short *dtris, *dmeshes, *polys;
329         int *dtrisToPolysMap, *dtrisToTrisMap;
330
331         if (!recastData) {
332                 printf("Converting navmesh: Error! Can't find recast custom data\n");
333                 return 0;
334         }
335
336         trisMapping = MEM_callocN(sizeof(int) * ntris, "buildNavMeshData trisMapping");
337
338         /* sort the triangles by polygon idx */
339         for (i = 0; i < ntris; i++)
340                 trisMapping[i] = i;
341         context.recastData = recastData;
342         context.trisToFacesMap = trisToFacesMap;
343         recast_qsort(trisMapping, ntris, sizeof(int), &context, compareByData);
344
345         /* search first valid triangle - triangle of convex polygon */
346         validTriStart = -1;
347         for (i = 0; i < ntris; i++) {
348                 if (recastData[trisToFacesMap[trisMapping[i]]] > 0) {
349                         validTriStart = i;
350                         break;
351                 }
352         }
353
354         if (validTriStart < 0) {
355                 printf("Converting navmesh: Error! No valid polygons in mesh\n");
356                 MEM_freeN(trisMapping);
357                 return 0;
358         }
359
360         ndtris = ntris - validTriStart;
361         /* fill dtris to faces mapping */
362         dtrisToTrisMap = MEM_callocN(sizeof(int) * ndtris, "buildNavMeshData dtrisToTrisMap");
363         memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris * sizeof(int));
364         MEM_freeN(trisMapping);
365
366         /* create detailed mesh triangles  - copy only valid triangles
367          * and reserve memory for adjacency info */
368         dtris = MEM_callocN(sizeof(unsigned short) * 3 * 2 * ndtris, "buildNavMeshData dtris");
369         memset(dtris, 0xffff, sizeof(unsigned short) * 3 * 2 * ndtris);
370         for (i = 0; i < ndtris; i++) {
371                 memcpy(dtris + 3 * 2 * i, tris + 3 * dtrisToTrisMap[i], sizeof(unsigned short) * 3);
372         }
373
374         /* create new recast data corresponded to dtris and renumber for continuous indices */
375         prevPolyIdx = -1;
376         newPolyIdx = 0;
377         dtrisToPolysMap = MEM_callocN(sizeof(int) * ndtris, "buildNavMeshData dtrisToPolysMap");
378         for (i = 0; i < ndtris; i++) {
379                 curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
380                 if (curPolyIdx != prevPolyIdx) {
381                         newPolyIdx++;
382                         prevPolyIdx = curPolyIdx;
383                 }
384                 dtrisToPolysMap[i] = newPolyIdx;
385         }
386
387
388         /* build adjacency info for detailed mesh triangles */
389         recast_buildMeshAdjacency(dtris, ndtris, nverts, 3);
390
391         /* create detailed mesh description for each navigation polygon */
392         npolys = dtrisToPolysMap[ndtris - 1];
393         dmeshes = MEM_callocN(sizeof(unsigned short) * npolys * 4, "buildNavMeshData dmeshes");
394         memset(dmeshes, 0, npolys * 4 * sizeof(unsigned short));
395         dmesh = NULL;
396         prevpolyidx = 0;
397         for (i = 0; i < ndtris; i++) {
398                 int curpolyidx = dtrisToPolysMap[i];
399                 if (curpolyidx != prevpolyidx) {
400                         if (curpolyidx != prevpolyidx + 1) {
401                                 printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
402                                 return 0;
403                         }
404                         dmesh = dmesh == NULL ? dmeshes : dmesh + 4;
405                         dmesh[2] = (unsigned short)i;  /* tbase */
406                         dmesh[3] = 0;  /* tnum */
407                         prevpolyidx = curpolyidx;
408                 }
409                 dmesh[3]++;
410         }
411
412         /* create navigation polygons */
413         vertsPerPoly = 6;
414         polys = MEM_callocN(sizeof(unsigned short) * npolys * vertsPerPoly * 2, "buildNavMeshData polys");
415         memset(polys, 0xff, sizeof(unsigned short) * vertsPerPoly * 2 * npolys);
416
417         buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
418
419         *ndtris_r = ndtris;
420         *npolys_r = npolys;
421         *vertsPerPoly_r = vertsPerPoly;
422         *dtris_r = dtris;
423         *dmeshes_r = dmeshes;
424         *polys_r = polys;
425         *dtrisToPolysMap_r = dtrisToPolysMap;
426         *dtrisToTrisMap_r = dtrisToTrisMap;
427
428         return 1;
429 }
430
431
432 int buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int *vertsPerPoly,
433                                   int *nverts, float **verts,
434                                   int *ndtris, unsigned short **dtris,
435                                   int *npolys, unsigned short **dmeshes,
436                                   unsigned short **polys, int **dtrisToPolysMap,
437                                   int **dtrisToTrisMap, int **trisToFacesMap)
438 {
439         int res = 1;
440         int ntris = 0, *recastData = NULL;
441         unsigned short *tris = NULL;
442
443         res = buildRawVertIndicesData(dm, nverts, verts, &ntris, &tris, trisToFacesMap, &recastData);
444         if (!res) {
445                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
446                 goto exit;
447         }
448
449         res = buildNavMeshData(*nverts, *verts, ntris, tris, recastData, *trisToFacesMap,
450                                ndtris, dtris, npolys, dmeshes, polys, vertsPerPoly,
451                                dtrisToPolysMap, dtrisToTrisMap);
452         if (!res) {
453                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
454                 goto exit;
455         }
456
457 exit:
458         if (tris)
459                 MEM_freeN(tris);
460
461         return res;
462 }
463
464 int polyFindVertex(const unsigned short *p, const int vertsPerPoly, unsigned short vertexIdx)
465 {
466         int i, res = -1;
467         for (i = 0; i < vertsPerPoly; i++) {
468                 if (p[i] == 0xffff)
469                         break;
470                 if (p[i] == vertexIdx) {
471                         res = i;
472                         break;
473                 }
474         }
475         return res;
476 }