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