code cleanup:
[blender.git] / extern / recastnavigation / Recast / Source / RecastContour.cpp
1 //
2 // Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
3 //
4 // This software is provided 'as-is', without any express or implied
5 // warranty.  In no event will the authors be held liable for any damages
6 // arising from the use of this software.
7 // Permission is granted to anyone to use this software for any purpose,
8 // including commercial applications, and to alter it and redistribute it
9 // freely, subject to the following restrictions:
10 // 1. The origin of this software must not be misrepresented; you must not
11 //    claim that you wrote the original software. If you use this software
12 //    in a product, an acknowledgment in the product documentation would be
13 //    appreciated but is not required.
14 // 2. Altered source versions must be plainly marked as such, and must not be
15 //    misrepresented as being the original software.
16 // 3. This notice may not be removed or altered from any source distribution.
17 //
18
19 #define _USE_MATH_DEFINES
20 #include <math.h>
21 #include <string.h>
22 #include <stdio.h>
23 #include "Recast.h"
24 #include "RecastAlloc.h"
25 #include "RecastAssert.h"
26
27
28 static int getCornerHeight(int x, int y, int i, int dir,
29                                                    const rcCompactHeightfield& chf,
30                                                    bool& isBorderVertex)
31 {
32         const rcCompactSpan& s = chf.spans[i];
33         int ch = (int)s.y;
34         int dirp = (dir+1) & 0x3;
35         
36         unsigned int regs[4] = {0,0,0,0};
37         
38         // Combine region and area codes in order to prevent
39         // border vertices which are in between two areas to be removed. 
40         regs[0] = chf.spans[i].reg | (chf.areas[i] << 16);
41         
42         if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
43         {
44                 const int ax = x + rcGetDirOffsetX(dir);
45                 const int ay = y + rcGetDirOffsetY(dir);
46                 const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
47                 const rcCompactSpan& as = chf.spans[ai];
48                 ch = rcMax(ch, (int)as.y);
49                 regs[1] = chf.spans[ai].reg | (chf.areas[ai] << 16);
50                 if (rcGetCon(as, dirp) != RC_NOT_CONNECTED)
51                 {
52                         const int ax2 = ax + rcGetDirOffsetX(dirp);
53                         const int ay2 = ay + rcGetDirOffsetY(dirp);
54                         const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dirp);
55                         const rcCompactSpan& as2 = chf.spans[ai2];
56                         ch = rcMax(ch, (int)as2.y);
57                         regs[2] = chf.spans[ai2].reg | (chf.areas[ai2] << 16);
58                 }
59         }
60         if (rcGetCon(s, dirp) != RC_NOT_CONNECTED)
61         {
62                 const int ax = x + rcGetDirOffsetX(dirp);
63                 const int ay = y + rcGetDirOffsetY(dirp);
64                 const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dirp);
65                 const rcCompactSpan& as = chf.spans[ai];
66                 ch = rcMax(ch, (int)as.y);
67                 regs[3] = chf.spans[ai].reg | (chf.areas[ai] << 16);
68                 if (rcGetCon(as, dir) != RC_NOT_CONNECTED)
69                 {
70                         const int ax2 = ax + rcGetDirOffsetX(dir);
71                         const int ay2 = ay + rcGetDirOffsetY(dir);
72                         const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dir);
73                         const rcCompactSpan& as2 = chf.spans[ai2];
74                         ch = rcMax(ch, (int)as2.y);
75                         regs[2] = chf.spans[ai2].reg | (chf.areas[ai2] << 16);
76                 }
77         }
78
79         // Check if the vertex is special edge vertex, these vertices will be removed later.
80         for (int j = 0; j < 4; ++j)
81         {
82                 const int a = j;
83                 const int b = (j+1) & 0x3;
84                 const int c = (j+2) & 0x3;
85                 const int d = (j+3) & 0x3;
86                 
87                 // The vertex is a border vertex there are two same exterior cells in a row,
88                 // followed by two interior cells and none of the regions are out of bounds.
89                 const bool twoSameExts = (regs[a] & regs[b] & RC_BORDER_REG) != 0 && regs[a] == regs[b];
90                 const bool twoInts = ((regs[c] | regs[d]) & RC_BORDER_REG) == 0;
91                 const bool intsSameArea = (regs[c]>>16) == (regs[d]>>16);
92                 const bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0;
93                 if (twoSameExts && twoInts && intsSameArea && noZeros)
94                 {
95                         isBorderVertex = true;
96                         break;
97                 }
98         }
99         
100         return ch;
101 }
102
103 static void walkContour(int x, int y, int i,
104                                                 rcCompactHeightfield& chf,
105                                                 unsigned char* flags, rcIntArray& points)
106 {
107         // Choose the first non-connected edge
108         unsigned char dir = 0;
109         while ((flags[i] & (1 << dir)) == 0)
110                 dir++;
111         
112         unsigned char startDir = dir;
113         int starti = i;
114         
115         const unsigned char area = chf.areas[i];
116         
117         int iter = 0;
118         while (++iter < 40000)
119         {
120                 if (flags[i] & (1 << dir))
121                 {
122                         // Choose the edge corner
123                         bool isBorderVertex = false;
124                         bool isAreaBorder = false;
125                         int px = x;
126                         int py = getCornerHeight(x, y, i, dir, chf, isBorderVertex);
127                         int pz = y;
128                         switch(dir)
129                         {
130                                 case 0: pz++; break;
131                                 case 1: px++; pz++; break;
132                                 case 2: px++; break;
133                         }
134                         int r = 0;
135                         const rcCompactSpan& s = chf.spans[i];
136                         if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
137                         {
138                                 const int ax = x + rcGetDirOffsetX(dir);
139                                 const int ay = y + rcGetDirOffsetY(dir);
140                                 const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir);
141                                 r = (int)chf.spans[ai].reg;
142                                 if (area != chf.areas[ai])
143                                         isAreaBorder = true;
144                         }
145                         if (isBorderVertex)
146                                 r |= RC_BORDER_VERTEX;
147                         if (isAreaBorder)
148                                 r |= RC_AREA_BORDER;
149                         points.push(px);
150                         points.push(py);
151                         points.push(pz);
152                         points.push(r);
153                         
154                         flags[i] &= ~(1 << dir); // Remove visited edges
155                         dir = (dir+1) & 0x3;  // Rotate CW
156                 }
157                 else
158                 {
159                         int ni = -1;
160                         const int nx = x + rcGetDirOffsetX(dir);
161                         const int ny = y + rcGetDirOffsetY(dir);
162                         const rcCompactSpan& s = chf.spans[i];
163                         if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
164                         {
165                                 const rcCompactCell& nc = chf.cells[nx+ny*chf.width];
166                                 ni = (int)nc.index + rcGetCon(s, dir);
167                         }
168                         if (ni == -1)
169                         {
170                                 // Should not happen.
171                                 return;
172                         }
173                         x = nx;
174                         y = ny;
175                         i = ni;
176                         dir = (dir+3) & 0x3;    // Rotate CCW
177                 }
178                 
179                 if (starti == i && startDir == dir)
180                 {
181                         break;
182                 }
183         }
184 }
185
186 static float distancePtSeg(const int x, const int z,
187                                                    const int px, const int pz,
188                                                    const int qx, const int qz)
189 {
190 /*      float pqx = (float)(qx - px);
191         float pqy = (float)(qy - py);
192         float pqz = (float)(qz - pz);
193         float dx = (float)(x - px);
194         float dy = (float)(y - py);
195         float dz = (float)(z - pz);
196         float d = pqx*pqx + pqy*pqy + pqz*pqz;
197         float t = pqx*dx + pqy*dy + pqz*dz;
198         if (d > 0)
199                 t /= d;
200         if (t < 0)
201                 t = 0;
202         else if (t > 1)
203                 t = 1;
204         
205         dx = px + t*pqx - x;
206         dy = py + t*pqy - y;
207         dz = pz + t*pqz - z;
208         
209         return dx*dx + dy*dy + dz*dz;*/
210
211         float pqx = (float)(qx - px);
212         float pqz = (float)(qz - pz);
213         float dx = (float)(x - px);
214         float dz = (float)(z - pz);
215         float d = pqx*pqx + pqz*pqz;
216         float t = pqx*dx + pqz*dz;
217         if (d > 0)
218                 t /= d;
219         if (t < 0)
220                 t = 0;
221         else if (t > 1)
222                 t = 1;
223         
224         dx = px + t*pqx - x;
225         dz = pz + t*pqz - z;
226         
227         return dx*dx + dz*dz;
228 }
229
230 static void simplifyContour(rcIntArray& points, rcIntArray& simplified,
231                                                         const float maxError, const int maxEdgeLen, const int buildFlags)
232 {
233         // Add initial points.
234         bool hasConnections = false;
235         for (int i = 0; i < points.size(); i += 4)
236         {
237                 if ((points[i+3] & RC_CONTOUR_REG_MASK) != 0)
238                 {
239                         hasConnections = true;
240                         break;
241                 }
242         }
243         
244         if (hasConnections)
245         {
246                 // The contour has some portals to other regions.
247                 // Add a new point to every location where the region changes.
248                 for (int i = 0, ni = points.size()/4; i < ni; ++i)
249                 {
250                         int ii = (i+1) % ni;
251                         const bool differentRegs = (points[i*4+3] & RC_CONTOUR_REG_MASK) != (points[ii*4+3] & RC_CONTOUR_REG_MASK);
252                         const bool areaBorders = (points[i*4+3] & RC_AREA_BORDER) != (points[ii*4+3] & RC_AREA_BORDER);
253                         if (differentRegs || areaBorders)
254                         {
255                                 simplified.push(points[i*4+0]);
256                                 simplified.push(points[i*4+1]);
257                                 simplified.push(points[i*4+2]);
258                                 simplified.push(i);
259                         }
260                 }       
261         }
262         
263         if (simplified.size() == 0)
264         {
265                 // If there is no connections at all,
266                 // create some initial points for the simplification process. 
267                 // Find lower-left and upper-right vertices of the contour.
268                 int llx = points[0];
269                 int lly = points[1];
270                 int llz = points[2];
271                 int lli = 0;
272                 int urx = points[0];
273                 int ury = points[1];
274                 int urz = points[2];
275                 int uri = 0;
276                 for (int i = 0; i < points.size(); i += 4)
277                 {
278                         int x = points[i+0];
279                         int y = points[i+1];
280                         int z = points[i+2];
281                         if (x < llx || (x == llx && z < llz))
282                         {
283                                 llx = x;
284                                 lly = y;
285                                 llz = z;
286                                 lli = i/4;
287                         }
288                         if (x > urx || (x == urx && z > urz))
289                         {
290                                 urx = x;
291                                 ury = y;
292                                 urz = z;
293                                 uri = i/4;
294                         }
295                 }
296                 simplified.push(llx);
297                 simplified.push(lly);
298                 simplified.push(llz);
299                 simplified.push(lli);
300                 
301                 simplified.push(urx);
302                 simplified.push(ury);
303                 simplified.push(urz);
304                 simplified.push(uri);
305         }
306         
307         // Add points until all raw points are within
308         // error tolerance to the simplified shape.
309         const int pn = points.size()/4;
310         for (int i = 0; i < simplified.size()/4; )
311         {
312                 int ii = (i+1) % (simplified.size()/4);
313                 
314                 const int ax = simplified[i*4+0];
315                 const int az = simplified[i*4+2];
316                 const int ai = simplified[i*4+3];
317                 
318                 const int bx = simplified[ii*4+0];
319                 const int bz = simplified[ii*4+2];
320                 const int bi = simplified[ii*4+3];
321
322                 // Find maximum deviation from the segment.
323                 float maxd = 0;
324                 int i_max = -1;
325                 int ci, cinc, endi;
326                 
327                 // Traverse the segment in lexilogical order so that the
328                 // max deviation is calculated similarly when traversing
329                 // opposite segments.
330                 if (bx > ax || (bx == ax && bz > az))
331                 {
332                         cinc = 1;
333                         ci = (ai+cinc) % pn;
334                         endi = bi;
335                 }
336                 else
337                 {
338                         cinc = pn-1;
339                         ci = (bi+cinc) % pn;
340                         endi = ai;
341                 }
342                 
343                 // Tessellate only outer edges or edges between areas.
344                 if ((points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0 ||
345                         (points[ci*4+3] & RC_AREA_BORDER))
346                 {
347                         while (ci != endi)
348                         {
349                                 float d = distancePtSeg(points[ci*4+0], points[ci*4+2], ax, az, bx, bz);
350                                 if (d > maxd)
351                                 {
352                                         maxd = d;
353                                         i_max = ci;
354                                 }
355                                 ci = (ci+cinc) % pn;
356                         }
357                 }
358                 
359                 
360                 // If the max deviation is larger than accepted error,
361                 // add new point, else continue to next segment.
362                 if (i_max != -1 && maxd > (maxError*maxError))
363                 {
364                         // Add space for the new point.
365                         simplified.resize(simplified.size()+4);
366                         const int n = simplified.size()/4;
367                         for (int j = n-1; j > i; --j)
368                         {
369                                 simplified[j*4+0] = simplified[(j-1)*4+0];
370                                 simplified[j*4+1] = simplified[(j-1)*4+1];
371                                 simplified[j*4+2] = simplified[(j-1)*4+2];
372                                 simplified[j*4+3] = simplified[(j-1)*4+3];
373                         }
374                         // Add the point.
375                         simplified[(i+1)*4+0] = points[i_max*4+0];
376                         simplified[(i+1)*4+1] = points[i_max*4+1];
377                         simplified[(i+1)*4+2] = points[i_max*4+2];
378                         simplified[(i+1)*4+3] = i_max;
379                 }
380                 else
381                 {
382                         ++i;
383                 }
384         }
385         
386         // Split too long edges.
387         if (maxEdgeLen > 0 && (buildFlags & (RC_CONTOUR_TESS_WALL_EDGES|RC_CONTOUR_TESS_AREA_EDGES)) != 0)
388         {
389                 for (int i = 0; i < simplified.size()/4; )
390                 {
391                         const int ii = (i+1) % (simplified.size()/4);
392                         
393                         const int ax = simplified[i*4+0];
394                         const int az = simplified[i*4+2];
395                         const int ai = simplified[i*4+3];
396                         
397                         const int bx = simplified[ii*4+0];
398                         const int bz = simplified[ii*4+2];
399                         const int bi = simplified[ii*4+3];
400
401                         // Find maximum deviation from the segment.
402                         int i_max = -1;
403                         int ci = (ai+1) % pn;
404
405                         // Tessellate only outer edges or edges between areas.
406                         bool tess = false;
407                         // Wall edges.
408                         if ((buildFlags & RC_CONTOUR_TESS_WALL_EDGES) && (points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0)
409                                 tess = true;
410                         // Edges between areas.
411                         if ((buildFlags & RC_CONTOUR_TESS_AREA_EDGES) && (points[ci*4+3] & RC_AREA_BORDER))
412                                 tess = true;
413                         
414                         if (tess)
415                         {
416                                 int dx = bx - ax;
417                                 int dz = bz - az;
418                                 if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen)
419                                 {
420                                         // Round based on the segments in lexilogical order so that the
421                                         // max tesselation is consistent regardles in which direction
422                                         // segments are traversed.
423                                         if (bx > ax || (bx == ax && bz > az))
424                                         {
425                                                 const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
426                                                 i_max = (ai + n/2) % pn;
427                                         }
428                                         else
429                                         {
430                                                 const int n = bi < ai ? (bi+pn - ai) : (bi - ai);
431                                                 i_max = (ai + (n+1)/2) % pn;
432                                         }
433                                 }
434                         }
435                         
436                         // If the max deviation is larger than accepted error,
437                         // add new point, else continue to next segment.
438                         if (i_max != -1)
439                         {
440                                 // Add space for the new point.
441                                 simplified.resize(simplified.size()+4);
442                                 const int n = simplified.size()/4;
443                                 for (int j = n-1; j > i; --j)
444                                 {
445                                         simplified[j*4+0] = simplified[(j-1)*4+0];
446                                         simplified[j*4+1] = simplified[(j-1)*4+1];
447                                         simplified[j*4+2] = simplified[(j-1)*4+2];
448                                         simplified[j*4+3] = simplified[(j-1)*4+3];
449                                 }
450                                 // Add the point.
451                                 simplified[(i+1)*4+0] = points[i_max*4+0];
452                                 simplified[(i+1)*4+1] = points[i_max*4+1];
453                                 simplified[(i+1)*4+2] = points[i_max*4+2];
454                                 simplified[(i+1)*4+3] = i_max;
455                         }
456                         else
457                         {
458                                 ++i;
459                         }
460                 }
461         }
462         
463         for (int i = 0; i < simplified.size()/4; ++i)
464         {
465                 // The edge vertex flag is take from the current raw point,
466                 // and the neighbour region is take from the next raw point.
467                 const int ai = (simplified[i*4+3]+1) % pn;
468                 const int bi = simplified[i*4+3];
469                 simplified[i*4+3] = (points[ai*4+3] & RC_CONTOUR_REG_MASK) | (points[bi*4+3] & RC_BORDER_VERTEX);
470         }
471         
472 }
473
474 static void removeDegenerateSegments(rcIntArray& simplified)
475 {
476         // Remove adjacent vertices which are equal on xz-plane,
477         // or else the triangulator will get confused.
478         for (int i = 0; i < simplified.size()/4; ++i)
479         {
480                 int ni = i+1;
481                 if (ni >= (simplified.size()/4))
482                         ni = 0;
483                         
484                 if (simplified[i*4+0] == simplified[ni*4+0] &&
485                         simplified[i*4+2] == simplified[ni*4+2])
486                 {
487                         // Degenerate segment, remove.
488                         for (int j = i; j < simplified.size()/4-1; ++j)
489                         {
490                                 simplified[j*4+0] = simplified[(j+1)*4+0];
491                                 simplified[j*4+1] = simplified[(j+1)*4+1];
492                                 simplified[j*4+2] = simplified[(j+1)*4+2];
493                                 simplified[j*4+3] = simplified[(j+1)*4+3];
494                         }
495                         simplified.resize(simplified.size()-4);
496                 }
497         }
498 }
499
500 static int calcAreaOfPolygon2D(const int* verts, const int nverts)
501 {
502         int area = 0;
503         for (int i = 0, j = nverts-1; i < nverts; j=i++)
504         {
505                 const int* vi = &verts[i*4];
506                 const int* vj = &verts[j*4];
507                 area += vi[0] * vj[2] - vj[0] * vi[2];
508         }
509         return (area+1) / 2;
510 }
511
512 inline bool ileft(const int* a, const int* b, const int* c)
513 {
514         return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]) <= 0;
515 }
516
517 static void getClosestIndices(const int* vertsa, const int nvertsa,
518                                                           const int* vertsb, const int nvertsb,
519                                                           int& ia, int& ib)
520 {
521         int closestDist = 0xfffffff;
522         ia = -1, ib = -1;
523         for (int i = 0; i < nvertsa; ++i)
524         {
525                 const int in = (i+1) % nvertsa;
526                 const int ip = (i+nvertsa-1) % nvertsa;
527                 const int* va = &vertsa[i*4];
528                 const int* van = &vertsa[in*4];
529                 const int* vap = &vertsa[ip*4];
530                 
531                 for (int j = 0; j < nvertsb; ++j)
532                 {
533                         const int* vb = &vertsb[j*4];
534                         // vb must be "infront" of va.
535                         if (ileft(vap,va,vb) && ileft(va,van,vb))
536                         {
537                                 const int dx = vb[0] - va[0];
538                                 const int dz = vb[2] - va[2];
539                                 const int d = dx*dx + dz*dz;
540                                 if (d < closestDist)
541                                 {
542                                         ia = i;
543                                         ib = j;
544                                         closestDist = d;
545                                 }
546                         }
547                 }
548         }
549 }
550
551 static bool mergeContours(rcContour& ca, rcContour& cb, int ia, int ib)
552 {
553         const int maxVerts = ca.nverts + cb.nverts + 2;
554         int* verts = (int*)rcAlloc(sizeof(int)*maxVerts*4, RC_ALLOC_PERM);
555         if (!verts)
556                 return false;
557
558         int nv = 0;
559
560         // Copy contour A.
561         for (int i = 0; i <= ca.nverts; ++i)
562         {
563                 int* dst = &verts[nv*4];
564                 const int* src = &ca.verts[((ia+i)%ca.nverts)*4];
565                 dst[0] = src[0];
566                 dst[1] = src[1];
567                 dst[2] = src[2];
568                 dst[3] = src[3];
569                 nv++;
570         }
571
572         // Copy contour B
573         for (int i = 0; i <= cb.nverts; ++i)
574         {
575                 int* dst = &verts[nv*4];
576                 const int* src = &cb.verts[((ib+i)%cb.nverts)*4];
577                 dst[0] = src[0];
578                 dst[1] = src[1];
579                 dst[2] = src[2];
580                 dst[3] = src[3];
581                 nv++;
582         }
583         
584         rcFree(ca.verts);
585         ca.verts = verts;
586         ca.nverts = nv;
587
588         rcFree(cb.verts);
589         cb.verts = 0;
590         cb.nverts = 0;
591         
592         return true;
593 }
594
595 /// @par
596 ///
597 /// The raw contours will match the region outlines exactly. The @p maxError and @p maxEdgeLen
598 /// parameters control how closely the simplified contours will match the raw contours.
599 ///
600 /// Simplified contours are generated such that the vertices for portals between areas match up. 
601 /// (They are considered mandatory vertices.)
602 ///
603 /// Setting @p maxEdgeLength to zero will disabled the edge length feature.
604 /// 
605 /// See the #rcConfig documentation for more information on the configuration parameters.
606 /// 
607 /// @see rcAllocContourSet, rcCompactHeightfield, rcContourSet, rcConfig
608 bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf,
609                                          const float maxError, const int maxEdgeLen,
610                                          rcContourSet& cset, const int buildFlags)
611 {
612         rcAssert(ctx);
613         
614         const int w = chf.width;
615         const int h = chf.height;
616         const int borderSize = chf.borderSize;
617         
618         ctx->startTimer(RC_TIMER_BUILD_CONTOURS);
619         
620         rcVcopy(cset.bmin, chf.bmin);
621         rcVcopy(cset.bmax, chf.bmax);
622         if (borderSize > 0)
623         {
624                 // If the heightfield was build with bordersize, remove the offset.
625                 const float pad = borderSize*chf.cs;
626                 cset.bmin[0] += pad;
627                 cset.bmin[2] += pad;
628                 cset.bmax[0] -= pad;
629                 cset.bmax[2] -= pad;
630         }
631         cset.cs = chf.cs;
632         cset.ch = chf.ch;
633         cset.width = chf.width - chf.borderSize*2;
634         cset.height = chf.height - chf.borderSize*2;
635         cset.borderSize = chf.borderSize;
636         
637         int maxContours = rcMax((int)chf.maxRegions, 8);
638         cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM);
639         if (!cset.conts)
640                 return false;
641         cset.nconts = 0;
642         
643         rcScopedDelete<unsigned char> flags = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
644         if (!flags)
645         {
646                 ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'flags' (%d).", chf.spanCount);
647                 return false;
648         }
649         
650         ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
651         
652         // Mark boundaries.
653         for (int y = 0; y < h; ++y)
654         {
655                 for (int x = 0; x < w; ++x)
656                 {
657                         const rcCompactCell& c = chf.cells[x+y*w];
658                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
659                         {
660                                 unsigned char res = 0;
661                                 const rcCompactSpan& s = chf.spans[i];
662                                 if (!chf.spans[i].reg || (chf.spans[i].reg & RC_BORDER_REG))
663                                 {
664                                         flags[i] = 0;
665                                         continue;
666                                 }
667                                 for (int dir = 0; dir < 4; ++dir)
668                                 {
669                                         unsigned short r = 0;
670                                         if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
671                                         {
672                                                 const int ax = x + rcGetDirOffsetX(dir);
673                                                 const int ay = y + rcGetDirOffsetY(dir);
674                                                 const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
675                                                 r = chf.spans[ai].reg;
676                                         }
677                                         if (r == chf.spans[i].reg)
678                                                 res |= (1 << dir);
679                                 }
680                                 flags[i] = res ^ 0xf; // Inverse, mark non connected edges.
681                         }
682                 }
683         }
684         
685         ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
686         
687         rcIntArray verts(256);
688         rcIntArray simplified(64);
689         
690         for (int y = 0; y < h; ++y)
691         {
692                 for (int x = 0; x < w; ++x)
693                 {
694                         const rcCompactCell& c = chf.cells[x+y*w];
695                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
696                         {
697                                 if (flags[i] == 0 || flags[i] == 0xf)
698                                 {
699                                         flags[i] = 0;
700                                         continue;
701                                 }
702                                 const unsigned short reg = chf.spans[i].reg;
703                                 if (!reg || (reg & RC_BORDER_REG))
704                                         continue;
705                                 const unsigned char area = chf.areas[i];
706                                 
707                                 verts.resize(0);
708                                 simplified.resize(0);
709
710                                 ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
711                                 walkContour(x, y, i, chf, flags, verts);
712                                 ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE);
713
714                                 ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
715                                 simplifyContour(verts, simplified, maxError, maxEdgeLen, buildFlags);
716                                 removeDegenerateSegments(simplified);
717                                 ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY);
718                                 
719
720                                 // Store region->contour remap info.
721                                 // Create contour.
722                                 if (simplified.size()/4 >= 3)
723                                 {
724                                         if (cset.nconts >= maxContours)
725                                         {
726                                                 // Allocate more contours.
727                                                 // This can happen when there are tiny holes in the heightfield.
728                                                 const int oldMax = maxContours;
729                                                 maxContours *= 2;
730                                                 rcContour* newConts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM);
731                                                 for (int j = 0; j < cset.nconts; ++j)
732                                                 {
733                                                         newConts[j] = cset.conts[j];
734                                                         // Reset source pointers to prevent data deletion.
735                                                         cset.conts[j].verts = 0;
736                                                         cset.conts[j].rverts = 0;
737                                                 }
738                                                 rcFree(cset.conts);
739                                                 cset.conts = newConts;
740                                         
741                                                 ctx->log(RC_LOG_WARNING, "rcBuildContours: Expanding max contours from %d to %d.", oldMax, maxContours);
742                                         }
743                                                 
744                                         rcContour* cont = &cset.conts[cset.nconts++];
745                                         
746                                         cont->nverts = simplified.size()/4;
747                                         cont->verts = (int*)rcAlloc(sizeof(int)*cont->nverts*4, RC_ALLOC_PERM);
748                                         if (!cont->verts)
749                                         {
750                                                 ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'verts' (%d).", cont->nverts);
751                                                 return false;
752                                         }
753                                         memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4);
754                                         if (borderSize > 0)
755                                         {
756                                                 // If the heightfield was build with bordersize, remove the offset.
757                                                 for (int i = 0; i < cont->nverts; ++i)
758                                                 {
759                                                         int* v = &cont->verts[i*4];
760                                                         v[0] -= borderSize;
761                                                         v[2] -= borderSize;
762                                                 }
763                                         }
764                                         
765                                         cont->nrverts = verts.size()/4;
766                                         cont->rverts = (int*)rcAlloc(sizeof(int)*cont->nrverts*4, RC_ALLOC_PERM);
767                                         if (!cont->rverts)
768                                         {
769                                                 ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'rverts' (%d).", cont->nrverts);
770                                                 return false;
771                                         }
772                                         memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4);
773                                         if (borderSize > 0)
774                                         {
775                                                 // If the heightfield was build with bordersize, remove the offset.
776                                                 for (int i = 0; i < cont->nrverts; ++i)
777                                                 {
778                                                         int* v = &cont->rverts[i*4];
779                                                         v[0] -= borderSize;
780                                                         v[2] -= borderSize;
781                                                 }
782                                         }
783                                         
784 /*                                      cont->cx = cont->cy = cont->cz = 0;
785                                         for (int i = 0; i < cont->nverts; ++i)
786                                         {
787                                                 cont->cx += cont->verts[i*4+0];
788                                                 cont->cy += cont->verts[i*4+1];
789                                                 cont->cz += cont->verts[i*4+2];
790                                         }
791                                         cont->cx /= cont->nverts;
792                                         cont->cy /= cont->nverts;
793                                         cont->cz /= cont->nverts;*/
794                                         
795                                         cont->reg = reg;
796                                         cont->area = area;
797                                 }
798                         }
799                 }
800         }
801         
802         // Check and merge droppings.
803         // Sometimes the previous algorithms can fail and create several contours
804         // per area. This pass will try to merge the holes into the main region.
805         for (int i = 0; i < cset.nconts; ++i)
806         {
807                 rcContour& cont = cset.conts[i];
808                 // Check if the contour is would backwards.
809                 if (calcAreaOfPolygon2D(cont.verts, cont.nverts) < 0)
810                 {
811                         // Find another contour which has the same region ID.
812                         int mergeIdx = -1;
813                         for (int j = 0; j < cset.nconts; ++j)
814                         {
815                                 if (i == j) continue;
816                                 if (cset.conts[j].nverts && cset.conts[j].reg == cont.reg)
817                                 {
818                                         // Make sure the polygon is correctly oriented.
819                                         if (calcAreaOfPolygon2D(cset.conts[j].verts, cset.conts[j].nverts))
820                                         {
821                                                 mergeIdx = j;
822                                                 break;
823                                         }
824                                 }
825                         }
826                         if (mergeIdx == -1)
827                         {
828                                 ctx->log(RC_LOG_WARNING, "rcBuildContours: Could not find merge target for bad contour %d.", i);
829                         }
830                         else
831                         {
832                                 rcContour& mcont = cset.conts[mergeIdx];
833                                 // Merge by closest points.
834                                 int ia = 0, ib = 0;
835                                 getClosestIndices(mcont.verts, mcont.nverts, cont.verts, cont.nverts, ia, ib);
836                                 if (ia == -1 || ib == -1)
837                                 {
838                                         ctx->log(RC_LOG_WARNING, "rcBuildContours: Failed to find merge points for %d and %d.", i, mergeIdx);
839                                         continue;
840                                 }
841                                 if (!mergeContours(mcont, cont, ia, ib))
842                                 {
843                                         ctx->log(RC_LOG_WARNING, "rcBuildContours: Failed to merge contours %d and %d.", i, mergeIdx);
844                                         continue;
845                                 }
846                         }
847                 }
848         }
849         
850         ctx->stopTimer(RC_TIMER_BUILD_CONTOURS);
851         
852         return true;
853 }