RecastArea.cpp 15 KB

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