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