svn merge ^/trunk/blender -r40644:40720
[blender-staging.git] / extern / recastnavigation / Recast / Source / RecastArea.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 /// @par 
30 /// 
31 /// Basically, any spans that are closer to a boundary or obstruction than the specified radius 
32 /// are marked as unwalkable.
33 ///
34 /// This method is usually called immediately after the heightfield has been built.
35 ///
36 /// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius
37 bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf)
38 {
39         rcAssert(ctx);
40         
41         const int w = chf.width;
42         const int h = chf.height;
43         
44         ctx->startTimer(RC_TIMER_ERODE_AREA);
45         
46         unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
47         if (!dist)
48         {
49                 ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount);
50                 return false;
51         }
52         
53         // Init distance.
54         memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount);
55         
56         // Mark boundary cells.
57         for (int y = 0; y < h; ++y)
58         {
59                 for (int x = 0; x < w; ++x)
60                 {
61                         const rcCompactCell& c = chf.cells[x+y*w];
62                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
63                         {
64                                 if (chf.areas[i] == RC_NULL_AREA)
65                                 {
66                                         dist[i] = 0;
67                                 }
68                                 else
69                                 {
70                                         const rcCompactSpan& s = chf.spans[i];
71                                         int nc = 0;
72                                         for (int dir = 0; dir < 4; ++dir)
73                                         {
74                                                 if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
75                                                 {
76                                                         const int nx = x + rcGetDirOffsetX(dir);
77                                                         const int ny = y + rcGetDirOffsetY(dir);
78                                                         const int ni = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir);
79                                                         if (chf.areas[ni] != RC_NULL_AREA)
80                                                         {
81                                                                 nc++;
82                                                         }
83                                                 }
84                                         }
85                                         // At least one missing neighbour.
86                                         if (nc != 4)
87                                                 dist[i] = 0;
88                                 }
89                         }
90                 }
91         }
92         
93         unsigned char nd;
94         
95         // Pass 1
96         for (int y = 0; y < h; ++y)
97         {
98                 for (int x = 0; x < w; ++x)
99                 {
100                         const rcCompactCell& c = chf.cells[x+y*w];
101                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
102                         {
103                                 const rcCompactSpan& s = chf.spans[i];
104                                 
105                                 if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
106                                 {
107                                         // (-1,0)
108                                         const int ax = x + rcGetDirOffsetX(0);
109                                         const int ay = y + rcGetDirOffsetY(0);
110                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
111                                         const rcCompactSpan& as = chf.spans[ai];
112                                         nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
113                                         if (nd < dist[i])
114                                                 dist[i] = nd;
115                                         
116                                         // (-1,-1)
117                                         if (rcGetCon(as, 3) != RC_NOT_CONNECTED)
118                                         {
119                                                 const int aax = ax + rcGetDirOffsetX(3);
120                                                 const int aay = ay + rcGetDirOffsetY(3);
121                                                 const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3);
122                                                 nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
123                                                 if (nd < dist[i])
124                                                         dist[i] = nd;
125                                         }
126                                 }
127                                 if (rcGetCon(s, 3) != RC_NOT_CONNECTED)
128                                 {
129                                         // (0,-1)
130                                         const int ax = x + rcGetDirOffsetX(3);
131                                         const int ay = y + rcGetDirOffsetY(3);
132                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
133                                         const rcCompactSpan& as = chf.spans[ai];
134                                         nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
135                                         if (nd < dist[i])
136                                                 dist[i] = nd;
137                                         
138                                         // (1,-1)
139                                         if (rcGetCon(as, 2) != RC_NOT_CONNECTED)
140                                         {
141                                                 const int aax = ax + rcGetDirOffsetX(2);
142                                                 const int aay = ay + rcGetDirOffsetY(2);
143                                                 const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2);
144                                                 nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
145                                                 if (nd < dist[i])
146                                                         dist[i] = nd;
147                                         }
148                                 }
149                         }
150                 }
151         }
152         
153         // Pass 2
154         for (int y = h-1; y >= 0; --y)
155         {
156                 for (int x = w-1; x >= 0; --x)
157                 {
158                         const rcCompactCell& c = chf.cells[x+y*w];
159                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
160                         {
161                                 const rcCompactSpan& s = chf.spans[i];
162                                 
163                                 if (rcGetCon(s, 2) != RC_NOT_CONNECTED)
164                                 {
165                                         // (1,0)
166                                         const int ax = x + rcGetDirOffsetX(2);
167                                         const int ay = y + rcGetDirOffsetY(2);
168                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2);
169                                         const rcCompactSpan& as = chf.spans[ai];
170                                         nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
171                                         if (nd < dist[i])
172                                                 dist[i] = nd;
173                                         
174                                         // (1,1)
175                                         if (rcGetCon(as, 1) != RC_NOT_CONNECTED)
176                                         {
177                                                 const int aax = ax + rcGetDirOffsetX(1);
178                                                 const int aay = ay + rcGetDirOffsetY(1);
179                                                 const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1);
180                                                 nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
181                                                 if (nd < dist[i])
182                                                         dist[i] = nd;
183                                         }
184                                 }
185                                 if (rcGetCon(s, 1) != RC_NOT_CONNECTED)
186                                 {
187                                         // (0,1)
188                                         const int ax = x + rcGetDirOffsetX(1);
189                                         const int ay = y + rcGetDirOffsetY(1);
190                                         const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1);
191                                         const rcCompactSpan& as = chf.spans[ai];
192                                         nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
193                                         if (nd < dist[i])
194                                                 dist[i] = nd;
195                                         
196                                         // (-1,1)
197                                         if (rcGetCon(as, 0) != RC_NOT_CONNECTED)
198                                         {
199                                                 const int aax = ax + rcGetDirOffsetX(0);
200                                                 const int aay = ay + rcGetDirOffsetY(0);
201                                                 const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0);
202                                                 nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
203                                                 if (nd < dist[i])
204                                                         dist[i] = nd;
205                                         }
206                                 }
207                         }
208                 }
209         }
210         
211         const unsigned char thr = (unsigned char)(radius*2);
212         for (int i = 0; i < chf.spanCount; ++i)
213                 if (dist[i] < thr)
214                         chf.areas[i] = RC_NULL_AREA;
215         
216         rcFree(dist);
217         
218         ctx->stopTimer(RC_TIMER_ERODE_AREA);
219         
220         return true;
221 }
222
223 static void insertSort(unsigned char* a, const int n)
224 {
225         int i, j;
226         for (i = 1; i < n; i++)
227         {
228                 const unsigned char value = a[i];
229                 for (j = i - 1; j >= 0 && a[j] > value; j--)
230                         a[j+1] = a[j];
231                 a[j+1] = value;
232         }
233 }
234
235 /// @par
236 ///
237 /// This filter is usually applied after applying area id's using functions
238 /// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea.
239 /// 
240 /// @see rcCompactHeightfield
241 bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf)
242 {
243         rcAssert(ctx);
244         
245         const int w = chf.width;
246         const int h = chf.height;
247         
248         ctx->startTimer(RC_TIMER_MEDIAN_AREA);
249         
250         unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
251         if (!areas)
252         {
253                 ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount);
254                 return false;
255         }
256         
257         // Init distance.
258         memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount);
259         
260         for (int y = 0; y < h; ++y)
261         {
262                 for (int x = 0; x < w; ++x)
263                 {
264                         const rcCompactCell& c = chf.cells[x+y*w];
265                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
266                         {
267                                 const rcCompactSpan& s = chf.spans[i];
268                                 if (chf.areas[i] == RC_NULL_AREA)
269                                 {
270                                         areas[i] = chf.areas[i];
271                                         continue;
272                                 }
273                                 
274                                 unsigned char nei[9];
275                                 for (int j = 0; j < 9; ++j)
276                                         nei[j] = chf.areas[i];
277                                 
278                                 for (int dir = 0; dir < 4; ++dir)
279                                 {
280                                         if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
281                                         {
282                                                 const int ax = x + rcGetDirOffsetX(dir);
283                                                 const int ay = y + rcGetDirOffsetY(dir);
284                                                 const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
285                                                 if (chf.areas[ai] != RC_NULL_AREA)
286                                                         nei[dir*2+0] = chf.areas[ai];
287                                                 
288                                                 const rcCompactSpan& as = chf.spans[ai];
289                                                 const int dir2 = (dir+1) & 0x3;
290                                                 if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
291                                                 {
292                                                         const int ax2 = ax + rcGetDirOffsetX(dir2);
293                                                         const int ay2 = ay + rcGetDirOffsetY(dir2);
294                                                         const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
295                                                         if (chf.areas[ai2] != RC_NULL_AREA)
296                                                                 nei[dir*2+1] = chf.areas[ai2];
297                                                 }
298                                         }
299                                 }
300                                 insertSort(nei, 9);
301                                 areas[i] = nei[4];
302                         }
303                 }
304         }
305         
306         memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount);
307         
308         rcFree(areas);
309
310         ctx->stopTimer(RC_TIMER_MEDIAN_AREA);
311         
312         return true;
313 }
314
315 /// @par
316 ///
317 /// The value of spacial parameters are in world units.
318 /// 
319 /// @see rcCompactHeightfield, rcMedianFilterWalkableArea
320 void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId,
321                                    rcCompactHeightfield& chf)
322 {
323         rcAssert(ctx);
324         
325         ctx->startTimer(RC_TIMER_MARK_BOX_AREA);
326
327         int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
328         int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
329         int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
330         int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
331         int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
332         int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
333         
334         if (maxx < 0) return;
335         if (minx >= chf.width) return;
336         if (maxz < 0) return;
337         if (minz >= chf.height) return;
338
339         if (minx < 0) minx = 0;
340         if (maxx >= chf.width) maxx = chf.width-1;
341         if (minz < 0) minz = 0;
342         if (maxz >= chf.height) maxz = chf.height-1;    
343         
344         for (int z = minz; z <= maxz; ++z)
345         {
346                 for (int x = minx; x <= maxx; ++x)
347                 {
348                         const rcCompactCell& c = chf.cells[x+z*chf.width];
349                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
350                         {
351                                 rcCompactSpan& s = chf.spans[i];
352                                 if ((int)s.y >= miny && (int)s.y <= maxy)
353                                 {
354                                         if (chf.areas[i] != RC_NULL_AREA)
355                                                 chf.areas[i] = areaId;
356                                 }
357                         }
358                 }
359         }
360
361         ctx->stopTimer(RC_TIMER_MARK_BOX_AREA);
362
363 }
364
365
366 static int pointInPoly(int nvert, const float* verts, const float* p)
367 {
368         int i, j, c = 0;
369         for (i = 0, j = nvert-1; i < nvert; j = i++)
370         {
371                 const float* vi = &verts[i*3];
372                 const float* vj = &verts[j*3];
373                 if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
374                         (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
375                         c = !c;
376         }
377         return c;
378 }
379
380 /// @par
381 ///
382 /// The value of spacial parameters are in world units.
383 /// 
384 /// The y-values of the polygon vertices are ignored. So the polygon is effectively 
385 /// projected onto the xz-plane at @p hmin, then extruded to @p hmax.
386 /// 
387 /// @see rcCompactHeightfield, rcMedianFilterWalkableArea
388 void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
389                                                   const float hmin, const float hmax, unsigned char areaId,
390                                                   rcCompactHeightfield& chf)
391 {
392         rcAssert(ctx);
393         
394         ctx->startTimer(RC_TIMER_MARK_CONVEXPOLY_AREA);
395
396         float bmin[3], bmax[3];
397         rcVcopy(bmin, verts);
398         rcVcopy(bmax, verts);
399         for (int i = 1; i < nverts; ++i)
400         {
401                 rcVmin(bmin, &verts[i*3]);
402                 rcVmax(bmax, &verts[i*3]);
403         }
404         bmin[1] = hmin;
405         bmax[1] = hmax;
406
407         int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
408         int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
409         int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
410         int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
411         int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
412         int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
413         
414         if (maxx < 0) return;
415         if (minx >= chf.width) return;
416         if (maxz < 0) return;
417         if (minz >= chf.height) return;
418         
419         if (minx < 0) minx = 0;
420         if (maxx >= chf.width) maxx = chf.width-1;
421         if (minz < 0) minz = 0;
422         if (maxz >= chf.height) maxz = chf.height-1;    
423         
424         
425         // TODO: Optimize.
426         for (int z = minz; z <= maxz; ++z)
427         {
428                 for (int x = minx; x <= maxx; ++x)
429                 {
430                         const rcCompactCell& c = chf.cells[x+z*chf.width];
431                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
432                         {
433                                 rcCompactSpan& s = chf.spans[i];
434                                 if (chf.areas[i] == RC_NULL_AREA)
435                                         continue;
436                                 if ((int)s.y >= miny && (int)s.y <= maxy)
437                                 {
438                                         float p[3];
439                                         p[0] = chf.bmin[0] + (x+0.5f)*chf.cs; 
440                                         p[1] = 0;
441                                         p[2] = chf.bmin[2] + (z+0.5f)*chf.cs; 
442
443                                         if (pointInPoly(nverts, verts, p))
444                                         {
445                                                 chf.areas[i] = areaId;
446                                         }
447                                 }
448                         }
449                 }
450         }
451
452         ctx->stopTimer(RC_TIMER_MARK_CONVEXPOLY_AREA);
453 }
454
455 /// @par
456 ///
457 /// The value of spacial parameters are in world units.
458 /// 
459 /// @see rcCompactHeightfield, rcMedianFilterWalkableArea
460 void rcMarkCylinderArea(rcContext* ctx, const float* pos,
461                                                 const float r, const float h, unsigned char areaId,
462                                                 rcCompactHeightfield& chf)
463 {
464         rcAssert(ctx);
465         
466         ctx->startTimer(RC_TIMER_MARK_CYLINDER_AREA);
467         
468         float bmin[3], bmax[3];
469         bmin[0] = pos[0] - r;
470         bmin[1] = pos[1];
471         bmin[2] = pos[2] - r;
472         bmax[0] = pos[0] + r;
473         bmax[1] = pos[1] + h;
474         bmax[2] = pos[2] + r;
475         const float r2 = r*r;
476         
477         int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
478         int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
479         int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
480         int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
481         int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
482         int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
483         
484         if (maxx < 0) return;
485         if (minx >= chf.width) return;
486         if (maxz < 0) return;
487         if (minz >= chf.height) return;
488         
489         if (minx < 0) minx = 0;
490         if (maxx >= chf.width) maxx = chf.width-1;
491         if (minz < 0) minz = 0;
492         if (maxz >= chf.height) maxz = chf.height-1;    
493         
494         
495         for (int z = minz; z <= maxz; ++z)
496         {
497                 for (int x = minx; x <= maxx; ++x)
498                 {
499                         const rcCompactCell& c = chf.cells[x+z*chf.width];
500                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
501                         {
502                                 rcCompactSpan& s = chf.spans[i];
503                                 
504                                 if (chf.areas[i] == RC_NULL_AREA)
505                                         continue;
506                                 
507                                 if ((int)s.y >= miny && (int)s.y <= maxy)
508                                 {
509                                         const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; 
510                                         const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; 
511                                         const float dx = sx - pos[0];
512                                         const float dz = sz - pos[2];
513                                         
514                                         if (dx*dx + dz*dz < r2)
515                                         {
516                                                 chf.areas[i] = areaId;
517                                         }
518                                 }
519                         }
520                 }
521         }
522         
523         ctx->stopTimer(RC_TIMER_MARK_CYLINDER_AREA);
524 }