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