aebd9624bc751813040b31388932b728c8804c9c
[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 #else
284 static int compareByData(const void * a, const void * b, void* data)
285 #endif
286 {
287         const SortContext* context = (const SortContext*)data;
288         return ( context->recastData[context->trisToFacesMap[*(int*)a]] - 
289                 context->recastData[context->trisToFacesMap[*(int*)b]] );
290 }
291
292 bool buildNavMeshData(const int nverts, const float* verts, 
293                                                          const int ntris, const unsigned short *tris, 
294                                                          const int* recastData, const int* trisToFacesMap,
295                                                          int &ndtris, unsigned short *&dtris,
296                                                          int &npolys, unsigned short *&dmeshes, unsigned short *&polys,
297                                                          int &vertsPerPoly, int *&dtrisToPolysMap, int *&dtrisToTrisMap)
298
299 {
300         if (!recastData)
301         {
302                 printf("Converting navmesh: Error! Can't find recast custom data\n");
303                 return false;
304         }
305
306         //sort the triangles by polygon idx
307         int* trisMapping = new int[ntris];
308         for (int i=0; i<ntris; i++)
309                 trisMapping[i]=i;
310         SortContext context;
311         context.recastData = recastData;
312         context.trisToFacesMap = trisToFacesMap;
313 #if defined(_MSC_VER)
314         qsort_s(trisMapping, ntris, sizeof(int), compareByData, &context);
315 #else
316         qsort_r(trisMapping, ntris, sizeof(int), compareByData, &context);
317 #endif
318         //search first valid triangle - triangle of convex polygon
319         int validTriStart = -1;
320         for (int i=0; i< ntris; i++)
321         {
322                 if (recastData[trisToFacesMap[trisMapping[i]]]>0)
323                 {
324                         validTriStart = i;
325                         break;
326                 }
327         }
328
329         if (validTriStart<0)
330         {
331                 printf("Converting navmesh: Error! No valid polygons in mesh\n");
332                 delete trisMapping;
333                 return false;
334         }
335
336         ndtris = ntris-validTriStart;
337         //fill dtris to faces mapping
338         dtrisToTrisMap = new int[ndtris];
339         memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
340         delete trisMapping; trisMapping=NULL;
341
342         //create detailed mesh triangles  - copy only valid triangles
343         //and reserve memory for adjacency info
344         dtris = new unsigned short[3*2*ndtris];
345         memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
346         for (int i=0; i<ndtris; i++)
347         {
348                 memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
349         }
350         //create new recast data corresponded to dtris and renumber for continuous indices
351         int prevPolyIdx=-1, curPolyIdx, newPolyIdx=0;
352         dtrisToPolysMap = new int[ndtris];
353         for (int i=0; i<ndtris; i++)
354         {
355                 curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
356                 if (curPolyIdx!=prevPolyIdx)
357                 {
358                         newPolyIdx++;
359                         prevPolyIdx=curPolyIdx;
360                 }
361                 dtrisToPolysMap[i] = newPolyIdx;
362         }
363
364
365         //build adjacency info for detailed mesh triangles
366         buildMeshAdjacency(dtris, ndtris, nverts, 3);
367
368         //create detailed mesh description for each navigation polygon
369         npolys = dtrisToPolysMap[ndtris-1];
370         dmeshes = new unsigned short[npolys*4];
371         memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
372         unsigned short *dmesh = NULL;
373         int prevpolyidx = 0;
374         for (int i=0; i<ndtris; i++)
375         {
376                 int curpolyidx = dtrisToPolysMap[i];
377                 if (curpolyidx!=prevpolyidx)
378                 {
379                         if (curpolyidx!=prevpolyidx+1)
380                         {
381                                 printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
382                                 return false;
383                         }
384                         dmesh = dmesh==NULL ? dmeshes : dmesh+4;
385                         dmesh[2] = (unsigned short)i;   //tbase
386                         dmesh[3] = 0;   //tnum
387                         prevpolyidx = curpolyidx;
388                 }
389                 dmesh[3]++;
390         }
391
392         //create navigation polygons
393         vertsPerPoly = 6;
394         polys = new unsigned short[npolys*vertsPerPoly*2];
395         memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
396
397         buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
398
399         return true;
400 }
401
402
403 bool buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int& vertsPerPoly, 
404                                                                                   int &nverts, float *&verts,
405                                                                                   int &ndtris, unsigned short *&dtris,
406                                                                                   int& npolys, unsigned short *&dmeshes,
407                                                                                   unsigned short*& polys, int *&dtrisToPolysMap,
408                                                                                   int *&dtrisToTrisMap, int *&trisToFacesMap)
409 {
410         bool res = true;
411         int ntris =0, *recastData=NULL;
412         unsigned short *tris=NULL;
413         res = buildRawVertIndicesData(dm, nverts, verts, ntris, tris, trisToFacesMap, recastData);
414         if (!res)
415         {
416                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
417                 goto exit;
418         }
419
420         res = buildNavMeshData(nverts, verts, ntris, tris, recastData, trisToFacesMap,
421                 ndtris, dtris, npolys, dmeshes,polys, vertsPerPoly, 
422                 dtrisToPolysMap, dtrisToTrisMap);
423         if (!res)
424         {
425                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
426                 goto exit;
427         }
428
429 exit:
430         if (tris)
431                 delete tris;
432
433         return res;
434 }
435
436 int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx)
437 {
438         int res = -1;
439         for(int i=0; i<vertsPerPoly; i++)
440         {
441                 if (p[i]==0xffff)
442                         break;
443                 if (p[i]==vertexIdx)
444                 {
445                         res = i;
446                         break;
447                 }
448         }
449         return res;
450 }