Merge branch 'master' into blender2.8
[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 #include "BLI_sort.h"
42
43 #include "BKE_navmesh_conversion.h"
44 #include "BKE_cdderivedmesh.h"
45
46 #include "recast-capi.h"
47
48 BLI_INLINE float area2(const float *a, const float *b, const float *c)
49 {
50         return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
51 }
52
53 BLI_INLINE int left(const float *a, const float *b, const float *c)
54 {
55         return area2(a, b, c) < 0;
56 }
57
58 int polyNumVerts(const unsigned short *p, const int vertsPerPoly)
59 {
60         int i, nv = 0;
61         for (i = 0; i < vertsPerPoly; i++) {
62                 if (p[i] == 0xffff)
63                         break;
64                 nv++;
65         }
66         return nv;
67 }
68
69 int polyIsConvex(const unsigned short *p, const int vertsPerPoly, const float *verts)
70 {
71         int j, nv = polyNumVerts(p, vertsPerPoly);
72         if (nv < 3)
73                 return 0;
74         for (j = 0; j < nv; j++) {
75                 const float *v = &verts[3 * p[j]];
76                 const float *v_next = &verts[3 * p[(j + 1) % nv]];
77                 const float *v_prev = &verts[3 * p[(nv + j - 1) % nv]];
78                 if (!left(v_prev, v, v_next))
79                         return 0;
80
81         }
82         return 1;
83 }
84
85 /* XXX, could replace with #dist_to_line_segment_v3(), or add a squared version */
86 float distPointToSegmentSq(const float point[3], const float a[3], const float b[3])
87 {
88         float abx[3], dx[3];
89         float d, t;
90
91         sub_v3_v3v3(abx, b, a);
92         sub_v3_v3v3(dx, point, a);
93
94         d = abx[0] * abx[0] + abx[2] * abx[2];
95         t = abx[0] * dx[0] + abx[2] * dx[2];
96
97         if (d > 0.0f)
98                 t /= d;
99         if (t < 0.0f)
100                 t = 0.0f;
101         else if (t > 1.0f)
102                 t = 1.0f;
103         dx[0] = a[0] + t * abx[0] - point[0];
104         dx[2] = a[2] + t * abx[2] - point[2];
105
106         return dx[0] * dx[0] + dx[2] * dx[2];
107 }
108
109 int buildRawVertIndicesData(DerivedMesh *dm, int *nverts_r, float **verts_r,
110                             int *ntris_r, unsigned short **tris_r, int **trisToFacesMap_r,
111                             int **recastData)
112 {
113         int vi, fi, triIdx;
114         int nverts, ntris;
115         int *trisToFacesMap;
116         float *verts;
117         unsigned short *tris, *tri;
118         int nfaces;
119         MFace *faces;
120
121         nverts = dm->getNumVerts(dm);
122         if (nverts >= 0xffff) {
123                 printf("Converting navmesh: Error! Too many vertices. Max number of vertices %d\n", 0xffff);
124                 return 0;
125         }
126         if (nverts == 0) {
127                 printf("Converting navmesh: Error! There are no vertices!\n");
128                 return 0;
129         }
130
131         verts = MEM_mallocN(sizeof(float[3]) * nverts, "buildRawVertIndicesData verts");
132         dm->getVertCos(dm, (float(*)[3])verts);
133
134         /* flip coordinates */
135         for (vi = 0; vi < nverts; vi++) {
136                 SWAP(float, verts[3 * vi + 1], verts[3 * vi + 2]);
137         }
138
139         /* calculate number of tris */
140         dm->recalcTessellation(dm);
141         nfaces = dm->getNumTessFaces(dm);
142         if (nfaces == 0) {
143                 printf("Converting navmesh: Error! There are %i vertices, but no faces!\n", nverts);
144                 return 0;
145         }
146
147         faces = dm->getTessFaceArray(dm);
148         ntris = nfaces;
149         for (fi = 0; fi < nfaces; fi++) {
150                 MFace *face = &faces[fi];
151                 if (face->v4)
152                         ntris++;
153         }
154
155         /* copy and transform to triangles (reorder on the run) */
156         trisToFacesMap = MEM_callocN(sizeof(int) * ntris, "buildRawVertIndicesData trisToFacesMap");
157         tris = MEM_callocN(sizeof(unsigned short) * 3 * ntris, "buildRawVertIndicesData tris");
158         tri = tris;
159         triIdx = 0;
160         for (fi = 0; fi < nfaces; fi++) {
161                 MFace *face = &faces[fi];
162                 tri[3 * triIdx + 0] = (unsigned short) face->v1;
163                 tri[3 * triIdx + 1] = (unsigned short) face->v3;
164                 tri[3 * triIdx + 2] = (unsigned short) face->v2;
165                 trisToFacesMap[triIdx++] = fi;
166                 if (face->v4) {
167                         tri[3 * triIdx + 0] = (unsigned short) face->v1;
168                         tri[3 * triIdx + 1] = (unsigned short) face->v4;
169                         tri[3 * triIdx + 2] = (unsigned short) face->v3;
170                         trisToFacesMap[triIdx++] = fi;
171                 }
172         }
173
174         /* carefully, recast data is just reference to data in derived mesh */
175         *recastData = (int *)CustomData_get_layer(&dm->polyData, CD_RECAST);
176
177         *nverts_r = nverts;
178         *verts_r = verts;
179         *ntris_r = ntris;
180         *tris_r = tris;
181         *trisToFacesMap_r = trisToFacesMap;
182
183         return 1;
184 }
185
186 int buildPolygonsByDetailedMeshes(const int vertsPerPoly, const int npolys,
187                                   unsigned short *polys, const unsigned short *dmeshes,
188                                   const float *verts, const unsigned short *dtris,
189                                   const int *dtrisToPolysMap)
190 {
191         int polyidx;
192         int capacity = vertsPerPoly;
193         unsigned short *newPoly = MEM_callocN(sizeof(unsigned short) * capacity, "buildPolygonsByDetailedMeshes newPoly");
194         memset(newPoly, 0xff, sizeof(unsigned short) * capacity);
195
196         for (polyidx = 0; polyidx < npolys; polyidx++) {
197                 size_t i;
198                 int j, k;
199                 int nv = 0;
200                 /* search border */
201                 int tri, btri = -1;
202                 int edge, bedge = -1;
203                 int dtrisNum = dmeshes[polyidx * 4 + 3];
204                 int dtrisBase = dmeshes[polyidx * 4 + 2];
205                 unsigned char *traversedTris = MEM_callocN(sizeof(unsigned char) * dtrisNum, "buildPolygonsByDetailedMeshes traversedTris");
206                 unsigned short *adjustedPoly;
207                 int adjustedNv;
208                 int allBorderTraversed;
209
210                 for (j = 0; j < dtrisNum && btri == -1; j++) {
211                         int curpolytri = dtrisBase + j;
212                         for (k = 0; k < 3; k++) {
213                                 unsigned short neighbortri = dtris[curpolytri * 3 * 2 + 3 + k];
214                                 if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
215                                         btri = curpolytri;
216                                         bedge = k;
217                                         break;
218                                 }
219                         }
220                 }
221                 if (btri == -1 || bedge == -1) {
222                         /* can't find triangle with border edge */
223                         MEM_freeN(traversedTris);
224                         MEM_freeN(newPoly);
225
226                         return 0;
227                 }
228
229                 newPoly[nv++] = dtris[btri * 3 * 2 + bedge];
230                 tri = btri;
231                 edge = (bedge + 1) % 3;
232                 traversedTris[tri - dtrisBase] = 1;
233                 while (tri != btri || edge != bedge) {
234                         int neighbortri = dtris[tri * 3 * 2 + 3 + edge];
235                         if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
236                                 if (nv == capacity) {
237                                         unsigned short *newPolyBig;
238                                         capacity += vertsPerPoly;
239                                         newPolyBig = MEM_callocN(sizeof(unsigned short) * capacity, "buildPolygonsByDetailedMeshes newPolyBig");
240                                         memset(newPolyBig, 0xff, sizeof(unsigned short) * capacity);
241                                         memcpy(newPolyBig, newPoly, sizeof(unsigned short) * nv);
242                                         MEM_freeN(newPoly);
243                                         newPoly = newPolyBig;
244                                 }
245                                 newPoly[nv++] = dtris[tri * 3 * 2 + edge];
246                                 /* move to next edge */
247                                 edge = (edge + 1) % 3;
248                         }
249                         else {
250                                 /* move to next tri */
251                                 int twinedge = -1;
252                                 for (k = 0; k < 3; k++) {
253                                         if (dtris[neighbortri * 3 * 2 + 3 + k] == tri) {
254                                                 twinedge = k;
255                                                 break;
256                                         }
257                                 }
258                                 if (twinedge == -1) {
259                                         printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
260                                         MEM_freeN(traversedTris);
261                                         goto returnLabel;
262                                 }
263                                 tri = neighbortri;
264                                 edge = (twinedge + 1) % 3;
265                                 traversedTris[tri - dtrisBase] = 1;
266                         }
267                 }
268
269                 adjustedPoly = MEM_callocN(sizeof(unsigned short) * nv, "buildPolygonsByDetailedMeshes adjustedPoly");
270                 adjustedNv = 0;
271                 for (i = 0; i < nv; i++) {
272                         unsigned short prev = newPoly[(nv + i - 1) % nv];
273                         unsigned short cur = newPoly[i];
274                         unsigned short next = newPoly[(i + 1) % nv];
275                         float distSq = distPointToSegmentSq(&verts[3 * cur], &verts[3 * prev], &verts[3 * next]);
276                         static const float tolerance = 0.001f;
277                         if (distSq > tolerance)
278                                 adjustedPoly[adjustedNv++] = cur;
279                 }
280                 memcpy(newPoly, adjustedPoly, adjustedNv * sizeof(unsigned short));
281                 MEM_freeN(adjustedPoly);
282                 nv = adjustedNv;
283
284                 allBorderTraversed = 1;
285                 for (i = 0; i < dtrisNum; i++) {
286                         if (traversedTris[i] == 0) {
287                                 /* check whether it has border edges */
288                                 int curpolytri = dtrisBase + i;
289                                 for (k = 0; k < 3; k++) {
290                                         unsigned short neighbortri = dtris[curpolytri * 3 * 2 + 3 + k];
291                                         if (neighbortri == 0xffff || dtrisToPolysMap[neighbortri] != polyidx + 1) {
292                                                 allBorderTraversed = 0;
293                                                 break;
294                                         }
295                                 }
296                         }
297                 }
298
299                 if (nv <= vertsPerPoly && allBorderTraversed) {
300                         for (i = 0; i < nv; i++) {
301                                 polys[polyidx * vertsPerPoly * 2 + i] = newPoly[i];
302                         }
303                 }
304
305                 MEM_freeN(traversedTris);
306         }
307
308 returnLabel:
309         MEM_freeN(newPoly);
310
311         return 1;
312 }
313
314 struct SortContext {
315         const int *recastData;
316         const int *trisToFacesMap;
317 };
318
319 static int compareByData(const void *a, const void *b, void *ctx)
320 {
321         return (((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int *)a]] -
322                 ((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int *)b]]);
323 }
324
325 int buildNavMeshData(const int nverts, const float *verts,
326                      const int ntris, const unsigned short *tris,
327                      const int *recastData, const int *trisToFacesMap,
328                      int *ndtris_r, unsigned short **dtris_r,
329                      int *npolys_r, unsigned short **dmeshes_r, unsigned short **polys_r,
330                      int *vertsPerPoly_r, int **dtrisToPolysMap_r, int **dtrisToTrisMap_r)
331
332 {
333         int *trisMapping;
334         int i;
335         struct SortContext context;
336         int validTriStart, prevPolyIdx, curPolyIdx, newPolyIdx, prevpolyidx;
337         unsigned short *dmesh;
338
339         int ndtris, npolys, vertsPerPoly;
340         unsigned short *dtris, *dmeshes, *polys;
341         int *dtrisToPolysMap, *dtrisToTrisMap;
342
343         if (!recastData) {
344                 printf("Converting navmesh: Error! Can't find recast custom data\n");
345                 return 0;
346         }
347
348         trisMapping = MEM_callocN(sizeof(int) * ntris, "buildNavMeshData trisMapping");
349
350         /* sort the triangles by polygon idx */
351         for (i = 0; i < ntris; i++)
352                 trisMapping[i] = i;
353         context.recastData = recastData;
354         context.trisToFacesMap = trisToFacesMap;
355         BLI_qsort_r(trisMapping, ntris, sizeof(int), compareByData, &context);
356
357         /* search first valid triangle - triangle of convex polygon */
358         validTriStart = -1;
359         for (i = 0; i < ntris; i++) {
360                 if (recastData[trisToFacesMap[trisMapping[i]]] > 0) {
361                         validTriStart = i;
362                         break;
363                 }
364         }
365
366         if (validTriStart < 0) {
367                 printf("Converting navmesh: Error! No valid polygons in mesh\n");
368                 MEM_freeN(trisMapping);
369                 return 0;
370         }
371
372         ndtris = ntris - validTriStart;
373         /* fill dtris to faces mapping */
374         dtrisToTrisMap = MEM_callocN(sizeof(int) * ndtris, "buildNavMeshData dtrisToTrisMap");
375         memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris * sizeof(int));
376         MEM_freeN(trisMapping);
377
378         /* create detailed mesh triangles  - copy only valid triangles
379          * and reserve memory for adjacency info */
380         dtris = MEM_callocN(sizeof(unsigned short) * 3 * 2 * ndtris, "buildNavMeshData dtris");
381         memset(dtris, 0xff, sizeof(unsigned short) * 3 * 2 * ndtris);
382         for (i = 0; i < ndtris; i++) {
383                 memcpy(dtris + 3 * 2 * i, tris + 3 * dtrisToTrisMap[i], sizeof(unsigned short) * 3);
384         }
385
386         /* create new recast data corresponded to dtris and renumber for continuous indices */
387         prevPolyIdx = -1;
388         newPolyIdx = 0;
389         dtrisToPolysMap = MEM_callocN(sizeof(int) * ndtris, "buildNavMeshData dtrisToPolysMap");
390         for (i = 0; i < ndtris; i++) {
391                 curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
392                 if (curPolyIdx != prevPolyIdx) {
393                         newPolyIdx++;
394                         prevPolyIdx = curPolyIdx;
395                 }
396                 dtrisToPolysMap[i] = newPolyIdx;
397         }
398
399
400         /* build adjacency info for detailed mesh triangles */
401         if (!recast_buildMeshAdjacency(dtris, ndtris, nverts, 3)) {
402                 printf("Converting navmesh: Error! Unable to build mesh adjacency information\n");
403                 MEM_freeN(trisMapping);
404                 MEM_freeN(dtrisToPolysMap);
405                 return 0;
406         }
407
408         /* create detailed mesh description for each navigation polygon */
409         npolys = dtrisToPolysMap[ndtris - 1];
410         dmeshes = MEM_callocN(sizeof(unsigned short) * npolys * 4, "buildNavMeshData dmeshes");
411         memset(dmeshes, 0, npolys * 4 * sizeof(unsigned short));
412         dmesh = NULL;
413         prevpolyidx = 0;
414         for (i = 0; i < ndtris; i++) {
415                 int curpolyidx = dtrisToPolysMap[i];
416                 if (curpolyidx != prevpolyidx) {
417                         if (curpolyidx != prevpolyidx + 1) {
418                                 printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
419                                 goto fail;
420                         }
421                         dmesh = dmesh == NULL ? dmeshes : dmesh + 4;
422                         dmesh[2] = (unsigned short)i;  /* tbase */
423                         dmesh[3] = 0;  /* tnum */
424                         prevpolyidx = curpolyidx;
425                 }
426                 dmesh[3]++;
427         }
428
429         /* create navigation polygons */
430         vertsPerPoly = 6;
431         polys = MEM_callocN(sizeof(unsigned short) * npolys * vertsPerPoly * 2, "buildNavMeshData polys");
432         memset(polys, 0xff, sizeof(unsigned short) * vertsPerPoly * 2 * npolys);
433
434         if (!buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap)) {
435                 printf("Converting navmesh: Error! Unable to build polygons from detailed mesh\n");
436                 goto fail;
437         }
438
439         *ndtris_r = ndtris;
440         *npolys_r = npolys;
441         *vertsPerPoly_r = vertsPerPoly;
442         *dtris_r = dtris;
443         *dmeshes_r = dmeshes;
444         *polys_r = polys;
445         *dtrisToPolysMap_r = dtrisToPolysMap;
446         *dtrisToTrisMap_r = dtrisToTrisMap;
447
448         return 1;
449
450 fail:
451         MEM_freeN(dmeshes);
452         MEM_freeN(dtrisToPolysMap);
453         MEM_freeN(dtrisToTrisMap);
454         return 0;
455 }
456
457
458 int buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int *vertsPerPoly,
459                                   int *nverts, float **verts,
460                                   int *ndtris, unsigned short **dtris,
461                                   int *npolys, unsigned short **dmeshes,
462                                   unsigned short **polys, int **dtrisToPolysMap,
463                                   int **dtrisToTrisMap, int **trisToFacesMap)
464 {
465         int res;
466         int ntris = 0, *recastData = NULL;
467         unsigned short *tris = NULL;
468
469         res = buildRawVertIndicesData(dm, nverts, verts, &ntris, &tris, trisToFacesMap, &recastData);
470         if (!res) {
471                 printf("Converting navmesh: Error! Can't get raw vertices and indices from mesh\n");
472                 goto exit;
473         }
474
475         res = buildNavMeshData(*nverts, *verts, ntris, tris, recastData, *trisToFacesMap,
476                                ndtris, dtris, npolys, dmeshes, polys, vertsPerPoly,
477                                dtrisToPolysMap, dtrisToTrisMap);
478         if (!res) {
479                 printf("Converting navmesh: Error! Can't build navmesh data from mesh\n");
480                 goto exit;
481         }
482
483 exit:
484         if (tris)
485                 MEM_freeN(tris);
486
487         return res;
488 }
489
490 int polyFindVertex(const unsigned short *p, const int vertsPerPoly, unsigned short vertexIdx)
491 {
492         int i, res = -1;
493         for (i = 0; i < vertsPerPoly; i++) {
494                 if (p[i] == 0xffff)
495                         break;
496                 if (p[i] == vertexIdx) {
497                         res = i;
498                         break;
499                 }
500         }
501         return res;
502 }