Fix selection when adding a primitive while already in edge/face-select mode
[blender-staging.git] / extern / recastnavigation / Recast / Source / RecastMesh.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 #define _USE_MATH_DEFINES
20 #include <math.h>
21 #include <string.h>
22 #include <stdio.h>
23 #include "Recast.h"
24 #include "RecastLog.h"
25 #include "RecastTimer.h"
26
27
28 struct rcEdge
29 {
30         unsigned short vert[2];
31         unsigned short polyEdge[2];
32         unsigned short poly[2];
33 };
34
35 /*static */bool buildMeshAdjacency(unsigned short* polys, const int npolys,
36                                                            const int nverts, const int vertsPerPoly)
37 {
38         // Based on code by Eric Lengyel from:
39         // http://www.terathon.com/code/edges.php
40         
41         int maxEdgeCount = npolys*vertsPerPoly;
42         unsigned short* firstEdge = new unsigned short[nverts + maxEdgeCount];
43         if (!firstEdge)
44                 return false;
45         unsigned short* nextEdge = firstEdge + nverts;
46         int edgeCount = 0;
47         
48         rcEdge* edges = new rcEdge[maxEdgeCount];
49         if (!edges)
50                 return false;
51         
52         for (int i = 0; i < nverts; i++)
53                 firstEdge[i] = 0xffff;
54         
55         // Invalida indices are marked as 0xffff, the following code
56         // handles them just fine.
57         
58         for (int i = 0; i < npolys; ++i)
59         {
60                 unsigned short* t = &polys[i*vertsPerPoly*2];
61                 for (int j = 0; j < vertsPerPoly; ++j)
62                 {
63                         unsigned short v0 = t[j];
64                         unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == 0xffff) ? t[0] : t[j+1];
65                         if (v0 < v1)
66                         {
67                                 rcEdge& edge = edges[edgeCount];
68                                 edge.vert[0] = v0;
69                                 edge.vert[1] = v1;
70                                 edge.poly[0] = (unsigned short)i;
71                                 edge.polyEdge[0] = (unsigned short)j;
72                                 edge.poly[1] = (unsigned short)i;
73                                 edge.polyEdge[1] = 0;
74                                 // Insert edge
75                                 nextEdge[edgeCount] = firstEdge[v0];
76                                 firstEdge[v0] = edgeCount;
77                                 edgeCount++;
78                         }
79                 }
80         }
81         
82         for (int i = 0; i < npolys; ++i)
83         {
84                 unsigned short* t = &polys[i*vertsPerPoly*2];
85                 for (int j = 0; j < vertsPerPoly; ++j)
86                 {
87                         unsigned short v0 = t[j];
88                         unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == 0xffff) ? t[0] : t[j+1];
89                         if (v0 > v1)
90                         {
91                                 for (unsigned short e = firstEdge[v1]; e != 0xffff; e = nextEdge[e])
92                                 {
93                                         rcEdge& edge = edges[e];
94                                         if (edge.vert[1] == v0 && edge.poly[0] == edge.poly[1])
95                                         {
96                                                 edge.poly[1] = (unsigned short)i;
97                                                 edge.polyEdge[1] = (unsigned short)j;
98                                                 break;
99                                         }
100                                 }
101                         }
102                 }
103         }
104         
105         // Store adjacency
106         for (int i = 0; i < edgeCount; ++i)
107         {
108                 const rcEdge& e = edges[i];
109                 if (e.poly[0] != e.poly[1])
110                 {
111                         unsigned short* p0 = &polys[e.poly[0]*vertsPerPoly*2];
112                         unsigned short* p1 = &polys[e.poly[1]*vertsPerPoly*2];
113                         p0[vertsPerPoly + e.polyEdge[0]] = e.poly[1];
114                         p1[vertsPerPoly + e.polyEdge[1]] = e.poly[0];
115                 }
116         }
117         
118         delete [] firstEdge;
119         delete [] edges;
120         
121         return true;
122 }
123
124
125 static const int VERTEX_BUCKET_COUNT = (1<<12);
126
127 inline int computeVertexHash(int x, int y, int z)
128 {
129         const unsigned int h1 = 0x8da6b343; // Large multiplicative constants;
130         const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes
131         const unsigned int h3 = 0xcb1ab31f;
132         unsigned int n = h1 * x + h2 * y + h3 * z;
133         return (int)(n & (VERTEX_BUCKET_COUNT-1));
134 }
135
136 static int addVertex(unsigned short x, unsigned short y, unsigned short z,
137                                          unsigned short* verts, int* firstVert, int* nextVert, int& nv)
138 {
139         int bucket = computeVertexHash(x, 0, z);
140         int i = firstVert[bucket];
141         
142         while (i != -1)
143         {
144                 const unsigned short* v = &verts[i*3];
145                 if (v[0] == x && (rcAbs(v[1] - y) <= 2) && v[2] == z)
146                         return i;
147                 i = nextVert[i]; // next
148         }
149         
150         // Could not find, create new.
151         i = nv; nv++;
152         unsigned short* v = &verts[i*3];
153         v[0] = x;
154         v[1] = y;
155         v[2] = z;
156         nextVert[i] = firstVert[bucket];
157         firstVert[bucket] = i;
158         
159         return i;
160 }
161
162 inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; }
163 inline int next(int i, int n) { return i+1 < n ? i+1 : 0; }
164
165 inline int area2(const int* a, const int* b, const int* c)
166 {
167         return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
168 }
169
170 //      Exclusive or: true iff exactly one argument is true.
171 //      The arguments are negated to ensure that they are 0/1
172 //      values.  Then the bitwise Xor operator may apply.
173 //      (This idea is due to Michael Baldwin.)
174 inline bool xorb(bool x, bool y)
175 {
176         return !x ^ !y;
177 }
178
179 // Returns true iff c is strictly to the left of the directed
180 // line through a to b.
181 inline bool left(const int* a, const int* b, const int* c)
182 {
183         return area2(a, b, c) < 0;
184 }
185
186 inline bool leftOn(const int* a, const int* b, const int* c)
187 {
188         return area2(a, b, c) <= 0;
189 }
190
191 inline bool collinear(const int* a, const int* b, const int* c)
192 {
193         return area2(a, b, c) == 0;
194 }
195
196 //      Returns true iff ab properly intersects cd: they share
197 //      a point interior to both segments.  The properness of the
198 //      intersection is ensured by using strict leftness.
199 bool intersectProp(const int* a, const int* b, const int* c, const int* d)
200 {
201         // Eliminate improper cases.
202         if (collinear(a,b,c) || collinear(a,b,d) ||
203                 collinear(c,d,a) || collinear(c,d,b))
204                 return false;
205         
206         return xorb(left(a,b,c), left(a,b,d)) && xorb(left(c,d,a), left(c,d,b));
207 }
208
209 // Returns T iff (a,b,c) are collinear and point c lies 
210 // on the closed segement ab.
211 static bool between(const int* a, const int* b, const int* c)
212 {
213         if (!collinear(a, b, c))
214                 return false;
215         // If ab not vertical, check betweenness on x; else on y.
216         if (a[0] != b[0])
217                 return  ((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0]));
218         else
219                 return  ((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2]));
220 }
221
222 // Returns true iff segments ab and cd intersect, properly or improperly.
223 static bool intersect(const int* a, const int* b, const int* c, const int* d)
224 {
225         if (intersectProp(a, b, c, d))
226                 return true;
227         else if (between(a, b, c) || between(a, b, d) ||
228                          between(c, d, a) || between(c, d, b))
229                 return true;
230         else
231                 return false;
232 }
233
234 static bool vequal(const int* a, const int* b)
235 {
236         return a[0] == b[0] && a[2] == b[2];
237 }
238
239 // Returns T iff (v_i, v_j) is a proper internal *or* external
240 // diagonal of P, *ignoring edges incident to v_i and v_j*.
241 static bool diagonalie(int i, int j, int n, const int* verts, int* indices)
242 {
243         const int* d0 = &verts[(indices[i] & 0x0fffffff) * 4];
244         const int* d1 = &verts[(indices[j] & 0x0fffffff) * 4];
245         
246         // For each edge (k,k+1) of P
247         for (int k = 0; k < n; k++)
248         {
249                 int k1 = next(k, n);
250                 // Skip edges incident to i or j
251                 if (!((k == i) || (k1 == i) || (k == j) || (k1 == j)))
252                 {
253                         const int* p0 = &verts[(indices[k] & 0x0fffffff) * 4];
254                         const int* p1 = &verts[(indices[k1] & 0x0fffffff) * 4];
255
256                         if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1))
257                                 continue;
258                         
259                         if (intersect(d0, d1, p0, p1))
260                                 return false;
261                 }
262         }
263         return true;
264 }
265
266 // Returns true iff the diagonal (i,j) is strictly internal to the 
267 // polygon P in the neighborhood of the i endpoint.
268 static bool     inCone(int i, int j, int n, const int* verts, int* indices)
269 {
270         const int* pi = &verts[(indices[i] & 0x0fffffff) * 4];
271         const int* pj = &verts[(indices[j] & 0x0fffffff) * 4];
272         const int* pi1 = &verts[(indices[next(i, n)] & 0x0fffffff) * 4];
273         const int* pin1 = &verts[(indices[prev(i, n)] & 0x0fffffff) * 4];
274
275         // If P[i] is a convex vertex [ i+1 left or on (i-1,i) ].
276         if (leftOn(pin1, pi, pi1))
277                 return left(pi, pj, pin1) && left(pj, pi, pi1);
278         // Assume (i-1,i,i+1) not collinear.
279         // else P[i] is reflex.
280         return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1));
281 }
282
283 // Returns T iff (v_i, v_j) is a proper internal
284 // diagonal of P.
285 static bool diagonal(int i, int j, int n, const int* verts, int* indices)
286 {
287         return inCone(i, j, n, verts, indices) && diagonalie(i, j, n, verts, indices);
288 }
289
290 int triangulate(int n, const int* verts, int* indices, int* tris)
291 {
292         int ntris = 0;
293         int* dst = tris;
294         
295         // The last bit of the index is used to indicate if the vertex can be removed.
296         for (int i = 0; i < n; i++)
297         {
298                 int i1 = next(i, n);
299                 int i2 = next(i1, n);
300                 if (diagonal(i, i2, n, verts, indices))
301                         indices[i1] |= 0x80000000;
302         }
303         
304         while (n > 3)
305         {
306                 int minLen = -1;
307                 int mini = -1;
308                 for (int i = 0; i < n; i++)
309                 {
310                         int i1 = next(i, n);
311                         if (indices[i1] & 0x80000000)
312                         {
313                                 const int* p0 = &verts[(indices[i] & 0x0fffffff) * 4];
314                                 const int* p2 = &verts[(indices[next(i1, n)] & 0x0fffffff) * 4];
315                                 
316                                 int dx = p2[0] - p0[0];
317                                 int dy = p2[2] - p0[2];
318                                 int len = dx*dx + dy*dy;
319                                 
320                                 if (minLen < 0 || len < minLen)
321                                 {
322                                         minLen = len;
323                                         mini = i;
324                                 }
325                         }
326                 }
327                 
328                 if (mini == -1)
329                 {
330                         // Should not happen.
331                         if (rcGetLog())
332                                 rcGetLog()->log(RC_LOG_WARNING, "triangulate: Failed to triangulate polygon.");
333 /*                      printf("mini == -1 ntris=%d n=%d\n", ntris, n);
334                         for (int i = 0; i < n; i++)
335                         {
336                                 printf("%d ", indices[i] & 0x0fffffff);
337                         }
338                         printf("\n");*/
339                         return -ntris;
340                 }
341                 
342                 int i = mini;
343                 int i1 = next(i, n);
344                 int i2 = next(i1, n);
345                 
346                 *dst++ = indices[i] & 0x0fffffff;
347                 *dst++ = indices[i1] & 0x0fffffff;
348                 *dst++ = indices[i2] & 0x0fffffff;
349                 ntris++;
350                 
351                 // Removes P[i1] by copying P[i+1]...P[n-1] left one index.
352                 n--;
353                 for (int k = i1; k < n; k++)
354                         indices[k] = indices[k+1];
355                 
356                 if (i1 >= n) i1 = 0;
357                 i = prev(i1,n);
358                 // Update diagonal flags.
359                 if (diagonal(prev(i, n), i1, n, verts, indices))
360                         indices[i] |= 0x80000000;
361                 else
362                         indices[i] &= 0x0fffffff;
363                 
364                 if (diagonal(i, next(i1, n), n, verts, indices))
365                         indices[i1] |= 0x80000000;
366                 else
367                         indices[i1] &= 0x0fffffff;
368         }
369         
370         // Append the remaining triangle.
371         *dst++ = indices[0] & 0x0fffffff;
372         *dst++ = indices[1] & 0x0fffffff;
373         *dst++ = indices[2] & 0x0fffffff;
374         ntris++;
375         
376         return ntris;
377 }
378
379 static int countPolyVerts(const unsigned short* p, const int nvp)
380 {
381         for (int i = 0; i < nvp; ++i)
382                 if (p[i] == 0xffff)
383                         return i;
384         return nvp;
385 }
386
387 inline bool uleft(const unsigned short* a, const unsigned short* b, const unsigned short* c)
388 {
389         return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) -
390                    ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) < 0;
391 }
392
393 static int getPolyMergeValue(unsigned short* pa, unsigned short* pb,
394                                                          const unsigned short* verts, int& ea, int& eb,
395                                                          const int nvp)
396 {
397         const int na = countPolyVerts(pa, nvp);
398         const int nb = countPolyVerts(pb, nvp);
399         
400         // If the merged polygon would be too big, do not merge.
401         if (na+nb-2 > nvp)
402                 return -1;
403         
404         // Check if the polygons share an edge.
405         ea = -1;
406         eb = -1;
407         
408         for (int i = 0; i < na; ++i)
409         {
410                 unsigned short va0 = pa[i];
411                 unsigned short va1 = pa[(i+1) % na];
412                 if (va0 > va1)
413                         rcSwap(va0, va1);
414                 for (int j = 0; j < nb; ++j)
415                 {
416                         unsigned short vb0 = pb[j];
417                         unsigned short vb1 = pb[(j+1) % nb];
418                         if (vb0 > vb1)
419                                 rcSwap(vb0, vb1);
420                         if (va0 == vb0 && va1 == vb1)
421                         {
422                                 ea = i;
423                                 eb = j;
424                                 break;
425                         }
426                 }
427         }
428         
429         // No common edge, cannot merge.
430         if (ea == -1 || eb == -1)
431                 return -1;
432         
433         // Check to see if the merged polygon would be convex.
434         unsigned short va, vb, vc;
435         
436         va = pa[(ea+na-1) % na];
437         vb = pa[ea];
438         vc = pb[(eb+2) % nb];
439         if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3]))
440                 return -1;
441         
442         va = pb[(eb+nb-1) % nb];
443         vb = pb[eb];
444         vc = pa[(ea+2) % na];
445         if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3]))
446                 return -1;
447         
448         va = pa[ea];
449         vb = pa[(ea+1)%na];
450         
451         int dx = (int)verts[va*3+0] - (int)verts[vb*3+0];
452         int dy = (int)verts[va*3+2] - (int)verts[vb*3+2];
453         
454         return dx*dx + dy*dy;
455 }
456
457 static void mergePolys(unsigned short* pa, unsigned short* pb,
458                                            const unsigned short* verts, int ea, int eb,
459                                            unsigned short* tmp, const int nvp)
460 {
461         const int na = countPolyVerts(pa, nvp);
462         const int nb = countPolyVerts(pb, nvp);
463         
464         // Merge polygons.
465         memset(tmp, 0xff, sizeof(unsigned short)*nvp);
466         int n = 0;
467         // Add pa
468         for (int i = 0; i < na-1; ++i)
469                 tmp[n++] = pa[(ea+1+i) % na];
470         // Add pb
471         for (int i = 0; i < nb-1; ++i)
472                 tmp[n++] = pb[(eb+1+i) % nb];
473         
474         memcpy(pa, tmp, sizeof(unsigned short)*nvp);
475 }
476
477 static void pushFront(int v, int* arr, int& an)
478 {
479         an++;
480         for (int i = an-1; i > 0; --i) arr[i] = arr[i-1];
481         arr[0] = v;
482 }
483
484 static void pushBack(int v, int* arr, int& an)
485 {
486         arr[an] = v;
487         an++;
488 }
489
490 static bool removeVertex(rcPolyMesh& mesh, const unsigned short rem, const int maxTris)
491 {
492         unsigned short* tmpPoly;
493         int ntris;
494
495         static const int nvp = mesh.nvp;
496
497         int* edges = 0;
498         int nedges = 0;
499         int* hole = 0;
500         int nhole = 0;
501         int* hreg = 0;
502         int nhreg = 0;
503         int* tris = 0;
504         int* tverts = 0;
505         int* thole = 0;
506         unsigned short* polys = 0;
507         unsigned short* pregs = 0;
508         int npolys = 0;
509
510         // Count number of polygons to remove.
511         int nrem = 0;
512         for (int i = 0; i < mesh.npolys; ++i)
513         {
514                 unsigned short* p = &mesh.polys[i*nvp*2];
515                 for (int j = 0; j < nvp; ++j)
516                         if (p[j] == rem) { nrem++; break; }
517         }
518
519         edges = new int[nrem*nvp*3];
520         if (!edges)
521         {
522                 if (rcGetLog())
523                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'edges' (%d).", nrem*nvp*3);
524                 goto failure;
525         }
526
527         hole = new int[nrem*nvp];
528         if (!hole)
529         {
530                 if (rcGetLog())
531                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hole' (%d).", nrem*nvp);
532                 goto failure;
533         }
534         hreg = new int[nrem*nvp];
535         if (!hreg)
536         {
537                 if (rcGetLog())
538                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hreg' (%d).", nrem*nvp);
539                 goto failure;
540         }
541         
542                 
543         for (int i = 0; i < mesh.npolys; ++i)
544         {
545                 unsigned short* p = &mesh.polys[i*nvp*2];
546                 const int nv = countPolyVerts(p, nvp);
547                 bool hasRem = false;
548                 for (int j = 0; j < nv; ++j)
549                         if (p[j] == rem) hasRem = true;
550                 if (hasRem)
551                 {
552                         // Collect edges which does not touch the removed vertex.
553                         for (int j = 0, k = nv-1; j < nv; k = j++)
554                         {
555                                 if (p[j] != rem && p[k] != rem)
556                                 {
557                                         int* e = &edges[nedges*3];
558                                         e[0] = p[k];
559                                         e[1] = p[j];
560                                         e[2] = mesh.regs[i];
561                                         nedges++;
562                                 }
563                         }
564                         // Remove the polygon.
565                         unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2];
566                         memcpy(p,p2,sizeof(unsigned short)*nvp);
567                         mesh.regs[i] = mesh.regs[mesh.npolys-1];
568                         mesh.npolys--;
569                         --i;
570                 }
571         }
572         
573         // Remove vertex.
574         for (int i = (int)rem; i < mesh.nverts; ++i)
575         {
576                 mesh.verts[i*3+0] = mesh.verts[(i+1)*3+0];
577                 mesh.verts[i*3+1] = mesh.verts[(i+1)*3+1];
578                 mesh.verts[i*3+2] = mesh.verts[(i+1)*3+2];
579         }
580         mesh.nverts--;
581
582         // Adjust indices to match the removed vertex layout.
583         for (int i = 0; i < mesh.npolys; ++i)
584         {
585                 unsigned short* p = &mesh.polys[i*nvp*2];
586                 const int nv = countPolyVerts(p, nvp);
587                 for (int j = 0; j < nv; ++j)
588                         if (p[j] > rem) p[j]--;
589         }
590         for (int i = 0; i < nedges; ++i)
591         {
592                 if (edges[i*3+0] > rem) edges[i*3+0]--;
593                 if (edges[i*3+1] > rem) edges[i*3+1]--;
594         }
595
596         if (nedges == 0)
597                 return true;
598
599         hole[nhole] = edges[0];
600         hreg[nhole] = edges[2];
601         nhole++;
602         
603         while (nedges)
604         {
605                 bool match = false;
606                 
607                 for (int i = 0; i < nedges; ++i)
608                 {
609                         const int ea = edges[i*3+0];
610                         const int eb = edges[i*3+1];
611                         const int r = edges[i*3+2];
612                         bool add = false;
613                         if (hole[0] == eb)
614                         {
615                                 pushFront(ea, hole, nhole);
616                                 pushFront(r, hreg, nhreg);
617                                 add = true;
618                         }
619                         else if (hole[nhole-1] == ea)
620                         {
621                                 pushBack(eb, hole, nhole);
622                                 pushBack(r, hreg, nhreg);
623                                 add = true;
624                         }
625                         if (add)
626                         {
627                                 // Remove edge.
628                                 edges[i*3+0] = edges[(nedges-1)*3+0];
629                                 edges[i*3+1] = edges[(nedges-1)*3+1];
630                                 edges[i*3+2] = edges[(nedges-1)*3+2];
631                                 --nedges;
632                                 match = true;
633                                 --i;
634                         }
635                 }
636                 
637                 if (!match)
638                         break;
639         }
640
641         tris = new int[nhole*3];
642         if (!tris)
643         {
644                 if (rcGetLog())
645                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tris' (%d).", nhole*3);
646                 goto failure;
647         }
648
649         tverts = new int[nhole*4];
650         if (!tverts)
651         {
652                 if (rcGetLog())
653                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tverts' (%d).", nhole*4);
654                 goto failure;
655         }
656
657         thole = new int[nhole];
658         if (!tverts)
659         {
660                 if (rcGetLog())
661                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'thole' (%d).", nhole);
662                 goto failure;
663         }
664
665         // Generate temp vertex array for triangulation.
666         for (int i = 0; i < nhole; ++i)
667         {
668                 const int pi = hole[i];
669                 tverts[i*4+0] = mesh.verts[pi*3+0];
670                 tverts[i*4+1] = mesh.verts[pi*3+1];
671                 tverts[i*4+2] = mesh.verts[pi*3+2];
672                 tverts[i*4+3] = 0;
673                 thole[i] = i;
674         }
675
676         // Triangulate the hole.
677         ntris = triangulate(nhole, &tverts[0], &thole[0], tris);
678
679         // Merge the hole triangles back to polygons.
680         polys = new unsigned short[(ntris+1)*nvp];
681         if (!polys)
682         {
683                 if (rcGetLog())
684                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'polys' (%d).", (ntris+1)*nvp);
685                 goto failure;
686         }
687         pregs = new unsigned short[ntris];
688         if (!pregs)
689         {
690                 if (rcGetLog())
691                         rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'pregs' (%d).", ntris);
692                 goto failure;
693         }
694         
695         tmpPoly = &polys[ntris*nvp];
696                         
697         // Build initial polygons.
698         memset(polys, 0xff, ntris*nvp*sizeof(unsigned short));
699         for (int j = 0; j < ntris; ++j)
700         {
701                 int* t = &tris[j*3];
702                 if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2])
703                 {
704                         polys[npolys*nvp+0] = (unsigned short)hole[t[0]];
705                         polys[npolys*nvp+1] = (unsigned short)hole[t[1]];
706                         polys[npolys*nvp+2] = (unsigned short)hole[t[2]];
707                         pregs[npolys] = hreg[t[0]];
708                         npolys++;
709                 }
710         }
711         if (!npolys)
712                 return true;
713         
714         // Merge polygons.
715         if (nvp > 3)
716         {
717                 while (true)
718                 {
719                         // Find best polygons to merge.
720                         int bestMergeVal = 0;
721                         int bestPa, bestPb, bestEa, bestEb;
722                         
723                         for (int j = 0; j < npolys-1; ++j)
724                         {
725                                 unsigned short* pj = &polys[j*nvp];
726                                 for (int k = j+1; k < npolys; ++k)
727                                 {
728                                         unsigned short* pk = &polys[k*nvp];
729                                         int ea, eb;
730                                         int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp);
731                                         if (v > bestMergeVal)
732                                         {
733                                                 bestMergeVal = v;
734                                                 bestPa = j;
735                                                 bestPb = k;
736                                                 bestEa = ea;
737                                                 bestEb = eb;
738                                         }
739                                 }
740                         }
741                         
742                         if (bestMergeVal > 0)
743                         {
744                                 // Found best, merge.
745                                 unsigned short* pa = &polys[bestPa*nvp];
746                                 unsigned short* pb = &polys[bestPb*nvp];
747                                 mergePolys(pa, pb, mesh.verts, bestEa, bestEb, tmpPoly, nvp);
748                                 memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp);
749                                 pregs[bestPb] = pregs[npolys-1];
750                                 npolys--;
751                         }
752                         else
753                         {
754                                 // Could not merge any polygons, stop.
755                                 break;
756                         }
757                 }
758         }
759         
760         // Store polygons.
761         for (int i = 0; i < npolys; ++i)
762         {
763                 if (mesh.npolys >= maxTris) break;
764                 unsigned short* p = &mesh.polys[mesh.npolys*nvp*2];
765                 memset(p,0xff,sizeof(unsigned short)*nvp*2);
766                 for (int j = 0; j < nvp; ++j)
767                         p[j] = polys[i*nvp+j];
768                 mesh.regs[mesh.npolys] = pregs[i];
769                 mesh.npolys++;
770         }
771         
772         delete [] edges;
773         delete [] hole;
774         delete [] hreg;
775         delete [] tris;
776         delete [] thole;
777         delete [] tverts;
778         delete [] polys;
779         delete [] pregs;
780         
781         return true;
782
783 failure:
784         delete [] edges;
785         delete [] hole;
786         delete [] hreg;
787         delete [] tris;
788         delete [] thole;
789         delete [] tverts;
790         delete [] polys;
791         delete [] pregs;
792         
793         return false;
794 }
795
796
797 bool rcBuildPolyMesh(rcContourSet& cset, int nvp, rcPolyMesh& mesh)
798 {
799         unsigned short* tmpPoly;
800         rcTimeVal startTime = rcGetPerformanceTimer();
801         rcTimeVal endTime;
802
803         vcopy(mesh.bmin, cset.bmin);
804         vcopy(mesh.bmax, cset.bmax);
805         mesh.cs = cset.cs;
806         mesh.ch = cset.ch;
807         
808         int maxVertices = 0;
809         int maxTris = 0;
810         int maxVertsPerCont = 0;
811         for (int i = 0; i < cset.nconts; ++i)
812         {
813                 maxVertices += cset.conts[i].nverts;
814                 maxTris += cset.conts[i].nverts - 2;
815                 maxVertsPerCont = rcMax(maxVertsPerCont, cset.conts[i].nverts);
816         }
817         
818         if (maxVertices >= 0xfffe)
819         {
820                 if (rcGetLog())
821                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many vertices %d.", maxVertices);
822                 return false;
823         }
824         
825         unsigned char* vflags = 0;
826         int* nextVert = 0;
827         int* firstVert = 0;
828         int* indices = 0;
829         int* tris = 0;
830         unsigned short* polys = 0;
831         
832         vflags = new unsigned char[maxVertices];
833         if (!vflags)
834         {
835                 if (rcGetLog())
836                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices);
837                 goto failure;
838         }
839         memset(vflags, 0, maxVertices);
840         
841         mesh.verts = new unsigned short[maxVertices*3];
842         if (!mesh.verts)
843         {
844                 if (rcGetLog())
845                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices);
846                 goto failure;
847         }
848         mesh.polys = new unsigned short[maxTris*nvp*2];
849         if (!mesh.polys)
850         {
851                 if (rcGetLog())
852                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2);
853                 goto failure;
854         }
855         mesh.regs = new unsigned short[maxTris];
856         if (!mesh.regs)
857         {
858                 if (rcGetLog())
859                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.regs' (%d).", maxTris);
860                 goto failure;
861         }
862         mesh.nverts = 0;
863         mesh.npolys = 0;
864         mesh.nvp = nvp;
865         
866         memset(mesh.verts, 0, sizeof(unsigned short)*maxVertices*3);
867         memset(mesh.polys, 0xff, sizeof(unsigned short)*maxTris*nvp*2);
868         memset(mesh.regs, 0, sizeof(unsigned short)*maxTris);
869         
870         nextVert = new int[maxVertices];
871         if (!nextVert)
872         {
873                 if (rcGetLog())
874                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'nextVert' (%d).", maxVertices);
875                 goto failure;
876         }
877         memset(nextVert, 0, sizeof(int)*maxVertices);
878         
879         firstVert = new int[VERTEX_BUCKET_COUNT];
880         if (!firstVert)
881         {
882                 if (rcGetLog())
883                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT);
884                 goto failure;
885         }
886         for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i)
887                 firstVert[i] = -1;
888         
889         indices = new int[maxVertsPerCont];
890         if (!indices)
891         {
892                 if (rcGetLog())
893                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'indices' (%d).", maxVertsPerCont);
894                 goto failure;
895         }
896         tris = new int[maxVertsPerCont*3];
897         if (!tris)
898         {
899                 if (rcGetLog())
900                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'tris' (%d).", maxVertsPerCont*3);
901                 goto failure;
902         }
903         polys = new unsigned short[(maxVertsPerCont+1)*nvp];
904         if (!polys)
905         {
906                 if (rcGetLog())
907                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'polys' (%d).", maxVertsPerCont*nvp);
908                 goto failure;
909         }
910         tmpPoly = &polys[maxVertsPerCont*nvp];
911
912         for (int i = 0; i < cset.nconts; ++i)
913         {
914                 rcContour& cont = cset.conts[i];
915                 
916                 // Skip empty contours.
917                 if (cont.nverts < 3)
918                         continue;
919                 
920                 // Triangulate contour
921                 for (int j = 0; j < cont.nverts; ++j)
922                         indices[j] = j;
923                         
924                 int ntris = triangulate(cont.nverts, cont.verts, &indices[0], &tris[0]);
925                 if (ntris <= 0)
926                 {
927                         // Bad triangulation, should not happen.
928 /*                      for (int k = 0; k < cont.nverts; ++k)
929                         {
930                                 const int* v = &cont.verts[k*4];
931                                 printf("\t\t%d,%d,%d,%d,\n", v[0], v[1], v[2], v[3]);
932                                 if (nBadPos < 100)
933                                 {
934                                         badPos[nBadPos*3+0] = v[0];
935                                         badPos[nBadPos*3+1] = v[1];
936                                         badPos[nBadPos*3+2] = v[2];
937                                         nBadPos++;
938                                 }
939                         }*/
940                         ntris = -ntris;
941                 }
942                 // Add and merge vertices.
943                 for (int j = 0; j < cont.nverts; ++j)
944                 {
945                         const int* v = &cont.verts[j*4];
946                         indices[j] = addVertex((unsigned short)v[0], (unsigned short)v[1], (unsigned short)v[2],
947                                                                    mesh.verts, firstVert, nextVert, mesh.nverts);
948                         if (v[3] & RC_BORDER_VERTEX)
949                         {
950                                 // This vertex should be removed.
951                                 vflags[indices[j]] = 1;
952                         }
953                 }
954                 
955                 // Build initial polygons.
956                 int npolys = 0;
957                 memset(polys, 0xff, maxVertsPerCont*nvp*sizeof(unsigned short));
958                 for (int j = 0; j < ntris; ++j)
959                 {
960                         int* t = &tris[j*3];
961                         if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2])
962                         {
963                                 polys[npolys*nvp+0] = (unsigned short)indices[t[0]];
964                                 polys[npolys*nvp+1] = (unsigned short)indices[t[1]];
965                                 polys[npolys*nvp+2] = (unsigned short)indices[t[2]];
966                                 npolys++;
967                         }
968                 }
969                 if (!npolys)
970                         continue;
971                 
972                 // Merge polygons.
973                 if (nvp > 3)
974                 {
975                         while (true)
976                         {
977                                 // Find best polygons to merge.
978                                 int bestMergeVal = 0;
979                                 int bestPa, bestPb, bestEa, bestEb;
980                                 
981                                 for (int j = 0; j < npolys-1; ++j)
982                                 {
983                                         unsigned short* pj = &polys[j*nvp];
984                                         for (int k = j+1; k < npolys; ++k)
985                                         {
986                                                 unsigned short* pk = &polys[k*nvp];
987                                                 int ea, eb;
988                                                 int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp);
989                                                 if (v > bestMergeVal)
990                                                 {
991                                                         bestMergeVal = v;
992                                                         bestPa = j;
993                                                         bestPb = k;
994                                                         bestEa = ea;
995                                                         bestEb = eb;
996                                                 }
997                                         }
998                                 }
999                                 
1000                                 if (bestMergeVal > 0)
1001                                 {
1002                                         // Found best, merge.
1003                                         unsigned short* pa = &polys[bestPa*nvp];
1004                                         unsigned short* pb = &polys[bestPb*nvp];
1005                                         mergePolys(pa, pb, mesh.verts, bestEa, bestEb, tmpPoly, nvp);
1006                                         memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp);
1007                                         npolys--;
1008                                 }
1009                                 else
1010                                 {
1011                                         // Could not merge any polygons, stop.
1012                                         break;
1013                                 }
1014                         }
1015                 }
1016                 
1017                 
1018                 // Store polygons.
1019                 for (int j = 0; j < npolys; ++j)
1020                 {
1021                         unsigned short* p = &mesh.polys[mesh.npolys*nvp*2];
1022                         unsigned short* q = &polys[j*nvp];
1023                         for (int k = 0; k < nvp; ++k)
1024                                 p[k] = q[k];
1025                         mesh.regs[mesh.npolys] = cont.reg;
1026                         mesh.npolys++;
1027                 }
1028         }
1029         
1030         
1031         // Remove edge vertices.
1032         for (int i = 0; i < mesh.nverts; ++i)
1033         {
1034                 if (vflags[i])
1035                 {
1036                         if (!removeVertex(mesh, i, maxTris))
1037                                 goto failure;
1038                         for (int j = i; j < mesh.nverts-1; ++j)
1039                                 vflags[j] = vflags[j+1];
1040                         --i;
1041                 }
1042         }
1043
1044         delete [] vflags;
1045         delete [] firstVert;
1046         delete [] nextVert;
1047         delete [] indices;
1048         delete [] tris;
1049         
1050         // Calculate adjacency.
1051         if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, nvp))
1052         {
1053                 if (rcGetLog())
1054                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed.");
1055                 return false;
1056         }
1057         
1058         endTime = rcGetPerformanceTimer();
1059         
1060 //      if (rcGetLog())
1061 //              rcGetLog()->log(RC_LOG_PROGRESS, "Build polymesh: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f);
1062         if (rcGetBuildTimes())
1063                 rcGetBuildTimes()->buildPolymesh += rcGetDeltaTimeUsec(startTime, endTime);
1064         
1065         return true;
1066
1067 failure:
1068         delete [] vflags;
1069         delete [] tmpPoly;
1070         delete [] firstVert;
1071         delete [] nextVert;
1072         delete [] indices;
1073         delete [] tris;
1074
1075         return false;
1076 }
1077
1078 bool rcMergePolyMeshes(rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh)
1079 {
1080         if (!nmeshes || !meshes)
1081                 return true;
1082
1083         rcTimeVal startTime = rcGetPerformanceTimer();
1084         rcTimeVal endTime;
1085
1086         int* nextVert = 0;
1087         int* firstVert = 0;
1088         unsigned short* vremap = 0;
1089
1090         mesh.nvp = meshes[0]->nvp;
1091         mesh.cs = meshes[0]->cs;
1092         mesh.ch = meshes[0]->ch;
1093         vcopy(mesh.bmin, meshes[0]->bmin);
1094         vcopy(mesh.bmax, meshes[0]->bmax);
1095
1096         int maxVerts = 0;
1097         int maxPolys = 0;
1098         int maxVertsPerMesh = 0;
1099         for (int i = 0; i < nmeshes; ++i)
1100         {
1101                 vmin(mesh.bmin, meshes[i]->bmin);
1102                 vmax(mesh.bmax, meshes[i]->bmax);
1103                 maxVertsPerMesh = rcMax(maxVertsPerMesh, meshes[i]->nverts);
1104                 maxVerts += meshes[i]->nverts;
1105                 maxPolys += meshes[i]->npolys;
1106         }
1107         
1108         mesh.nverts = 0;
1109         mesh.verts = new unsigned short[maxVerts*3];
1110         if (!mesh.verts)
1111         {
1112                 if (rcGetLog())
1113                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.verts' (%d).", maxVerts*3);
1114                 return false;
1115         }
1116
1117         mesh.npolys = 0;
1118         mesh.polys = new unsigned short[maxPolys*2*mesh.nvp];
1119         if (!mesh.polys)
1120         {
1121                 if (rcGetLog())
1122                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.polys' (%d).", maxPolys*2*mesh.nvp);
1123                 return false;
1124         }
1125         memset(mesh.polys, 0xff, sizeof(unsigned short)*maxPolys*2*mesh.nvp);
1126
1127         mesh.regs = new unsigned short[maxPolys];
1128         if (!mesh.regs)
1129         {
1130                 if (rcGetLog())
1131                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.regs' (%d).", maxPolys);
1132                 return false;
1133         }
1134         memset(mesh.regs, 0, sizeof(unsigned short)*maxPolys);
1135         
1136         nextVert = new int[maxVerts];
1137         if (!nextVert)
1138         {
1139                 if (rcGetLog())
1140                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'nextVert' (%d).", maxVerts);
1141                 goto failure;
1142         }
1143         memset(nextVert, 0, sizeof(int)*maxVerts);
1144         
1145         firstVert = new int[VERTEX_BUCKET_COUNT];
1146         if (!firstVert)
1147         {
1148                 if (rcGetLog())
1149                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT);
1150                 goto failure;
1151         }
1152         for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i)
1153                 firstVert[i] = -1;
1154
1155         vremap = new unsigned short[maxVertsPerMesh];
1156         if (!vremap)
1157         {
1158                 if (rcGetLog())
1159                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh);
1160                 goto failure;
1161         }
1162         memset(nextVert, 0, sizeof(int)*maxVerts);
1163         
1164         for (int i = 0; i < nmeshes; ++i)
1165         {
1166                 const rcPolyMesh* pmesh = meshes[i];
1167                 
1168                 const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f);
1169                 const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f);
1170                 
1171                 for (int j = 0; j < pmesh->nverts; ++j)
1172                 {
1173                         unsigned short* v = &pmesh->verts[j*3];
1174                         vremap[j] = addVertex(v[0]+ox, v[1], v[2]+oz,
1175                                                    mesh.verts, firstVert, nextVert, mesh.nverts);
1176                 }
1177                 
1178                 for (int j = 0; j < pmesh->npolys; ++j)
1179                 {
1180                         unsigned short* tgt = &mesh.polys[mesh.npolys*2*mesh.nvp];
1181                         unsigned short* src = &pmesh->polys[j*2*mesh.nvp];
1182                         mesh.regs[mesh.npolys] = pmesh->regs[j];
1183                         mesh.npolys++;
1184                         for (int k = 0; k < mesh.nvp; ++k)
1185                         {
1186                                 if (src[k] == 0xffff) break;
1187                                 tgt[k] = vremap[src[k]];
1188                         }
1189                 }
1190         }
1191
1192         // Calculate adjacency.
1193         if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, mesh.nvp))
1194         {
1195                 if (rcGetLog())
1196                         rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Adjacency failed.");
1197                 return false;
1198         }
1199                 
1200
1201         delete [] firstVert;
1202         delete [] nextVert;
1203         delete [] vremap;
1204         
1205         endTime = rcGetPerformanceTimer();
1206         
1207         if (rcGetBuildTimes())
1208                 rcGetBuildTimes()->mergePolyMesh += rcGetDeltaTimeUsec(startTime, endTime);
1209         
1210         return true;
1211         
1212 failure:
1213         delete [] firstVert;
1214         delete [] nextVert;
1215         delete [] vremap;
1216         
1217         return false;
1218 }