style cleanup, brackets in else/if, some indentation.
[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                                 //move to next tri
253                                 int twinedge = -1;
254                                 for (k=0; k<3; k++)
255                                 {
256                                         if (dtris[neighbortri*3*2+3+k] == tri)
257                                         {
258                                                 twinedge = k;
259                                                 break;
260                                         }
261                                 }
262                                 if (twinedge==-1)
263                                 {
264                                         printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
265                                         MEM_freeN(traversedTris);
266                                         goto returnLabel;
267                                 }
268                                 tri = neighbortri;
269                                 edge = (twinedge+1)%3;
270                                 traversedTris[tri-dtrisBase] = 1;
271                         }
272                 }
273
274                 adjustedPoly = MEM_callocN(sizeof(unsigned short)*nv, "buildPolygonsByDetailedMeshes adjustedPoly");
275                 adjustedNv = 0;
276                 for (i=0; i<nv; i++)
277                 {
278                         unsigned short prev = newPoly[(nv+i-1)%nv];
279                         unsigned short cur = newPoly[i];
280                         unsigned short next = newPoly[(i+1)%nv];
281                         float distSq = distPointToSegmentSq(&verts[3*cur], &verts[3*prev], &verts[3*next]);
282                         static const float tolerance = 0.001f;
283                         if (distSq>tolerance)
284                                 adjustedPoly[adjustedNv++] = cur;
285                 }
286                 memcpy(newPoly, adjustedPoly, adjustedNv*sizeof(unsigned short));
287                 MEM_freeN(adjustedPoly);
288                 nv = adjustedNv;
289
290                 allBorderTraversed = 1;
291                 for (i=0; i<dtrisNum; i++)
292                 {
293                         if (traversedTris[i]==0)
294                         {
295                                 //check whether it has border edges
296                                 int curpolytri = dtrisBase+i;
297                                 for (k=0; k<3; k++)
298                                 {
299                                         unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
300                                         if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
301                                         {
302                                                 allBorderTraversed = 0;
303                                                 break;
304                                         }
305                                 }
306                         }                               
307                 }
308
309                 if (nv<=vertsPerPoly && allBorderTraversed)
310                 {
311                         for (i=0; i<nv; i++)
312                         {
313                                 polys[polyidx*vertsPerPoly*2+i] = newPoly[i];
314                         }
315                 }
316
317                 MEM_freeN(traversedTris);
318         }
319
320 returnLabel:
321         MEM_freeN(newPoly);
322
323         return 1;
324 }
325
326 struct SortContext
327 {
328         const int* recastData;
329         const int* trisToFacesMap;
330 };
331
332 static int compareByData(void *ctx, const void * a, const void * b)
333 {
334         return (((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int*)a]] -
335                         ((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int*)b]] );
336 }
337
338 int buildNavMeshData(const int nverts, const float* verts, 
339                                                          const int ntris, const unsigned short *tris, 
340                                                          const int* recastData, const int* trisToFacesMap,
341                                                          int *ndtris_r, unsigned short **dtris_r,
342                                                          int *npolys_r, unsigned short **dmeshes_r, unsigned short **polys_r,
343                                                          int *vertsPerPoly_r, int **dtrisToPolysMap_r, int **dtrisToTrisMap_r)
344
345 {
346         int *trisMapping;
347         int i;
348         struct SortContext context;
349         int validTriStart, prevPolyIdx, curPolyIdx, newPolyIdx, prevpolyidx;
350         unsigned short *dmesh;
351
352         int ndtris, npolys, vertsPerPoly;
353         unsigned short *dtris, *dmeshes, *polys;
354         int *dtrisToPolysMap, *dtrisToTrisMap;
355
356         if (!recastData)
357         {
358                 printf("Converting navmesh: Error! Can't find recast custom data\n");
359                 return 0;
360         }
361
362         trisMapping = MEM_callocN(sizeof(int)*ntris, "buildNavMeshData trisMapping");
363
364         //sort the triangles by polygon idx
365         for (i=0; i<ntris; i++)
366                 trisMapping[i]=i;
367         context.recastData = recastData;
368         context.trisToFacesMap = trisToFacesMap;
369         recast_qsort(trisMapping, ntris, sizeof(int), &context, compareByData);
370
371         //search first valid triangle - triangle of convex polygon
372         validTriStart = -1;
373         for (i=0; i< ntris; i++)
374         {
375                 if (recastData[trisToFacesMap[trisMapping[i]]]>0)
376                 {
377                         validTriStart = i;
378                         break;
379                 }
380         }
381
382         if (validTriStart<0)
383         {
384                 printf("Converting navmesh: Error! No valid polygons in mesh\n");
385                 MEM_freeN(trisMapping);
386                 return 0;
387         }
388
389         ndtris = ntris-validTriStart;
390         //fill dtris to faces mapping
391         dtrisToTrisMap = MEM_callocN(sizeof(int)*ndtris, "buildNavMeshData dtrisToTrisMap");
392         memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
393         MEM_freeN(trisMapping);
394
395         //create detailed mesh triangles  - copy only valid triangles
396         //and reserve memory for adjacency info
397         dtris = MEM_callocN(sizeof(unsigned short)*3*2*ndtris, "buildNavMeshData dtris");
398         memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
399         for (i=0; i<ndtris; i++)
400         {
401                 memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
402         }
403
404         //create new recast data corresponded to dtris and renumber for continuous indices
405         prevPolyIdx = -1;
406         newPolyIdx = 0;
407         dtrisToPolysMap = MEM_callocN(sizeof(int)*ndtris, "buildNavMeshData dtrisToPolysMap");
408         for (i=0; i<ndtris; i++)
409         {
410                 curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
411                 if (curPolyIdx!=prevPolyIdx)
412                 {
413                         newPolyIdx++;
414                         prevPolyIdx=curPolyIdx;
415                 }
416                 dtrisToPolysMap[i] = newPolyIdx;
417         }
418
419
420         //build adjacency info for detailed mesh triangles
421         recast_buildMeshAdjacency(dtris, ndtris, nverts, 3);
422
423         //create detailed mesh description for each navigation polygon
424         npolys = dtrisToPolysMap[ndtris-1];
425         dmeshes = MEM_callocN(sizeof(unsigned short)*npolys*4, "buildNavMeshData dmeshes");
426         memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
427         dmesh = NULL;
428         prevpolyidx = 0;
429         for (i=0; i<ndtris; i++)
430         {
431                 int curpolyidx = dtrisToPolysMap[i];
432                 if (curpolyidx!=prevpolyidx)
433                 {
434                         if (curpolyidx!=prevpolyidx+1)
435                         {
436                                 printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
437                                 return 0;
438                         }
439                         dmesh = dmesh==NULL ? dmeshes : dmesh+4;
440                         dmesh[2] = (unsigned short)i;   //tbase
441                         dmesh[3] = 0;   //tnum
442                         prevpolyidx = curpolyidx;
443                 }
444                 dmesh[3]++;
445         }
446
447         //create navigation polygons
448         vertsPerPoly = 6;
449         polys = MEM_callocN(sizeof(unsigned short)*npolys*vertsPerPoly*2, "buildNavMeshData polys");
450         memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
451
452         buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
453
454         *ndtris_r = ndtris;
455         *npolys_r = npolys;
456         *vertsPerPoly_r = vertsPerPoly;
457         *dtris_r = dtris;
458         *dmeshes_r = dmeshes;
459         *polys_r = polys;
460         *dtrisToPolysMap_r = dtrisToPolysMap;
461         *dtrisToTrisMap_r = dtrisToTrisMap;
462
463         return 1;
464 }
465
466
467 int buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int *vertsPerPoly, 
468                                                                                   int *nverts, float **verts,
469                                                                                   int *ndtris, unsigned short **dtris,
470                                                                                   int *npolys, unsigned short **dmeshes,
471                                                                                   unsigned short **polys, int **dtrisToPolysMap,
472                                                                                   int **dtrisToTrisMap, int **trisToFacesMap)
473 {
474         int res = 1;
475         int ntris = 0, *recastData=NULL;
476         unsigned short *tris=NULL;
477
478         res = buildRawVertIndicesData(dm, nverts, verts, &ntris, &tris, trisToFacesMap, &recastData);
479         if (!res)
480         {
481                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
482                 goto exit;
483         }
484
485         res = buildNavMeshData(*nverts, *verts, ntris, tris, recastData, *trisToFacesMap,
486                 ndtris, dtris, npolys, dmeshes,polys, vertsPerPoly, 
487                 dtrisToPolysMap, dtrisToTrisMap);
488         if (!res)
489         {
490                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
491                 goto exit;
492         }
493
494 exit:
495         if (tris)
496                 MEM_freeN(tris);
497
498         return res;
499 }
500
501 int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx)
502 {
503         int i, res = -1;
504         for(i=0; i<vertsPerPoly; i++)
505         {
506                 if (p[i]==0xffff)
507                         break;
508                 if (p[i]==vertexIdx)
509                 {
510                         res = i;
511                         break;
512                 }
513         }
514         return res;
515 }