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