Fixed for navmesh on FreeBSD. Oatch by sambler, thanks!
[blender.git] / extern / recastnavigation / Recast / Source / RecastMeshDetail.cpp
1 //
2 // Copyright (c) 2009 Mikko Mononen memon@inside.org
3 //
4 // This software is provided 'as-is', without any express or implied
5 // warranty.  In no event will the authors be held liable for any damages
6 // arising from the use of this software.
7 // Permission is granted to anyone to use this software for any purpose,
8 // including commercial applications, and to alter it and redistribute it
9 // freely, subject to the following restrictions:
10 // 1. The origin of this software must not be misrepresented; you must not
11 //    claim that you wrote the original software. If you use this software
12 //    in a product, an acknowledgment in the product documentation would be
13 //    appreciated but is not required.
14 // 2. Altered source versions must be plainly marked as such, and must not be
15 //    misrepresented as being the original software.
16 // 3. This notice may not be removed or altered from any source distribution.
17 //
18
19 #include <float.h>
20 #define _USE_MATH_DEFINES
21 #include <math.h>
22 #include <string.h>
23 #include <stdlib.h>
24 #include <stdio.h>
25 #include "Recast.h"
26 #include "RecastLog.h"
27 #include "RecastTimer.h"
28
29
30 struct rcHeightPatch
31 {
32         inline rcHeightPatch() : data(0) {}
33         inline ~rcHeightPatch() { delete [] data; }
34         unsigned short* data;
35         int xmin, ymin, width, height;
36 };
37
38
39 static int circumCircle(const float xp, const float yp,
40                                                 const float x1, const float y1,
41                                                 const float x2, const float y2,
42                                                 const float x3, const float y3,
43                                                 float& xc, float& yc, float& rsqr)
44 {
45         static const float EPSILON = 1e-6f;
46         
47         const float fabsy1y2 = rcAbs(y1-y2);
48         const float fabsy2y3 = rcAbs(y2-y3);
49         
50         /* Check for coincident points */
51         if (fabsy1y2 < EPSILON && fabsy2y3 < EPSILON)
52                 return 0;
53         
54         if (fabsy1y2 < EPSILON)
55         {
56                 const float m2 = - (x3-x2) / (y3-y2);
57                 const float mx2 = (x2 + x3) / 2.0f;
58                 const float my2 = (y2 + y3) / 2.0f;
59                 xc = (x2 + x1) / 2.0f;
60                 yc = m2 * (xc - mx2) + my2;
61         }
62         else if (fabsy2y3 < EPSILON)
63         {
64                 const float m1 = - (x2-x1) / (y2-y1);
65                 const float mx1 = (x1 + x2) / 2.0f;
66                 const float my1 = (y1 + y2) / 2.0f;
67                 xc = (x3 + x2) / 2.0f;
68                 yc = m1 * (xc - mx1) + my1;
69         }
70         else
71         {
72                 const float m1 = - (x2-x1) / (y2-y1);
73                 const float m2 = - (x3-x2) / (y3-y2);
74                 const float mx1 = (x1 + x2) / 2.0f;
75                 const float mx2 = (x2 + x3) / 2.0f;
76                 const float my1 = (y1 + y2) / 2.0f;
77                 const float my2 = (y2 + y3) / 2.0f;
78                 xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
79                 if (fabsy1y2 > fabsy2y3)
80                         yc = m1 * (xc - mx1) + my1;
81                 else
82                         yc = m2 * (xc - mx2) + my2;
83         }
84         
85         float dx,dy;
86         
87         dx = x2 - xc;
88         dy = y2 - yc;
89         rsqr = dx*dx + dy*dy;
90         
91         dx = xp - xc;
92         dy = yp - yc;
93         const float drsqr = dx*dx + dy*dy;
94         
95         return (drsqr <= rsqr) ? 1 : 0;
96 }
97
98 #if defined(_MSC_VER)
99 static int ptcmp(void* up, const void *v1, const void *v2)
100 #elif defined(__APPLE__) || defined(__FreeBSD__)
101 static int ptcmp(void* up, const void *v1, const void *v2)
102 #else
103 static int ptcmp(const void *v1, const void *v2, void* up)
104 #endif
105 {
106         const float* verts = (const float*)up;
107         const float* p1 = &verts[(*(const int*)v1)*3];
108         const float* p2 = &verts[(*(const int*)v2)*3];
109         if (p1[0] < p2[0])
110                 return -1;
111         else if (p1[0] > p2[0])
112                 return 1;
113         else
114                 return 0;
115 }
116
117 // Based on Paul Bourke's triangulate.c
118 //  http://astronomy.swin.edu.au/~pbourke/terrain/triangulate/triangulate.c
119 static void delaunay(const int nv, float *verts, rcIntArray& idx, rcIntArray& tris, rcIntArray& edges)
120 {
121         // Sort vertices
122         idx.resize(nv);
123         for (int i = 0; i < nv; ++i)
124                 idx[i] = i;
125 #if defined(_MSC_VER)
126         qsort_s(&idx[0], idx.size(), sizeof(int), ptcmp, verts);
127 #elif defined(__APPLE__) || defined(__FreeBSD__)
128         qsort_r(&idx[0], idx.size(), sizeof(int), verts, ptcmp);
129 #else
130         qsort_r(&idx[0], idx.size(), sizeof(int), ptcmp, verts);
131 #endif
132
133         // Find the maximum and minimum vertex bounds.
134         // This is to allow calculation of the bounding triangle
135         float xmin = verts[0];
136         float ymin = verts[2];
137         float xmax = xmin;
138         float ymax = ymin;
139         for (int i = 1; i < nv; ++i)
140         {
141                 xmin = rcMin(xmin, verts[i*3+0]);
142                 xmax = rcMax(xmax, verts[i*3+0]);
143                 ymin = rcMin(ymin, verts[i*3+2]);
144                 ymax = rcMax(ymax, verts[i*3+2]);
145         }
146         float dx = xmax - xmin;
147         float dy = ymax - ymin;
148         float dmax = (dx > dy) ? dx : dy;
149         float xmid = (xmax + xmin) / 2.0f;
150         float ymid = (ymax + ymin) / 2.0f;
151         
152         // Set up the supertriangle
153         // This is a triangle which encompasses all the sample points.
154         // The supertriangle coordinates are added to the end of the
155         // vertex list. The supertriangle is the first triangle in
156         // the triangle list.
157         float sv[3*3];
158         
159         sv[0] = xmid - 20 * dmax;
160         sv[1] = 0;
161         sv[2] = ymid - dmax;
162         
163         sv[3] = xmid;
164         sv[4] = 0;
165         sv[5] = ymid + 20 * dmax;
166         
167         sv[6] = xmid + 20 * dmax;
168         sv[7] = 0;
169         sv[8] = ymid - dmax;
170         
171         tris.push(-3);
172         tris.push(-2);
173         tris.push(-1);
174         tris.push(0); // not completed
175         
176         for (int i = 0; i < nv; ++i)
177         {
178                 const float xp = verts[idx[i]*3+0];
179                 const float yp = verts[idx[i]*3+2];
180                 
181                 edges.resize(0);
182                 
183                 // Set up the edge buffer.
184                 // If the point (xp,yp) lies inside the circumcircle then the
185                 // three edges of that triangle are added to the edge buffer
186                 // and that triangle is removed.
187                 for (int j = 0; j < tris.size()/4; ++j)
188                 {
189                         int* t = &tris[j*4];
190                         if (t[3]) // completed?
191                                 continue;
192                         const float* v1 = t[0] < 0 ? &sv[(t[0]+3)*3] : &verts[idx[t[0]]*3];
193                         const float* v2 = t[1] < 0 ? &sv[(t[1]+3)*3] : &verts[idx[t[1]]*3];
194                         const float* v3 = t[2] < 0 ? &sv[(t[2]+3)*3] : &verts[idx[t[2]]*3];
195                         float xc,yc,rsqr;
196                         int inside = circumCircle(xp,yp, v1[0],v1[2], v2[0],v2[2], v3[0],v3[2], xc,yc,rsqr);
197                         if (xc < xp && rcSqr(xp-xc) > rsqr)
198                                 t[3] = 1;
199                         if (inside)
200                         {
201                                 // Collect triangle edges.
202                                 edges.push(t[0]);
203                                 edges.push(t[1]);
204                                 edges.push(t[1]);
205                                 edges.push(t[2]);
206                                 edges.push(t[2]);
207                                 edges.push(t[0]);
208                                 // Remove triangle j.
209                                 t[0] = tris[tris.size()-4];
210                                 t[1] = tris[tris.size()-3];
211                                 t[2] = tris[tris.size()-2];
212                                 t[3] = tris[tris.size()-1];
213                                 tris.resize(tris.size()-4);
214                                 j--;
215                         }
216                 }
217                 
218                 // Remove duplicate edges.
219                 const int ne = edges.size()/2;
220                 for (int j = 0; j < ne-1; ++j)
221                 {
222                         for (int k = j+1; k < ne; ++k)
223                         {
224                                 // Dupe?, make null.
225                                 if ((edges[j*2+0] == edges[k*2+1]) && (edges[j*2+1] == edges[k*2+0]))
226                                 {
227                                         edges[j*2+0] = 0;
228                                         edges[j*2+1] = 0;
229                                         edges[k*2+0] = 0;
230                                         edges[k*2+1] = 0;
231                                 }
232                         }
233                 }
234                 
235                 // Form new triangles for the current point
236                 // Skipping over any null.
237                 // All edges are arranged in clockwise order.
238                 for (int j = 0; j < ne; ++j)
239                 {
240                         if (edges[j*2+0] == edges[j*2+1]) continue;
241                         tris.push(edges[j*2+0]);
242                         tris.push(edges[j*2+1]);
243                         tris.push(i);
244                         tris.push(0); // not completed
245                 }
246         }
247         
248         // Remove triangles with supertriangle vertices
249         // These are triangles which have a vertex number greater than nv
250         for (int i = 0; i < tris.size()/4; ++i)
251         {
252                 int* t = &tris[i*4];
253                 if (t[0] < 0 || t[1] < 0 || t[2] < 0)
254                 {
255                         t[0] = tris[tris.size()-4];
256                         t[1] = tris[tris.size()-3];
257                         t[2] = tris[tris.size()-2];
258                         t[3] = tris[tris.size()-1];
259                         tris.resize(tris.size()-4);
260                         i--;
261                 }
262         }
263         // Triangle vertices are pointing to sorted vertices, remap indices.
264         for (int i = 0; i < tris.size(); ++i)
265                 tris[i] = idx[tris[i]];
266 }
267
268 inline float vdot2(const float* a, const float* b)
269 {
270         return a[0]*b[0] + a[2]*b[2];
271 }
272
273 static float distPtTri(const float* p, const float* a, const float* b, const float* c)
274 {
275         float v0[3], v1[3], v2[3];
276         vsub(v0, c,a);
277         vsub(v1, b,a);
278         vsub(v2, p,a);
279
280         const float dot00 = vdot2(v0, v0);
281         const float dot01 = vdot2(v0, v1);
282         const float dot02 = vdot2(v0, v2);
283         const float dot11 = vdot2(v1, v1);
284         const float dot12 = vdot2(v1, v2);
285         
286         // Compute barycentric coordinates
287         float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
288         float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
289         float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
290         
291         // If point lies inside the triangle, return interpolated y-coord.
292         static const float EPS = 1e-4f;
293         if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
294         {
295                 float y = a[1] + v0[1]*u + v1[1]*v;
296                 return fabsf(y-p[1]);
297         }
298         return FLT_MAX;
299 }
300
301 static float distancePtSeg(const float* pt, const float* p, const float* q)
302 {
303         float pqx = q[0] - p[0];
304         float pqy = q[1] - p[1];
305         float pqz = q[2] - p[2];
306         float dx = pt[0] - p[0];
307         float dy = pt[1] - p[1];
308         float dz = pt[2] - p[2];
309         float d = pqx*pqx + pqy*pqy + pqz*pqz;
310         float t = pqx*dx + pqy*dy + pqz*dz;
311         if (d > 0)
312                 t /= d;
313         if (t < 0)
314                 t = 0;
315         else if (t > 1)
316                 t = 1;
317         
318         dx = p[0] + t*pqx - pt[0];
319         dy = p[1] + t*pqy - pt[1];
320         dz = p[2] + t*pqz - pt[2];
321         
322         return dx*dx + dy*dy + dz*dz;
323 }
324
325 static float distancePtSeg2d(const float* pt, const float* p, const float* q)
326 {
327         float pqx = q[0] - p[0];
328         float pqz = q[2] - p[2];
329         float dx = pt[0] - p[0];
330         float dz = pt[2] - p[2];
331         float d = pqx*pqx + pqz*pqz;
332         float t = pqx*dx + pqz*dz;
333         if (d > 0)
334                 t /= d;
335         if (t < 0)
336                 t = 0;
337         else if (t > 1)
338                 t = 1;
339         
340         dx = p[0] + t*pqx - pt[0];
341         dz = p[2] + t*pqz - pt[2];
342         
343         return dx*dx + dz*dz;
344 }
345
346 static float distToTriMesh(const float* p, const float* verts, int nverts, const int* tris, int ntris)
347 {
348         float dmin = FLT_MAX;
349         for (int i = 0; i < ntris; ++i)
350         {
351                 const float* va = &verts[tris[i*4+0]*3];
352                 const float* vb = &verts[tris[i*4+1]*3];
353                 const float* vc = &verts[tris[i*4+2]*3];
354                 float d = distPtTri(p, va,vb,vc);
355                 if (d < dmin)
356                         dmin = d;
357         }
358         if (dmin == FLT_MAX) return -1;
359         return dmin;
360 }
361
362 static float distToPoly(int nvert, const float* verts, const float* p)
363 {
364
365         float dmin = FLT_MAX;
366         int i, j, c = 0;
367         for (i = 0, j = nvert-1; i < nvert; j = i++)
368         {
369                 const float* vi = &verts[i*3];
370                 const float* vj = &verts[j*3];
371                 if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
372                         (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
373                         c = !c;
374                 dmin = rcMin(dmin, distancePtSeg2d(p, vj, vi));
375         }
376         return c ? -dmin : dmin;
377 }
378
379
380 static unsigned short getHeight(const float* pos, const float* bmin, const float ics, const rcHeightPatch& hp)
381 {
382         int ix = (int)floorf((pos[0]-bmin[0])*ics + 0.01f);
383         int iz = (int)floorf((pos[2]-bmin[2])*ics + 0.01f);
384         ix = rcClamp(ix-hp.xmin, 0, hp.width);
385         iz = rcClamp(iz-hp.ymin, 0, hp.height);
386         unsigned short h = hp.data[ix+iz*hp.width];
387         return h;
388 }
389
390 static bool buildPolyDetail(const float* in, const int nin, unsigned short reg,
391                                                         const float sampleDist, const float sampleMaxError,
392                                                         const rcCompactHeightfield& chf, const rcHeightPatch& hp,
393                                                         float* verts, int& nverts, rcIntArray& tris,
394                                                         rcIntArray& edges, rcIntArray& idx, rcIntArray& samples)
395 {
396         static const int MAX_VERTS = 256;
397         static const int MAX_EDGE = 64;
398         float edge[(MAX_EDGE+1)*3];
399
400         nverts = 0;
401
402         for (int i = 0; i < nin; ++i)
403                 vcopy(&verts[i*3], &in[i*3]);
404         nverts = nin;
405         
406         const float ics = 1.0f/chf.cs;
407         
408         // Tesselate outlines.
409         // This is done in separate pass in order to ensure
410         // seamless height values across the ply boundaries.
411         if (sampleDist > 0)
412         {
413                 for (int i = 0, j = nin-1; i < nin; j=i++)
414                 {
415                         const float* vj = &in[j*3];
416                         const float* vi = &in[i*3];
417                         // Make sure the segments are always handled in same order
418                         // using lexological sort or else there will be seams.
419                         if (fabsf(vj[0]-vi[0]) < 1e-6f)
420                         {
421                                 if (vj[2] > vi[2])
422                                         rcSwap(vj,vi);
423                         }
424                         else
425                         {
426                                 if (vj[0] > vi[0])
427                                         rcSwap(vj,vi);
428                         }
429                         // Create samples along the edge.
430                         float dx = vi[0] - vj[0];
431                         float dy = vi[1] - vj[1];
432                         float dz = vi[2] - vj[2];
433                         float d = sqrtf(dx*dx + dz*dz);
434                         int nn = 1 + (int)floorf(d/sampleDist);
435                         if (nn > MAX_EDGE) nn = MAX_EDGE;
436                         if (nverts+nn >= MAX_VERTS)
437                                 nn = MAX_VERTS-1-nverts;
438                         for (int k = 0; k <= nn; ++k)
439                         {
440                                 float u = (float)k/(float)nn;
441                                 float* pos = &edge[k*3];
442                                 pos[0] = vj[0] + dx*u;
443                                 pos[1] = vj[1] + dy*u;
444                                 pos[2] = vj[2] + dz*u;
445                                 pos[1] = chf.bmin[1] + getHeight(pos, chf.bmin, ics, hp)*chf.ch;
446                         }
447                         // Simplify samples.
448                         int idx[MAX_EDGE] = {0,nn};
449                         int nidx = 2;
450                         for (int k = 0; k < nidx-1; )
451                         {
452                                 const int a = idx[k];
453                                 const int b = idx[k+1];
454                                 const float* va = &edge[a*3];
455                                 const float* vb = &edge[b*3];
456                                 // Find maximum deviation along the segment.
457                                 float maxd = 0;
458                                 int maxi = -1;
459                                 for (int m = a+1; m < b; ++m)
460                                 {
461                                         float d = distancePtSeg(&edge[m*3],va,vb);
462                                         if (d > maxd)
463                                         {
464                                                 maxd = d;
465                                                 maxi = m;
466                                         }
467                                 }
468                                 // If the max deviation is larger than accepted error,
469                                 // add new point, else continue to next segment.
470                                 if (maxi != -1 && maxd > rcSqr(sampleMaxError))
471                                 {
472                                         for (int m = nidx; m > k; --m)
473                                                 idx[m] = idx[m-1];
474                                         idx[k+1] = maxi;
475                                         nidx++;
476                                 }
477                                 else
478                                 {
479                                         ++k;
480                                 }
481                         }
482                         // Add new vertices.
483                         for (int k = 1; k < nidx-1; ++k)
484                         {
485                                 vcopy(&verts[nverts*3], &edge[idx[k]*3]);
486                                 nverts++;
487                         }
488                 }
489         }
490         
491         // Tesselate the base mesh.
492         edges.resize(0);
493         tris.resize(0);
494         idx.resize(0);
495         delaunay(nverts, verts, idx, tris, edges);
496
497         if (sampleDist > 0)
498         {
499                 // Create sample locations in a grid.
500                 float bmin[3], bmax[3];
501                 vcopy(bmin, in);
502                 vcopy(bmax, in);
503                 for (int i = 1; i < nin; ++i)
504                 {
505                         vmin(bmin, &in[i*3]);
506                         vmax(bmax, &in[i*3]);
507                 }
508                 int x0 = (int)floorf(bmin[0]/sampleDist);
509                 int x1 = (int)ceilf(bmax[0]/sampleDist);
510                 int z0 = (int)floorf(bmin[2]/sampleDist);
511                 int z1 = (int)ceilf(bmax[2]/sampleDist);
512                 samples.resize(0);
513                 for (int z = z0; z < z1; ++z)
514                 {
515                         for (int x = x0; x < x1; ++x)
516                         {
517                                 float pt[3];
518                                 pt[0] = x*sampleDist;
519                                 pt[2] = z*sampleDist;
520                                 // Make sure the samples are not too close to the edges.
521                                 if (distToPoly(nin,in,pt) > -sampleDist/2) continue;
522                                 samples.push(x);
523                                 samples.push(getHeight(pt, chf.bmin, ics, hp));
524                                 samples.push(z);
525                         }
526                 }
527                                 
528                 // Add the samples starting from the one that has the most
529                 // error. The procedure stops when all samples are added
530                 // or when the max error is within treshold.
531                 const int nsamples = samples.size()/3;
532                 for (int iter = 0; iter < nsamples; ++iter)
533                 {
534                         // Find sample with most error.
535                         float bestpt[3];
536                         float bestd = 0;
537                         for (int i = 0; i < nsamples; ++i)
538                         {
539                                 float pt[3];
540                                 pt[0] = samples[i*3+0]*sampleDist;
541                                 pt[1] = chf.bmin[1] + samples[i*3+1]*chf.ch;
542                                 pt[2] = samples[i*3+2]*sampleDist;
543                                 float d = distToTriMesh(pt, verts, nverts, &tris[0], tris.size()/4);
544                                 if (d < 0) continue; // did not hit the mesh.
545                                 if (d > bestd)
546                                 {
547                                         bestd = d;
548                                         vcopy(bestpt,pt);
549                                 }
550                         }
551                         // If the max error is within accepted threshold, stop tesselating.
552                         if (bestd <= sampleMaxError)
553                                 break;
554
555                         // Add the new sample point.
556                         vcopy(&verts[nverts*3],bestpt);
557                         nverts++;
558                         
559                         // Create new triangulation.
560                         // TODO: Incremental add instead of full rebuild.
561                         edges.resize(0);
562                         tris.resize(0);
563                         idx.resize(0);
564                         delaunay(nverts, verts, idx, tris, edges);
565
566                         if (nverts >= MAX_VERTS)
567                                 break;
568                 }
569         }
570
571         return true;
572 }
573
574 static void getHeightData(const rcCompactHeightfield& chf,
575                                                   const unsigned short* poly, const int npoly,
576                                                   const unsigned short* verts,
577                                                   rcHeightPatch& hp, rcIntArray& stack)
578 {
579         // Floodfill the heightfield to get 2D height data,
580         // starting at vertex locations as seeds.
581         
582         memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height);
583
584         stack.resize(0);
585         
586         // Use poly vertices as seed points for the flood fill.
587         for (int j = 0; j < npoly; ++j)
588         {
589                 const int ax = (int)verts[poly[j]*3+0];
590                 const int ay = (int)verts[poly[j]*3+1];
591                 const int az = (int)verts[poly[j]*3+2];
592                 if (ax < hp.xmin || ax >= hp.xmin+hp.width ||
593                         az < hp.ymin || az >= hp.ymin+hp.height)
594                         continue;
595                         
596                 const rcCompactCell& c = chf.cells[ax+az*chf.width];
597                 int dmin = 0xffff;
598                 int ai = -1;
599                 for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
600                 {
601                         const rcCompactSpan& s = chf.spans[i];
602                         int d = rcAbs(ay - (int)s.y);
603                         if (d < dmin)
604                         {
605                                 ai = i;
606                                 dmin = d;
607                         }
608                 }
609                 if (ai != -1)
610                 {
611                         stack.push(ax);
612                         stack.push(az);
613                         stack.push(ai);
614                 }
615         }
616
617         while (stack.size() > 0)
618         {
619                 int ci = stack.pop();
620                 int cy = stack.pop();
621                 int cx = stack.pop();
622
623                 // Skip already visited locations.
624                 int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width;
625                 if (hp.data[idx] != 0xffff)
626                         continue;
627                 
628                 const rcCompactSpan& cs = chf.spans[ci];
629                 hp.data[idx] = cs.y;
630                 
631                 for (int dir = 0; dir < 4; ++dir)
632                 {
633                         if (rcGetCon(cs, dir) == 0xf) continue;
634                         
635                         const int ax = cx + rcGetDirOffsetX(dir);
636                         const int ay = cy + rcGetDirOffsetY(dir);
637                 
638                         if (ax < hp.xmin || ax >= (hp.xmin+hp.width) ||
639                                 ay < hp.ymin || ay >= (hp.ymin+hp.height))
640                                 continue;
641
642                         if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0xffff)
643                                 continue;
644
645                         const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir);
646                         
647                         stack.push(ax);
648                         stack.push(ay);
649                         stack.push(ai);
650                 }
651         }       
652 }
653
654 static unsigned char getEdgeFlags(const float* va, const float* vb,
655                                                                   const float* vpoly, const int npoly)
656 {
657         // Return true if edge (va,vb) is part of the polygon.
658         static const float thrSqr = rcSqr(0.001f);
659         for (int i = 0, j = npoly-1; i < npoly; j=i++)
660         {
661                 if (distancePtSeg2d(va, &vpoly[j*3], &vpoly[i*3]) < thrSqr && 
662                         distancePtSeg2d(vb, &vpoly[j*3], &vpoly[i*3]) < thrSqr)
663                         return 1;
664         }
665         return 0;
666 }
667
668 static unsigned char getTriFlags(const float* va, const float* vb, const float* vc,
669                                                                  const float* vpoly, const int npoly)
670 {
671         unsigned char flags = 0;
672         flags |= getEdgeFlags(va,vb,vpoly,npoly) << 0;
673         flags |= getEdgeFlags(vb,vc,vpoly,npoly) << 2;
674         flags |= getEdgeFlags(vc,va,vpoly,npoly) << 4;
675         return flags;
676 }
677
678
679
680 bool rcBuildPolyMeshDetail(const rcPolyMesh& mesh, const rcCompactHeightfield& chf,
681                                                    const float sampleDist, const float sampleMaxError,
682                                                    rcPolyMeshDetail& dmesh)
683 {
684         if (mesh.nverts == 0 || mesh.npolys == 0)
685                 return true;
686
687         rcTimeVal startTime = rcGetPerformanceTimer();
688         rcTimeVal endTime;
689
690         int vcap;
691         int tcap;
692
693         const int nvp = mesh.nvp;
694         const float cs = mesh.cs;
695         const float ch = mesh.ch;
696         const float* orig = mesh.bmin;
697         
698         rcIntArray edges(64);
699         rcIntArray tris(512);
700         rcIntArray idx(512);
701         rcIntArray stack(512);
702         rcIntArray samples(512);
703         float verts[256*3];
704         float* poly = 0;
705         int* bounds = 0;
706         rcHeightPatch hp;
707         int nPolyVerts = 0;
708         int maxhw = 0, maxhh = 0;
709         
710         bounds = new int[mesh.npolys*4];
711         if (!bounds)
712         {
713                 if (rcGetLog())
714                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'bounds' (%d).", mesh.npolys*4);
715                 goto failure;
716         }
717         poly = new float[nvp*3];
718         if (!bounds)
719         {
720                 if (rcGetLog())
721                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'poly' (%d).", nvp*3);
722                 goto failure;
723         }
724         
725         // Find max size for a polygon area.
726         for (int i = 0; i < mesh.npolys; ++i)
727         {
728                 const unsigned short* p = &mesh.polys[i*nvp*2];
729                 int& xmin = bounds[i*4+0];
730                 int& xmax = bounds[i*4+1];
731                 int& ymin = bounds[i*4+2];
732                 int& ymax = bounds[i*4+3];
733                 xmin = chf.width;
734                 xmax = 0;
735                 ymin = chf.height;
736                 ymax = 0;
737                 for (int j = 0; j < nvp; ++j)
738                 {
739                         if(p[j] == 0xffff) break;
740                         const unsigned short* v = &mesh.verts[p[j]*3];
741                         xmin = rcMin(xmin, (int)v[0]);
742                         xmax = rcMax(xmax, (int)v[0]);
743                         ymin = rcMin(ymin, (int)v[2]);
744                         ymax = rcMax(ymax, (int)v[2]);
745                         nPolyVerts++;
746                 }
747                 xmin = rcMax(0,xmin-1);
748                 xmax = rcMin(chf.width,xmax+1);
749                 ymin = rcMax(0,ymin-1);
750                 ymax = rcMin(chf.height,ymax+1);
751                 if (xmin >= xmax || ymin >= ymax) continue;
752                 maxhw = rcMax(maxhw, xmax-xmin);
753                 maxhh = rcMax(maxhh, ymax-ymin);
754         }
755         
756         hp.data = new unsigned short[maxhw*maxhh];
757         if (!hp.data)
758         {
759                 if (rcGetLog())
760                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'hp.data' (%d).", maxhw*maxhh);
761                 goto failure;
762         }
763                 
764         dmesh.nmeshes = mesh.npolys;
765         dmesh.nverts = 0;
766         dmesh.ntris = 0;
767         dmesh.meshes = new unsigned short[dmesh.nmeshes*4];
768         if (!dmesh.meshes)
769         {
770                 if (rcGetLog())
771                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.meshes' (%d).", dmesh.nmeshes*4);
772                 goto failure;
773         }
774
775         vcap = nPolyVerts+nPolyVerts/2;
776         tcap = vcap*2;
777
778         dmesh.nverts = 0;
779         dmesh.verts = new float[vcap*3];
780         if (!dmesh.verts)
781         {
782                 if (rcGetLog())
783                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", vcap*3);
784                 goto failure;
785         }
786         dmesh.ntris = 0;
787         dmesh.tris = new unsigned char[tcap*4];
788         if (!dmesh.tris)
789         {
790                 if (rcGetLog())
791                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", tcap*4);
792                 goto failure;
793         }
794         
795         for (int i = 0; i < mesh.npolys; ++i)
796         {
797                 const unsigned short* p = &mesh.polys[i*nvp*2];
798                 
799                 // Find polygon bounding box.
800                 int npoly = 0;
801                 for (int j = 0; j < nvp; ++j)
802                 {
803                         if(p[j] == 0xffff) break;
804                         const unsigned short* v = &mesh.verts[p[j]*3];
805                         poly[j*3+0] = orig[0] + v[0]*cs;
806                         poly[j*3+1] = orig[1] + v[1]*ch;
807                         poly[j*3+2] = orig[2] + v[2]*cs;
808                         npoly++;
809                 }
810                 
811                 // Get the height data from the area of the polygon.
812                 hp.xmin = bounds[i*4+0];
813                 hp.ymin = bounds[i*4+2];
814                 hp.width = bounds[i*4+1]-bounds[i*4+0];
815                 hp.height = bounds[i*4+3]-bounds[i*4+2];
816                 getHeightData(chf, p, npoly, mesh.verts, hp, stack);
817                 
818                 // Build detail mesh.
819                 int nverts = 0;
820                 if (!buildPolyDetail(poly, npoly, mesh.regs[i],
821                                                          sampleDist, sampleMaxError,
822                                                          chf, hp, verts, nverts, tris,
823                                                          edges, idx, samples))
824                 {
825                         goto failure;
826                 }
827
828                 // Offset detail vertices, unnecassary?
829                 for (int j = 0; j < nverts; ++j)
830                         verts[j*3+1] += chf.ch;
831         
832                 // Store detail submesh.
833                 const int ntris = tris.size()/4;
834                 
835                 dmesh.meshes[i*4+0] = dmesh.nverts;
836                 dmesh.meshes[i*4+1] = (unsigned short)nverts;
837                 dmesh.meshes[i*4+2] = dmesh.ntris;
838                 dmesh.meshes[i*4+3] = (unsigned short)ntris;
839                 
840                 // Store vertices, allocate more memory if necessary.
841                 if (dmesh.nverts+nverts > vcap)
842                 {
843                         while (dmesh.nverts+nverts > vcap)
844                                 vcap += 256;
845                                 
846                         float* newv = new float[vcap*3];
847                         if (!newv)
848                         {
849                                 if (rcGetLog())
850                                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newv' (%d).", vcap*3);
851                                 goto failure;
852                         }
853                         if (dmesh.nverts)
854                                 memcpy(newv, dmesh.verts, sizeof(float)*3*dmesh.nverts);
855                         delete [] dmesh.verts;
856                         dmesh.verts = newv;
857                 }
858                 for (int j = 0; j < nverts; ++j)
859                 {
860                         dmesh.verts[dmesh.nverts*3+0] = verts[j*3+0];
861                         dmesh.verts[dmesh.nverts*3+1] = verts[j*3+1];
862                         dmesh.verts[dmesh.nverts*3+2] = verts[j*3+2];
863                         dmesh.nverts++;
864                 }
865                 
866                 // Store triangles, allocate more memory if necessary.
867                 if (dmesh.ntris+ntris > tcap)
868                 {
869                         while (dmesh.ntris+ntris > tcap)
870                                 tcap += 256;
871                         unsigned char* newt = new unsigned char[tcap*4];
872                         if (!newt)
873                         {
874                                 if (rcGetLog())
875                                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newt' (%d).", tcap*4);
876                                 goto failure;
877                         }
878                         if (dmesh.ntris)
879                                 memcpy(newt, dmesh.tris, sizeof(unsigned char)*4*dmesh.ntris);
880                         delete [] dmesh.tris;
881                         dmesh.tris = newt;
882                 }
883                 for (int j = 0; j < ntris; ++j)
884                 {
885                         const int* t = &tris[j*4];
886                         dmesh.tris[dmesh.ntris*4+0] = (unsigned char)t[0];
887                         dmesh.tris[dmesh.ntris*4+1] = (unsigned char)t[1];
888                         dmesh.tris[dmesh.ntris*4+2] = (unsigned char)t[2];
889                         dmesh.tris[dmesh.ntris*4+3] = getTriFlags(&verts[t[0]*3], &verts[t[1]*3], &verts[t[2]*3], poly, npoly);
890                         dmesh.ntris++;
891                 }
892         }
893         
894         delete [] bounds;
895         delete [] poly;
896         
897         endTime = rcGetPerformanceTimer();
898         
899         if (rcGetBuildTimes())
900                 rcGetBuildTimes()->buildDetailMesh += rcGetDeltaTimeUsec(startTime, endTime);
901
902         return true;
903
904 failure:
905
906         delete [] bounds;
907         delete [] poly;
908
909         return false;
910 }
911
912 bool rcMergePolyMeshDetails(rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh)
913 {
914         rcTimeVal startTime = rcGetPerformanceTimer();
915         
916         int maxVerts = 0;
917         int maxTris = 0;
918         int maxMeshes = 0;
919
920         for (int i = 0; i < nmeshes; ++i)
921         {
922                 if (!meshes[i]) continue;
923                 maxVerts += meshes[i]->nverts;
924                 maxTris += meshes[i]->ntris;
925                 maxMeshes += meshes[i]->nmeshes;
926         }
927
928         mesh.nmeshes = 0;
929         mesh.meshes = new unsigned short[maxMeshes*4];
930         if (!mesh.meshes)
931         {
932                 if (rcGetLog())
933                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'pmdtl.meshes' (%d).", maxMeshes*4);
934                 return false;
935         }
936
937         mesh.ntris = 0;
938         mesh.tris = new unsigned char[maxTris*4];
939         if (!mesh.tris)
940         {
941                 if (rcGetLog())
942                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", maxTris*4);
943                 return false;
944         }
945
946         mesh.nverts = 0;
947         mesh.verts = new float[maxVerts*3];
948         if (!mesh.verts)
949         {
950                 if (rcGetLog())
951                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", maxVerts*3);
952                 return false;
953         }
954         
955         // Merge datas.
956         for (int i = 0; i < nmeshes; ++i)
957         {
958                 rcPolyMeshDetail* dm = meshes[i];
959                 if (!dm) continue;
960                 for (int j = 0; j < dm->nmeshes; ++j)
961                 {
962                         unsigned short* dst = &mesh.meshes[mesh.nmeshes*4];
963                         unsigned short* src = &dm->meshes[j*4];
964                         dst[0] = mesh.nverts+src[0];
965                         dst[1] = src[1];
966                         dst[2] = mesh.ntris+src[2];
967                         dst[3] = src[3];
968                         mesh.nmeshes++;
969                 }
970                         
971                 for (int k = 0; k < dm->nverts; ++k)
972                 {
973                         vcopy(&mesh.verts[mesh.nverts*3], &dm->verts[k*3]);
974                         mesh.nverts++;
975                 }
976                 for (int k = 0; k < dm->ntris; ++k)
977                 {
978                         mesh.tris[mesh.ntris*4+0] = dm->tris[k*4+0];
979                         mesh.tris[mesh.ntris*4+1] = dm->tris[k*4+1];
980                         mesh.tris[mesh.ntris*4+2] = dm->tris[k*4+2];
981                         mesh.tris[mesh.ntris*4+3] = dm->tris[k*4+3];
982                         mesh.ntris++;
983                 }
984         }
985
986         rcTimeVal endTime = rcGetPerformanceTimer();
987         
988         if (rcGetBuildTimes())
989                 rcGetBuildTimes()->mergePolyMeshDetail += rcGetDeltaTimeUsec(startTime, endTime);
990         
991         return true;
992 }
993