svn merge ^/trunk/blender -r40644:40720
[blender-staging.git] / extern / recastnavigation / Recast / Source / RecastLayers.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 #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 "RecastAlloc.h"
27 #include "RecastAssert.h"
28
29
30 static const int RC_MAX_LAYERS = RC_NOT_CONNECTED;
31 static const int RC_MAX_NEIS = 16;
32
33 struct rcLayerRegion
34 {
35         unsigned char layers[RC_MAX_LAYERS];
36         unsigned char neis[RC_MAX_NEIS];
37         unsigned short ymin, ymax;
38         unsigned char layerId;          // Layer ID
39         unsigned char nlayers;          // Layer count
40         unsigned char nneis;            // Neighbour count
41         unsigned char base;                     // Flag indicating if the region is hte base of merged regions.
42 };
43
44
45 static void addUnique(unsigned char* a, unsigned char& an, unsigned char v)
46 {
47         const int n = (int)an;
48         for (int i = 0; i < n; ++i)
49                 if (a[i] == v)
50                         return;
51         a[an] = v;
52         an++;
53 }
54
55 static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v)
56 {
57         const int n = (int)an;
58         for (int i = 0; i < n; ++i)
59                 if (a[i] == v)
60                         return true;
61         return false;
62 }
63
64 inline bool overlapRange(const unsigned short amin, const unsigned short amax,
65                                                  const unsigned short bmin, const unsigned short bmax)
66 {
67         return (amin > bmax || amax < bmin) ? false : true;
68 }
69
70
71
72 struct rcLayerSweepSpan
73 {
74         unsigned short ns;      // number samples
75         unsigned char id;       // region id
76         unsigned char nei;      // neighbour id
77 };
78
79 /// @par
80 /// 
81 /// See the #rcConfig documentation for more information on the configuration parameters.
82 /// 
83 /// @see rcAllocHeightfieldLayerSet, rcCompactHeightfield, rcHeightfieldLayerSet, rcConfig
84 bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf,
85                                                           const int borderSize, const int walkableHeight,
86                                                           rcHeightfieldLayerSet& lset)
87 {
88         rcAssert(ctx);
89         
90         ctx->startTimer(RC_TIMER_BUILD_LAYERS);
91         
92         const int w = chf.width;
93         const int h = chf.height;
94         
95         rcScopedDelete<unsigned char> srcReg = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
96         if (!srcReg)
97         {
98                 ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'srcReg' (%d).", chf.spanCount);
99                 return false;
100         }
101         memset(srcReg,0xff,sizeof(unsigned char)*chf.spanCount);
102         
103         const int nsweeps = chf.width;
104         rcScopedDelete<rcLayerSweepSpan> sweeps = (rcLayerSweepSpan*)rcAlloc(sizeof(rcLayerSweepSpan)*nsweeps, RC_ALLOC_TEMP);
105         if (!sweeps)
106         {
107                 ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'sweeps' (%d).", nsweeps);
108                 return false;
109         }
110         
111         
112         // Partition walkable area into monotone regions.
113         int prevCount[256];
114         unsigned char regId = 0;
115
116         for (int y = borderSize; y < h-borderSize; ++y)
117         {
118                 memset(prevCount,0,sizeof(int)*regId);
119                 unsigned char sweepId = 0;
120                 
121                 for (int x = borderSize; x < w-borderSize; ++x)
122                 {
123                         const rcCompactCell& c = chf.cells[x+y*w];
124                         
125                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
126                         {
127                                 const rcCompactSpan& s = chf.spans[i];
128                                 if (chf.areas[i] == RC_NULL_AREA) continue;
129
130                                 unsigned char sid = 0xff;
131
132                                 // -x
133                                 if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
134                                 {
135                                         const int ax = x + rcGetDirOffsetX(0);
136                                         const int ay = y + rcGetDirOffsetY(0);
137                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
138                                         if (chf.areas[ai] != RC_NULL_AREA && srcReg[ai] != 0xff)
139                                                 sid = srcReg[ai];
140                                 }
141                                 
142                                 if (sid == 0xff)
143                                 {
144                                         sid = sweepId++;
145                                         sweeps[sid].nei = 0xff;
146                                         sweeps[sid].ns = 0;
147                                 }
148                                 
149                                 // -y
150                                 if (rcGetCon(s,3) != RC_NOT_CONNECTED)
151                                 {
152                                         const int ax = x + rcGetDirOffsetX(3);
153                                         const int ay = y + rcGetDirOffsetY(3);
154                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
155                                         const unsigned char nr = srcReg[ai];
156                                         if (nr != 0xff)
157                                         {
158                                                 // Set neighbour when first valid neighbour is encoutered.
159                                                 if (sweeps[sid].ns == 0)
160                                                         sweeps[sid].nei = nr;
161                                                 
162                                                 if (sweeps[sid].nei == nr)
163                                                 {
164                                                         // Update existing neighbour
165                                                         sweeps[sid].ns++;
166                                                         prevCount[nr]++;
167                                                 }
168                                                 else
169                                                 {
170                                                         // This is hit if there is nore than one neighbour.
171                                                         // Invalidate the neighbour.
172                                                         sweeps[sid].nei = 0xff;
173                                                 }
174                                         }
175                                 }
176                                 
177                                 srcReg[i] = sid;
178                         }
179                 }
180                 
181                 // Create unique ID.
182                 for (int i = 0; i < sweepId; ++i)
183                 {
184                         // If the neighbour is set and there is only one continuous connection to it,
185                         // the sweep will be merged with the previous one, else new region is created.
186                         if (sweeps[i].nei != 0xff && prevCount[sweeps[i].nei] == (int)sweeps[i].ns)
187                         {
188                                 sweeps[i].id = sweeps[i].nei;
189                         }
190                         else
191                         {
192                                 if (regId == 255)
193                                 {
194                                         ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Region ID overflow.");
195                                         return false;
196                                 }
197                                 sweeps[i].id = regId++;
198                         }
199                 }
200                 
201                 // Remap local sweep ids to region ids.
202                 for (int x = borderSize; x < w-borderSize; ++x)
203                 {
204                         const rcCompactCell& c = chf.cells[x+y*w];
205                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
206                         {
207                                 if (srcReg[i] != 0xff)
208                                         srcReg[i] = sweeps[srcReg[i]].id;
209                         }
210                 }
211         }
212
213         // Allocate and init layer regions.
214         const int nregs = (int)regId;
215         rcScopedDelete<rcLayerRegion> regs = (rcLayerRegion*)rcAlloc(sizeof(rcLayerRegion)*nregs, RC_ALLOC_TEMP);
216         if (!regs)
217         {
218                 ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'regs' (%d).", nregs);
219                 return false;
220         }
221         memset(regs, 0, sizeof(rcLayerRegion)*nregs);
222         for (int i = 0; i < nregs; ++i)
223         {
224                 regs[i].layerId = 0xff;
225                 regs[i].ymin = 0xffff;
226                 regs[i].ymax = 0;
227         }
228         
229         // Find region neighbours and overlapping regions.
230         for (int y = 0; y < h; ++y)
231         {
232                 for (int x = 0; x < w; ++x)
233                 {
234                         const rcCompactCell& c = chf.cells[x+y*w];
235                         
236                         unsigned char lregs[RC_MAX_LAYERS];
237                         int nlregs = 0;
238                         
239                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
240                         {
241                                 const rcCompactSpan& s = chf.spans[i];
242                                 const unsigned char ri = srcReg[i];
243                                 if (ri == 0xff) continue;
244                                 
245                                 regs[ri].ymin = rcMin(regs[ri].ymin, s.y);
246                                 regs[ri].ymax = rcMax(regs[ri].ymax, s.y);
247                                 
248                                 // Collect all region layers.
249                                 if (nlregs < RC_MAX_LAYERS)
250                                         lregs[nlregs++] = ri;
251                                 
252                                 // Update neighbours
253                                 for (int dir = 0; dir < 4; ++dir)
254                                 {
255                                         if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
256                                         {
257                                                 const int ax = x + rcGetDirOffsetX(dir);
258                                                 const int ay = y + rcGetDirOffsetY(dir);
259                                                 const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
260                                                 const unsigned char rai = srcReg[ai];
261                                                 if (rai != 0xff && rai != ri)
262                                                         addUnique(regs[ri].neis, regs[ri].nneis, rai);
263                                         }
264                                 }
265                                 
266                         }
267                         
268                         // Update overlapping regions.
269                         for (int i = 0; i < nlregs-1; ++i)
270                         {
271                                 for (int j = i+1; j < nlregs; ++j)
272                                 {
273                                         if (lregs[i] != lregs[j])
274                                         {
275                                                 rcLayerRegion& ri = regs[lregs[i]];
276                                                 rcLayerRegion& rj = regs[lregs[j]];
277                                                 addUnique(ri.layers, ri.nlayers, lregs[j]);
278                                                 addUnique(rj.layers, rj.nlayers, lregs[i]);
279                                         }
280                                 }
281                         }
282                         
283                 }
284         }
285         
286         // Create 2D layers from regions.
287         unsigned char layerId = 0;
288         
289         static const int MAX_STACK = 64;
290         unsigned char stack[MAX_STACK];
291         int nstack = 0;
292         
293         for (int i = 0; i < nregs; ++i)
294         {
295                 rcLayerRegion& root = regs[i];
296                 // Skip alreadu visited.
297                 if (root.layerId != 0xff)
298                         continue;
299
300                 // Start search.
301                 root.layerId = layerId;
302                 root.base = 1;
303                 
304                 nstack = 0;
305                 stack[nstack++] = (unsigned char)i;
306                 
307                 while (nstack)
308                 {
309                         // Pop front
310                         rcLayerRegion& reg = regs[stack[0]];
311                         nstack--;
312                         for (int j = 0; j < nstack; ++j)
313                                 stack[j] = stack[j+1];
314                         
315                         const int nneis = (int)reg.nneis;
316                         for (int j = 0; j < nneis; ++j)
317                         {
318                                 const unsigned char nei = reg.neis[j];
319                                 rcLayerRegion& regn = regs[nei];
320                                 // Skip already visited.
321                                 if (regn.layerId != 0xff)
322                                         continue;
323                                 // Skip if the neighbour is overlapping root region.
324                                 if (contains(root.layers, root.nlayers, nei))
325                                         continue;
326                                 // Skip if the height range would become too large.
327                                 const int ymin = rcMin(root.ymin, regn.ymin);
328                                 const int ymax = rcMin(root.ymax, regn.ymax);
329                                 if ((ymax - ymin) >= 255)
330                                          continue;
331
332                                 if (nstack < MAX_STACK)
333                                 {
334                                         // Deepen
335                                         stack[nstack++] = (unsigned char)nei;
336                                         
337                                         // Mark layer id
338                                         regn.layerId = layerId;
339                                         // Merge current layers to root.
340                                         for (int k = 0; k < regn.nlayers; ++k)
341                                                 addUnique(root.layers, root.nlayers, regn.layers[k]);
342                                         root.ymin = rcMin(root.ymin, regn.ymin);
343                                         root.ymax = rcMax(root.ymax, regn.ymax);
344                                 }
345                         }
346                 }
347                 
348                 layerId++;
349         }
350         
351         // Merge non-overlapping regions that are close in height.
352         const unsigned short mergeHeight = (unsigned short)walkableHeight * 4;
353         
354         for (int i = 0; i < nregs; ++i)
355         {
356                 rcLayerRegion& ri = regs[i];
357                 if (!ri.base) continue;
358                 
359                 unsigned char newId = ri.layerId;
360                 
361                 for (;;)
362                 {
363                         unsigned char oldId = 0xff;
364                         
365                         for (int j = 0; j < nregs; ++j)
366                         {
367                                 if (i == j) continue;
368                                 rcLayerRegion& rj = regs[j];
369                                 if (!rj.base) continue;
370                                 
371                                 // Skip if teh regions are not close to each other.
372                                 if (!overlapRange(ri.ymin,ri.ymax+mergeHeight, rj.ymin,rj.ymax+mergeHeight))
373                                         continue;
374                                 // Skip if the height range would become too large.
375                                 const int ymin = rcMin(ri.ymin, rj.ymin);
376                                 const int ymax = rcMin(ri.ymax, rj.ymax);
377                                 if ((ymax - ymin) >= 255)
378                                   continue;
379                                                   
380                                 // Make sure that there is no overlap when mergin 'ri' and 'rj'.
381                                 bool overlap = false;
382                                 // Iterate over all regions which have the same layerId as 'rj'
383                                 for (int k = 0; k < nregs; ++k)
384                                 {
385                                         if (regs[k].layerId != rj.layerId)
386                                                 continue;
387                                         // Check if region 'k' is overlapping region 'ri'
388                                         // Index to 'regs' is the same as region id.
389                                         if (contains(ri.layers,ri.nlayers, (unsigned char)k))
390                                         {
391                                                 overlap = true;
392                                                 break;
393                                         }
394                                 }
395                                 // Cannot merge of regions overlap.
396                                 if (overlap)
397                                         continue;
398                                 
399                                 // Can merge i and j.
400                                 oldId = rj.layerId;
401                                 break;
402                         }
403                         
404                         // Could not find anything to merge with, stop.
405                         if (oldId == 0xff)
406                                 break;
407                         
408                         // Merge
409                         for (int j = 0; j < nregs; ++j)
410                         {
411                                 rcLayerRegion& rj = regs[j];
412                                 if (rj.layerId == oldId)
413                                 {
414                                         rj.base = 0;
415                                         // Remap layerIds.
416                                         rj.layerId = newId;
417                                         // Add overlaid layers from 'rj' to 'ri'.
418                                         for (int k = 0; k < rj.nlayers; ++k)
419                                                 addUnique(ri.layers, ri.nlayers, rj.layers[k]);
420                                         // Update heigh bounds.
421                                         ri.ymin = rcMin(ri.ymin, rj.ymin);
422                                         ri.ymax = rcMax(ri.ymax, rj.ymax);
423                                 }
424                         }
425                 }
426         }
427         
428         // Compact layerIds
429         unsigned char remap[256];
430         memset(remap, 0, 256);
431
432         // Find number of unique layers.
433         layerId = 0;
434         for (int i = 0; i < nregs; ++i)
435                 remap[regs[i].layerId] = 1;
436         for (int i = 0; i < 256; ++i)
437         {
438                 if (remap[i])
439                         remap[i] = layerId++;
440                 else
441                         remap[i] = 0xff;
442         }
443         // Remap ids.
444         for (int i = 0; i < nregs; ++i)
445                 regs[i].layerId = remap[regs[i].layerId];
446         
447         // No layers, return empty.
448         if (layerId == 0)
449         {
450                 ctx->stopTimer(RC_TIMER_BUILD_LAYERS);
451                 return true;
452         }
453         
454         // Create layers.
455         rcAssert(lset.layers == 0);
456         
457         const int lw = w - borderSize*2;
458         const int lh = h - borderSize*2;
459
460         // Build contracted bbox for layers.
461         float bmin[3], bmax[3];
462         rcVcopy(bmin, chf.bmin);
463         rcVcopy(bmax, chf.bmax);
464         bmin[0] += borderSize*chf.cs;
465         bmin[2] += borderSize*chf.cs;
466         bmax[0] -= borderSize*chf.cs;
467         bmax[2] -= borderSize*chf.cs;
468         
469         lset.nlayers = (int)layerId;
470         
471         lset.layers = (rcHeightfieldLayer*)rcAlloc(sizeof(rcHeightfieldLayer)*lset.nlayers, RC_ALLOC_PERM);
472         if (!lset.layers)
473         {
474                 ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'layers' (%d).", lset.nlayers);
475                 return false;
476         }
477         memset(lset.layers, 0, sizeof(rcHeightfieldLayer)*lset.nlayers);
478
479         
480         // Store layers.
481         for (int i = 0; i < lset.nlayers; ++i)
482         {
483                 unsigned char curId = (unsigned char)i;
484                 
485                 // Allocate memory for the current layer.
486                 rcHeightfieldLayer* layer = &lset.layers[i];
487                 memset(layer, 0, sizeof(rcHeightfieldLayer));
488
489                 const int gridSize = sizeof(unsigned char)*lw*lh;
490
491                 layer->heights = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
492                 if (!layer->heights)
493                 {
494                         ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'heights' (%d).", gridSize);
495                         return false;
496                 }
497                 memset(layer->heights, 0xff, gridSize);
498
499                 layer->areas = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
500                 if (!layer->areas)
501                 {
502                         ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'areas' (%d).", gridSize);
503                         return false;
504                 }
505                 memset(layer->areas, 0, gridSize);
506
507                 layer->cons = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
508                 if (!layer->cons)
509                 {
510                         ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'cons' (%d).", gridSize);
511                         return false;
512                 }
513                 memset(layer->cons, 0, gridSize);
514                 
515                 // Find layer height bounds.
516                 int hmin = 0, hmax = 0;
517                 for (int j = 0; j < nregs; ++j)
518                 {
519                         if (regs[j].base && regs[j].layerId == curId)
520                         {
521                                 hmin = (int)regs[j].ymin;
522                                 hmax = (int)regs[j].ymax;
523                         }
524                 }
525
526                 layer->width = lw;
527                 layer->height = lh;
528                 layer->cs = chf.cs;
529                 layer->ch = chf.ch;
530                 
531                 // Adjust the bbox to fit the heighfield.
532                 rcVcopy(layer->bmin, bmin);
533                 rcVcopy(layer->bmax, bmax);
534                 layer->bmin[1] = bmin[1] + hmin*chf.ch;
535                 layer->bmax[1] = bmin[1] + hmax*chf.ch;
536                 layer->hmin = hmin;
537                 layer->hmax = hmax;
538
539                 // Update usable data region.
540                 layer->minx = layer->width;
541                 layer->maxx = 0;
542                 layer->miny = layer->height;
543                 layer->maxy = 0;
544                 
545                 // Copy height and area from compact heighfield. 
546                 for (int y = 0; y < lh; ++y)
547                 {
548                         for (int x = 0; x < lw; ++x)
549                         {
550                                 const int cx = borderSize+x;
551                                 const int cy = borderSize+y;
552                                 const rcCompactCell& c = chf.cells[cx+cy*w];
553                                 for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
554                                 {
555                                         const rcCompactSpan& s = chf.spans[i];
556                                         // Skip unassigned regions.
557                                         if (srcReg[i] == 0xff)
558                                                 continue;
559                                         // Skip of does nto belong to current layer.
560                                         unsigned char lid = regs[srcReg[i]].layerId;
561                                         if (lid != curId)
562                                                 continue;
563                                         
564                                         // Update data bounds.
565                                         layer->minx = rcMin(layer->minx, x);
566                                         layer->maxx = rcMax(layer->maxx, x);
567                                         layer->miny = rcMin(layer->miny, y);
568                                         layer->maxy = rcMax(layer->maxy, y);
569                                         
570                                         // Store height and area type.
571                                         const int idx = x+y*lw;
572                                         layer->heights[idx] = (unsigned char)(s.y - hmin);
573                                         layer->areas[idx] = chf.areas[i];
574                                         
575                                         // Check connection.
576                                         unsigned char portal = 0;
577                                         unsigned char con = 0;
578                                         for (int dir = 0; dir < 4; ++dir)
579                                         {
580                                                 if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
581                                                 {
582                                                         const int ax = cx + rcGetDirOffsetX(dir);
583                                                         const int ay = cy + rcGetDirOffsetY(dir);
584                                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
585                                                         unsigned char alid = srcReg[ai] != 0xff ? regs[srcReg[ai]].layerId : 0xff;
586                                                         // Portal mask
587                                                         if (chf.areas[ai] != RC_NULL_AREA && lid != alid)
588                                                         {
589                                                                 portal |= (unsigned char)(1<<dir);
590                                                                 // Update height so that it matches on both sides of the portal.
591                                                                 const rcCompactSpan& as = chf.spans[ai];
592                                                                 if (as.y > hmin)
593                                                                         layer->heights[idx] = rcMax(layer->heights[idx], (unsigned char)(as.y - hmin));
594                                                         }
595                                                         // Valid connection mask
596                                                         if (chf.areas[ai] != RC_NULL_AREA && lid == alid)
597                                                         {
598                                                                 const int nx = ax - borderSize;
599                                                                 const int ny = ay - borderSize;
600                                                                 if (nx >= 0 && ny >= 0 && nx < lw && ny < lh)
601                                                                         con |= (unsigned char)(1<<dir);
602                                                         }
603                                                 }
604                                         }
605                                         
606                                         layer->cons[idx] = (portal << 4) | con;
607                                 }
608                         }
609                 }
610                 
611                 if (layer->minx > layer->maxx)
612                         layer->minx = layer->maxx = 0;
613                 if (layer->miny > layer->maxy)
614                         layer->miny = layer->maxy = 0;
615         }
616         
617         ctx->stopTimer(RC_TIMER_BUILD_LAYERS);
618         
619         return true;
620 }