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