9
3

Recast.cpp 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575
  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 <stdarg.h>
  25. #include "Recast.h"
  26. #include "RecastAlloc.h"
  27. #include "RecastAssert.h"
  28. namespace
  29. {
  30. /// Allocates and constructs an object of the given type, returning a pointer.
  31. /// TODO: Support constructor args.
  32. /// @param[in] hint Hint to the allocator.
  33. template <typename T>
  34. T* rcNew(rcAllocHint hint) {
  35. T* ptr = (T*)rcAlloc(sizeof(T), hint);
  36. ::new(rcNewTag(), (void*)ptr) T();
  37. return ptr;
  38. }
  39. /// Destroys and frees an object allocated with rcNew.
  40. /// @param[in] ptr The object pointer to delete.
  41. template <typename T>
  42. void rcDelete(T* ptr) {
  43. if (ptr) {
  44. ptr->~T();
  45. rcFree((void*)ptr);
  46. }
  47. }
  48. } // namespace
  49. float rcSqrt(float x)
  50. {
  51. return sqrtf(x);
  52. }
  53. /// @class rcContext
  54. /// @par
  55. ///
  56. /// This class does not provide logging or timer functionality on its
  57. /// own. Both must be provided by a concrete implementation
  58. /// by overriding the protected member functions. Also, this class does not
  59. /// provide an interface for extracting log messages. (Only adding them.)
  60. /// So concrete implementations must provide one.
  61. ///
  62. /// If no logging or timers are required, just pass an instance of this
  63. /// class through the Recast build process.
  64. ///
  65. /// @par
  66. ///
  67. /// Example:
  68. /// @code
  69. /// // Where ctx is an instance of rcContext and filepath is a char array.
  70. /// ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath);
  71. /// @endcode
  72. void rcContext::log(const rcLogCategory category, const char* format, ...)
  73. {
  74. if (!m_logEnabled)
  75. return;
  76. static const int MSG_SIZE = 512;
  77. char msg[MSG_SIZE];
  78. va_list ap;
  79. va_start(ap, format);
  80. int len = vsnprintf(msg, MSG_SIZE, format, ap);
  81. if (len >= MSG_SIZE)
  82. {
  83. len = MSG_SIZE-1;
  84. msg[MSG_SIZE-1] = '\0';
  85. }
  86. va_end(ap);
  87. doLog(category, msg, len);
  88. }
  89. rcHeightfield* rcAllocHeightfield()
  90. {
  91. return rcNew<rcHeightfield>(RC_ALLOC_PERM);
  92. }
  93. rcHeightfield::rcHeightfield()
  94. : width()
  95. , height()
  96. , bmin()
  97. , bmax()
  98. , cs()
  99. , ch()
  100. , spans()
  101. , pools()
  102. , freelist()
  103. {
  104. }
  105. rcHeightfield::~rcHeightfield()
  106. {
  107. // Delete span array.
  108. rcFree(spans);
  109. // Delete span pools.
  110. while (pools)
  111. {
  112. rcSpanPool* next = pools->next;
  113. rcFree(pools);
  114. pools = next;
  115. }
  116. }
  117. void rcFreeHeightField(rcHeightfield* hf)
  118. {
  119. rcDelete(hf);
  120. }
  121. rcCompactHeightfield* rcAllocCompactHeightfield()
  122. {
  123. return rcNew<rcCompactHeightfield>(RC_ALLOC_PERM);
  124. }
  125. void rcFreeCompactHeightfield(rcCompactHeightfield* chf)
  126. {
  127. rcDelete(chf);
  128. }
  129. rcCompactHeightfield::rcCompactHeightfield()
  130. : width(),
  131. height(),
  132. spanCount(),
  133. walkableHeight(),
  134. walkableClimb(),
  135. borderSize(),
  136. maxDistance(),
  137. maxRegions(),
  138. bmin(),
  139. bmax(),
  140. cs(),
  141. ch(),
  142. cells(),
  143. spans(),
  144. dist(),
  145. areas()
  146. {
  147. }
  148. rcCompactHeightfield::~rcCompactHeightfield()
  149. {
  150. rcFree(cells);
  151. rcFree(spans);
  152. rcFree(dist);
  153. rcFree(areas);
  154. }
  155. rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
  156. {
  157. return rcNew<rcHeightfieldLayerSet>(RC_ALLOC_PERM);
  158. }
  159. void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset)
  160. {
  161. rcDelete(lset);
  162. }
  163. rcHeightfieldLayerSet::rcHeightfieldLayerSet()
  164. : layers(), nlayers() {}
  165. rcHeightfieldLayerSet::~rcHeightfieldLayerSet()
  166. {
  167. for (int i = 0; i < nlayers; ++i)
  168. {
  169. rcFree(layers[i].heights);
  170. rcFree(layers[i].areas);
  171. rcFree(layers[i].cons);
  172. }
  173. rcFree(layers);
  174. }
  175. rcContourSet* rcAllocContourSet()
  176. {
  177. return rcNew<rcContourSet>(RC_ALLOC_PERM);
  178. }
  179. void rcFreeContourSet(rcContourSet* cset)
  180. {
  181. rcDelete(cset);
  182. }
  183. rcContourSet::rcContourSet()
  184. : conts(),
  185. nconts(),
  186. bmin(),
  187. bmax(),
  188. cs(),
  189. ch(),
  190. width(),
  191. height(),
  192. borderSize(),
  193. maxError() {}
  194. rcContourSet::~rcContourSet()
  195. {
  196. for (int i = 0; i < nconts; ++i)
  197. {
  198. rcFree(conts[i].verts);
  199. rcFree(conts[i].rverts);
  200. }
  201. rcFree(conts);
  202. }
  203. rcPolyMesh* rcAllocPolyMesh()
  204. {
  205. return rcNew<rcPolyMesh>(RC_ALLOC_PERM);
  206. }
  207. void rcFreePolyMesh(rcPolyMesh* pmesh)
  208. {
  209. rcDelete(pmesh);
  210. }
  211. rcPolyMesh::rcPolyMesh()
  212. : verts(),
  213. polys(),
  214. regs(),
  215. flags(),
  216. areas(),
  217. nverts(),
  218. npolys(),
  219. maxpolys(),
  220. nvp(),
  221. bmin(),
  222. bmax(),
  223. cs(),
  224. ch(),
  225. borderSize(),
  226. maxEdgeError() {}
  227. rcPolyMesh::~rcPolyMesh()
  228. {
  229. rcFree(verts);
  230. rcFree(polys);
  231. rcFree(regs);
  232. rcFree(flags);
  233. rcFree(areas);
  234. }
  235. rcPolyMeshDetail* rcAllocPolyMeshDetail()
  236. {
  237. rcPolyMeshDetail* dmesh = (rcPolyMeshDetail*)rcAlloc(sizeof(rcPolyMeshDetail), RC_ALLOC_PERM);
  238. memset(dmesh, 0, sizeof(rcPolyMeshDetail));
  239. return dmesh;
  240. }
  241. void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh)
  242. {
  243. if (!dmesh) return;
  244. rcFree(dmesh->meshes);
  245. rcFree(dmesh->verts);
  246. rcFree(dmesh->tris);
  247. rcFree(dmesh);
  248. }
  249. void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
  250. {
  251. // Calculate bounding box.
  252. rcVcopy(bmin, verts);
  253. rcVcopy(bmax, verts);
  254. for (int i = 1; i < nv; ++i)
  255. {
  256. const float* v = &verts[i*3];
  257. rcVmin(bmin, v);
  258. rcVmax(bmax, v);
  259. }
  260. }
  261. void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h)
  262. {
  263. *w = (int)((bmax[0] - bmin[0])/cs+0.5f);
  264. *h = (int)((bmax[2] - bmin[2])/cs+0.5f);
  265. }
  266. /// @par
  267. ///
  268. /// See the #rcConfig documentation for more information on the configuration parameters.
  269. ///
  270. /// @see rcAllocHeightfield, rcHeightfield
  271. bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height,
  272. const float* bmin, const float* bmax,
  273. float cs, float ch)
  274. {
  275. rcIgnoreUnused(ctx);
  276. hf.width = width;
  277. hf.height = height;
  278. rcVcopy(hf.bmin, bmin);
  279. rcVcopy(hf.bmax, bmax);
  280. hf.cs = cs;
  281. hf.ch = ch;
  282. hf.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*)*hf.width*hf.height, RC_ALLOC_PERM);
  283. if (!hf.spans)
  284. return false;
  285. memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height);
  286. return true;
  287. }
  288. static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm)
  289. {
  290. float e0[3], e1[3];
  291. rcVsub(e0, v1, v0);
  292. rcVsub(e1, v2, v0);
  293. rcVcross(norm, e0, e1);
  294. rcVnormalize(norm);
  295. }
  296. /// @par
  297. ///
  298. /// Only sets the area id's for the walkable triangles. Does not alter the
  299. /// area id's for unwalkable triangles.
  300. ///
  301. /// See the #rcConfig documentation for more information on the configuration parameters.
  302. ///
  303. /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
  304. void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
  305. const float* verts, int nv,
  306. const int* tris, int nt,
  307. unsigned char* areas)
  308. {
  309. rcIgnoreUnused(ctx);
  310. rcIgnoreUnused(nv);
  311. const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
  312. float norm[3];
  313. for (int i = 0; i < nt; ++i)
  314. {
  315. const int* tri = &tris[i*3];
  316. calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
  317. // Check if the face is walkable.
  318. if (norm[1] > walkableThr)
  319. areas[i] = RC_WALKABLE_AREA;
  320. }
  321. }
  322. /// @par
  323. ///
  324. /// Only sets the area id's for the unwalkable triangles. Does not alter the
  325. /// area id's for walkable triangles.
  326. ///
  327. /// See the #rcConfig documentation for more information on the configuration parameters.
  328. ///
  329. /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
  330. void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
  331. const float* verts, int /*nv*/,
  332. const int* tris, int nt,
  333. unsigned char* areas)
  334. {
  335. rcIgnoreUnused(ctx);
  336. const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
  337. float norm[3];
  338. for (int i = 0; i < nt; ++i)
  339. {
  340. const int* tri = &tris[i*3];
  341. calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
  342. // Check if the face is walkable.
  343. if (norm[1] <= walkableThr)
  344. areas[i] = RC_NULL_AREA;
  345. }
  346. }
  347. int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf)
  348. {
  349. rcIgnoreUnused(ctx);
  350. const int w = hf.width;
  351. const int h = hf.height;
  352. int spanCount = 0;
  353. for (int y = 0; y < h; ++y)
  354. {
  355. for (int x = 0; x < w; ++x)
  356. {
  357. for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next)
  358. {
  359. if (s->area != RC_NULL_AREA)
  360. spanCount++;
  361. }
  362. }
  363. }
  364. return spanCount;
  365. }
  366. /// @par
  367. ///
  368. /// This is just the beginning of the process of fully building a compact heightfield.
  369. /// Various filters may be applied, then the distance field and regions built.
  370. /// E.g: #rcBuildDistanceField and #rcBuildRegions
  371. ///
  372. /// See the #rcConfig documentation for more information on the configuration parameters.
  373. ///
  374. /// @see rcAllocCompactHeightfield, rcHeightfield, rcCompactHeightfield, rcConfig
  375. bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb,
  376. rcHeightfield& hf, rcCompactHeightfield& chf)
  377. {
  378. rcAssert(ctx);
  379. rcScopedTimer timer(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
  380. const int w = hf.width;
  381. const int h = hf.height;
  382. const int spanCount = rcGetHeightFieldSpanCount(ctx, hf);
  383. // Fill in header.
  384. chf.width = w;
  385. chf.height = h;
  386. chf.spanCount = spanCount;
  387. chf.walkableHeight = walkableHeight;
  388. chf.walkableClimb = walkableClimb;
  389. chf.maxRegions = 0;
  390. rcVcopy(chf.bmin, hf.bmin);
  391. rcVcopy(chf.bmax, hf.bmax);
  392. chf.bmax[1] += walkableHeight*hf.ch;
  393. chf.cs = hf.cs;
  394. chf.ch = hf.ch;
  395. chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*w*h, RC_ALLOC_PERM);
  396. if (!chf.cells)
  397. {
  398. ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h);
  399. return false;
  400. }
  401. memset(chf.cells, 0, sizeof(rcCompactCell)*w*h);
  402. chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*spanCount, RC_ALLOC_PERM);
  403. if (!chf.spans)
  404. {
  405. ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
  406. return false;
  407. }
  408. memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount);
  409. chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*spanCount, RC_ALLOC_PERM);
  410. if (!chf.areas)
  411. {
  412. ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount);
  413. return false;
  414. }
  415. memset(chf.areas, RC_NULL_AREA, sizeof(unsigned char)*spanCount);
  416. const int MAX_HEIGHT = 0xffff;
  417. // Fill in cells and spans.
  418. int idx = 0;
  419. for (int y = 0; y < h; ++y)
  420. {
  421. for (int x = 0; x < w; ++x)
  422. {
  423. const rcSpan* s = hf.spans[x + y*w];
  424. // If there are no spans at this cell, just leave the data to index=0, count=0.
  425. if (!s) continue;
  426. rcCompactCell& c = chf.cells[x+y*w];
  427. c.index = idx;
  428. c.count = 0;
  429. while (s)
  430. {
  431. if (s->area != RC_NULL_AREA)
  432. {
  433. const int bot = (int)s->smax;
  434. const int top = s->next ? (int)s->next->smin : MAX_HEIGHT;
  435. chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff);
  436. chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
  437. chf.areas[idx] = s->area;
  438. idx++;
  439. c.count++;
  440. }
  441. s = s->next;
  442. }
  443. }
  444. }
  445. // Find neighbour connections.
  446. const int MAX_LAYERS = RC_NOT_CONNECTED-1;
  447. int tooHighNeighbour = 0;
  448. for (int y = 0; y < h; ++y)
  449. {
  450. for (int x = 0; x < w; ++x)
  451. {
  452. const rcCompactCell& c = chf.cells[x+y*w];
  453. for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
  454. {
  455. rcCompactSpan& s = chf.spans[i];
  456. for (int dir = 0; dir < 4; ++dir)
  457. {
  458. rcSetCon(s, dir, RC_NOT_CONNECTED);
  459. const int nx = x + rcGetDirOffsetX(dir);
  460. const int ny = y + rcGetDirOffsetY(dir);
  461. // First check that the neighbour cell is in bounds.
  462. if (nx < 0 || ny < 0 || nx >= w || ny >= h)
  463. continue;
  464. // Iterate over all neighbour spans and check if any of the is
  465. // accessible from current cell.
  466. const rcCompactCell& nc = chf.cells[nx+ny*w];
  467. for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k)
  468. {
  469. const rcCompactSpan& ns = chf.spans[k];
  470. const int bot = rcMax(s.y, ns.y);
  471. const int top = rcMin(s.y+s.h, ns.y+ns.h);
  472. // Check that the gap between the spans is walkable,
  473. // and that the climb height between the gaps is not too high.
  474. if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
  475. {
  476. // Mark direction as walkable.
  477. const int lidx = k - (int)nc.index;
  478. if (lidx < 0 || lidx > MAX_LAYERS)
  479. {
  480. tooHighNeighbour = rcMax(tooHighNeighbour, lidx);
  481. continue;
  482. }
  483. rcSetCon(s, dir, lidx);
  484. break;
  485. }
  486. }
  487. }
  488. }
  489. }
  490. }
  491. if (tooHighNeighbour > MAX_LAYERS)
  492. {
  493. ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)",
  494. tooHighNeighbour, MAX_LAYERS);
  495. }
  496. return true;
  497. }
  498. /*
  499. static int getHeightfieldMemoryUsage(const rcHeightfield& hf)
  500. {
  501. int size = 0;
  502. size += sizeof(hf);
  503. size += hf.width * hf.height * sizeof(rcSpan*);
  504. rcSpanPool* pool = hf.pools;
  505. while (pool)
  506. {
  507. size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL;
  508. pool = pool->next;
  509. }
  510. return size;
  511. }
  512. static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf)
  513. {
  514. int size = 0;
  515. size += sizeof(rcCompactHeightfield);
  516. size += sizeof(rcCompactSpan) * chf.spanCount;
  517. size += sizeof(rcCompactCell) * chf.width * chf.height;
  518. return size;
  519. }
  520. */