4d89f78bdf5401f0e74a18badca588f5d9aae14d
[blender.git] / source / blender / editors / util / navmesh_conversion.cpp
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 #include <math.h>
30 #include "Recast.h"
31
32
33 extern "C"{
34 #include "ED_navmesh_conversion.h"
35
36 #include "DNA_meshdata_types.h"
37 #include "BKE_cdderivedmesh.h"
38 #include "BLI_math.h"
39 }
40
41 int polyNumVerts(const unsigned short* p, const int vertsPerPoly)
42 {
43         int nv = 0;
44         for (int i=0; i<vertsPerPoly; i++)
45         {
46                 if (p[i]==0xffff)
47                         break;
48                 nv++;
49         }
50         return nv;
51 }
52
53 bool polyIsConvex(const unsigned short* p, const int vertsPerPoly, const float* verts)
54 {
55         int nv = polyNumVerts(p, vertsPerPoly);
56         if (nv<3)
57                 return false;
58         for (int j=0; j<nv; j++)
59         {
60                 const float* v = &verts[3*p[j]];
61                 const float* v_next = &verts[3*p[(j+1)%nv]];
62                 const float* v_prev = &verts[3*p[(nv+j-1)%nv]];
63                 if (!left(v_prev, v, v_next))
64                         return false;
65
66         }
67         return true;
68 }
69
70 float distPointToSegmentSq(const float* point, const float* a, const float* b)
71 {
72         float abx[3], dx[3];
73         vsub(abx, b,a);
74         vsub(dx, point,a);
75         float d = abx[0]*abx[0]+abx[2]*abx[2];
76         float t = abx[0]*dx[0]+abx[2]*dx[2];
77         if (d > 0)
78                 t /= d;
79         if (t < 0)
80                 t = 0;
81         else if (t > 1)
82                 t = 1;
83         dx[0] = a[0] + t*abx[0] - point[0];
84         dx[2] = a[2] + t*abx[2] - point[2];
85         return dx[0]*dx[0] + dx[2]*dx[2];
86 }
87
88 bool buildRawVertIndicesData(DerivedMesh* dm, int &nverts, float *&verts, 
89                                                                         int &ntris, unsigned short *&tris, int *&trisToFacesMap,
90                                                                         int *&recastData)
91 {
92         nverts = dm->getNumVerts(dm);
93         if (nverts>=0xffff)
94         {
95                 printf("Converting navmesh: Error! Too many vertices. Max number of vertices %d\n", 0xffff);
96                 return false;
97         }
98         verts = new float[3*nverts];
99         dm->getVertCos(dm, (float(*)[3])verts);
100
101         //flip coordinates
102         for (int vi=0; vi<nverts; vi++)
103         {
104                 SWAP(float, verts[3*vi+1], verts[3*vi+2]);
105         }
106
107         //calculate number of tris
108         int nfaces = dm->getNumFaces(dm);
109         MFace *faces = dm->getFaceArray(dm);
110         ntris = nfaces;
111         for (int fi=0; fi<nfaces; fi++)
112         {
113                 MFace* face = &faces[fi];
114                 if (face->v4)
115                         ntris++;
116         }
117
118         //copy and transform to triangles (reorder on the run)
119         trisToFacesMap = new int[ntris];
120         tris = new unsigned short[3*ntris];
121         unsigned short* tri = tris;
122         int triIdx = 0;
123         for (int fi=0; fi<nfaces; fi++)
124         {
125                 MFace* face = &faces[fi];
126                 tri[3*triIdx+0] = (unsigned short) face->v1;
127                 tri[3*triIdx+1] = (unsigned short) face->v3;
128                 tri[3*triIdx+2] = (unsigned short) face->v2;
129                 trisToFacesMap[triIdx++]=fi;
130                 if (face->v4)
131                 {
132                         tri[3*triIdx+0] = (unsigned short) face->v1;
133                         tri[3*triIdx+1] = (unsigned short) face->v4;
134                         tri[3*triIdx+2] = (unsigned short) face->v3;
135                         trisToFacesMap[triIdx++]=fi;
136                 }
137         }
138
139         //carefully, recast data is just reference to data in derived mesh
140         recastData = (int*)CustomData_get_layer(&dm->faceData, CD_RECAST);
141         return true;
142 }
143
144 bool buildPolygonsByDetailedMeshes(const int vertsPerPoly, const int npolys, 
145                                                                                   unsigned short* polys, const unsigned short* dmeshes, 
146                                                                                   const float* verts, const unsigned short* dtris, 
147                                                                                   const int* dtrisToPolysMap)
148 {
149         int capacity = vertsPerPoly;
150         unsigned short* newPoly =  new unsigned short[capacity];
151         memset(newPoly, 0xff, sizeof(unsigned short)*capacity);
152         for (int polyidx=0; polyidx<npolys; polyidx++)
153         {
154                 int nv = 0;
155                 //search border 
156                 int btri = -1;
157                 int bedge = -1;
158                 int dtrisNum = dmeshes[polyidx*4+3];
159                 int dtrisBase = dmeshes[polyidx*4+2];
160                 unsigned char *traversedTris = new unsigned char[dtrisNum];
161                 memset(traversedTris, 0, dtrisNum*sizeof(unsigned char));
162                 for (int j=0; j<dtrisNum && btri==-1;j++)
163                 {
164                         int curpolytri = dtrisBase+j;
165                         for (int k=0; k<3; k++)
166                         {
167                                 unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
168                                 if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
169                                 {
170                                         btri = curpolytri;
171                                         bedge = k;
172                                         break;
173                                 }
174                         }                                                       
175                 }
176                 if (btri==-1 || bedge==-1)
177                 {
178                         //can't find triangle with border edge
179                         return false;
180                 }
181
182                 newPoly[nv++] = dtris[btri*3*2+bedge];
183                 int tri = btri;
184                 int edge = (bedge+1)%3;
185                 traversedTris[tri-dtrisBase] = 1;
186                 while (tri!=btri || edge!=bedge)
187                 {
188                         int neighbortri = dtris[tri*3*2+3+edge];
189                         if (neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
190                         {
191                                 if (nv==capacity)
192                                 {
193                                         capacity += vertsPerPoly;
194                                         unsigned short* newPolyBig =  new unsigned short[capacity];
195                                         memset(newPolyBig, 0xff, sizeof(unsigned short)*capacity);
196                                         memcpy(newPolyBig, newPoly, sizeof(unsigned short)*nv);
197                                         delete newPoly;
198                                         newPoly = newPolyBig;                   
199                                 }
200                                 newPoly[nv++] = dtris[tri*3*2+edge];
201                                 //move to next edge                                     
202                                 edge = (edge+1)%3;
203                         }
204                         else
205                         {
206                                 //move to next tri
207                                 int twinedge = -1;
208                                 for (int k=0; k<3; k++)
209                                 {
210                                         if (dtris[neighbortri*3*2+3+k] == tri)
211                                         {
212                                                 twinedge = k;
213                                                 break;
214                                         }
215                                 }
216                                 if (twinedge==-1)
217                                 {
218                                         printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
219                                         goto returnLabel;                                       
220                                 }
221                                 tri = neighbortri;
222                                 edge = (twinedge+1)%3;
223                                 traversedTris[tri-dtrisBase] = 1;
224                         }
225                 }
226
227                 unsigned short* adjustedPoly = new unsigned short[nv];
228                 int adjustedNv = 0;
229                 for (size_t i=0; i<(size_t)nv; i++)
230                 {
231                         unsigned short prev = newPoly[(nv+i-1)%nv];
232                         unsigned short cur = newPoly[i];
233                         unsigned short next = newPoly[(i+1)%nv];
234                         float distSq = distPointToSegmentSq(&verts[3*cur], &verts[3*prev], &verts[3*next]);
235                         static const float tolerance = 0.001f;
236                         if (distSq>tolerance)
237                                 adjustedPoly[adjustedNv++] = cur;
238                 }
239                 memcpy(newPoly, adjustedPoly, adjustedNv*sizeof(unsigned short));
240                 delete adjustedPoly;
241                 nv = adjustedNv;
242
243                 bool allBorderTraversed = true;
244                 for (size_t i=0; i<(size_t)dtrisNum; i++)
245                 {
246                         if (traversedTris[i]==0)
247                         {
248                                 //check whether it has border edges
249                                 int curpolytri = dtrisBase+i;
250                                 for (int k=0; k<3; k++)
251                                 {
252                                         unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
253                                         if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
254                                         {
255                                                 allBorderTraversed = false;
256                                                 break;
257                                         }
258                                 }
259                         }                               
260                 }
261
262                 if (nv<=vertsPerPoly && allBorderTraversed)
263                 {
264                         for (int i=0; i<nv; i++)
265                         {
266                                 polys[polyidx*vertsPerPoly*2+i] = newPoly[i];
267                         }
268                 }
269         }
270
271 returnLabel:
272         delete newPoly;
273         return true;
274 }
275
276 struct SortContext
277 {
278         const int* recastData;
279         const int* trisToFacesMap;
280 };
281 #if defined(_MSC_VER)
282 static int compareByData(void* data, const void * a, const void * b)
283 #elif defined(__APPLE__)
284 static int compareByData(void* data, const void * a, const void * b)
285 #else
286 static int compareByData(const void * a, const void * b, void* data)
287 #endif
288 {
289         const SortContext* context = (const SortContext*)data;
290         return ( context->recastData[context->trisToFacesMap[*(int*)a]] - 
291                 context->recastData[context->trisToFacesMap[*(int*)b]] );
292 }
293
294 bool buildNavMeshData(const int nverts, const float* verts, 
295                                                          const int ntris, const unsigned short *tris, 
296                                                          const int* recastData, const int* trisToFacesMap,
297                                                          int &ndtris, unsigned short *&dtris,
298                                                          int &npolys, unsigned short *&dmeshes, unsigned short *&polys,
299                                                          int &vertsPerPoly, int *&dtrisToPolysMap, int *&dtrisToTrisMap)
300
301 {
302         if (!recastData)
303         {
304                 printf("Converting navmesh: Error! Can't find recast custom data\n");
305                 return false;
306         }
307
308         //sort the triangles by polygon idx
309         int* trisMapping = new int[ntris];
310         for (int i=0; i<ntris; i++)
311                 trisMapping[i]=i;
312         SortContext context;
313         context.recastData = recastData;
314         context.trisToFacesMap = trisToFacesMap;
315 #if defined(_MSC_VER)
316         qsort_s(trisMapping, ntris, sizeof(int), compareByData, &context);
317 #elif defined(__APPLE__)
318         qsort_r(trisMapping, ntris, sizeof(int), &context, compareByData);
319 #else
320         qsort_r(trisMapping, ntris, sizeof(int), compareByData, &context);
321 #endif
322         //search first valid triangle - triangle of convex polygon
323         int validTriStart = -1;
324         for (int i=0; i< ntris; i++)
325         {
326                 if (recastData[trisToFacesMap[trisMapping[i]]]>0)
327                 {
328                         validTriStart = i;
329                         break;
330                 }
331         }
332
333         if (validTriStart<0)
334         {
335                 printf("Converting navmesh: Error! No valid polygons in mesh\n");
336                 delete trisMapping;
337                 return false;
338         }
339
340         ndtris = ntris-validTriStart;
341         //fill dtris to faces mapping
342         dtrisToTrisMap = new int[ndtris];
343         memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
344         delete trisMapping; trisMapping=NULL;
345
346         //create detailed mesh triangles  - copy only valid triangles
347         //and reserve memory for adjacency info
348         dtris = new unsigned short[3*2*ndtris];
349         memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
350         for (int i=0; i<ndtris; i++)
351         {
352                 memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
353         }
354         //create new recast data corresponded to dtris and renumber for continuous indices
355         int prevPolyIdx=-1, curPolyIdx, newPolyIdx=0;
356         dtrisToPolysMap = new int[ndtris];
357         for (int i=0; i<ndtris; i++)
358         {
359                 curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
360                 if (curPolyIdx!=prevPolyIdx)
361                 {
362                         newPolyIdx++;
363                         prevPolyIdx=curPolyIdx;
364                 }
365                 dtrisToPolysMap[i] = newPolyIdx;
366         }
367
368
369         //build adjacency info for detailed mesh triangles
370         buildMeshAdjacency(dtris, ndtris, nverts, 3);
371
372         //create detailed mesh description for each navigation polygon
373         npolys = dtrisToPolysMap[ndtris-1];
374         dmeshes = new unsigned short[npolys*4];
375         memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
376         unsigned short *dmesh = NULL;
377         int prevpolyidx = 0;
378         for (int i=0; i<ndtris; i++)
379         {
380                 int curpolyidx = dtrisToPolysMap[i];
381                 if (curpolyidx!=prevpolyidx)
382                 {
383                         if (curpolyidx!=prevpolyidx+1)
384                         {
385                                 printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
386                                 return false;
387                         }
388                         dmesh = dmesh==NULL ? dmeshes : dmesh+4;
389                         dmesh[2] = (unsigned short)i;   //tbase
390                         dmesh[3] = 0;   //tnum
391                         prevpolyidx = curpolyidx;
392                 }
393                 dmesh[3]++;
394         }
395
396         //create navigation polygons
397         vertsPerPoly = 6;
398         polys = new unsigned short[npolys*vertsPerPoly*2];
399         memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
400
401         buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
402
403         return true;
404 }
405
406
407 bool buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int& vertsPerPoly, 
408                                                                                   int &nverts, float *&verts,
409                                                                                   int &ndtris, unsigned short *&dtris,
410                                                                                   int& npolys, unsigned short *&dmeshes,
411                                                                                   unsigned short*& polys, int *&dtrisToPolysMap,
412                                                                                   int *&dtrisToTrisMap, int *&trisToFacesMap)
413 {
414         bool res = true;
415         int ntris =0, *recastData=NULL;
416         unsigned short *tris=NULL;
417         res = buildRawVertIndicesData(dm, nverts, verts, ntris, tris, trisToFacesMap, recastData);
418         if (!res)
419         {
420                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
421                 goto exit;
422         }
423
424         res = buildNavMeshData(nverts, verts, ntris, tris, recastData, trisToFacesMap,
425                 ndtris, dtris, npolys, dmeshes,polys, vertsPerPoly, 
426                 dtrisToPolysMap, dtrisToTrisMap);
427         if (!res)
428         {
429                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
430                 goto exit;
431         }
432
433 exit:
434         if (tris)
435                 delete tris;
436
437         return res;
438 }
439
440 int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx)
441 {
442         int res = -1;
443         for(int i=0; i<vertsPerPoly; i++)
444         {
445                 if (p[i]==0xffff)
446                         break;
447                 if (p[i]==vertexIdx)
448                 {
449                         res = i;
450                         break;
451                 }
452         }
453         return res;
454 }