9
3

DetourNavMesh.cpp 44 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584
  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. #include <string.h>
  20. #include <stdio.h>
  21. #include "DetourNavMesh.h"
  22. #include "DetourNode.h"
  23. #include "DetourCommon.h"
  24. #include "DetourMath.h"
  25. #include "DetourAlloc.h"
  26. #include "DetourAssert.h"
  27. #include <new>
  28. inline bool overlapSlabs(const float* amin, const float* amax,
  29. const float* bmin, const float* bmax,
  30. const float px, const float py)
  31. {
  32. // Check for horizontal overlap.
  33. // The segment is shrunken a little so that slabs which touch
  34. // at end points are not connected.
  35. const float minx = dtMax(amin[0]+px,bmin[0]+px);
  36. const float maxx = dtMin(amax[0]-px,bmax[0]-px);
  37. if (minx > maxx)
  38. return false;
  39. // Check vertical overlap.
  40. const float ad = (amax[1]-amin[1]) / (amax[0]-amin[0]);
  41. const float ak = amin[1] - ad*amin[0];
  42. const float bd = (bmax[1]-bmin[1]) / (bmax[0]-bmin[0]);
  43. const float bk = bmin[1] - bd*bmin[0];
  44. const float aminy = ad*minx + ak;
  45. const float amaxy = ad*maxx + ak;
  46. const float bminy = bd*minx + bk;
  47. const float bmaxy = bd*maxx + bk;
  48. const float dmin = bminy - aminy;
  49. const float dmax = bmaxy - amaxy;
  50. // Crossing segments always overlap.
  51. if (dmin*dmax < 0)
  52. return true;
  53. // Check for overlap at endpoints.
  54. const float thr = dtSqr(py*2);
  55. if (dmin*dmin <= thr || dmax*dmax <= thr)
  56. return true;
  57. return false;
  58. }
  59. static float getSlabCoord(const float* va, const int side)
  60. {
  61. if (side == 0 || side == 4)
  62. return va[0];
  63. else if (side == 2 || side == 6)
  64. return va[2];
  65. return 0;
  66. }
  67. static void calcSlabEndPoints(const float* va, const float* vb, float* bmin, float* bmax, const int side)
  68. {
  69. if (side == 0 || side == 4)
  70. {
  71. if (va[2] < vb[2])
  72. {
  73. bmin[0] = va[2];
  74. bmin[1] = va[1];
  75. bmax[0] = vb[2];
  76. bmax[1] = vb[1];
  77. }
  78. else
  79. {
  80. bmin[0] = vb[2];
  81. bmin[1] = vb[1];
  82. bmax[0] = va[2];
  83. bmax[1] = va[1];
  84. }
  85. }
  86. else if (side == 2 || side == 6)
  87. {
  88. if (va[0] < vb[0])
  89. {
  90. bmin[0] = va[0];
  91. bmin[1] = va[1];
  92. bmax[0] = vb[0];
  93. bmax[1] = vb[1];
  94. }
  95. else
  96. {
  97. bmin[0] = vb[0];
  98. bmin[1] = vb[1];
  99. bmax[0] = va[0];
  100. bmax[1] = va[1];
  101. }
  102. }
  103. }
  104. inline int computeTileHash(int x, int y, const int mask)
  105. {
  106. const unsigned int h1 = 0x8da6b343; // Large multiplicative constants;
  107. const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes
  108. unsigned int n = h1 * x + h2 * y;
  109. return (int)(n & mask);
  110. }
  111. inline unsigned int allocLink(dtMeshTile* tile)
  112. {
  113. if (tile->linksFreeList == DT_NULL_LINK)
  114. return DT_NULL_LINK;
  115. unsigned int link = tile->linksFreeList;
  116. tile->linksFreeList = tile->links[link].next;
  117. return link;
  118. }
  119. inline void freeLink(dtMeshTile* tile, unsigned int link)
  120. {
  121. tile->links[link].next = tile->linksFreeList;
  122. tile->linksFreeList = link;
  123. }
  124. dtNavMesh* dtAllocNavMesh()
  125. {
  126. void* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM);
  127. if (!mem) return 0;
  128. return new(mem) dtNavMesh;
  129. }
  130. /// @par
  131. ///
  132. /// This function will only free the memory for tiles with the #DT_TILE_FREE_DATA
  133. /// flag set.
  134. void dtFreeNavMesh(dtNavMesh* navmesh)
  135. {
  136. if (!navmesh) return;
  137. navmesh->~dtNavMesh();
  138. dtFree(navmesh);
  139. }
  140. //////////////////////////////////////////////////////////////////////////////////////////
  141. /**
  142. @class dtNavMesh
  143. The navigation mesh consists of one or more tiles defining three primary types of structural data:
  144. A polygon mesh which defines most of the navigation graph. (See rcPolyMesh for its structure.)
  145. A detail mesh used for determining surface height on the polygon mesh. (See rcPolyMeshDetail for its structure.)
  146. Off-mesh connections, which define custom point-to-point edges within the navigation graph.
  147. The general build process is as follows:
  148. -# Create rcPolyMesh and rcPolyMeshDetail data using the Recast build pipeline.
  149. -# Optionally, create off-mesh connection data.
  150. -# Combine the source data into a dtNavMeshCreateParams structure.
  151. -# Create a tile data array using dtCreateNavMeshData().
  152. -# Allocate at dtNavMesh object and initialize it. (For single tile navigation meshes,
  153. the tile data is loaded during this step.)
  154. -# For multi-tile navigation meshes, load the tile data using dtNavMesh::addTile().
  155. Notes:
  156. - This class is usually used in conjunction with the dtNavMeshQuery class for pathfinding.
  157. - Technically, all navigation meshes are tiled. A 'solo' mesh is simply a navigation mesh initialized
  158. to have only a single tile.
  159. - This class does not implement any asynchronous methods. So the ::dtStatus result of all methods will
  160. always contain either a success or failure flag.
  161. @see dtNavMeshQuery, dtCreateNavMeshData, dtNavMeshCreateParams, #dtAllocNavMesh, #dtFreeNavMesh
  162. */
  163. dtNavMesh::dtNavMesh() :
  164. m_tileWidth(0),
  165. m_tileHeight(0),
  166. m_maxTiles(0),
  167. m_tileLutSize(0),
  168. m_tileLutMask(0),
  169. m_posLookup(0),
  170. m_nextFree(0),
  171. m_tiles(0)
  172. {
  173. #ifndef DT_POLYREF64
  174. m_saltBits = 0;
  175. m_tileBits = 0;
  176. m_polyBits = 0;
  177. #endif
  178. memset(&m_params, 0, sizeof(dtNavMeshParams));
  179. m_orig[0] = 0;
  180. m_orig[1] = 0;
  181. m_orig[2] = 0;
  182. }
  183. dtNavMesh::~dtNavMesh()
  184. {
  185. for (int i = 0; i < m_maxTiles; ++i)
  186. {
  187. if (m_tiles[i].flags & DT_TILE_FREE_DATA)
  188. {
  189. dtFree(m_tiles[i].data);
  190. m_tiles[i].data = 0;
  191. m_tiles[i].dataSize = 0;
  192. }
  193. }
  194. dtFree(m_posLookup);
  195. dtFree(m_tiles);
  196. }
  197. dtStatus dtNavMesh::init(const dtNavMeshParams* params)
  198. {
  199. memcpy(&m_params, params, sizeof(dtNavMeshParams));
  200. dtVcopy(m_orig, params->orig);
  201. m_tileWidth = params->tileWidth;
  202. m_tileHeight = params->tileHeight;
  203. // Init tiles
  204. m_maxTiles = params->maxTiles;
  205. m_tileLutSize = dtNextPow2(params->maxTiles/4);
  206. if (!m_tileLutSize) m_tileLutSize = 1;
  207. m_tileLutMask = m_tileLutSize-1;
  208. m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM);
  209. if (!m_tiles)
  210. return DT_FAILURE | DT_OUT_OF_MEMORY;
  211. m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM);
  212. if (!m_posLookup)
  213. return DT_FAILURE | DT_OUT_OF_MEMORY;
  214. memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles);
  215. memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize);
  216. m_nextFree = 0;
  217. for (int i = m_maxTiles-1; i >= 0; --i)
  218. {
  219. m_tiles[i].salt = 1;
  220. m_tiles[i].next = m_nextFree;
  221. m_nextFree = &m_tiles[i];
  222. }
  223. // Init ID generator values.
  224. #ifndef DT_POLYREF64
  225. m_tileBits = dtIlog2(dtNextPow2((unsigned int)params->maxTiles));
  226. m_polyBits = dtIlog2(dtNextPow2((unsigned int)params->maxPolys));
  227. // Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow.
  228. m_saltBits = dtMin((unsigned int)31, 32 - m_tileBits - m_polyBits);
  229. if (m_saltBits < 10)
  230. return DT_FAILURE | DT_INVALID_PARAM;
  231. #endif
  232. return DT_SUCCESS;
  233. }
  234. dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flags)
  235. {
  236. // Make sure the data is in right format.
  237. dtMeshHeader* header = (dtMeshHeader*)data;
  238. if (header->magic != DT_NAVMESH_MAGIC)
  239. return DT_FAILURE | DT_WRONG_MAGIC;
  240. if (header->version != DT_NAVMESH_VERSION)
  241. return DT_FAILURE | DT_WRONG_VERSION;
  242. dtNavMeshParams params;
  243. dtVcopy(params.orig, header->bmin);
  244. params.tileWidth = header->bmax[0] - header->bmin[0];
  245. params.tileHeight = header->bmax[2] - header->bmin[2];
  246. params.maxTiles = 1;
  247. params.maxPolys = header->polyCount;
  248. dtStatus status = init(&params);
  249. if (dtStatusFailed(status))
  250. return status;
  251. return addTile(data, dataSize, flags, 0, 0);
  252. }
  253. /// @par
  254. ///
  255. /// @note The parameters are created automatically when the single tile
  256. /// initialization is performed.
  257. const dtNavMeshParams* dtNavMesh::getParams() const
  258. {
  259. return &m_params;
  260. }
  261. //////////////////////////////////////////////////////////////////////////////////////////
  262. int dtNavMesh::findConnectingPolys(const float* va, const float* vb,
  263. const dtMeshTile* tile, int side,
  264. dtPolyRef* con, float* conarea, int maxcon) const
  265. {
  266. if (!tile) return 0;
  267. float amin[2], amax[2];
  268. calcSlabEndPoints(va, vb, amin, amax, side);
  269. const float apos = getSlabCoord(va, side);
  270. // Remove links pointing to 'side' and compact the links array.
  271. float bmin[2], bmax[2];
  272. unsigned short m = DT_EXT_LINK | (unsigned short)side;
  273. int n = 0;
  274. dtPolyRef base = getPolyRefBase(tile);
  275. for (int i = 0; i < tile->header->polyCount; ++i)
  276. {
  277. dtPoly* poly = &tile->polys[i];
  278. const int nv = poly->vertCount;
  279. for (int j = 0; j < nv; ++j)
  280. {
  281. // Skip edges which do not point to the right side.
  282. if (poly->neis[j] != m) continue;
  283. const float* vc = &tile->verts[poly->verts[j]*3];
  284. const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3];
  285. const float bpos = getSlabCoord(vc, side);
  286. // Segments are not close enough.
  287. if (dtAbs(apos-bpos) > 0.01f)
  288. continue;
  289. // Check if the segments touch.
  290. calcSlabEndPoints(vc,vd, bmin,bmax, side);
  291. if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue;
  292. // Add return value.
  293. if (n < maxcon)
  294. {
  295. conarea[n*2+0] = dtMax(amin[0], bmin[0]);
  296. conarea[n*2+1] = dtMin(amax[0], bmax[0]);
  297. con[n] = base | (dtPolyRef)i;
  298. n++;
  299. }
  300. break;
  301. }
  302. }
  303. return n;
  304. }
  305. void dtNavMesh::unconnectLinks(dtMeshTile* tile, dtMeshTile* target)
  306. {
  307. if (!tile || !target) return;
  308. const unsigned int targetNum = decodePolyIdTile(getTileRef(target));
  309. for (int i = 0; i < tile->header->polyCount; ++i)
  310. {
  311. dtPoly* poly = &tile->polys[i];
  312. unsigned int j = poly->firstLink;
  313. unsigned int pj = DT_NULL_LINK;
  314. while (j != DT_NULL_LINK)
  315. {
  316. if (decodePolyIdTile(tile->links[j].ref) == targetNum)
  317. {
  318. // Remove link.
  319. unsigned int nj = tile->links[j].next;
  320. if (pj == DT_NULL_LINK)
  321. poly->firstLink = nj;
  322. else
  323. tile->links[pj].next = nj;
  324. freeLink(tile, j);
  325. j = nj;
  326. }
  327. else
  328. {
  329. // Advance
  330. pj = j;
  331. j = tile->links[j].next;
  332. }
  333. }
  334. }
  335. }
  336. void dtNavMesh::connectExtLinks(dtMeshTile* tile, dtMeshTile* target, int side)
  337. {
  338. if (!tile) return;
  339. // Connect border links.
  340. for (int i = 0; i < tile->header->polyCount; ++i)
  341. {
  342. dtPoly* poly = &tile->polys[i];
  343. // Create new links.
  344. // unsigned short m = DT_EXT_LINK | (unsigned short)side;
  345. const int nv = poly->vertCount;
  346. for (int j = 0; j < nv; ++j)
  347. {
  348. // Skip non-portal edges.
  349. if ((poly->neis[j] & DT_EXT_LINK) == 0)
  350. continue;
  351. const int dir = (int)(poly->neis[j] & 0xff);
  352. if (side != -1 && dir != side)
  353. continue;
  354. // Create new links
  355. const float* va = &tile->verts[poly->verts[j]*3];
  356. const float* vb = &tile->verts[poly->verts[(j+1) % nv]*3];
  357. dtPolyRef nei[4];
  358. float neia[4*2];
  359. int nnei = findConnectingPolys(va,vb, target, dtOppositeTile(dir), nei,neia,4);
  360. for (int k = 0; k < nnei; ++k)
  361. {
  362. unsigned int idx = allocLink(tile);
  363. if (idx != DT_NULL_LINK)
  364. {
  365. dtLink* link = &tile->links[idx];
  366. link->ref = nei[k];
  367. link->edge = (unsigned char)j;
  368. link->side = (unsigned char)dir;
  369. link->next = poly->firstLink;
  370. poly->firstLink = idx;
  371. // Compress portal limits to a byte value.
  372. if (dir == 0 || dir == 4)
  373. {
  374. float tmin = (neia[k*2+0]-va[2]) / (vb[2]-va[2]);
  375. float tmax = (neia[k*2+1]-va[2]) / (vb[2]-va[2]);
  376. if (tmin > tmax)
  377. dtSwap(tmin,tmax);
  378. link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
  379. link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f);
  380. }
  381. else if (dir == 2 || dir == 6)
  382. {
  383. float tmin = (neia[k*2+0]-va[0]) / (vb[0]-va[0]);
  384. float tmax = (neia[k*2+1]-va[0]) / (vb[0]-va[0]);
  385. if (tmin > tmax)
  386. dtSwap(tmin,tmax);
  387. link->bmin = (unsigned char)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
  388. link->bmax = (unsigned char)(dtClamp(tmax, 0.0f, 1.0f)*255.0f);
  389. }
  390. }
  391. }
  392. }
  393. }
  394. }
  395. void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side)
  396. {
  397. if (!tile) return;
  398. // Connect off-mesh links.
  399. // We are interested on links which land from target tile to this tile.
  400. const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side);
  401. for (int i = 0; i < target->header->offMeshConCount; ++i)
  402. {
  403. dtOffMeshConnection* targetCon = &target->offMeshCons[i];
  404. if (targetCon->side != oppositeSide)
  405. continue;
  406. dtPoly* targetPoly = &target->polys[targetCon->poly];
  407. // Skip off-mesh connections which start location could not be connected at all.
  408. if (targetPoly->firstLink == DT_NULL_LINK)
  409. continue;
  410. const float halfExtents[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad };
  411. // Find polygon to connect to.
  412. const float* p = &targetCon->pos[3];
  413. float nearestPt[3];
  414. dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt);
  415. if (!ref)
  416. continue;
  417. // findNearestPoly may return too optimistic results, further check to make sure.
  418. if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(targetCon->rad))
  419. continue;
  420. // Make sure the location is on current mesh.
  421. float* v = &target->verts[targetPoly->verts[1]*3];
  422. dtVcopy(v, nearestPt);
  423. // Link off-mesh connection to target poly.
  424. unsigned int idx = allocLink(target);
  425. if (idx != DT_NULL_LINK)
  426. {
  427. dtLink* link = &target->links[idx];
  428. link->ref = ref;
  429. link->edge = (unsigned char)1;
  430. link->side = oppositeSide;
  431. link->bmin = link->bmax = 0;
  432. // Add to linked list.
  433. link->next = targetPoly->firstLink;
  434. targetPoly->firstLink = idx;
  435. }
  436. // Link target poly to off-mesh connection.
  437. if (targetCon->flags & DT_OFFMESH_CON_BIDIR)
  438. {
  439. unsigned int tidx = allocLink(tile);
  440. if (tidx != DT_NULL_LINK)
  441. {
  442. const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
  443. dtPoly* landPoly = &tile->polys[landPolyIdx];
  444. dtLink* link = &tile->links[tidx];
  445. link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly);
  446. link->edge = 0xff;
  447. link->side = (unsigned char)(side == -1 ? 0xff : side);
  448. link->bmin = link->bmax = 0;
  449. // Add to linked list.
  450. link->next = landPoly->firstLink;
  451. landPoly->firstLink = tidx;
  452. }
  453. }
  454. }
  455. }
  456. void dtNavMesh::connectIntLinks(dtMeshTile* tile)
  457. {
  458. if (!tile) return;
  459. dtPolyRef base = getPolyRefBase(tile);
  460. for (int i = 0; i < tile->header->polyCount; ++i)
  461. {
  462. dtPoly* poly = &tile->polys[i];
  463. poly->firstLink = DT_NULL_LINK;
  464. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  465. continue;
  466. // Build edge links backwards so that the links will be
  467. // in the linked list from lowest index to highest.
  468. for (int j = poly->vertCount-1; j >= 0; --j)
  469. {
  470. // Skip hard and non-internal edges.
  471. if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue;
  472. unsigned int idx = allocLink(tile);
  473. if (idx != DT_NULL_LINK)
  474. {
  475. dtLink* link = &tile->links[idx];
  476. link->ref = base | (dtPolyRef)(poly->neis[j]-1);
  477. link->edge = (unsigned char)j;
  478. link->side = 0xff;
  479. link->bmin = link->bmax = 0;
  480. // Add to linked list.
  481. link->next = poly->firstLink;
  482. poly->firstLink = idx;
  483. }
  484. }
  485. }
  486. }
  487. void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
  488. {
  489. if (!tile) return;
  490. dtPolyRef base = getPolyRefBase(tile);
  491. // Base off-mesh connection start points.
  492. for (int i = 0; i < tile->header->offMeshConCount; ++i)
  493. {
  494. dtOffMeshConnection* con = &tile->offMeshCons[i];
  495. dtPoly* poly = &tile->polys[con->poly];
  496. const float halfExtents[3] = { con->rad, tile->header->walkableClimb, con->rad };
  497. // Find polygon to connect to.
  498. const float* p = &con->pos[0]; // First vertex
  499. float nearestPt[3];
  500. dtPolyRef ref = findNearestPolyInTile(tile, p, halfExtents, nearestPt);
  501. if (!ref) continue;
  502. // findNearestPoly may return too optimistic results, further check to make sure.
  503. if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad))
  504. continue;
  505. // Make sure the location is on current mesh.
  506. float* v = &tile->verts[poly->verts[0]*3];
  507. dtVcopy(v, nearestPt);
  508. // Link off-mesh connection to target poly.
  509. unsigned int idx = allocLink(tile);
  510. if (idx != DT_NULL_LINK)
  511. {
  512. dtLink* link = &tile->links[idx];
  513. link->ref = ref;
  514. link->edge = (unsigned char)0;
  515. link->side = 0xff;
  516. link->bmin = link->bmax = 0;
  517. // Add to linked list.
  518. link->next = poly->firstLink;
  519. poly->firstLink = idx;
  520. }
  521. // Start end-point is always connect back to off-mesh connection.
  522. unsigned int tidx = allocLink(tile);
  523. if (tidx != DT_NULL_LINK)
  524. {
  525. const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
  526. dtPoly* landPoly = &tile->polys[landPolyIdx];
  527. dtLink* link = &tile->links[tidx];
  528. link->ref = base | (dtPolyRef)(con->poly);
  529. link->edge = 0xff;
  530. link->side = 0xff;
  531. link->bmin = link->bmax = 0;
  532. // Add to linked list.
  533. link->next = landPoly->firstLink;
  534. landPoly->firstLink = tidx;
  535. }
  536. }
  537. }
  538. namespace
  539. {
  540. template<bool onlyBoundary>
  541. void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest)
  542. {
  543. const unsigned int ip = (unsigned int)(poly - tile->polys);
  544. const dtPolyDetail* pd = &tile->detailMeshes[ip];
  545. float dmin = FLT_MAX;
  546. float tmin = 0;
  547. const float* pmin = 0;
  548. const float* pmax = 0;
  549. for (int i = 0; i < pd->triCount; i++)
  550. {
  551. const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4];
  552. const int ANY_BOUNDARY_EDGE =
  553. (DT_DETAIL_EDGE_BOUNDARY << 0) |
  554. (DT_DETAIL_EDGE_BOUNDARY << 2) |
  555. (DT_DETAIL_EDGE_BOUNDARY << 4);
  556. if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0)
  557. continue;
  558. const float* v[3];
  559. for (int j = 0; j < 3; ++j)
  560. {
  561. if (tris[j] < poly->vertCount)
  562. v[j] = &tile->verts[poly->verts[tris[j]] * 3];
  563. else
  564. v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3];
  565. }
  566. for (int k = 0, j = 2; k < 3; j = k++)
  567. {
  568. if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 &&
  569. (onlyBoundary || tris[j] < tris[k]))
  570. {
  571. // Only looking at boundary edges and this is internal, or
  572. // this is an inner edge that we will see again or have already seen.
  573. continue;
  574. }
  575. float t;
  576. float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t);
  577. if (d < dmin)
  578. {
  579. dmin = d;
  580. tmin = t;
  581. pmin = v[j];
  582. pmax = v[k];
  583. }
  584. }
  585. }
  586. dtVlerp(closest, pmin, pmax, tmin);
  587. }
  588. }
  589. bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const
  590. {
  591. // Off-mesh connections do not have detail polys and getting height
  592. // over them does not make sense.
  593. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  594. return false;
  595. const unsigned int ip = (unsigned int)(poly - tile->polys);
  596. const dtPolyDetail* pd = &tile->detailMeshes[ip];
  597. float verts[DT_VERTS_PER_POLYGON*3];
  598. const int nv = poly->vertCount;
  599. for (int i = 0; i < nv; ++i)
  600. dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
  601. if (!dtPointInPolygon(pos, verts, nv))
  602. return false;
  603. if (!height)
  604. return true;
  605. // Find height at the location.
  606. for (int j = 0; j < pd->triCount; ++j)
  607. {
  608. const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
  609. const float* v[3];
  610. for (int k = 0; k < 3; ++k)
  611. {
  612. if (t[k] < poly->vertCount)
  613. v[k] = &tile->verts[poly->verts[t[k]]*3];
  614. else
  615. v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
  616. }
  617. float h;
  618. if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
  619. {
  620. *height = h;
  621. return true;
  622. }
  623. }
  624. // If all triangle checks failed above (can happen with degenerate triangles
  625. // or larger floating point values) the point is on an edge, so just select
  626. // closest. This should almost never happen so the extra iteration here is
  627. // ok.
  628. float closest[3];
  629. closestPointOnDetailEdges<false>(tile, poly, pos, closest);
  630. *height = closest[1];
  631. return true;
  632. }
  633. void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
  634. {
  635. const dtMeshTile* tile = 0;
  636. const dtPoly* poly = 0;
  637. getTileAndPolyByRefUnsafe(ref, &tile, &poly);
  638. dtVcopy(closest, pos);
  639. if (getPolyHeight(tile, poly, pos, &closest[1]))
  640. {
  641. if (posOverPoly)
  642. *posOverPoly = true;
  643. return;
  644. }
  645. if (posOverPoly)
  646. *posOverPoly = false;
  647. // Off-mesh connections don't have detail polygons.
  648. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  649. {
  650. const float* v0 = &tile->verts[poly->verts[0]*3];
  651. const float* v1 = &tile->verts[poly->verts[1]*3];
  652. float t;
  653. dtDistancePtSegSqr2D(pos, v0, v1, t);
  654. dtVlerp(closest, v0, v1, t);
  655. return;
  656. }
  657. // Outside poly that is not an offmesh connection.
  658. closestPointOnDetailEdges<true>(tile, poly, pos, closest);
  659. }
  660. dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
  661. const float* center, const float* halfExtents,
  662. float* nearestPt) const
  663. {
  664. float bmin[3], bmax[3];
  665. dtVsub(bmin, center, halfExtents);
  666. dtVadd(bmax, center, halfExtents);
  667. // Get nearby polygons from proximity grid.
  668. dtPolyRef polys[128];
  669. int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128);
  670. // Find nearest polygon amongst the nearby polygons.
  671. dtPolyRef nearest = 0;
  672. float nearestDistanceSqr = FLT_MAX;
  673. for (int i = 0; i < polyCount; ++i)
  674. {
  675. dtPolyRef ref = polys[i];
  676. float closestPtPoly[3];
  677. float diff[3];
  678. bool posOverPoly = false;
  679. float d;
  680. closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
  681. // If a point is directly over a polygon and closer than
  682. // climb height, favor that instead of straight line nearest point.
  683. dtVsub(diff, center, closestPtPoly);
  684. if (posOverPoly)
  685. {
  686. d = dtAbs(diff[1]) - tile->header->walkableClimb;
  687. d = d > 0 ? d*d : 0;
  688. }
  689. else
  690. {
  691. d = dtVlenSqr(diff);
  692. }
  693. if (d < nearestDistanceSqr)
  694. {
  695. dtVcopy(nearestPt, closestPtPoly);
  696. nearestDistanceSqr = d;
  697. nearest = ref;
  698. }
  699. }
  700. return nearest;
  701. }
  702. int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
  703. dtPolyRef* polys, const int maxPolys) const
  704. {
  705. if (tile->bvTree)
  706. {
  707. const dtBVNode* node = &tile->bvTree[0];
  708. const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
  709. const float* tbmin = tile->header->bmin;
  710. const float* tbmax = tile->header->bmax;
  711. const float qfac = tile->header->bvQuantFactor;
  712. // Calculate quantized box
  713. unsigned short bmin[3], bmax[3];
  714. // dtClamp query box to world box.
  715. float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
  716. float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
  717. float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
  718. float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
  719. float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
  720. float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
  721. // Quantize
  722. bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
  723. bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
  724. bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
  725. bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
  726. bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
  727. bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
  728. // Traverse tree
  729. dtPolyRef base = getPolyRefBase(tile);
  730. int n = 0;
  731. while (node < end)
  732. {
  733. const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
  734. const bool isLeafNode = node->i >= 0;
  735. if (isLeafNode && overlap)
  736. {
  737. if (n < maxPolys)
  738. polys[n++] = base | (dtPolyRef)node->i;
  739. }
  740. if (overlap || isLeafNode)
  741. node++;
  742. else
  743. {
  744. const int escapeIndex = -node->i;
  745. node += escapeIndex;
  746. }
  747. }
  748. return n;
  749. }
  750. else
  751. {
  752. float bmin[3], bmax[3];
  753. int n = 0;
  754. dtPolyRef base = getPolyRefBase(tile);
  755. for (int i = 0; i < tile->header->polyCount; ++i)
  756. {
  757. dtPoly* p = &tile->polys[i];
  758. // Do not return off-mesh connection polygons.
  759. if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  760. continue;
  761. // Calc polygon bounds.
  762. const float* v = &tile->verts[p->verts[0]*3];
  763. dtVcopy(bmin, v);
  764. dtVcopy(bmax, v);
  765. for (int j = 1; j < p->vertCount; ++j)
  766. {
  767. v = &tile->verts[p->verts[j]*3];
  768. dtVmin(bmin, v);
  769. dtVmax(bmax, v);
  770. }
  771. if (dtOverlapBounds(qmin,qmax, bmin,bmax))
  772. {
  773. if (n < maxPolys)
  774. polys[n++] = base | (dtPolyRef)i;
  775. }
  776. }
  777. return n;
  778. }
  779. }
  780. /// @par
  781. ///
  782. /// The add operation will fail if the data is in the wrong format, the allocated tile
  783. /// space is full, or there is a tile already at the specified reference.
  784. ///
  785. /// The lastRef parameter is used to restore a tile with the same tile
  786. /// reference it had previously used. In this case the #dtPolyRef's for the
  787. /// tile will be restored to the same values they were before the tile was
  788. /// removed.
  789. ///
  790. /// The nav mesh assumes exclusive access to the data passed and will make
  791. /// changes to the dynamic portion of the data. For that reason the data
  792. /// should not be reused in other nav meshes until the tile has been successfully
  793. /// removed from this nav mesh.
  794. ///
  795. /// @see dtCreateNavMeshData, #removeTile
  796. dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
  797. dtTileRef lastRef, dtTileRef* result)
  798. {
  799. // Make sure the data is in right format.
  800. dtMeshHeader* header = (dtMeshHeader*)data;
  801. if (header->magic != DT_NAVMESH_MAGIC)
  802. return DT_FAILURE | DT_WRONG_MAGIC;
  803. if (header->version != DT_NAVMESH_VERSION)
  804. return DT_FAILURE | DT_WRONG_VERSION;
  805. // Make sure the location is free.
  806. if (getTileAt(header->x, header->y, header->layer))
  807. return DT_FAILURE | DT_ALREADY_OCCUPIED;
  808. // Allocate a tile.
  809. dtMeshTile* tile = 0;
  810. if (!lastRef)
  811. {
  812. if (m_nextFree)
  813. {
  814. tile = m_nextFree;
  815. m_nextFree = tile->next;
  816. tile->next = 0;
  817. }
  818. }
  819. else
  820. {
  821. // Try to relocate the tile to specific index with same salt.
  822. int tileIndex = (int)decodePolyIdTile((dtPolyRef)lastRef);
  823. if (tileIndex >= m_maxTiles)
  824. return DT_FAILURE | DT_OUT_OF_MEMORY;
  825. // Try to find the specific tile id from the free list.
  826. dtMeshTile* target = &m_tiles[tileIndex];
  827. dtMeshTile* prev = 0;
  828. tile = m_nextFree;
  829. while (tile && tile != target)
  830. {
  831. prev = tile;
  832. tile = tile->next;
  833. }
  834. // Could not find the correct location.
  835. if (tile != target)
  836. return DT_FAILURE | DT_OUT_OF_MEMORY;
  837. // Remove from freelist
  838. if (!prev)
  839. m_nextFree = tile->next;
  840. else
  841. prev->next = tile->next;
  842. // Restore salt.
  843. tile->salt = decodePolyIdSalt((dtPolyRef)lastRef);
  844. }
  845. // Make sure we could allocate a tile.
  846. if (!tile)
  847. return DT_FAILURE | DT_OUT_OF_MEMORY;
  848. // Insert tile into the position lut.
  849. int h = computeTileHash(header->x, header->y, m_tileLutMask);
  850. tile->next = m_posLookup[h];
  851. m_posLookup[h] = tile;
  852. // Patch header pointers.
  853. const int headerSize = dtAlign4(sizeof(dtMeshHeader));
  854. const int vertsSize = dtAlign4(sizeof(float)*3*header->vertCount);
  855. const int polysSize = dtAlign4(sizeof(dtPoly)*header->polyCount);
  856. const int linksSize = dtAlign4(sizeof(dtLink)*(header->maxLinkCount));
  857. const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*header->detailMeshCount);
  858. const int detailVertsSize = dtAlign4(sizeof(float)*3*header->detailVertCount);
  859. const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*header->detailTriCount);
  860. const int bvtreeSize = dtAlign4(sizeof(dtBVNode)*header->bvNodeCount);
  861. const int offMeshLinksSize = dtAlign4(sizeof(dtOffMeshConnection)*header->offMeshConCount);
  862. unsigned char* d = data + headerSize;
  863. tile->verts = dtGetThenAdvanceBufferPointer<float>(d, vertsSize);
  864. tile->polys = dtGetThenAdvanceBufferPointer<dtPoly>(d, polysSize);
  865. tile->links = dtGetThenAdvanceBufferPointer<dtLink>(d, linksSize);
  866. tile->detailMeshes = dtGetThenAdvanceBufferPointer<dtPolyDetail>(d, detailMeshesSize);
  867. tile->detailVerts = dtGetThenAdvanceBufferPointer<float>(d, detailVertsSize);
  868. tile->detailTris = dtGetThenAdvanceBufferPointer<unsigned char>(d, detailTrisSize);
  869. tile->bvTree = dtGetThenAdvanceBufferPointer<dtBVNode>(d, bvtreeSize);
  870. tile->offMeshCons = dtGetThenAdvanceBufferPointer<dtOffMeshConnection>(d, offMeshLinksSize);
  871. // If there are no items in the bvtree, reset the tree pointer.
  872. if (!bvtreeSize)
  873. tile->bvTree = 0;
  874. // Build links freelist
  875. tile->linksFreeList = 0;
  876. tile->links[header->maxLinkCount-1].next = DT_NULL_LINK;
  877. for (int i = 0; i < header->maxLinkCount-1; ++i)
  878. tile->links[i].next = i+1;
  879. // Init tile.
  880. tile->header = header;
  881. tile->data = data;
  882. tile->dataSize = dataSize;
  883. tile->flags = flags;
  884. connectIntLinks(tile);
  885. // Base off-mesh connections to their starting polygons and connect connections inside the tile.
  886. baseOffMeshLinks(tile);
  887. connectExtOffMeshLinks(tile, tile, -1);
  888. // Create connections with neighbour tiles.
  889. static const int MAX_NEIS = 32;
  890. dtMeshTile* neis[MAX_NEIS];
  891. int nneis;
  892. // Connect with layers in current tile.
  893. nneis = getTilesAt(header->x, header->y, neis, MAX_NEIS);
  894. for (int j = 0; j < nneis; ++j)
  895. {
  896. if (neis[j] == tile)
  897. continue;
  898. connectExtLinks(tile, neis[j], -1);
  899. connectExtLinks(neis[j], tile, -1);
  900. connectExtOffMeshLinks(tile, neis[j], -1);
  901. connectExtOffMeshLinks(neis[j], tile, -1);
  902. }
  903. // Connect with neighbour tiles.
  904. for (int i = 0; i < 8; ++i)
  905. {
  906. nneis = getNeighbourTilesAt(header->x, header->y, i, neis, MAX_NEIS);
  907. for (int j = 0; j < nneis; ++j)
  908. {
  909. connectExtLinks(tile, neis[j], i);
  910. connectExtLinks(neis[j], tile, dtOppositeTile(i));
  911. connectExtOffMeshLinks(tile, neis[j], i);
  912. connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i));
  913. }
  914. }
  915. if (result)
  916. *result = getTileRef(tile);
  917. return DT_SUCCESS;
  918. }
  919. const dtMeshTile* dtNavMesh::getTileAt(const int x, const int y, const int layer) const
  920. {
  921. // Find tile based on hash.
  922. int h = computeTileHash(x,y,m_tileLutMask);
  923. dtMeshTile* tile = m_posLookup[h];
  924. while (tile)
  925. {
  926. if (tile->header &&
  927. tile->header->x == x &&
  928. tile->header->y == y &&
  929. tile->header->layer == layer)
  930. {
  931. return tile;
  932. }
  933. tile = tile->next;
  934. }
  935. return 0;
  936. }
  937. int dtNavMesh::getNeighbourTilesAt(const int x, const int y, const int side, dtMeshTile** tiles, const int maxTiles) const
  938. {
  939. int nx = x, ny = y;
  940. switch (side)
  941. {
  942. case 0: nx++; break;
  943. case 1: nx++; ny++; break;
  944. case 2: ny++; break;
  945. case 3: nx--; ny++; break;
  946. case 4: nx--; break;
  947. case 5: nx--; ny--; break;
  948. case 6: ny--; break;
  949. case 7: nx++; ny--; break;
  950. };
  951. return getTilesAt(nx, ny, tiles, maxTiles);
  952. }
  953. int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile** tiles, const int maxTiles) const
  954. {
  955. int n = 0;
  956. // Find tile based on hash.
  957. int h = computeTileHash(x,y,m_tileLutMask);
  958. dtMeshTile* tile = m_posLookup[h];
  959. while (tile)
  960. {
  961. if (tile->header &&
  962. tile->header->x == x &&
  963. tile->header->y == y)
  964. {
  965. if (n < maxTiles)
  966. tiles[n++] = tile;
  967. }
  968. tile = tile->next;
  969. }
  970. return n;
  971. }
  972. /// @par
  973. ///
  974. /// This function will not fail if the tiles array is too small to hold the
  975. /// entire result set. It will simply fill the array to capacity.
  976. int dtNavMesh::getTilesAt(const int x, const int y, dtMeshTile const** tiles, const int maxTiles) const
  977. {
  978. int n = 0;
  979. // Find tile based on hash.
  980. int h = computeTileHash(x,y,m_tileLutMask);
  981. dtMeshTile* tile = m_posLookup[h];
  982. while (tile)
  983. {
  984. if (tile->header &&
  985. tile->header->x == x &&
  986. tile->header->y == y)
  987. {
  988. if (n < maxTiles)
  989. tiles[n++] = tile;
  990. }
  991. tile = tile->next;
  992. }
  993. return n;
  994. }
  995. dtTileRef dtNavMesh::getTileRefAt(const int x, const int y, const int layer) const
  996. {
  997. // Find tile based on hash.
  998. int h = computeTileHash(x,y,m_tileLutMask);
  999. dtMeshTile* tile = m_posLookup[h];
  1000. while (tile)
  1001. {
  1002. if (tile->header &&
  1003. tile->header->x == x &&
  1004. tile->header->y == y &&
  1005. tile->header->layer == layer)
  1006. {
  1007. return getTileRef(tile);
  1008. }
  1009. tile = tile->next;
  1010. }
  1011. return 0;
  1012. }
  1013. const dtMeshTile* dtNavMesh::getTileByRef(dtTileRef ref) const
  1014. {
  1015. if (!ref)
  1016. return 0;
  1017. unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref);
  1018. unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref);
  1019. if ((int)tileIndex >= m_maxTiles)
  1020. return 0;
  1021. const dtMeshTile* tile = &m_tiles[tileIndex];
  1022. if (tile->salt != tileSalt)
  1023. return 0;
  1024. return tile;
  1025. }
  1026. int dtNavMesh::getMaxTiles() const
  1027. {
  1028. return m_maxTiles;
  1029. }
  1030. dtMeshTile* dtNavMesh::getTile(int i)
  1031. {
  1032. return &m_tiles[i];
  1033. }
  1034. const dtMeshTile* dtNavMesh::getTile(int i) const
  1035. {
  1036. return &m_tiles[i];
  1037. }
  1038. void dtNavMesh::calcTileLoc(const float* pos, int* tx, int* ty) const
  1039. {
  1040. *tx = (int)floorf((pos[0]-m_orig[0]) / m_tileWidth);
  1041. *ty = (int)floorf((pos[2]-m_orig[2]) / m_tileHeight);
  1042. }
  1043. dtStatus dtNavMesh::getTileAndPolyByRef(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const
  1044. {
  1045. if (!ref) return DT_FAILURE;
  1046. unsigned int salt, it, ip;
  1047. decodePolyId(ref, salt, it, ip);
  1048. if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
  1049. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
  1050. if (ip >= (unsigned int)m_tiles[it].header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
  1051. *tile = &m_tiles[it];
  1052. *poly = &m_tiles[it].polys[ip];
  1053. return DT_SUCCESS;
  1054. }
  1055. /// @par
  1056. ///
  1057. /// @warning Only use this function if it is known that the provided polygon
  1058. /// reference is valid. This function is faster than #getTileAndPolyByRef, but
  1059. /// it does not validate the reference.
  1060. void dtNavMesh::getTileAndPolyByRefUnsafe(const dtPolyRef ref, const dtMeshTile** tile, const dtPoly** poly) const
  1061. {
  1062. unsigned int salt, it, ip;
  1063. decodePolyId(ref, salt, it, ip);
  1064. *tile = &m_tiles[it];
  1065. *poly = &m_tiles[it].polys[ip];
  1066. }
  1067. bool dtNavMesh::isValidPolyRef(dtPolyRef ref) const
  1068. {
  1069. if (!ref) return false;
  1070. unsigned int salt, it, ip;
  1071. decodePolyId(ref, salt, it, ip);
  1072. if (it >= (unsigned int)m_maxTiles) return false;
  1073. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return false;
  1074. if (ip >= (unsigned int)m_tiles[it].header->polyCount) return false;
  1075. return true;
  1076. }
  1077. /// @par
  1078. ///
  1079. /// This function returns the data for the tile so that, if desired,
  1080. /// it can be added back to the navigation mesh at a later point.
  1081. ///
  1082. /// @see #addTile
  1083. dtStatus dtNavMesh::removeTile(dtTileRef ref, unsigned char** data, int* dataSize)
  1084. {
  1085. if (!ref)
  1086. return DT_FAILURE | DT_INVALID_PARAM;
  1087. unsigned int tileIndex = decodePolyIdTile((dtPolyRef)ref);
  1088. unsigned int tileSalt = decodePolyIdSalt((dtPolyRef)ref);
  1089. if ((int)tileIndex >= m_maxTiles)
  1090. return DT_FAILURE | DT_INVALID_PARAM;
  1091. dtMeshTile* tile = &m_tiles[tileIndex];
  1092. if (tile->salt != tileSalt)
  1093. return DT_FAILURE | DT_INVALID_PARAM;
  1094. // Remove tile from hash lookup.
  1095. int h = computeTileHash(tile->header->x,tile->header->y,m_tileLutMask);
  1096. dtMeshTile* prev = 0;
  1097. dtMeshTile* cur = m_posLookup[h];
  1098. while (cur)
  1099. {
  1100. if (cur == tile)
  1101. {
  1102. if (prev)
  1103. prev->next = cur->next;
  1104. else
  1105. m_posLookup[h] = cur->next;
  1106. break;
  1107. }
  1108. prev = cur;
  1109. cur = cur->next;
  1110. }
  1111. // Remove connections to neighbour tiles.
  1112. static const int MAX_NEIS = 32;
  1113. dtMeshTile* neis[MAX_NEIS];
  1114. int nneis;
  1115. // Disconnect from other layers in current tile.
  1116. nneis = getTilesAt(tile->header->x, tile->header->y, neis, MAX_NEIS);
  1117. for (int j = 0; j < nneis; ++j)
  1118. {
  1119. if (neis[j] == tile) continue;
  1120. unconnectLinks(neis[j], tile);
  1121. }
  1122. // Disconnect from neighbour tiles.
  1123. for (int i = 0; i < 8; ++i)
  1124. {
  1125. nneis = getNeighbourTilesAt(tile->header->x, tile->header->y, i, neis, MAX_NEIS);
  1126. for (int j = 0; j < nneis; ++j)
  1127. unconnectLinks(neis[j], tile);
  1128. }
  1129. // Reset tile.
  1130. if (tile->flags & DT_TILE_FREE_DATA)
  1131. {
  1132. // Owns data
  1133. dtFree(tile->data);
  1134. tile->data = 0;
  1135. tile->dataSize = 0;
  1136. if (data) *data = 0;
  1137. if (dataSize) *dataSize = 0;
  1138. }
  1139. else
  1140. {
  1141. if (data) *data = tile->data;
  1142. if (dataSize) *dataSize = tile->dataSize;
  1143. }
  1144. tile->header = 0;
  1145. tile->flags = 0;
  1146. tile->linksFreeList = 0;
  1147. tile->polys = 0;
  1148. tile->verts = 0;
  1149. tile->links = 0;
  1150. tile->detailMeshes = 0;
  1151. tile->detailVerts = 0;
  1152. tile->detailTris = 0;
  1153. tile->bvTree = 0;
  1154. tile->offMeshCons = 0;
  1155. // Update salt, salt should never be zero.
  1156. #ifdef DT_POLYREF64
  1157. tile->salt = (tile->salt+1) & ((1<<DT_SALT_BITS)-1);
  1158. #else
  1159. tile->salt = (tile->salt+1) & ((1<<m_saltBits)-1);
  1160. #endif
  1161. if (tile->salt == 0)
  1162. tile->salt++;
  1163. // Add to free list.
  1164. tile->next = m_nextFree;
  1165. m_nextFree = tile;
  1166. return DT_SUCCESS;
  1167. }
  1168. dtTileRef dtNavMesh::getTileRef(const dtMeshTile* tile) const
  1169. {
  1170. if (!tile) return 0;
  1171. const unsigned int it = (unsigned int)(tile - m_tiles);
  1172. return (dtTileRef)encodePolyId(tile->salt, it, 0);
  1173. }
  1174. /// @par
  1175. ///
  1176. /// Example use case:
  1177. /// @code
  1178. ///
  1179. /// const dtPolyRef base = navmesh->getPolyRefBase(tile);
  1180. /// for (int i = 0; i < tile->header->polyCount; ++i)
  1181. /// {
  1182. /// const dtPoly* p = &tile->polys[i];
  1183. /// const dtPolyRef ref = base | (dtPolyRef)i;
  1184. ///
  1185. /// // Use the reference to access the polygon data.
  1186. /// }
  1187. /// @endcode
  1188. dtPolyRef dtNavMesh::getPolyRefBase(const dtMeshTile* tile) const
  1189. {
  1190. if (!tile) return 0;
  1191. const unsigned int it = (unsigned int)(tile - m_tiles);
  1192. return encodePolyId(tile->salt, it, 0);
  1193. }
  1194. struct dtTileState
  1195. {
  1196. int magic; // Magic number, used to identify the data.
  1197. int version; // Data version number.
  1198. dtTileRef ref; // Tile ref at the time of storing the data.
  1199. };
  1200. struct dtPolyState
  1201. {
  1202. unsigned short flags; // Flags (see dtPolyFlags).
  1203. unsigned char area; // Area ID of the polygon.
  1204. };
  1205. /// @see #storeTileState
  1206. int dtNavMesh::getTileStateSize(const dtMeshTile* tile) const
  1207. {
  1208. if (!tile) return 0;
  1209. const int headerSize = dtAlign4(sizeof(dtTileState));
  1210. const int polyStateSize = dtAlign4(sizeof(dtPolyState) * tile->header->polyCount);
  1211. return headerSize + polyStateSize;
  1212. }
  1213. /// @par
  1214. ///
  1215. /// Tile state includes non-structural data such as polygon flags, area ids, etc.
  1216. /// @note The state data is only valid until the tile reference changes.
  1217. /// @see #getTileStateSize, #restoreTileState
  1218. dtStatus dtNavMesh::storeTileState(const dtMeshTile* tile, unsigned char* data, const int maxDataSize) const
  1219. {
  1220. // Make sure there is enough space to store the state.
  1221. const int sizeReq = getTileStateSize(tile);
  1222. if (maxDataSize < sizeReq)
  1223. return DT_FAILURE | DT_BUFFER_TOO_SMALL;
  1224. dtTileState* tileState = dtGetThenAdvanceBufferPointer<dtTileState>(data, dtAlign4(sizeof(dtTileState)));
  1225. dtPolyState* polyStates = dtGetThenAdvanceBufferPointer<dtPolyState>(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount));
  1226. // Store tile state.
  1227. tileState->magic = DT_NAVMESH_STATE_MAGIC;
  1228. tileState->version = DT_NAVMESH_STATE_VERSION;
  1229. tileState->ref = getTileRef(tile);
  1230. // Store per poly state.
  1231. for (int i = 0; i < tile->header->polyCount; ++i)
  1232. {
  1233. const dtPoly* p = &tile->polys[i];
  1234. dtPolyState* s = &polyStates[i];
  1235. s->flags = p->flags;
  1236. s->area = p->getArea();
  1237. }
  1238. return DT_SUCCESS;
  1239. }
  1240. /// @par
  1241. ///
  1242. /// Tile state includes non-structural data such as polygon flags, area ids, etc.
  1243. /// @note This function does not impact the tile's #dtTileRef and #dtPolyRef's.
  1244. /// @see #storeTileState
  1245. dtStatus dtNavMesh::restoreTileState(dtMeshTile* tile, const unsigned char* data, const int maxDataSize)
  1246. {
  1247. // Make sure there is enough space to store the state.
  1248. const int sizeReq = getTileStateSize(tile);
  1249. if (maxDataSize < sizeReq)
  1250. return DT_FAILURE | DT_INVALID_PARAM;
  1251. const dtTileState* tileState = dtGetThenAdvanceBufferPointer<const dtTileState>(data, dtAlign4(sizeof(dtTileState)));
  1252. const dtPolyState* polyStates = dtGetThenAdvanceBufferPointer<const dtPolyState>(data, dtAlign4(sizeof(dtPolyState) * tile->header->polyCount));
  1253. // Check that the restore is possible.
  1254. if (tileState->magic != DT_NAVMESH_STATE_MAGIC)
  1255. return DT_FAILURE | DT_WRONG_MAGIC;
  1256. if (tileState->version != DT_NAVMESH_STATE_VERSION)
  1257. return DT_FAILURE | DT_WRONG_VERSION;
  1258. if (tileState->ref != getTileRef(tile))
  1259. return DT_FAILURE | DT_INVALID_PARAM;
  1260. // Restore per poly state.
  1261. for (int i = 0; i < tile->header->polyCount; ++i)
  1262. {
  1263. dtPoly* p = &tile->polys[i];
  1264. const dtPolyState* s = &polyStates[i];
  1265. p->flags = s->flags;
  1266. p->setArea(s->area);
  1267. }
  1268. return DT_SUCCESS;
  1269. }
  1270. /// @par
  1271. ///
  1272. /// Off-mesh connections are stored in the navigation mesh as special 2-vertex
  1273. /// polygons with a single edge. At least one of the vertices is expected to be
  1274. /// inside a normal polygon. So an off-mesh connection is "entered" from a
  1275. /// normal polygon at one of its endpoints. This is the polygon identified by
  1276. /// the prevRef parameter.
  1277. dtStatus dtNavMesh::getOffMeshConnectionPolyEndPoints(dtPolyRef prevRef, dtPolyRef polyRef, float* startPos, float* endPos) const
  1278. {
  1279. unsigned int salt, it, ip;
  1280. if (!polyRef)
  1281. return DT_FAILURE;
  1282. // Get current polygon
  1283. decodePolyId(polyRef, salt, it, ip);
  1284. if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
  1285. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
  1286. const dtMeshTile* tile = &m_tiles[it];
  1287. if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
  1288. const dtPoly* poly = &tile->polys[ip];
  1289. // Make sure that the current poly is indeed off-mesh link.
  1290. if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION)
  1291. return DT_FAILURE;
  1292. // Figure out which way to hand out the vertices.
  1293. int idx0 = 0, idx1 = 1;
  1294. // Find link that points to first vertex.
  1295. for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
  1296. {
  1297. if (tile->links[i].edge == 0)
  1298. {
  1299. if (tile->links[i].ref != prevRef)
  1300. {
  1301. idx0 = 1;
  1302. idx1 = 0;
  1303. }
  1304. break;
  1305. }
  1306. }
  1307. dtVcopy(startPos, &tile->verts[poly->verts[idx0]*3]);
  1308. dtVcopy(endPos, &tile->verts[poly->verts[idx1]*3]);
  1309. return DT_SUCCESS;
  1310. }
  1311. const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) const
  1312. {
  1313. unsigned int salt, it, ip;
  1314. if (!ref)
  1315. return 0;
  1316. // Get current polygon
  1317. decodePolyId(ref, salt, it, ip);
  1318. if (it >= (unsigned int)m_maxTiles) return 0;
  1319. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return 0;
  1320. const dtMeshTile* tile = &m_tiles[it];
  1321. if (ip >= (unsigned int)tile->header->polyCount) return 0;
  1322. const dtPoly* poly = &tile->polys[ip];
  1323. // Make sure that the current poly is indeed off-mesh link.
  1324. if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION)
  1325. return 0;
  1326. const unsigned int idx = ip - tile->header->offMeshBase;
  1327. dtAssert(idx < (unsigned int)tile->header->offMeshConCount);
  1328. return &tile->offMeshCons[idx];
  1329. }
  1330. dtStatus dtNavMesh::setPolyFlags(dtPolyRef ref, unsigned short flags)
  1331. {
  1332. if (!ref) return DT_FAILURE;
  1333. unsigned int salt, it, ip;
  1334. decodePolyId(ref, salt, it, ip);
  1335. if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
  1336. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
  1337. dtMeshTile* tile = &m_tiles[it];
  1338. if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
  1339. dtPoly* poly = &tile->polys[ip];
  1340. // Change flags.
  1341. poly->flags = flags;
  1342. return DT_SUCCESS;
  1343. }
  1344. dtStatus dtNavMesh::getPolyFlags(dtPolyRef ref, unsigned short* resultFlags) const
  1345. {
  1346. if (!ref) return DT_FAILURE;
  1347. unsigned int salt, it, ip;
  1348. decodePolyId(ref, salt, it, ip);
  1349. if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
  1350. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
  1351. const dtMeshTile* tile = &m_tiles[it];
  1352. if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
  1353. const dtPoly* poly = &tile->polys[ip];
  1354. *resultFlags = poly->flags;
  1355. return DT_SUCCESS;
  1356. }
  1357. dtStatus dtNavMesh::setPolyArea(dtPolyRef ref, unsigned char area)
  1358. {
  1359. if (!ref) return DT_FAILURE;
  1360. unsigned int salt, it, ip;
  1361. decodePolyId(ref, salt, it, ip);
  1362. if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
  1363. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
  1364. dtMeshTile* tile = &m_tiles[it];
  1365. if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
  1366. dtPoly* poly = &tile->polys[ip];
  1367. poly->setArea(area);
  1368. return DT_SUCCESS;
  1369. }
  1370. dtStatus dtNavMesh::getPolyArea(dtPolyRef ref, unsigned char* resultArea) const
  1371. {
  1372. if (!ref) return DT_FAILURE;
  1373. unsigned int salt, it, ip;
  1374. decodePolyId(ref, salt, it, ip);
  1375. if (it >= (unsigned int)m_maxTiles) return DT_FAILURE | DT_INVALID_PARAM;
  1376. if (m_tiles[it].salt != salt || m_tiles[it].header == 0) return DT_FAILURE | DT_INVALID_PARAM;
  1377. const dtMeshTile* tile = &m_tiles[it];
  1378. if (ip >= (unsigned int)tile->header->polyCount) return DT_FAILURE | DT_INVALID_PARAM;
  1379. const dtPoly* poly = &tile->polys[ip];
  1380. *resultArea = poly->getArea();
  1381. return DT_SUCCESS;
  1382. }