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