Recast: upgrade library.
[blender.git] / extern / recastnavigation / Recast / Source / RecastMeshDetail.cpp
1 //
2 // Copyright (c) 2009-2010 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 "RecastAlloc.h"
27 #include "RecastAssert.h"
28
29
30 static const unsigned RC_UNSET_HEIGHT = 0xffff;
31
32 struct rcHeightPatch
33 {
34         inline rcHeightPatch() : data(0), xmin(0), ymin(0), width(0), height(0) {}
35         inline ~rcHeightPatch() { rcFree(data); }
36         unsigned short* data;
37         int xmin, ymin, width, height;
38 };
39
40 inline float vdot2(const float* a, const float* b)
41 {
42         return a[0]*b[0] + a[2]*b[2];
43 }
44   
45 inline float vdistSq2(const float* p, const float* q)
46 {
47         const float dx = q[0] - p[0];
48         const float dy = q[2] - p[2];
49         return dx*dx + dy*dy;
50 }
51
52 inline float vdist2(const float* p, const float* q)
53 {
54         return sqrtf(vdistSq2(p,q));
55 }
56
57 inline float vcross2(const float* p1, const float* p2, const float* p3)
58
59         const float u1 = p2[0] - p1[0];
60         const float v1 = p2[2] - p1[2];
61         const float u2 = p3[0] - p1[0];
62         const float v2 = p3[2] - p1[2];
63         return u1 * v2 - v1 * u2;
64 }
65
66 static bool circumCircle(const float* p1, const float* p2, const float* p3,
67                                                  float* c, float& r)
68 {
69         static const float EPS = 1e-6f;
70         
71         const float cp = vcross2(p1, p2, p3);
72         if (fabsf(cp) > EPS)
73         {
74                 const float p1Sq = vdot2(p1,p1);
75                 const float p2Sq = vdot2(p2,p2);
76                 const float p3Sq = vdot2(p3,p3);
77                 c[0] = (p1Sq*(p2[2]-p3[2]) + p2Sq*(p3[2]-p1[2]) + p3Sq*(p1[2]-p2[2])) / (2*cp);
78                 c[2] = (p1Sq*(p3[0]-p2[0]) + p2Sq*(p1[0]-p3[0]) + p3Sq*(p2[0]-p1[0])) / (2*cp);
79                 r = vdist2(c, p1);
80                 return true;
81         }
82
83         c[0] = p1[0];
84         c[2] = p1[2];
85         r = 0;
86         return false;
87 }
88
89 static float distPtTri(const float* p, const float* a, const float* b, const float* c)
90 {
91         float v0[3], v1[3], v2[3];
92         rcVsub(v0, c,a);
93         rcVsub(v1, b,a);
94         rcVsub(v2, p,a);
95
96         const float dot00 = vdot2(v0, v0);
97         const float dot01 = vdot2(v0, v1);
98         const float dot02 = vdot2(v0, v2);
99         const float dot11 = vdot2(v1, v1);
100         const float dot12 = vdot2(v1, v2);
101         
102         // Compute barycentric coordinates
103         const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
104         const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
105         float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
106         
107         // If point lies inside the triangle, return interpolated y-coord.
108         static const float EPS = 1e-4f;
109         if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
110         {
111                 const float y = a[1] + v0[1]*u + v1[1]*v;
112                 return fabsf(y-p[1]);
113         }
114         return FLT_MAX;
115 }
116
117 static float distancePtSeg(const float* pt, const float* p, const float* q)
118 {
119         float pqx = q[0] - p[0];
120         float pqy = q[1] - p[1];
121         float pqz = q[2] - p[2];
122         float dx = pt[0] - p[0];
123         float dy = pt[1] - p[1];
124         float dz = pt[2] - p[2];
125         float d = pqx*pqx + pqy*pqy + pqz*pqz;
126         float t = pqx*dx + pqy*dy + pqz*dz;
127         if (d > 0)
128                 t /= d;
129         if (t < 0)
130                 t = 0;
131         else if (t > 1)
132                 t = 1;
133         
134         dx = p[0] + t*pqx - pt[0];
135         dy = p[1] + t*pqy - pt[1];
136         dz = p[2] + t*pqz - pt[2];
137         
138         return dx*dx + dy*dy + dz*dz;
139 }
140
141 static float distancePtSeg2d(const float* pt, const float* p, const float* q)
142 {
143         float pqx = q[0] - p[0];
144         float pqz = q[2] - p[2];
145         float dx = pt[0] - p[0];
146         float dz = pt[2] - p[2];
147         float d = pqx*pqx + pqz*pqz;
148         float t = pqx*dx + pqz*dz;
149         if (d > 0)
150                 t /= d;
151         if (t < 0)
152                 t = 0;
153         else if (t > 1)
154                 t = 1;
155         
156         dx = p[0] + t*pqx - pt[0];
157         dz = p[2] + t*pqz - pt[2];
158         
159         return dx*dx + dz*dz;
160 }
161
162 static float distToTriMesh(const float* p, const float* verts, const int /*nverts*/, const int* tris, const int ntris)
163 {
164         float dmin = FLT_MAX;
165         for (int i = 0; i < ntris; ++i)
166         {
167                 const float* va = &verts[tris[i*4+0]*3];
168                 const float* vb = &verts[tris[i*4+1]*3];
169                 const float* vc = &verts[tris[i*4+2]*3];
170                 float d = distPtTri(p, va,vb,vc);
171                 if (d < dmin)
172                         dmin = d;
173         }
174         if (dmin == FLT_MAX) return -1;
175         return dmin;
176 }
177
178 static float distToPoly(int nvert, const float* verts, const float* p)
179 {
180
181         float dmin = FLT_MAX;
182         int i, j, c = 0;
183         for (i = 0, j = nvert-1; i < nvert; j = i++)
184         {
185                 const float* vi = &verts[i*3];
186                 const float* vj = &verts[j*3];
187                 if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
188                         (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
189                         c = !c;
190                 dmin = rcMin(dmin, distancePtSeg2d(p, vj, vi));
191         }
192         return c ? -dmin : dmin;
193 }
194
195
196 static unsigned short getHeight(const float fx, const float fy, const float fz,
197                                                                 const float /*cs*/, const float ics, const float ch,
198                                                                 const rcHeightPatch& hp)
199 {
200         int ix = (int)floorf(fx*ics + 0.01f);
201         int iz = (int)floorf(fz*ics + 0.01f);
202         ix = rcClamp(ix-hp.xmin, 0, hp.width);
203         iz = rcClamp(iz-hp.ymin, 0, hp.height);
204         unsigned short h = hp.data[ix+iz*hp.width];
205         if (h == RC_UNSET_HEIGHT)
206         {
207                 // Special case when data might be bad.
208                 // Find nearest neighbour pixel which has valid height.
209                 const int off[8*2] = { -1,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1};
210                 float dmin = FLT_MAX;
211                 for (int i = 0; i < 8; ++i)
212                 {
213                         const int nx = ix+off[i*2+0];
214                         const int nz = iz+off[i*2+1];
215                         if (nx < 0 || nz < 0 || nx >= hp.width || nz >= hp.height) continue;
216                         const unsigned short nh = hp.data[nx+nz*hp.width];
217                         if (nh == RC_UNSET_HEIGHT) continue;
218
219                         const float d = fabsf(nh*ch - fy);
220                         if (d < dmin)
221                         {
222                                 h = nh;
223                                 dmin = d;
224                         }
225                         
226 /*                      const float dx = (nx+0.5f)*cs - fx; 
227                         const float dz = (nz+0.5f)*cs - fz;
228                         const float d = dx*dx+dz*dz;
229                         if (d < dmin)
230                         {
231                                 h = nh;
232                                 dmin = d;
233                         } */
234                 }
235         }
236         return h;
237 }
238
239
240 enum EdgeValues
241 {
242         UNDEF = -1,
243         HULL = -2,
244 };
245
246 static int findEdge(const int* edges, int nedges, int s, int t)
247 {
248         for (int i = 0; i < nedges; i++)
249         {
250                 const int* e = &edges[i*4];
251                 if ((e[0] == s && e[1] == t) || (e[0] == t && e[1] == s))
252                         return i;
253         }
254         return UNDEF;
255 }
256
257 static int addEdge(rcContext* ctx, int* edges, int& nedges, const int maxEdges, int s, int t, int l, int r)
258 {
259         if (nedges >= maxEdges)
260         {
261                 ctx->log(RC_LOG_ERROR, "addEdge: Too many edges (%d/%d).", nedges, maxEdges);
262                 return UNDEF;
263         }
264         
265         // Add edge if not already in the triangulation. 
266         int e = findEdge(edges, nedges, s, t);
267         if (e == UNDEF)
268         {
269                 int* e = &edges[nedges*4];
270                 e[0] = s;
271                 e[1] = t;
272                 e[2] = l;
273                 e[3] = r;
274                 return nedges++;
275         }
276         else
277         {
278                 return UNDEF;
279         }
280 }
281
282 static void updateLeftFace(int* e, int s, int t, int f)
283 {
284         if (e[0] == s && e[1] == t && e[2] == UNDEF)
285                 e[2] = f;
286         else if (e[1] == s && e[0] == t && e[3] == UNDEF)
287                 e[3] = f;
288 }       
289
290 static int overlapSegSeg2d(const float* a, const float* b, const float* c, const float* d)
291 {
292         const float a1 = vcross2(a, b, d);
293         const float a2 = vcross2(a, b, c);
294         if (a1*a2 < 0.0f)
295         {
296                 float a3 = vcross2(c, d, a);
297                 float a4 = a3 + a2 - a1;
298                 if (a3 * a4 < 0.0f)
299                         return 1;
300         }       
301         return 0;
302 }
303
304 static bool overlapEdges(const float* pts, const int* edges, int nedges, int s1, int t1)
305 {
306         for (int i = 0; i < nedges; ++i)
307         {
308                 const int s0 = edges[i*4+0];
309                 const int t0 = edges[i*4+1];
310                 // Same or connected edges do not overlap.
311                 if (s0 == s1 || s0 == t1 || t0 == s1 || t0 == t1)
312                         continue;
313                 if (overlapSegSeg2d(&pts[s0*3],&pts[t0*3], &pts[s1*3],&pts[t1*3]))
314                         return true;
315         }
316         return false;
317 }
318
319 static void completeFacet(rcContext* ctx, const float* pts, int npts, int* edges, int& nedges, const int maxEdges, int& nfaces, int e)
320 {
321         static const float EPS = 1e-5f;
322
323         int* edge = &edges[e*4];
324         
325         // Cache s and t.
326         int s,t;
327         if (edge[2] == UNDEF)
328         {
329                 s = edge[0];
330                 t = edge[1];
331         }
332         else if (edge[3] == UNDEF)
333         {
334                 s = edge[1];
335                 t = edge[0];
336         }
337         else
338         {
339             // Edge already completed. 
340             return;
341         }
342     
343         // Find best point on left of edge. 
344         int pt = npts;
345         float c[3] = {0,0,0};
346         float r = -1;
347         for (int u = 0; u < npts; ++u)
348         {
349                 if (u == s || u == t) continue;
350                 if (vcross2(&pts[s*3], &pts[t*3], &pts[u*3]) > EPS)
351                 {
352                         if (r < 0)
353                         {
354                                 // The circle is not updated yet, do it now.
355                                 pt = u;
356                                 circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
357                                 continue;
358                         }
359                         const float d = vdist2(c, &pts[u*3]);
360                         const float tol = 0.001f;
361                         if (d > r*(1+tol))
362                         {
363                                 // Outside current circumcircle, skip.
364                                 continue;
365                         }
366                         else if (d < r*(1-tol))
367                         {
368                                 // Inside safe circumcircle, update circle.
369                                 pt = u;
370                                 circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
371                         }
372                         else
373                         {
374                                 // Inside epsilon circum circle, do extra tests to make sure the edge is valid.
375                                 // s-u and t-u cannot overlap with s-pt nor t-pt if they exists.
376                                 if (overlapEdges(pts, edges, nedges, s,u))
377                                         continue;
378                                 if (overlapEdges(pts, edges, nedges, t,u))
379                                         continue;
380                                 // Edge is valid.
381                                 pt = u;
382                                 circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
383                         }
384                 }
385         }
386         
387         // Add new triangle or update edge info if s-t is on hull. 
388         if (pt < npts)
389         {
390                 // Update face information of edge being completed. 
391                 updateLeftFace(&edges[e*4], s, t, nfaces);
392                 
393                 // Add new edge or update face info of old edge. 
394                 e = findEdge(edges, nedges, pt, s);
395                 if (e == UNDEF)
396                     addEdge(ctx, edges, nedges, maxEdges, pt, s, nfaces, UNDEF);
397                 else
398                     updateLeftFace(&edges[e*4], pt, s, nfaces);
399                 
400                 // Add new edge or update face info of old edge. 
401                 e = findEdge(edges, nedges, t, pt);
402                 if (e == UNDEF)
403                     addEdge(ctx, edges, nedges, maxEdges, t, pt, nfaces, UNDEF);
404                 else
405                     updateLeftFace(&edges[e*4], t, pt, nfaces);
406                 
407                 nfaces++;
408         }
409         else
410         {
411                 updateLeftFace(&edges[e*4], s, t, HULL);
412         }
413 }
414
415 static void delaunayHull(rcContext* ctx, const int npts, const float* pts,
416                                                  const int nhull, const int* hull,
417                                                  rcIntArray& tris, rcIntArray& edges)
418 {
419         int nfaces = 0;
420         int nedges = 0;
421         const int maxEdges = npts*10;
422         edges.resize(maxEdges*4);
423         
424         for (int i = 0, j = nhull-1; i < nhull; j=i++)
425                 addEdge(ctx, &edges[0], nedges, maxEdges, hull[j],hull[i], HULL, UNDEF);
426         
427         int currentEdge = 0;
428         while (currentEdge < nedges)
429         {
430                 if (edges[currentEdge*4+2] == UNDEF)
431                         completeFacet(ctx, pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge);
432                 if (edges[currentEdge*4+3] == UNDEF)
433                         completeFacet(ctx, pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge);
434                 currentEdge++;
435         }
436
437         // Create tris
438         tris.resize(nfaces*4);
439         for (int i = 0; i < nfaces*4; ++i)
440                 tris[i] = -1;
441         
442         for (int i = 0; i < nedges; ++i)
443         {
444                 const int* e = &edges[i*4];
445                 if (e[3] >= 0)
446                 {
447                         // Left face
448                         int* t = &tris[e[3]*4];
449                         if (t[0] == -1)
450                         {
451                                 t[0] = e[0];
452                                 t[1] = e[1];
453                         }
454                         else if (t[0] == e[1])
455                                 t[2] = e[0];
456                         else if (t[1] == e[0])
457                                 t[2] = e[1];
458                 }
459                 if (e[2] >= 0)
460                 {
461                         // Right
462                         int* t = &tris[e[2]*4];
463                         if (t[0] == -1)
464                         {
465                                 t[0] = e[1];
466                                 t[1] = e[0];
467                         }
468                         else if (t[0] == e[0])
469                                 t[2] = e[1];
470                         else if (t[1] == e[1])
471                                 t[2] = e[0];
472                 }
473         }
474         
475         for (int i = 0; i < tris.size()/4; ++i)
476         {
477                 int* t = &tris[i*4];
478                 if (t[0] == -1 || t[1] == -1 || t[2] == -1)
479                 {
480                         ctx->log(RC_LOG_WARNING, "delaunayHull: Removing dangling face %d [%d,%d,%d].", i, t[0],t[1],t[2]);
481                         t[0] = tris[tris.size()-4];
482                         t[1] = tris[tris.size()-3];
483                         t[2] = tris[tris.size()-2];
484                         t[3] = tris[tris.size()-1];
485                         tris.resize(tris.size()-4);
486                         --i;
487                 }
488         }
489 }
490
491
492 inline float getJitterX(const int i)
493 {
494         return (((i * 0x8da6b343) & 0xffff) / 65535.0f * 2.0f) - 1.0f;
495 }
496
497 inline float getJitterY(const int i)
498 {
499         return (((i * 0xd8163841) & 0xffff) / 65535.0f * 2.0f) - 1.0f;
500 }
501
502 static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin,
503                                                         const float sampleDist, const float sampleMaxError,
504                                                         const rcCompactHeightfield& chf, const rcHeightPatch& hp,
505                                                         float* verts, int& nverts, rcIntArray& tris,
506                                                         rcIntArray& edges, rcIntArray& samples)
507 {
508         static const int MAX_VERTS = 127;
509         static const int MAX_TRIS = 255;        // Max tris for delaunay is 2n-2-k (n=num verts, k=num hull verts).
510         static const int MAX_VERTS_PER_EDGE = 32;
511         float edge[(MAX_VERTS_PER_EDGE+1)*3];
512         int hull[MAX_VERTS];
513         int nhull = 0;
514
515         nverts = 0;
516
517         for (int i = 0; i < nin; ++i)
518                 rcVcopy(&verts[i*3], &in[i*3]);
519         nverts = nin;
520         
521         const float cs = chf.cs;
522         const float ics = 1.0f/cs;
523         
524         // Tessellate outlines.
525         // This is done in separate pass in order to ensure
526         // seamless height values across the ply boundaries.
527         if (sampleDist > 0)
528         {
529                 for (int i = 0, j = nin-1; i < nin; j=i++)
530                 {
531                         const float* vj = &in[j*3];
532                         const float* vi = &in[i*3];
533                         bool swapped = false;
534                         // Make sure the segments are always handled in same order
535                         // using lexological sort or else there will be seams.
536                         if (fabsf(vj[0]-vi[0]) < 1e-6f)
537                         {
538                                 if (vj[2] > vi[2])
539                                 {
540                                         rcSwap(vj,vi);
541                                         swapped = true;
542                                 }
543                         }
544                         else
545                         {
546                                 if (vj[0] > vi[0])
547                                 {
548                                         rcSwap(vj,vi);
549                                         swapped = true;
550                                 }
551                         }
552                         // Create samples along the edge.
553                         float dx = vi[0] - vj[0];
554                         float dy = vi[1] - vj[1];
555                         float dz = vi[2] - vj[2];
556                         float d = sqrtf(dx*dx + dz*dz);
557                         int nn = 1 + (int)floorf(d/sampleDist);
558                         if (nn >= MAX_VERTS_PER_EDGE) nn = MAX_VERTS_PER_EDGE-1;
559                         if (nverts+nn >= MAX_VERTS)
560                                 nn = MAX_VERTS-1-nverts;
561                         
562                         for (int k = 0; k <= nn; ++k)
563                         {
564                                 float u = (float)k/(float)nn;
565                                 float* pos = &edge[k*3];
566                                 pos[0] = vj[0] + dx*u;
567                                 pos[1] = vj[1] + dy*u;
568                                 pos[2] = vj[2] + dz*u;
569                                 pos[1] = getHeight(pos[0],pos[1],pos[2], cs, ics, chf.ch, hp)*chf.ch;
570                         }
571                         // Simplify samples.
572                         int idx[MAX_VERTS_PER_EDGE] = {0,nn};
573                         int nidx = 2;
574                         for (int k = 0; k < nidx-1; )
575                         {
576                                 const int a = idx[k];
577                                 const int b = idx[k+1];
578                                 const float* va = &edge[a*3];
579                                 const float* vb = &edge[b*3];
580                                 // Find maximum deviation along the segment.
581                                 float maxd = 0;
582                                 int maxi = -1;
583                                 for (int m = a+1; m < b; ++m)
584                                 {
585                                         float d = distancePtSeg(&edge[m*3],va,vb);
586                                         if (d > maxd)
587                                         {
588                                                 maxd = d;
589                                                 maxi = m;
590                                         }
591                                 }
592                                 // If the max deviation is larger than accepted error,
593                                 // add new point, else continue to next segment.
594                                 if (maxi != -1 && maxd > rcSqr(sampleMaxError))
595                                 {
596                                         for (int m = nidx; m > k; --m)
597                                                 idx[m] = idx[m-1];
598                                         idx[k+1] = maxi;
599                                         nidx++;
600                                 }
601                                 else
602                                 {
603                                         ++k;
604                                 }
605                         }
606                         
607                         hull[nhull++] = j;
608                         // Add new vertices.
609                         if (swapped)
610                         {
611                                 for (int k = nidx-2; k > 0; --k)
612                                 {
613                                         rcVcopy(&verts[nverts*3], &edge[idx[k]*3]);
614                                         hull[nhull++] = nverts;
615                                         nverts++;
616                                 }
617                         }
618                         else
619                         {
620                                 for (int k = 1; k < nidx-1; ++k)
621                                 {
622                                         rcVcopy(&verts[nverts*3], &edge[idx[k]*3]);
623                                         hull[nhull++] = nverts;
624                                         nverts++;
625                                 }
626                         }
627                 }
628         }
629         
630
631         // Tessellate the base mesh.
632         edges.resize(0);
633         tris.resize(0);
634
635         delaunayHull(ctx, nverts, verts, nhull, hull, tris, edges);
636         
637         if (tris.size() == 0)
638         {
639                 // Could not triangulate the poly, make sure there is some valid data there.
640                 ctx->log(RC_LOG_WARNING, "buildPolyDetail: Could not triangulate polygon, adding default data.");
641                 for (int i = 2; i < nverts; ++i)
642                 {
643                         tris.push(0);
644                         tris.push(i-1);
645                         tris.push(i);
646                         tris.push(0);
647                 }
648                 return true;
649         }
650
651         if (sampleDist > 0)
652         {
653                 // Create sample locations in a grid.
654                 float bmin[3], bmax[3];
655                 rcVcopy(bmin, in);
656                 rcVcopy(bmax, in);
657                 for (int i = 1; i < nin; ++i)
658                 {
659                         rcVmin(bmin, &in[i*3]);
660                         rcVmax(bmax, &in[i*3]);
661                 }
662                 int x0 = (int)floorf(bmin[0]/sampleDist);
663                 int x1 = (int)ceilf(bmax[0]/sampleDist);
664                 int z0 = (int)floorf(bmin[2]/sampleDist);
665                 int z1 = (int)ceilf(bmax[2]/sampleDist);
666                 samples.resize(0);
667                 for (int z = z0; z < z1; ++z)
668                 {
669                         for (int x = x0; x < x1; ++x)
670                         {
671                                 float pt[3];
672                                 pt[0] = x*sampleDist;
673                                 pt[1] = (bmax[1]+bmin[1])*0.5f;
674                                 pt[2] = z*sampleDist;
675                                 // Make sure the samples are not too close to the edges.
676                                 if (distToPoly(nin,in,pt) > -sampleDist/2) continue;
677                                 samples.push(x);
678                                 samples.push(getHeight(pt[0], pt[1], pt[2], cs, ics, chf.ch, hp));
679                                 samples.push(z);
680                                 samples.push(0); // Not added
681                         }
682                 }
683                                 
684                 // Add the samples starting from the one that has the most
685                 // error. The procedure stops when all samples are added
686                 // or when the max error is within treshold.
687                 const int nsamples = samples.size()/4;
688                 for (int iter = 0; iter < nsamples; ++iter)
689                 {
690                         if (nverts >= MAX_VERTS)
691                                 break;
692
693                         // Find sample with most error.
694                         float bestpt[3] = {0,0,0};
695                         float bestd = 0;
696                         int besti = -1;
697                         for (int i = 0; i < nsamples; ++i)
698                         {
699                                 const int* s = &samples[i*4];
700                                 if (s[3]) continue; // skip added.
701                                 float pt[3];
702                                 // The sample location is jittered to get rid of some bad triangulations
703                                 // which are cause by symmetrical data from the grid structure.
704                                 pt[0] = s[0]*sampleDist + getJitterX(i)*cs*0.1f;
705                                 pt[1] = s[1]*chf.ch;
706                                 pt[2] = s[2]*sampleDist + getJitterY(i)*cs*0.1f;
707                                 float d = distToTriMesh(pt, verts, nverts, &tris[0], tris.size()/4);
708                                 if (d < 0) continue; // did not hit the mesh.
709                                 if (d > bestd)
710                                 {
711                                         bestd = d;
712                                         besti = i;
713                                         rcVcopy(bestpt,pt);
714                                 }
715                         }
716                         // If the max error is within accepted threshold, stop tesselating.
717                         if (bestd <= sampleMaxError || besti == -1)
718                                 break;
719                         // Mark sample as added.
720                         samples[besti*4+3] = 1;
721                         // Add the new sample point.
722                         rcVcopy(&verts[nverts*3],bestpt);
723                         nverts++;
724                         
725                         // Create new triangulation.
726                         // TODO: Incremental add instead of full rebuild.
727                         edges.resize(0);
728                         tris.resize(0);
729                         delaunayHull(ctx, nverts, verts, nhull, hull, tris, edges);
730                 }               
731         }
732
733         const int ntris = tris.size()/4;
734         if (ntris > MAX_TRIS)
735         {
736                 tris.resize(MAX_TRIS*4);
737                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Shrinking triangle count from %d to max %d.", ntris, MAX_TRIS);
738         }
739
740         return true;
741 }
742
743 static void getHeightData(const rcCompactHeightfield& chf,
744                                                   const unsigned short* poly, const int npoly,
745                                                   const unsigned short* verts, const int bs,
746                                                   rcHeightPatch& hp, rcIntArray& stack)
747 {
748         // Floodfill the heightfield to get 2D height data,
749         // starting at vertex locations as seeds.
750         
751         // Note: Reads to the compact heightfield are offset by border size (bs)
752         // since border size offset is already removed from the polymesh vertices.
753         
754         memset(hp.data, 0, sizeof(unsigned short)*hp.width*hp.height);
755         
756         stack.resize(0);
757         
758         static const int offset[9*2] =
759         {
760                 0,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1, -1,0,
761         };
762         
763         // Use poly vertices as seed points for the flood fill.
764         for (int j = 0; j < npoly; ++j)
765         {
766                 int cx = 0, cz = 0, ci =-1;
767                 int dmin = RC_UNSET_HEIGHT;
768                 for (int k = 0; k < 9; ++k)
769                 {
770                         const int ax = (int)verts[poly[j]*3+0] + offset[k*2+0];
771                         const int ay = (int)verts[poly[j]*3+1];
772                         const int az = (int)verts[poly[j]*3+2] + offset[k*2+1];
773                         if (ax < hp.xmin || ax >= hp.xmin+hp.width ||
774                                 az < hp.ymin || az >= hp.ymin+hp.height)
775                                 continue;
776                         
777                         const rcCompactCell& c = chf.cells[(ax+bs)+(az+bs)*chf.width];
778                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
779                         {
780                                 const rcCompactSpan& s = chf.spans[i];
781                                 int d = rcAbs(ay - (int)s.y);
782                                 if (d < dmin)
783                                 {
784                                         cx = ax;
785                                         cz = az;
786                                         ci = i;
787                                         dmin = d;
788                                 }
789                         }
790                 }
791                 if (ci != -1)
792                 {
793                         stack.push(cx);
794                         stack.push(cz);
795                         stack.push(ci);
796                 }
797         }
798         
799         // Find center of the polygon using flood fill.
800         int pcx = 0, pcz = 0;
801         for (int j = 0; j < npoly; ++j)
802         {
803                 pcx += (int)verts[poly[j]*3+0];
804                 pcz += (int)verts[poly[j]*3+2];
805         }
806         pcx /= npoly;
807         pcz /= npoly;
808         
809         for (int i = 0; i < stack.size(); i += 3)
810         {
811                 int cx = stack[i+0];
812                 int cy = stack[i+1];
813                 int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width;
814                 hp.data[idx] = 1;
815         }
816         
817         while (stack.size() > 0)
818         {
819                 int ci = stack.pop();
820                 int cy = stack.pop();
821                 int cx = stack.pop();
822                 
823                 // Check if close to center of the polygon.
824                 if (rcAbs(cx-pcx) <= 1 && rcAbs(cy-pcz) <= 1)
825                 {
826                         stack.resize(0);
827                         stack.push(cx);
828                         stack.push(cy);
829                         stack.push(ci);
830                         break;
831                 }
832                 
833                 const rcCompactSpan& cs = chf.spans[ci];
834                 
835                 for (int dir = 0; dir < 4; ++dir)
836                 {
837                         if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue;
838                         
839                         const int ax = cx + rcGetDirOffsetX(dir);
840                         const int ay = cy + rcGetDirOffsetY(dir);
841                         
842                         if (ax < hp.xmin || ax >= (hp.xmin+hp.width) ||
843                                 ay < hp.ymin || ay >= (hp.ymin+hp.height))
844                                 continue;
845                         
846                         if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0)
847                                 continue;
848                         
849                         const int ai = (int)chf.cells[(ax+bs)+(ay+bs)*chf.width].index + rcGetCon(cs, dir);
850
851                         int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width;
852                         hp.data[idx] = 1;
853                         
854                         stack.push(ax);
855                         stack.push(ay);
856                         stack.push(ai);
857                 }
858         }
859
860         memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height);
861
862         // Mark start locations.
863         for (int i = 0; i < stack.size(); i += 3)
864         {
865                 int cx = stack[i+0];
866                 int cy = stack[i+1];
867                 int ci = stack[i+2];
868                 int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width;
869                 const rcCompactSpan& cs = chf.spans[ci];
870                 hp.data[idx] = cs.y;
871         }
872         
873         static const int RETRACT_SIZE = 256;
874         int head = 0;
875         
876         while (head*3 < stack.size())
877         {
878                 int cx = stack[head*3+0];
879                 int cy = stack[head*3+1];
880                 int ci = stack[head*3+2];
881                 head++;
882                 if (head >= RETRACT_SIZE)
883                 {
884                         head = 0;
885                         if (stack.size() > RETRACT_SIZE*3)
886                                 memmove(&stack[0], &stack[RETRACT_SIZE*3], sizeof(int)*(stack.size()-RETRACT_SIZE*3));
887                         stack.resize(stack.size()-RETRACT_SIZE*3);
888                 }
889
890                 const rcCompactSpan& cs = chf.spans[ci];
891                 for (int dir = 0; dir < 4; ++dir)
892                 {
893                         if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue;
894                         
895                         const int ax = cx + rcGetDirOffsetX(dir);
896                         const int ay = cy + rcGetDirOffsetY(dir);
897                         
898                         if (ax < hp.xmin || ax >= (hp.xmin+hp.width) ||
899                                 ay < hp.ymin || ay >= (hp.ymin+hp.height))
900                                 continue;
901                         
902                         if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != RC_UNSET_HEIGHT)
903                                 continue;
904                         
905                         const int ai = (int)chf.cells[(ax+bs)+(ay+bs)*chf.width].index + rcGetCon(cs, dir);
906                         
907                         const rcCompactSpan& as = chf.spans[ai];
908                         int idx = ax-hp.xmin+(ay-hp.ymin)*hp.width;
909                         hp.data[idx] = as.y;
910
911                         stack.push(ax);
912                         stack.push(ay);
913                         stack.push(ai);
914                 }
915         }
916         
917 }
918
919 static unsigned char getEdgeFlags(const float* va, const float* vb,
920                                                                   const float* vpoly, const int npoly)
921 {
922         // Return true if edge (va,vb) is part of the polygon.
923         static const float thrSqr = rcSqr(0.001f);
924         for (int i = 0, j = npoly-1; i < npoly; j=i++)
925         {
926                 if (distancePtSeg2d(va, &vpoly[j*3], &vpoly[i*3]) < thrSqr && 
927                         distancePtSeg2d(vb, &vpoly[j*3], &vpoly[i*3]) < thrSqr)
928                         return 1;
929         }
930         return 0;
931 }
932
933 static unsigned char getTriFlags(const float* va, const float* vb, const float* vc,
934                                                                  const float* vpoly, const int npoly)
935 {
936         unsigned char flags = 0;
937         flags |= getEdgeFlags(va,vb,vpoly,npoly) << 0;
938         flags |= getEdgeFlags(vb,vc,vpoly,npoly) << 2;
939         flags |= getEdgeFlags(vc,va,vpoly,npoly) << 4;
940         return flags;
941 }
942
943 /// @par
944 ///
945 /// See the #rcConfig documentation for more information on the configuration parameters.
946 ///
947 /// @see rcAllocPolyMeshDetail, rcPolyMesh, rcCompactHeightfield, rcPolyMeshDetail, rcConfig
948 bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompactHeightfield& chf,
949                                                    const float sampleDist, const float sampleMaxError,
950                                                    rcPolyMeshDetail& dmesh)
951 {
952         rcAssert(ctx);
953         
954         ctx->startTimer(RC_TIMER_BUILD_POLYMESHDETAIL);
955
956         if (mesh.nverts == 0 || mesh.npolys == 0)
957                 return true;
958         
959         const int nvp = mesh.nvp;
960         const float cs = mesh.cs;
961         const float ch = mesh.ch;
962         const float* orig = mesh.bmin;
963         const int borderSize = mesh.borderSize;
964         
965         rcIntArray edges(64);
966         rcIntArray tris(512);
967         rcIntArray stack(512);
968         rcIntArray samples(512);
969         float verts[256*3];
970         rcHeightPatch hp;
971         int nPolyVerts = 0;
972         int maxhw = 0, maxhh = 0;
973         
974         rcScopedDelete<int> bounds = (int*)rcAlloc(sizeof(int)*mesh.npolys*4, RC_ALLOC_TEMP);
975         if (!bounds)
976         {
977                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'bounds' (%d).", mesh.npolys*4);
978                 return false;
979         }
980         rcScopedDelete<float> poly = (float*)rcAlloc(sizeof(float)*nvp*3, RC_ALLOC_TEMP);
981         if (!poly)
982         {
983                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'poly' (%d).", nvp*3);
984                 return false;
985         }
986         
987         // Find max size for a polygon area.
988         for (int i = 0; i < mesh.npolys; ++i)
989         {
990                 const unsigned short* p = &mesh.polys[i*nvp*2];
991                 int& xmin = bounds[i*4+0];
992                 int& xmax = bounds[i*4+1];
993                 int& ymin = bounds[i*4+2];
994                 int& ymax = bounds[i*4+3];
995                 xmin = chf.width;
996                 xmax = 0;
997                 ymin = chf.height;
998                 ymax = 0;
999                 for (int j = 0; j < nvp; ++j)
1000                 {
1001                         if(p[j] == RC_MESH_NULL_IDX) break;
1002                         const unsigned short* v = &mesh.verts[p[j]*3];
1003                         xmin = rcMin(xmin, (int)v[0]);
1004                         xmax = rcMax(xmax, (int)v[0]);
1005                         ymin = rcMin(ymin, (int)v[2]);
1006                         ymax = rcMax(ymax, (int)v[2]);
1007                         nPolyVerts++;
1008                 }
1009                 xmin = rcMax(0,xmin-1);
1010                 xmax = rcMin(chf.width,xmax+1);
1011                 ymin = rcMax(0,ymin-1);
1012                 ymax = rcMin(chf.height,ymax+1);
1013                 if (xmin >= xmax || ymin >= ymax) continue;
1014                 maxhw = rcMax(maxhw, xmax-xmin);
1015                 maxhh = rcMax(maxhh, ymax-ymin);
1016         }
1017         
1018         hp.data = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxhw*maxhh, RC_ALLOC_TEMP);
1019         if (!hp.data)
1020         {
1021                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'hp.data' (%d).", maxhw*maxhh);
1022                 return false;
1023         }
1024         
1025         dmesh.nmeshes = mesh.npolys;
1026         dmesh.nverts = 0;
1027         dmesh.ntris = 0;
1028         dmesh.meshes = (unsigned int*)rcAlloc(sizeof(unsigned int)*dmesh.nmeshes*4, RC_ALLOC_PERM);
1029         if (!dmesh.meshes)
1030         {
1031                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.meshes' (%d).", dmesh.nmeshes*4);
1032                 return false;
1033         }
1034
1035         int vcap = nPolyVerts+nPolyVerts/2;
1036         int tcap = vcap*2;
1037
1038         dmesh.nverts = 0;
1039         dmesh.verts = (float*)rcAlloc(sizeof(float)*vcap*3, RC_ALLOC_PERM);
1040         if (!dmesh.verts)
1041         {
1042                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", vcap*3);
1043                 return false;
1044         }
1045         dmesh.ntris = 0;
1046         dmesh.tris = (unsigned char*)rcAlloc(sizeof(unsigned char*)*tcap*4, RC_ALLOC_PERM);
1047         if (!dmesh.tris)
1048         {
1049                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", tcap*4);
1050                 return false;
1051         }
1052         
1053         for (int i = 0; i < mesh.npolys; ++i)
1054         {
1055                 const unsigned short* p = &mesh.polys[i*nvp*2];
1056                 
1057                 // Store polygon vertices for processing.
1058                 int npoly = 0;
1059                 for (int j = 0; j < nvp; ++j)
1060                 {
1061                         if(p[j] == RC_MESH_NULL_IDX) break;
1062                         const unsigned short* v = &mesh.verts[p[j]*3];
1063                         poly[j*3+0] = v[0]*cs;
1064                         poly[j*3+1] = v[1]*ch;
1065                         poly[j*3+2] = v[2]*cs;
1066                         npoly++;
1067                 }
1068                 
1069                 // Get the height data from the area of the polygon.
1070                 hp.xmin = bounds[i*4+0];
1071                 hp.ymin = bounds[i*4+2];
1072                 hp.width = bounds[i*4+1]-bounds[i*4+0];
1073                 hp.height = bounds[i*4+3]-bounds[i*4+2];
1074                 getHeightData(chf, p, npoly, mesh.verts, borderSize, hp, stack);
1075                 
1076                 // Build detail mesh.
1077                 int nverts = 0;
1078                 if (!buildPolyDetail(ctx, poly, npoly,
1079                                                          sampleDist, sampleMaxError,
1080                                                          chf, hp, verts, nverts, tris,
1081                                                          edges, samples))
1082                 {
1083                         return false;
1084                 }
1085
1086                 // Move detail verts to world space.
1087                 for (int j = 0; j < nverts; ++j)
1088                 {
1089                         verts[j*3+0] += orig[0];
1090                         verts[j*3+1] += orig[1] + chf.ch; // Is this offset necessary?
1091                         verts[j*3+2] += orig[2];
1092                 }
1093                 // Offset poly too, will be used to flag checking.
1094                 for (int j = 0; j < npoly; ++j)
1095                 {
1096                         poly[j*3+0] += orig[0];
1097                         poly[j*3+1] += orig[1];
1098                         poly[j*3+2] += orig[2];
1099                 }
1100         
1101                 // Store detail submesh.
1102                 const int ntris = tris.size()/4;
1103
1104                 dmesh.meshes[i*4+0] = (unsigned int)dmesh.nverts;
1105                 dmesh.meshes[i*4+1] = (unsigned int)nverts;
1106                 dmesh.meshes[i*4+2] = (unsigned int)dmesh.ntris;
1107                 dmesh.meshes[i*4+3] = (unsigned int)ntris;              
1108                 
1109                 // Store vertices, allocate more memory if necessary.
1110                 if (dmesh.nverts+nverts > vcap)
1111                 {
1112                         while (dmesh.nverts+nverts > vcap)
1113                                 vcap += 256;
1114                                 
1115                         float* newv = (float*)rcAlloc(sizeof(float)*vcap*3, RC_ALLOC_PERM);
1116                         if (!newv)
1117                         {
1118                                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newv' (%d).", vcap*3);
1119                                 return false;
1120                         }
1121                         if (dmesh.nverts)
1122                                 memcpy(newv, dmesh.verts, sizeof(float)*3*dmesh.nverts);
1123                         rcFree(dmesh.verts);
1124                         dmesh.verts = newv;
1125                 }
1126                 for (int j = 0; j < nverts; ++j)
1127                 {
1128                         dmesh.verts[dmesh.nverts*3+0] = verts[j*3+0];
1129                         dmesh.verts[dmesh.nverts*3+1] = verts[j*3+1];
1130                         dmesh.verts[dmesh.nverts*3+2] = verts[j*3+2];
1131                         dmesh.nverts++;
1132                 }
1133                 
1134                 // Store triangles, allocate more memory if necessary.
1135                 if (dmesh.ntris+ntris > tcap)
1136                 {
1137                         while (dmesh.ntris+ntris > tcap)
1138                                 tcap += 256;
1139                         unsigned char* newt = (unsigned char*)rcAlloc(sizeof(unsigned char)*tcap*4, RC_ALLOC_PERM);
1140                         if (!newt)
1141                         {
1142                                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newt' (%d).", tcap*4);
1143                                 return false;
1144                         }
1145                         if (dmesh.ntris)
1146                                 memcpy(newt, dmesh.tris, sizeof(unsigned char)*4*dmesh.ntris);
1147                         rcFree(dmesh.tris);
1148                         dmesh.tris = newt;
1149                 }
1150                 for (int j = 0; j < ntris; ++j)
1151                 {
1152                         const int* t = &tris[j*4];
1153                         dmesh.tris[dmesh.ntris*4+0] = (unsigned char)t[0];
1154                         dmesh.tris[dmesh.ntris*4+1] = (unsigned char)t[1];
1155                         dmesh.tris[dmesh.ntris*4+2] = (unsigned char)t[2];
1156                         dmesh.tris[dmesh.ntris*4+3] = getTriFlags(&verts[t[0]*3], &verts[t[1]*3], &verts[t[2]*3], poly, npoly);
1157                         dmesh.ntris++;
1158                 }
1159         }
1160                 
1161         ctx->stopTimer(RC_TIMER_BUILD_POLYMESHDETAIL);
1162
1163         return true;
1164 }
1165
1166 /// @see rcAllocPolyMeshDetail, rcPolyMeshDetail
1167 bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh)
1168 {
1169         rcAssert(ctx);
1170         
1171         ctx->startTimer(RC_TIMER_MERGE_POLYMESHDETAIL);
1172
1173         int maxVerts = 0;
1174         int maxTris = 0;
1175         int maxMeshes = 0;
1176
1177         for (int i = 0; i < nmeshes; ++i)
1178         {
1179                 if (!meshes[i]) continue;
1180                 maxVerts += meshes[i]->nverts;
1181                 maxTris += meshes[i]->ntris;
1182                 maxMeshes += meshes[i]->nmeshes;
1183         }
1184
1185         mesh.nmeshes = 0;
1186         mesh.meshes = (unsigned int*)rcAlloc(sizeof(unsigned int)*maxMeshes*4, RC_ALLOC_PERM);
1187         if (!mesh.meshes)
1188         {
1189                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'pmdtl.meshes' (%d).", maxMeshes*4);
1190                 return false;
1191         }
1192
1193         mesh.ntris = 0;
1194         mesh.tris = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxTris*4, RC_ALLOC_PERM);
1195         if (!mesh.tris)
1196         {
1197                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", maxTris*4);
1198                 return false;
1199         }
1200
1201         mesh.nverts = 0;
1202         mesh.verts = (float*)rcAlloc(sizeof(float)*maxVerts*3, RC_ALLOC_PERM);
1203         if (!mesh.verts)
1204         {
1205                 ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", maxVerts*3);
1206                 return false;
1207         }
1208         
1209         // Merge datas.
1210         for (int i = 0; i < nmeshes; ++i)
1211         {
1212                 rcPolyMeshDetail* dm = meshes[i];
1213                 if (!dm) continue;
1214                 for (int j = 0; j < dm->nmeshes; ++j)
1215                 {
1216                         unsigned int* dst = &mesh.meshes[mesh.nmeshes*4];
1217                         unsigned int* src = &dm->meshes[j*4];
1218                         dst[0] = (unsigned int)mesh.nverts+src[0];
1219                         dst[1] = src[1];
1220                         dst[2] = (unsigned int)mesh.ntris+src[2];
1221                         dst[3] = src[3];
1222                         mesh.nmeshes++;
1223                 }
1224                         
1225                 for (int k = 0; k < dm->nverts; ++k)
1226                 {
1227                         rcVcopy(&mesh.verts[mesh.nverts*3], &dm->verts[k*3]);
1228                         mesh.nverts++;
1229                 }
1230                 for (int k = 0; k < dm->ntris; ++k)
1231                 {
1232                         mesh.tris[mesh.ntris*4+0] = dm->tris[k*4+0];
1233                         mesh.tris[mesh.ntris*4+1] = dm->tris[k*4+1];
1234                         mesh.tris[mesh.ntris*4+2] = dm->tris[k*4+2];
1235                         mesh.tris[mesh.ntris*4+3] = dm->tris[k*4+3];
1236                         mesh.ntris++;
1237                 }
1238         }
1239
1240         ctx->stopTimer(RC_TIMER_MERGE_POLYMESHDETAIL);
1241         
1242         return true;
1243 }
1244