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