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