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