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