9
3

DetourNavMeshQuery.cpp 102 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667
  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 "DetourNavMeshQuery.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. /// @class dtQueryFilter
  29. ///
  30. /// <b>The Default Implementation</b>
  31. ///
  32. /// At construction: All area costs default to 1.0. All flags are included
  33. /// and none are excluded.
  34. ///
  35. /// If a polygon has both an include and an exclude flag, it will be excluded.
  36. ///
  37. /// The way filtering works, a navigation mesh polygon must have at least one flag
  38. /// set to ever be considered by a query. So a polygon with no flags will never
  39. /// be considered.
  40. ///
  41. /// Setting the include flags to 0 will result in all polygons being excluded.
  42. ///
  43. /// <b>Custom Implementations</b>
  44. ///
  45. /// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class.
  46. ///
  47. /// Implement a custom query filter by overriding the virtual passFilter()
  48. /// and getCost() functions. If this is done, both functions should be as
  49. /// fast as possible. Use cached local copies of data rather than accessing
  50. /// your own objects where possible.
  51. ///
  52. /// Custom implementations do not need to adhere to the flags or cost logic
  53. /// used by the default implementation.
  54. ///
  55. /// In order for A* searches to work properly, the cost should be proportional to
  56. /// the travel distance. Implementing a cost modifier less than 1.0 is likely
  57. /// to lead to problems during pathfinding.
  58. ///
  59. /// @see dtNavMeshQuery
  60. dtQueryFilter::dtQueryFilter() :
  61. m_includeFlags(0xffff),
  62. m_excludeFlags(0)
  63. {
  64. for (int i = 0; i < DT_MAX_AREAS; ++i)
  65. m_areaCost[i] = 1.0f;
  66. }
  67. #ifdef DT_VIRTUAL_QUERYFILTER
  68. bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
  69. const dtMeshTile* /*tile*/,
  70. const dtPoly* poly) const
  71. {
  72. return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
  73. }
  74. float dtQueryFilter::getCost(const float* pa, const float* pb,
  75. const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
  76. const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
  77. const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
  78. {
  79. return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
  80. }
  81. #else
  82. inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
  83. const dtMeshTile* /*tile*/,
  84. const dtPoly* poly) const
  85. {
  86. return (poly->flags & m_includeFlags) != 0 && (poly->flags & m_excludeFlags) == 0;
  87. }
  88. inline float dtQueryFilter::getCost(const float* pa, const float* pb,
  89. const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
  90. const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
  91. const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
  92. {
  93. return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
  94. }
  95. #endif
  96. static const float H_SCALE = 0.999f; // Search heuristic scale.
  97. dtNavMeshQuery* dtAllocNavMeshQuery()
  98. {
  99. void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM);
  100. if (!mem) return 0;
  101. return new(mem) dtNavMeshQuery;
  102. }
  103. void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh)
  104. {
  105. if (!navmesh) return;
  106. navmesh->~dtNavMeshQuery();
  107. dtFree(navmesh);
  108. }
  109. //////////////////////////////////////////////////////////////////////////////////////////
  110. /// @class dtNavMeshQuery
  111. ///
  112. /// For methods that support undersized buffers, if the buffer is too small
  113. /// to hold the entire result set the return status of the method will include
  114. /// the #DT_BUFFER_TOO_SMALL flag.
  115. ///
  116. /// Constant member functions can be used by multiple clients without side
  117. /// effects. (E.g. No change to the closed list. No impact on an in-progress
  118. /// sliced path query. Etc.)
  119. ///
  120. /// Walls and portals: A @e wall is a polygon segment that is
  121. /// considered impassable. A @e portal is a passable segment between polygons.
  122. /// A portal may be treated as a wall based on the dtQueryFilter used for a query.
  123. ///
  124. /// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery()
  125. dtNavMeshQuery::dtNavMeshQuery() :
  126. m_nav(0),
  127. m_tinyNodePool(0),
  128. m_nodePool(0),
  129. m_openList(0)
  130. {
  131. memset(&m_query, 0, sizeof(dtQueryData));
  132. }
  133. dtNavMeshQuery::~dtNavMeshQuery()
  134. {
  135. if (m_tinyNodePool)
  136. m_tinyNodePool->~dtNodePool();
  137. if (m_nodePool)
  138. m_nodePool->~dtNodePool();
  139. if (m_openList)
  140. m_openList->~dtNodeQueue();
  141. dtFree(m_tinyNodePool);
  142. dtFree(m_nodePool);
  143. dtFree(m_openList);
  144. }
  145. /// @par
  146. ///
  147. /// Must be the first function called after construction, before other
  148. /// functions are used.
  149. ///
  150. /// This function can be used multiple times.
  151. dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
  152. {
  153. if (maxNodes > DT_NULL_IDX || maxNodes > (1 << DT_NODE_PARENT_BITS) - 1)
  154. return DT_FAILURE | DT_INVALID_PARAM;
  155. m_nav = nav;
  156. if (!m_nodePool || m_nodePool->getMaxNodes() < maxNodes)
  157. {
  158. if (m_nodePool)
  159. {
  160. m_nodePool->~dtNodePool();
  161. dtFree(m_nodePool);
  162. m_nodePool = 0;
  163. }
  164. m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4));
  165. if (!m_nodePool)
  166. return DT_FAILURE | DT_OUT_OF_MEMORY;
  167. }
  168. else
  169. {
  170. m_nodePool->clear();
  171. }
  172. if (!m_tinyNodePool)
  173. {
  174. m_tinyNodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(64, 32);
  175. if (!m_tinyNodePool)
  176. return DT_FAILURE | DT_OUT_OF_MEMORY;
  177. }
  178. else
  179. {
  180. m_tinyNodePool->clear();
  181. }
  182. if (!m_openList || m_openList->getCapacity() < maxNodes)
  183. {
  184. if (m_openList)
  185. {
  186. m_openList->~dtNodeQueue();
  187. dtFree(m_openList);
  188. m_openList = 0;
  189. }
  190. m_openList = new (dtAlloc(sizeof(dtNodeQueue), DT_ALLOC_PERM)) dtNodeQueue(maxNodes);
  191. if (!m_openList)
  192. return DT_FAILURE | DT_OUT_OF_MEMORY;
  193. }
  194. else
  195. {
  196. m_openList->clear();
  197. }
  198. return DT_SUCCESS;
  199. }
  200. dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
  201. dtPolyRef* randomRef, float* randomPt) const
  202. {
  203. dtAssert(m_nav);
  204. if (!filter || !frand || !randomRef || !randomPt)
  205. return DT_FAILURE | DT_INVALID_PARAM;
  206. // Randomly pick one tile. Assume that all tiles cover roughly the same area.
  207. const dtMeshTile* tile = 0;
  208. float tsum = 0.0f;
  209. for (int i = 0; i < m_nav->getMaxTiles(); i++)
  210. {
  211. const dtMeshTile* t = m_nav->getTile(i);
  212. if (!t || !t->header) continue;
  213. // Choose random tile using reservoi sampling.
  214. const float area = 1.0f; // Could be tile area too.
  215. tsum += area;
  216. const float u = frand();
  217. if (u*tsum <= area)
  218. tile = t;
  219. }
  220. if (!tile)
  221. return DT_FAILURE;
  222. // Randomly pick one polygon weighted by polygon area.
  223. const dtPoly* poly = 0;
  224. dtPolyRef polyRef = 0;
  225. const dtPolyRef base = m_nav->getPolyRefBase(tile);
  226. float areaSum = 0.0f;
  227. for (int i = 0; i < tile->header->polyCount; ++i)
  228. {
  229. const dtPoly* p = &tile->polys[i];
  230. // Do not return off-mesh connection polygons.
  231. if (p->getType() != DT_POLYTYPE_GROUND)
  232. continue;
  233. // Must pass filter
  234. const dtPolyRef ref = base | (dtPolyRef)i;
  235. if (!filter->passFilter(ref, tile, p))
  236. continue;
  237. // Calc area of the polygon.
  238. float polyArea = 0.0f;
  239. for (int j = 2; j < p->vertCount; ++j)
  240. {
  241. const float* va = &tile->verts[p->verts[0]*3];
  242. const float* vb = &tile->verts[p->verts[j-1]*3];
  243. const float* vc = &tile->verts[p->verts[j]*3];
  244. polyArea += dtTriArea2D(va,vb,vc);
  245. }
  246. // Choose random polygon weighted by area, using reservoi sampling.
  247. areaSum += polyArea;
  248. const float u = frand();
  249. if (u*areaSum <= polyArea)
  250. {
  251. poly = p;
  252. polyRef = ref;
  253. }
  254. }
  255. if (!poly)
  256. return DT_FAILURE;
  257. // Randomly pick point on polygon.
  258. const float* v = &tile->verts[poly->verts[0]*3];
  259. float verts[3*DT_VERTS_PER_POLYGON];
  260. float areas[DT_VERTS_PER_POLYGON];
  261. dtVcopy(&verts[0*3],v);
  262. for (int j = 1; j < poly->vertCount; ++j)
  263. {
  264. v = &tile->verts[poly->verts[j]*3];
  265. dtVcopy(&verts[j*3],v);
  266. }
  267. const float s = frand();
  268. const float t = frand();
  269. float pt[3];
  270. dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
  271. float h = 0.0f;
  272. dtStatus status = getPolyHeight(polyRef, pt, &h);
  273. if (dtStatusFailed(status))
  274. return status;
  275. pt[1] = h;
  276. dtVcopy(randomPt, pt);
  277. *randomRef = polyRef;
  278. return DT_SUCCESS;
  279. }
  280. dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius,
  281. const dtQueryFilter* filter, float (*frand)(),
  282. dtPolyRef* randomRef, float* randomPt) const
  283. {
  284. dtAssert(m_nav);
  285. dtAssert(m_nodePool);
  286. dtAssert(m_openList);
  287. // Validate input
  288. if (!m_nav->isValidPolyRef(startRef) ||
  289. !centerPos || !dtVisfinite(centerPos) ||
  290. maxRadius < 0 || !dtMathIsfinite(maxRadius) ||
  291. !filter || !frand || !randomRef || !randomPt)
  292. {
  293. return DT_FAILURE | DT_INVALID_PARAM;
  294. }
  295. const dtMeshTile* startTile = 0;
  296. const dtPoly* startPoly = 0;
  297. m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
  298. if (!filter->passFilter(startRef, startTile, startPoly))
  299. return DT_FAILURE | DT_INVALID_PARAM;
  300. m_nodePool->clear();
  301. m_openList->clear();
  302. dtNode* startNode = m_nodePool->getNode(startRef);
  303. dtVcopy(startNode->pos, centerPos);
  304. startNode->pidx = 0;
  305. startNode->cost = 0;
  306. startNode->total = 0;
  307. startNode->id = startRef;
  308. startNode->flags = DT_NODE_OPEN;
  309. m_openList->push(startNode);
  310. dtStatus status = DT_SUCCESS;
  311. const float radiusSqr = dtSqr(maxRadius);
  312. float areaSum = 0.0f;
  313. const dtMeshTile* randomTile = 0;
  314. const dtPoly* randomPoly = 0;
  315. dtPolyRef randomPolyRef = 0;
  316. while (!m_openList->empty())
  317. {
  318. dtNode* bestNode = m_openList->pop();
  319. bestNode->flags &= ~DT_NODE_OPEN;
  320. bestNode->flags |= DT_NODE_CLOSED;
  321. // Get poly and tile.
  322. // The API input has been cheked already, skip checking internal data.
  323. const dtPolyRef bestRef = bestNode->id;
  324. const dtMeshTile* bestTile = 0;
  325. const dtPoly* bestPoly = 0;
  326. m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
  327. // Place random locations on on ground.
  328. if (bestPoly->getType() == DT_POLYTYPE_GROUND)
  329. {
  330. // Calc area of the polygon.
  331. float polyArea = 0.0f;
  332. for (int j = 2; j < bestPoly->vertCount; ++j)
  333. {
  334. const float* va = &bestTile->verts[bestPoly->verts[0]*3];
  335. const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3];
  336. const float* vc = &bestTile->verts[bestPoly->verts[j]*3];
  337. polyArea += dtTriArea2D(va,vb,vc);
  338. }
  339. // Choose random polygon weighted by area, using reservoi sampling.
  340. areaSum += polyArea;
  341. const float u = frand();
  342. if (u*areaSum <= polyArea)
  343. {
  344. randomTile = bestTile;
  345. randomPoly = bestPoly;
  346. randomPolyRef = bestRef;
  347. }
  348. }
  349. // Get parent poly and tile.
  350. dtPolyRef parentRef = 0;
  351. const dtMeshTile* parentTile = 0;
  352. const dtPoly* parentPoly = 0;
  353. if (bestNode->pidx)
  354. parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
  355. if (parentRef)
  356. m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
  357. for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
  358. {
  359. const dtLink* link = &bestTile->links[i];
  360. dtPolyRef neighbourRef = link->ref;
  361. // Skip invalid neighbours and do not follow back to parent.
  362. if (!neighbourRef || neighbourRef == parentRef)
  363. continue;
  364. // Expand to neighbour
  365. const dtMeshTile* neighbourTile = 0;
  366. const dtPoly* neighbourPoly = 0;
  367. m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
  368. // Do not advance if the polygon is excluded by the filter.
  369. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  370. continue;
  371. // Find edge and calc distance to the edge.
  372. float va[3], vb[3];
  373. if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
  374. continue;
  375. // If the circle is not touching the next polygon, skip it.
  376. float tseg;
  377. float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
  378. if (distSqr > radiusSqr)
  379. continue;
  380. dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
  381. if (!neighbourNode)
  382. {
  383. status |= DT_OUT_OF_NODES;
  384. continue;
  385. }
  386. if (neighbourNode->flags & DT_NODE_CLOSED)
  387. continue;
  388. // Cost
  389. if (neighbourNode->flags == 0)
  390. dtVlerp(neighbourNode->pos, va, vb, 0.5f);
  391. const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
  392. // The node is already in open list and the new result is worse, skip.
  393. if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
  394. continue;
  395. neighbourNode->id = neighbourRef;
  396. neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
  397. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
  398. neighbourNode->total = total;
  399. if (neighbourNode->flags & DT_NODE_OPEN)
  400. {
  401. m_openList->modify(neighbourNode);
  402. }
  403. else
  404. {
  405. neighbourNode->flags = DT_NODE_OPEN;
  406. m_openList->push(neighbourNode);
  407. }
  408. }
  409. }
  410. if (!randomPoly)
  411. return DT_FAILURE;
  412. // Randomly pick point on polygon.
  413. const float* v = &randomTile->verts[randomPoly->verts[0]*3];
  414. float verts[3*DT_VERTS_PER_POLYGON];
  415. float areas[DT_VERTS_PER_POLYGON];
  416. dtVcopy(&verts[0*3],v);
  417. for (int j = 1; j < randomPoly->vertCount; ++j)
  418. {
  419. v = &randomTile->verts[randomPoly->verts[j]*3];
  420. dtVcopy(&verts[j*3],v);
  421. }
  422. const float s = frand();
  423. const float t = frand();
  424. float pt[3];
  425. dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
  426. float h = 0.0f;
  427. dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
  428. if (dtStatusFailed(status))
  429. return stat;
  430. pt[1] = h;
  431. dtVcopy(randomPt, pt);
  432. *randomRef = randomPolyRef;
  433. return DT_SUCCESS;
  434. }
  435. //////////////////////////////////////////////////////////////////////////////////////////
  436. /// @par
  437. ///
  438. /// Uses the detail polygons to find the surface height. (Most accurate.)
  439. ///
  440. /// @p pos does not have to be within the bounds of the polygon or navigation mesh.
  441. ///
  442. /// See closestPointOnPolyBoundary() for a limited but faster option.
  443. ///
  444. dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
  445. {
  446. dtAssert(m_nav);
  447. if (!m_nav->isValidPolyRef(ref) ||
  448. !pos || !dtVisfinite(pos) ||
  449. !closest)
  450. {
  451. return DT_FAILURE | DT_INVALID_PARAM;
  452. }
  453. m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly);
  454. return DT_SUCCESS;
  455. }
  456. /// @par
  457. ///
  458. /// Much faster than closestPointOnPoly().
  459. ///
  460. /// If the provided position lies within the polygon's xz-bounds (above or below),
  461. /// then @p pos and @p closest will be equal.
  462. ///
  463. /// The height of @p closest will be the polygon boundary. The height detail is not used.
  464. ///
  465. /// @p pos does not have to be within the bounds of the polybon or the navigation mesh.
  466. ///
  467. dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const
  468. {
  469. dtAssert(m_nav);
  470. const dtMeshTile* tile = 0;
  471. const dtPoly* poly = 0;
  472. if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
  473. return DT_FAILURE | DT_INVALID_PARAM;
  474. if (!pos || !dtVisfinite(pos) || !closest)
  475. return DT_FAILURE | DT_INVALID_PARAM;
  476. // Collect vertices.
  477. float verts[DT_VERTS_PER_POLYGON*3];
  478. float edged[DT_VERTS_PER_POLYGON];
  479. float edget[DT_VERTS_PER_POLYGON];
  480. int nv = 0;
  481. for (int i = 0; i < (int)poly->vertCount; ++i)
  482. {
  483. dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
  484. nv++;
  485. }
  486. bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
  487. if (inside)
  488. {
  489. // Point is inside the polygon, return the point.
  490. dtVcopy(closest, pos);
  491. }
  492. else
  493. {
  494. // Point is outside the polygon, dtClamp to nearest edge.
  495. float dmin = edged[0];
  496. int imin = 0;
  497. for (int i = 1; i < nv; ++i)
  498. {
  499. if (edged[i] < dmin)
  500. {
  501. dmin = edged[i];
  502. imin = i;
  503. }
  504. }
  505. const float* va = &verts[imin*3];
  506. const float* vb = &verts[((imin+1)%nv)*3];
  507. dtVlerp(closest, va, vb, edget[imin]);
  508. }
  509. return DT_SUCCESS;
  510. }
  511. /// @par
  512. ///
  513. /// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds
  514. /// of the polygon.
  515. ///
  516. dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
  517. {
  518. dtAssert(m_nav);
  519. const dtMeshTile* tile = 0;
  520. const dtPoly* poly = 0;
  521. if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
  522. return DT_FAILURE | DT_INVALID_PARAM;
  523. if (!pos || !dtVisfinite2D(pos))
  524. return DT_FAILURE | DT_INVALID_PARAM;
  525. // We used to return success for offmesh connections, but the
  526. // getPolyHeight in DetourNavMesh does not do this, so special
  527. // case it here.
  528. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  529. {
  530. const float* v0 = &tile->verts[poly->verts[0]*3];
  531. const float* v1 = &tile->verts[poly->verts[1]*3];
  532. float t;
  533. dtDistancePtSegSqr2D(pos, v0, v1, t);
  534. if (height)
  535. *height = v0[1] + (v1[1] - v0[1])*t;
  536. return DT_SUCCESS;
  537. }
  538. return m_nav->getPolyHeight(tile, poly, pos, height)
  539. ? DT_SUCCESS
  540. : DT_FAILURE | DT_INVALID_PARAM;
  541. }
  542. class dtFindNearestPolyQuery : public dtPolyQuery
  543. {
  544. const dtNavMeshQuery* m_query;
  545. const float* m_center;
  546. float m_nearestDistanceSqr;
  547. dtPolyRef m_nearestRef;
  548. float m_nearestPoint[3];
  549. public:
  550. dtFindNearestPolyQuery(const dtNavMeshQuery* query, const float* center)
  551. : m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint()
  552. {
  553. }
  554. dtPolyRef nearestRef() const { return m_nearestRef; }
  555. const float* nearestPoint() const { return m_nearestPoint; }
  556. void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count)
  557. {
  558. dtIgnoreUnused(polys);
  559. for (int i = 0; i < count; ++i)
  560. {
  561. dtPolyRef ref = refs[i];
  562. float closestPtPoly[3];
  563. float diff[3];
  564. bool posOverPoly = false;
  565. float d;
  566. m_query->closestPointOnPoly(ref, m_center, closestPtPoly, &posOverPoly);
  567. // If a point is directly over a polygon and closer than
  568. // climb height, favor that instead of straight line nearest point.
  569. dtVsub(diff, m_center, closestPtPoly);
  570. if (posOverPoly)
  571. {
  572. d = dtAbs(diff[1]) - tile->header->walkableClimb;
  573. d = d > 0 ? d*d : 0;
  574. }
  575. else
  576. {
  577. d = dtVlenSqr(diff);
  578. }
  579. if (d < m_nearestDistanceSqr)
  580. {
  581. dtVcopy(m_nearestPoint, closestPtPoly);
  582. m_nearestDistanceSqr = d;
  583. m_nearestRef = ref;
  584. }
  585. }
  586. }
  587. };
  588. /// @par
  589. ///
  590. /// @note If the search box does not intersect any polygons the search will
  591. /// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check
  592. /// @p nearestRef before using @p nearestPt.
  593. ///
  594. dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* halfExtents,
  595. const dtQueryFilter* filter,
  596. dtPolyRef* nearestRef, float* nearestPt) const
  597. {
  598. dtAssert(m_nav);
  599. if (!nearestRef)
  600. return DT_FAILURE | DT_INVALID_PARAM;
  601. // queryPolygons below will check rest of params
  602. dtFindNearestPolyQuery query(this, center);
  603. dtStatus status = queryPolygons(center, halfExtents, filter, &query);
  604. if (dtStatusFailed(status))
  605. return status;
  606. *nearestRef = query.nearestRef();
  607. // Only override nearestPt if we actually found a poly so the nearest point
  608. // is valid.
  609. if (nearestPt && *nearestRef)
  610. dtVcopy(nearestPt, query.nearestPoint());
  611. return DT_SUCCESS;
  612. }
  613. void dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
  614. const dtQueryFilter* filter, dtPolyQuery* query) const
  615. {
  616. dtAssert(m_nav);
  617. static const int batchSize = 32;
  618. dtPolyRef polyRefs[batchSize];
  619. dtPoly* polys[batchSize];
  620. int n = 0;
  621. if (tile->bvTree)
  622. {
  623. const dtBVNode* node = &tile->bvTree[0];
  624. const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
  625. const float* tbmin = tile->header->bmin;
  626. const float* tbmax = tile->header->bmax;
  627. const float qfac = tile->header->bvQuantFactor;
  628. // Calculate quantized box
  629. unsigned short bmin[3], bmax[3];
  630. // dtClamp query box to world box.
  631. float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
  632. float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
  633. float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
  634. float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
  635. float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
  636. float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
  637. // Quantize
  638. bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
  639. bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
  640. bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
  641. bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
  642. bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
  643. bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
  644. // Traverse tree
  645. const dtPolyRef base = m_nav->getPolyRefBase(tile);
  646. while (node < end)
  647. {
  648. const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
  649. const bool isLeafNode = node->i >= 0;
  650. if (isLeafNode && overlap)
  651. {
  652. dtPolyRef ref = base | (dtPolyRef)node->i;
  653. if (filter->passFilter(ref, tile, &tile->polys[node->i]))
  654. {
  655. polyRefs[n] = ref;
  656. polys[n] = &tile->polys[node->i];
  657. if (n == batchSize - 1)
  658. {
  659. query->process(tile, polys, polyRefs, batchSize);
  660. n = 0;
  661. }
  662. else
  663. {
  664. n++;
  665. }
  666. }
  667. }
  668. if (overlap || isLeafNode)
  669. node++;
  670. else
  671. {
  672. const int escapeIndex = -node->i;
  673. node += escapeIndex;
  674. }
  675. }
  676. }
  677. else
  678. {
  679. float bmin[3], bmax[3];
  680. const dtPolyRef base = m_nav->getPolyRefBase(tile);
  681. for (int i = 0; i < tile->header->polyCount; ++i)
  682. {
  683. dtPoly* p = &tile->polys[i];
  684. // Do not return off-mesh connection polygons.
  685. if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  686. continue;
  687. // Must pass filter
  688. const dtPolyRef ref = base | (dtPolyRef)i;
  689. if (!filter->passFilter(ref, tile, p))
  690. continue;
  691. // Calc polygon bounds.
  692. const float* v = &tile->verts[p->verts[0]*3];
  693. dtVcopy(bmin, v);
  694. dtVcopy(bmax, v);
  695. for (int j = 1; j < p->vertCount; ++j)
  696. {
  697. v = &tile->verts[p->verts[j]*3];
  698. dtVmin(bmin, v);
  699. dtVmax(bmax, v);
  700. }
  701. if (dtOverlapBounds(qmin, qmax, bmin, bmax))
  702. {
  703. polyRefs[n] = ref;
  704. polys[n] = p;
  705. if (n == batchSize - 1)
  706. {
  707. query->process(tile, polys, polyRefs, batchSize);
  708. n = 0;
  709. }
  710. else
  711. {
  712. n++;
  713. }
  714. }
  715. }
  716. }
  717. // Process the last polygons that didn't make a full batch.
  718. if (n > 0)
  719. query->process(tile, polys, polyRefs, n);
  720. }
  721. class dtCollectPolysQuery : public dtPolyQuery
  722. {
  723. dtPolyRef* m_polys;
  724. const int m_maxPolys;
  725. int m_numCollected;
  726. bool m_overflow;
  727. public:
  728. dtCollectPolysQuery(dtPolyRef* polys, const int maxPolys)
  729. : m_polys(polys), m_maxPolys(maxPolys), m_numCollected(0), m_overflow(false)
  730. {
  731. }
  732. int numCollected() const { return m_numCollected; }
  733. bool overflowed() const { return m_overflow; }
  734. void process(const dtMeshTile* tile, dtPoly** polys, dtPolyRef* refs, int count)
  735. {
  736. dtIgnoreUnused(tile);
  737. dtIgnoreUnused(polys);
  738. int numLeft = m_maxPolys - m_numCollected;
  739. int toCopy = count;
  740. if (toCopy > numLeft)
  741. {
  742. m_overflow = true;
  743. toCopy = numLeft;
  744. }
  745. memcpy(m_polys + m_numCollected, refs, (size_t)toCopy * sizeof(dtPolyRef));
  746. m_numCollected += toCopy;
  747. }
  748. };
  749. /// @par
  750. ///
  751. /// If no polygons are found, the function will return #DT_SUCCESS with a
  752. /// @p polyCount of zero.
  753. ///
  754. /// If @p polys is too small to hold the entire result set, then the array will
  755. /// be filled to capacity. The method of choosing which polygons from the
  756. /// full set are included in the partial result set is undefined.
  757. ///
  758. dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
  759. const dtQueryFilter* filter,
  760. dtPolyRef* polys, int* polyCount, const int maxPolys) const
  761. {
  762. if (!polys || !polyCount || maxPolys < 0)
  763. return DT_FAILURE | DT_INVALID_PARAM;
  764. dtCollectPolysQuery collector(polys, maxPolys);
  765. dtStatus status = queryPolygons(center, halfExtents, filter, &collector);
  766. if (dtStatusFailed(status))
  767. return status;
  768. *polyCount = collector.numCollected();
  769. return collector.overflowed() ? DT_SUCCESS | DT_BUFFER_TOO_SMALL : DT_SUCCESS;
  770. }
  771. /// @par
  772. ///
  773. /// The query will be invoked with batches of polygons. Polygons passed
  774. /// to the query have bounding boxes that overlap with the center and halfExtents
  775. /// passed to this function. The dtPolyQuery::process function is invoked multiple
  776. /// times until all overlapping polygons have been processed.
  777. ///
  778. dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
  779. const dtQueryFilter* filter, dtPolyQuery* query) const
  780. {
  781. dtAssert(m_nav);
  782. if (!center || !dtVisfinite(center) ||
  783. !halfExtents || !dtVisfinite(halfExtents) ||
  784. !filter || !query)
  785. {
  786. return DT_FAILURE | DT_INVALID_PARAM;
  787. }
  788. float bmin[3], bmax[3];
  789. dtVsub(bmin, center, halfExtents);
  790. dtVadd(bmax, center, halfExtents);
  791. // Find tiles the query touches.
  792. int minx, miny, maxx, maxy;
  793. m_nav->calcTileLoc(bmin, &minx, &miny);
  794. m_nav->calcTileLoc(bmax, &maxx, &maxy);
  795. static const int MAX_NEIS = 32;
  796. const dtMeshTile* neis[MAX_NEIS];
  797. for (int y = miny; y <= maxy; ++y)
  798. {
  799. for (int x = minx; x <= maxx; ++x)
  800. {
  801. const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS);
  802. for (int j = 0; j < nneis; ++j)
  803. {
  804. queryPolygonsInTile(neis[j], bmin, bmax, filter, query);
  805. }
  806. }
  807. }
  808. return DT_SUCCESS;
  809. }
  810. /// @par
  811. ///
  812. /// If the end polygon cannot be reached through the navigation graph,
  813. /// the last polygon in the path will be the nearest the end polygon.
  814. ///
  815. /// If the path array is to small to hold the full result, it will be filled as
  816. /// far as possible from the start polygon toward the end polygon.
  817. ///
  818. /// The start and end positions are used to calculate traversal costs.
  819. /// (The y-values impact the result.)
  820. ///
  821. dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
  822. const float* startPos, const float* endPos,
  823. const dtQueryFilter* filter,
  824. dtPolyRef* path, int* pathCount, const int maxPath) const
  825. {
  826. dtAssert(m_nav);
  827. dtAssert(m_nodePool);
  828. dtAssert(m_openList);
  829. if (!pathCount)
  830. return DT_FAILURE | DT_INVALID_PARAM;
  831. *pathCount = 0;
  832. // Validate input
  833. if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
  834. !startPos || !dtVisfinite(startPos) ||
  835. !endPos || !dtVisfinite(endPos) ||
  836. !filter || !path || maxPath <= 0)
  837. {
  838. return DT_FAILURE | DT_INVALID_PARAM;
  839. }
  840. if (startRef == endRef)
  841. {
  842. path[0] = startRef;
  843. *pathCount = 1;
  844. return DT_SUCCESS;
  845. }
  846. m_nodePool->clear();
  847. m_openList->clear();
  848. dtNode* startNode = m_nodePool->getNode(startRef);
  849. dtVcopy(startNode->pos, startPos);
  850. startNode->pidx = 0;
  851. startNode->cost = 0;
  852. startNode->total = dtVdist(startPos, endPos) * H_SCALE;
  853. startNode->id = startRef;
  854. startNode->flags = DT_NODE_OPEN;
  855. m_openList->push(startNode);
  856. dtNode* lastBestNode = startNode;
  857. float lastBestNodeCost = startNode->total;
  858. bool outOfNodes = false;
  859. while (!m_openList->empty())
  860. {
  861. // Remove node from open list and put it in closed list.
  862. dtNode* bestNode = m_openList->pop();
  863. bestNode->flags &= ~DT_NODE_OPEN;
  864. bestNode->flags |= DT_NODE_CLOSED;
  865. // Reached the goal, stop searching.
  866. if (bestNode->id == endRef)
  867. {
  868. lastBestNode = bestNode;
  869. break;
  870. }
  871. // Get current poly and tile.
  872. // The API input has been cheked already, skip checking internal data.
  873. const dtPolyRef bestRef = bestNode->id;
  874. const dtMeshTile* bestTile = 0;
  875. const dtPoly* bestPoly = 0;
  876. m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
  877. // Get parent poly and tile.
  878. dtPolyRef parentRef = 0;
  879. const dtMeshTile* parentTile = 0;
  880. const dtPoly* parentPoly = 0;
  881. if (bestNode->pidx)
  882. parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
  883. if (parentRef)
  884. m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
  885. for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
  886. {
  887. dtPolyRef neighbourRef = bestTile->links[i].ref;
  888. // Skip invalid ids and do not expand back to where we came from.
  889. if (!neighbourRef || neighbourRef == parentRef)
  890. continue;
  891. // Get neighbour poly and tile.
  892. // The API input has been cheked already, skip checking internal data.
  893. const dtMeshTile* neighbourTile = 0;
  894. const dtPoly* neighbourPoly = 0;
  895. if (dtStatusFailed(m_nav->getTileAndPolyByRef(neighbourRef, &neighbourTile, &neighbourPoly))) {
  896. continue;
  897. }
  898. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  899. continue;
  900. // deal explicitly with crossing tile boundaries
  901. unsigned char crossSide = 0;
  902. if (bestTile->links[i].side != 0xff)
  903. crossSide = bestTile->links[i].side >> 1;
  904. // get the node
  905. dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
  906. if (!neighbourNode)
  907. {
  908. outOfNodes = true;
  909. continue;
  910. }
  911. // If the node is visited the first time, calculate node position.
  912. if (neighbourNode->flags == 0)
  913. {
  914. getEdgeMidPoint(bestRef, bestPoly, bestTile,
  915. neighbourRef, neighbourPoly, neighbourTile,
  916. neighbourNode->pos);
  917. }
  918. // Calculate cost and heuristic.
  919. float cost = 0;
  920. float heuristic = 0;
  921. // Special case for last node.
  922. if (neighbourRef == endRef)
  923. {
  924. // Cost
  925. const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
  926. parentRef, parentTile, parentPoly,
  927. bestRef, bestTile, bestPoly,
  928. neighbourRef, neighbourTile, neighbourPoly);
  929. const float endCost = filter->getCost(neighbourNode->pos, endPos,
  930. bestRef, bestTile, bestPoly,
  931. neighbourRef, neighbourTile, neighbourPoly,
  932. 0, 0, 0);
  933. cost = bestNode->cost + curCost + endCost;
  934. heuristic = 0;
  935. }
  936. else
  937. {
  938. // Cost
  939. const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
  940. parentRef, parentTile, parentPoly,
  941. bestRef, bestTile, bestPoly,
  942. neighbourRef, neighbourTile, neighbourPoly);
  943. cost = bestNode->cost + curCost;
  944. heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
  945. }
  946. const float total = cost + heuristic;
  947. // The node is already in open list and the new result is worse, skip.
  948. if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
  949. continue;
  950. // The node is already visited and process, and the new result is worse, skip.
  951. if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
  952. continue;
  953. // Add or update the node.
  954. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
  955. neighbourNode->id = neighbourRef;
  956. neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
  957. neighbourNode->cost = cost;
  958. neighbourNode->total = total;
  959. if (neighbourNode->flags & DT_NODE_OPEN)
  960. {
  961. // Already in open, update node location.
  962. m_openList->modify(neighbourNode);
  963. }
  964. else
  965. {
  966. // Put the node in open list.
  967. neighbourNode->flags |= DT_NODE_OPEN;
  968. m_openList->push(neighbourNode);
  969. }
  970. // Update nearest node to target so far.
  971. if (heuristic < lastBestNodeCost)
  972. {
  973. lastBestNodeCost = heuristic;
  974. lastBestNode = neighbourNode;
  975. }
  976. }
  977. }
  978. dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath);
  979. if (lastBestNode->id != endRef)
  980. status |= DT_PARTIAL_RESULT;
  981. if (outOfNodes)
  982. status |= DT_OUT_OF_NODES;
  983. return status;
  984. }
  985. dtStatus dtNavMeshQuery::getPathToNode(dtNode* endNode, dtPolyRef* path, int* pathCount, int maxPath) const
  986. {
  987. // Find the length of the entire path.
  988. dtNode* curNode = endNode;
  989. int length = 0;
  990. do
  991. {
  992. length++;
  993. curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
  994. } while (curNode);
  995. // If the path cannot be fully stored then advance to the last node we will be able to store.
  996. curNode = endNode;
  997. int writeCount;
  998. for (writeCount = length; writeCount > maxPath; writeCount--)
  999. {
  1000. dtAssert(curNode);
  1001. curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
  1002. }
  1003. // Write path
  1004. for (int i = writeCount - 1; i >= 0; i--)
  1005. {
  1006. dtAssert(curNode);
  1007. path[i] = curNode->id;
  1008. curNode = m_nodePool->getNodeAtIdx(curNode->pidx);
  1009. }
  1010. dtAssert(!curNode);
  1011. *pathCount = dtMin(length, maxPath);
  1012. if (length > maxPath)
  1013. return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
  1014. return DT_SUCCESS;
  1015. }
  1016. /// @par
  1017. ///
  1018. /// @warning Calling any non-slice methods before calling finalizeSlicedFindPath()
  1019. /// or finalizeSlicedFindPathPartial() may result in corrupted data!
  1020. ///
  1021. /// The @p filter pointer is stored and used for the duration of the sliced
  1022. /// path query.
  1023. ///
  1024. dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
  1025. const float* startPos, const float* endPos,
  1026. const dtQueryFilter* filter, const unsigned int options)
  1027. {
  1028. dtAssert(m_nav);
  1029. dtAssert(m_nodePool);
  1030. dtAssert(m_openList);
  1031. // Init path state.
  1032. memset(&m_query, 0, sizeof(dtQueryData));
  1033. m_query.status = DT_FAILURE;
  1034. m_query.startRef = startRef;
  1035. m_query.endRef = endRef;
  1036. if (startPos)
  1037. dtVcopy(m_query.startPos, startPos);
  1038. if (endPos)
  1039. dtVcopy(m_query.endPos, endPos);
  1040. m_query.filter = filter;
  1041. m_query.options = options;
  1042. m_query.raycastLimitSqr = FLT_MAX;
  1043. // Validate input
  1044. if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
  1045. !startPos || !dtVisfinite(startPos) ||
  1046. !endPos || !dtVisfinite(endPos) || !filter)
  1047. {
  1048. return DT_FAILURE | DT_INVALID_PARAM;
  1049. }
  1050. // trade quality with performance?
  1051. if (options & DT_FINDPATH_ANY_ANGLE)
  1052. {
  1053. // limiting to several times the character radius yields nice results. It is not sensitive
  1054. // so it is enough to compute it from the first tile.
  1055. const dtMeshTile* tile = m_nav->getTileByRef(startRef);
  1056. float agentRadius = tile->header->walkableRadius;
  1057. m_query.raycastLimitSqr = dtSqr(agentRadius * DT_RAY_CAST_LIMIT_PROPORTIONS);
  1058. }
  1059. if (startRef == endRef)
  1060. {
  1061. m_query.status = DT_SUCCESS;
  1062. return DT_SUCCESS;
  1063. }
  1064. m_nodePool->clear();
  1065. m_openList->clear();
  1066. dtNode* startNode = m_nodePool->getNode(startRef);
  1067. dtVcopy(startNode->pos, startPos);
  1068. startNode->pidx = 0;
  1069. startNode->cost = 0;
  1070. startNode->total = dtVdist(startPos, endPos) * H_SCALE;
  1071. startNode->id = startRef;
  1072. startNode->flags = DT_NODE_OPEN;
  1073. m_openList->push(startNode);
  1074. m_query.status = DT_IN_PROGRESS;
  1075. m_query.lastBestNode = startNode;
  1076. m_query.lastBestNodeCost = startNode->total;
  1077. return m_query.status;
  1078. }
  1079. dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
  1080. {
  1081. if (!dtStatusInProgress(m_query.status))
  1082. return m_query.status;
  1083. // Make sure the request is still valid.
  1084. if (!m_nav->isValidPolyRef(m_query.startRef) || !m_nav->isValidPolyRef(m_query.endRef))
  1085. {
  1086. m_query.status = DT_FAILURE;
  1087. return DT_FAILURE;
  1088. }
  1089. dtRaycastHit rayHit;
  1090. rayHit.maxPath = 0;
  1091. int iter = 0;
  1092. while (iter < maxIter && !m_openList->empty())
  1093. {
  1094. iter++;
  1095. // Remove node from open list and put it in closed list.
  1096. dtNode* bestNode = m_openList->pop();
  1097. bestNode->flags &= ~DT_NODE_OPEN;
  1098. bestNode->flags |= DT_NODE_CLOSED;
  1099. // Reached the goal, stop searching.
  1100. if (bestNode->id == m_query.endRef)
  1101. {
  1102. m_query.lastBestNode = bestNode;
  1103. const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
  1104. m_query.status = DT_SUCCESS | details;
  1105. if (doneIters)
  1106. *doneIters = iter;
  1107. return m_query.status;
  1108. }
  1109. // Get current poly and tile.
  1110. // The API input has been cheked already, skip checking internal data.
  1111. const dtPolyRef bestRef = bestNode->id;
  1112. const dtMeshTile* bestTile = 0;
  1113. const dtPoly* bestPoly = 0;
  1114. if (dtStatusFailed(m_nav->getTileAndPolyByRef(bestRef, &bestTile, &bestPoly)))
  1115. {
  1116. // The polygon has disappeared during the sliced query, fail.
  1117. m_query.status = DT_FAILURE;
  1118. if (doneIters)
  1119. *doneIters = iter;
  1120. return m_query.status;
  1121. }
  1122. // Get parent and grand parent poly and tile.
  1123. dtPolyRef parentRef = 0, grandpaRef = 0;
  1124. const dtMeshTile* parentTile = 0;
  1125. const dtPoly* parentPoly = 0;
  1126. dtNode* parentNode = 0;
  1127. if (bestNode->pidx)
  1128. {
  1129. parentNode = m_nodePool->getNodeAtIdx(bestNode->pidx);
  1130. parentRef = parentNode->id;
  1131. if (parentNode->pidx)
  1132. grandpaRef = m_nodePool->getNodeAtIdx(parentNode->pidx)->id;
  1133. }
  1134. if (parentRef)
  1135. {
  1136. bool invalidParent = dtStatusFailed(m_nav->getTileAndPolyByRef(parentRef, &parentTile, &parentPoly));
  1137. if (invalidParent || (grandpaRef && !m_nav->isValidPolyRef(grandpaRef)) )
  1138. {
  1139. // The polygon has disappeared during the sliced query, fail.
  1140. m_query.status = DT_FAILURE;
  1141. if (doneIters)
  1142. *doneIters = iter;
  1143. return m_query.status;
  1144. }
  1145. }
  1146. // decide whether to test raycast to previous nodes
  1147. bool tryLOS = false;
  1148. if (m_query.options & DT_FINDPATH_ANY_ANGLE)
  1149. {
  1150. if ((parentRef != 0) && (dtVdistSqr(parentNode->pos, bestNode->pos) < m_query.raycastLimitSqr))
  1151. tryLOS = true;
  1152. }
  1153. for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
  1154. {
  1155. dtPolyRef neighbourRef = bestTile->links[i].ref;
  1156. // Skip invalid ids and do not expand back to where we came from.
  1157. if (!neighbourRef || neighbourRef == parentRef)
  1158. continue;
  1159. // Get neighbour poly and tile.
  1160. // The API input has been cheked already, skip checking internal data.
  1161. const dtMeshTile* neighbourTile = 0;
  1162. const dtPoly* neighbourPoly = 0;
  1163. if (dtStatusFailed(m_nav->getTileAndPolyByRef(neighbourRef, &neighbourTile, &neighbourPoly))) {
  1164. continue;
  1165. }
  1166. if (!m_query.filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  1167. continue;
  1168. // get the neighbor node
  1169. dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, 0);
  1170. if (!neighbourNode)
  1171. {
  1172. m_query.status |= DT_OUT_OF_NODES;
  1173. continue;
  1174. }
  1175. // do not expand to nodes that were already visited from the same parent
  1176. if (neighbourNode->pidx != 0 && neighbourNode->pidx == bestNode->pidx)
  1177. continue;
  1178. // If the node is visited the first time, calculate node position.
  1179. if (neighbourNode->flags == 0)
  1180. {
  1181. getEdgeMidPoint(bestRef, bestPoly, bestTile,
  1182. neighbourRef, neighbourPoly, neighbourTile,
  1183. neighbourNode->pos);
  1184. }
  1185. // Calculate cost and heuristic.
  1186. float cost = 0;
  1187. float heuristic = 0;
  1188. // raycast parent
  1189. bool foundShortCut = false;
  1190. rayHit.pathCost = rayHit.t = 0;
  1191. if (tryLOS)
  1192. {
  1193. raycast(parentRef, parentNode->pos, neighbourNode->pos, m_query.filter, DT_RAYCAST_USE_COSTS, &rayHit, grandpaRef);
  1194. foundShortCut = rayHit.t >= 1.0f;
  1195. }
  1196. // update move cost
  1197. if (foundShortCut)
  1198. {
  1199. // shortcut found using raycast. Using shorter cost instead
  1200. cost = parentNode->cost + rayHit.pathCost;
  1201. }
  1202. else
  1203. {
  1204. // No shortcut found.
  1205. const float curCost = m_query.filter->getCost(bestNode->pos, neighbourNode->pos,
  1206. parentRef, parentTile, parentPoly,
  1207. bestRef, bestTile, bestPoly,
  1208. neighbourRef, neighbourTile, neighbourPoly);
  1209. cost = bestNode->cost + curCost;
  1210. }
  1211. // Special case for last node.
  1212. if (neighbourRef == m_query.endRef)
  1213. {
  1214. const float endCost = m_query.filter->getCost(neighbourNode->pos, m_query.endPos,
  1215. bestRef, bestTile, bestPoly,
  1216. neighbourRef, neighbourTile, neighbourPoly,
  1217. 0, 0, 0);
  1218. cost = cost + endCost;
  1219. heuristic = 0;
  1220. }
  1221. else
  1222. {
  1223. heuristic = dtVdist(neighbourNode->pos, m_query.endPos)*H_SCALE;
  1224. }
  1225. const float total = cost + heuristic;
  1226. // The node is already in open list and the new result is worse, skip.
  1227. if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
  1228. continue;
  1229. // The node is already visited and process, and the new result is worse, skip.
  1230. if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
  1231. continue;
  1232. // Add or update the node.
  1233. neighbourNode->pidx = foundShortCut ? bestNode->pidx : m_nodePool->getNodeIdx(bestNode);
  1234. neighbourNode->id = neighbourRef;
  1235. neighbourNode->flags = (neighbourNode->flags & ~(DT_NODE_CLOSED | DT_NODE_PARENT_DETACHED));
  1236. neighbourNode->cost = cost;
  1237. neighbourNode->total = total;
  1238. if (foundShortCut)
  1239. neighbourNode->flags = (neighbourNode->flags | DT_NODE_PARENT_DETACHED);
  1240. if (neighbourNode->flags & DT_NODE_OPEN)
  1241. {
  1242. // Already in open, update node location.
  1243. m_openList->modify(neighbourNode);
  1244. }
  1245. else
  1246. {
  1247. // Put the node in open list.
  1248. neighbourNode->flags |= DT_NODE_OPEN;
  1249. m_openList->push(neighbourNode);
  1250. }
  1251. // Update nearest node to target so far.
  1252. if (heuristic < m_query.lastBestNodeCost)
  1253. {
  1254. m_query.lastBestNodeCost = heuristic;
  1255. m_query.lastBestNode = neighbourNode;
  1256. }
  1257. }
  1258. }
  1259. // Exhausted all nodes, but could not find path.
  1260. if (m_openList->empty())
  1261. {
  1262. const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
  1263. m_query.status = DT_SUCCESS | details;
  1264. }
  1265. if (doneIters)
  1266. *doneIters = iter;
  1267. return m_query.status;
  1268. }
  1269. dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath)
  1270. {
  1271. if (!pathCount)
  1272. return DT_FAILURE | DT_INVALID_PARAM;
  1273. *pathCount = 0;
  1274. if (!path || maxPath <= 0)
  1275. return DT_FAILURE | DT_INVALID_PARAM;
  1276. if (dtStatusFailed(m_query.status))
  1277. {
  1278. // Reset query.
  1279. memset(&m_query, 0, sizeof(dtQueryData));
  1280. return DT_FAILURE;
  1281. }
  1282. int n = 0;
  1283. if (m_query.startRef == m_query.endRef)
  1284. {
  1285. // Special case: the search starts and ends at same poly.
  1286. path[n++] = m_query.startRef;
  1287. }
  1288. else
  1289. {
  1290. // Reverse the path.
  1291. dtAssert(m_query.lastBestNode);
  1292. if (m_query.lastBestNode->id != m_query.endRef)
  1293. m_query.status |= DT_PARTIAL_RESULT;
  1294. dtNode* prev = 0;
  1295. dtNode* node = m_query.lastBestNode;
  1296. int prevRay = 0;
  1297. do
  1298. {
  1299. dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
  1300. node->pidx = m_nodePool->getNodeIdx(prev);
  1301. prev = node;
  1302. int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
  1303. node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
  1304. prevRay = nextRay;
  1305. node = next;
  1306. }
  1307. while (node);
  1308. // Store path
  1309. node = prev;
  1310. do
  1311. {
  1312. dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
  1313. dtStatus status = 0;
  1314. if (node->flags & DT_NODE_PARENT_DETACHED)
  1315. {
  1316. float t, normal[3];
  1317. int m;
  1318. status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n);
  1319. n += m;
  1320. // raycast ends on poly boundary and the path might include the next poly boundary.
  1321. if (path[n-1] == next->id)
  1322. n--; // remove to avoid duplicates
  1323. }
  1324. else
  1325. {
  1326. path[n++] = node->id;
  1327. if (n >= maxPath)
  1328. status = DT_BUFFER_TOO_SMALL;
  1329. }
  1330. if (status & DT_STATUS_DETAIL_MASK)
  1331. {
  1332. m_query.status |= status & DT_STATUS_DETAIL_MASK;
  1333. break;
  1334. }
  1335. node = next;
  1336. }
  1337. while (node);
  1338. }
  1339. const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
  1340. // Reset query.
  1341. memset(&m_query, 0, sizeof(dtQueryData));
  1342. *pathCount = n;
  1343. return DT_SUCCESS | details;
  1344. }
  1345. dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
  1346. dtPolyRef* path, int* pathCount, const int maxPath)
  1347. {
  1348. if (!pathCount)
  1349. return DT_FAILURE | DT_INVALID_PARAM;
  1350. *pathCount = 0;
  1351. if (!existing || existingSize <= 0 || !path || !pathCount || maxPath <= 0)
  1352. return DT_FAILURE | DT_INVALID_PARAM;
  1353. if (dtStatusFailed(m_query.status))
  1354. {
  1355. // Reset query.
  1356. memset(&m_query, 0, sizeof(dtQueryData));
  1357. return DT_FAILURE;
  1358. }
  1359. int n = 0;
  1360. if (m_query.startRef == m_query.endRef)
  1361. {
  1362. // Special case: the search starts and ends at same poly.
  1363. path[n++] = m_query.startRef;
  1364. }
  1365. else
  1366. {
  1367. // Find furthest existing node that was visited.
  1368. dtNode* prev = 0;
  1369. dtNode* node = 0;
  1370. for (int i = existingSize-1; i >= 0; --i)
  1371. {
  1372. m_nodePool->findNodes(existing[i], &node, 1);
  1373. if (node)
  1374. break;
  1375. }
  1376. if (!node)
  1377. {
  1378. m_query.status |= DT_PARTIAL_RESULT;
  1379. dtAssert(m_query.lastBestNode);
  1380. node = m_query.lastBestNode;
  1381. }
  1382. // Reverse the path.
  1383. int prevRay = 0;
  1384. do
  1385. {
  1386. dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
  1387. node->pidx = m_nodePool->getNodeIdx(prev);
  1388. prev = node;
  1389. int nextRay = node->flags & DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
  1390. node->flags = (node->flags & ~DT_NODE_PARENT_DETACHED) | prevRay; // and store it in the reversed path's node
  1391. prevRay = nextRay;
  1392. node = next;
  1393. }
  1394. while (node);
  1395. // Store path
  1396. node = prev;
  1397. do
  1398. {
  1399. dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
  1400. dtStatus status = 0;
  1401. if (node->flags & DT_NODE_PARENT_DETACHED)
  1402. {
  1403. float t, normal[3];
  1404. int m;
  1405. status = raycast(node->id, node->pos, next->pos, m_query.filter, &t, normal, path+n, &m, maxPath-n);
  1406. n += m;
  1407. // raycast ends on poly boundary and the path might include the next poly boundary.
  1408. if (path[n-1] == next->id)
  1409. n--; // remove to avoid duplicates
  1410. }
  1411. else
  1412. {
  1413. path[n++] = node->id;
  1414. if (n >= maxPath)
  1415. status = DT_BUFFER_TOO_SMALL;
  1416. }
  1417. if (status & DT_STATUS_DETAIL_MASK)
  1418. {
  1419. m_query.status |= status & DT_STATUS_DETAIL_MASK;
  1420. break;
  1421. }
  1422. node = next;
  1423. }
  1424. while (node);
  1425. }
  1426. const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
  1427. // Reset query.
  1428. memset(&m_query, 0, sizeof(dtQueryData));
  1429. *pathCount = n;
  1430. return DT_SUCCESS | details;
  1431. }
  1432. dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
  1433. float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
  1434. int* straightPathCount, const int maxStraightPath) const
  1435. {
  1436. if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
  1437. {
  1438. // The vertices are equal, update flags and poly.
  1439. if (straightPathFlags)
  1440. straightPathFlags[(*straightPathCount)-1] = flags;
  1441. if (straightPathRefs)
  1442. straightPathRefs[(*straightPathCount)-1] = ref;
  1443. }
  1444. else
  1445. {
  1446. // Append new vertex.
  1447. dtVcopy(&straightPath[(*straightPathCount)*3], pos);
  1448. if (straightPathFlags)
  1449. straightPathFlags[(*straightPathCount)] = flags;
  1450. if (straightPathRefs)
  1451. straightPathRefs[(*straightPathCount)] = ref;
  1452. (*straightPathCount)++;
  1453. // If there is no space to append more vertices, return.
  1454. if ((*straightPathCount) >= maxStraightPath)
  1455. {
  1456. return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
  1457. }
  1458. // If reached end of path, return.
  1459. if (flags == DT_STRAIGHTPATH_END)
  1460. {
  1461. return DT_SUCCESS;
  1462. }
  1463. }
  1464. return DT_IN_PROGRESS;
  1465. }
  1466. dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
  1467. float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
  1468. int* straightPathCount, const int maxStraightPath, const int options) const
  1469. {
  1470. const float* startPos = &straightPath[(*straightPathCount-1)*3];
  1471. // Append or update last vertex
  1472. dtStatus stat = 0;
  1473. for (int i = startIdx; i < endIdx; i++)
  1474. {
  1475. // Calculate portal
  1476. const dtPolyRef from = path[i];
  1477. const dtMeshTile* fromTile = 0;
  1478. const dtPoly* fromPoly = 0;
  1479. if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
  1480. return DT_FAILURE | DT_INVALID_PARAM;
  1481. const dtPolyRef to = path[i+1];
  1482. const dtMeshTile* toTile = 0;
  1483. const dtPoly* toPoly = 0;
  1484. if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
  1485. return DT_FAILURE | DT_INVALID_PARAM;
  1486. float left[3], right[3];
  1487. if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
  1488. break;
  1489. if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
  1490. {
  1491. // Skip intersection if only area crossings are requested.
  1492. if (fromPoly->getArea() == toPoly->getArea())
  1493. continue;
  1494. }
  1495. // Append intersection
  1496. float s,t;
  1497. if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
  1498. {
  1499. float pt[3];
  1500. dtVlerp(pt, left,right, t);
  1501. stat = appendVertex(pt, 0, path[i+1],
  1502. straightPath, straightPathFlags, straightPathRefs,
  1503. straightPathCount, maxStraightPath);
  1504. if (stat != DT_IN_PROGRESS)
  1505. return stat;
  1506. }
  1507. }
  1508. return DT_IN_PROGRESS;
  1509. }
  1510. /// @par
  1511. ///
  1512. /// This method peforms what is often called 'string pulling'.
  1513. ///
  1514. /// The start position is clamped to the first polygon in the path, and the
  1515. /// end position is clamped to the last. So the start and end positions should
  1516. /// normally be within or very near the first and last polygons respectively.
  1517. ///
  1518. /// The returned polygon references represent the reference id of the polygon
  1519. /// that is entered at the associated path position. The reference id associated
  1520. /// with the end point will always be zero. This allows, for example, matching
  1521. /// off-mesh link points to their representative polygons.
  1522. ///
  1523. /// If the provided result buffers are too small for the entire result set,
  1524. /// they will be filled as far as possible from the start toward the end
  1525. /// position.
  1526. ///
  1527. dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
  1528. const dtPolyRef* path, const int pathSize,
  1529. float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
  1530. int* straightPathCount, const int maxStraightPath, const int options) const
  1531. {
  1532. dtAssert(m_nav);
  1533. if (!straightPathCount)
  1534. return DT_FAILURE | DT_INVALID_PARAM;
  1535. *straightPathCount = 0;
  1536. if (!startPos || !dtVisfinite(startPos) ||
  1537. !endPos || !dtVisfinite(endPos) ||
  1538. !path || pathSize <= 0 || !path[0] ||
  1539. maxStraightPath <= 0)
  1540. {
  1541. return DT_FAILURE | DT_INVALID_PARAM;
  1542. }
  1543. dtStatus stat = 0;
  1544. // TODO: Should this be callers responsibility?
  1545. float closestStartPos[3];
  1546. if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
  1547. return DT_FAILURE | DT_INVALID_PARAM;
  1548. float closestEndPos[3];
  1549. if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
  1550. return DT_FAILURE | DT_INVALID_PARAM;
  1551. // Add start point.
  1552. stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
  1553. straightPath, straightPathFlags, straightPathRefs,
  1554. straightPathCount, maxStraightPath);
  1555. if (stat != DT_IN_PROGRESS)
  1556. return stat;
  1557. if (pathSize > 1)
  1558. {
  1559. float portalApex[3], portalLeft[3], portalRight[3];
  1560. dtVcopy(portalApex, closestStartPos);
  1561. dtVcopy(portalLeft, portalApex);
  1562. dtVcopy(portalRight, portalApex);
  1563. int apexIndex = 0;
  1564. int leftIndex = 0;
  1565. int rightIndex = 0;
  1566. unsigned char leftPolyType = 0;
  1567. unsigned char rightPolyType = 0;
  1568. dtPolyRef leftPolyRef = path[0];
  1569. dtPolyRef rightPolyRef = path[0];
  1570. for (int i = 0; i < pathSize; ++i)
  1571. {
  1572. float left[3], right[3];
  1573. unsigned char toType;
  1574. if (i+1 < pathSize)
  1575. {
  1576. unsigned char fromType; // fromType is ignored.
  1577. // Next portal.
  1578. if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
  1579. {
  1580. // Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
  1581. // Clamp the end point to path[i], and return the path so far.
  1582. if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
  1583. {
  1584. // This should only happen when the first polygon is invalid.
  1585. return DT_FAILURE | DT_INVALID_PARAM;
  1586. }
  1587. // Apeend portals along the current straight path segment.
  1588. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
  1589. {
  1590. // Ignore status return value as we're just about to return anyway.
  1591. appendPortals(apexIndex, i, closestEndPos, path,
  1592. straightPath, straightPathFlags, straightPathRefs,
  1593. straightPathCount, maxStraightPath, options);
  1594. }
  1595. // Ignore status return value as we're just about to return anyway.
  1596. appendVertex(closestEndPos, 0, path[i],
  1597. straightPath, straightPathFlags, straightPathRefs,
  1598. straightPathCount, maxStraightPath);
  1599. return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
  1600. }
  1601. // If starting really close the portal, advance.
  1602. if (i == 0)
  1603. {
  1604. float t;
  1605. if (dtDistancePtSegSqr2D(portalApex, left, right, t) < dtSqr(0.001f))
  1606. continue;
  1607. }
  1608. }
  1609. else
  1610. {
  1611. // End of the path.
  1612. dtVcopy(left, closestEndPos);
  1613. dtVcopy(right, closestEndPos);
  1614. toType = DT_POLYTYPE_GROUND;
  1615. }
  1616. // Right vertex.
  1617. if (dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
  1618. {
  1619. if (dtVequal(portalApex, portalRight) || dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
  1620. {
  1621. dtVcopy(portalRight, right);
  1622. rightPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
  1623. rightPolyType = toType;
  1624. rightIndex = i;
  1625. }
  1626. else
  1627. {
  1628. // Append portals along the current straight path segment.
  1629. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
  1630. {
  1631. stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
  1632. straightPath, straightPathFlags, straightPathRefs,
  1633. straightPathCount, maxStraightPath, options);
  1634. if (stat != DT_IN_PROGRESS)
  1635. return stat;
  1636. }
  1637. dtVcopy(portalApex, portalLeft);
  1638. apexIndex = leftIndex;
  1639. unsigned char flags = 0;
  1640. if (!leftPolyRef)
  1641. flags = DT_STRAIGHTPATH_END;
  1642. else if (leftPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
  1643. flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
  1644. dtPolyRef ref = leftPolyRef;
  1645. // Append or update vertex
  1646. stat = appendVertex(portalApex, flags, ref,
  1647. straightPath, straightPathFlags, straightPathRefs,
  1648. straightPathCount, maxStraightPath);
  1649. if (stat != DT_IN_PROGRESS)
  1650. return stat;
  1651. dtVcopy(portalLeft, portalApex);
  1652. dtVcopy(portalRight, portalApex);
  1653. leftIndex = apexIndex;
  1654. rightIndex = apexIndex;
  1655. // Restart
  1656. i = apexIndex;
  1657. continue;
  1658. }
  1659. }
  1660. // Left vertex.
  1661. if (dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
  1662. {
  1663. if (dtVequal(portalApex, portalLeft) || dtTriArea2D(portalApex, portalRight, left) < 0.0f)
  1664. {
  1665. dtVcopy(portalLeft, left);
  1666. leftPolyRef = (i+1 < pathSize) ? path[i+1] : 0;
  1667. leftPolyType = toType;
  1668. leftIndex = i;
  1669. }
  1670. else
  1671. {
  1672. // Append portals along the current straight path segment.
  1673. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
  1674. {
  1675. stat = appendPortals(apexIndex, rightIndex, portalRight, path,
  1676. straightPath, straightPathFlags, straightPathRefs,
  1677. straightPathCount, maxStraightPath, options);
  1678. if (stat != DT_IN_PROGRESS)
  1679. return stat;
  1680. }
  1681. dtVcopy(portalApex, portalRight);
  1682. apexIndex = rightIndex;
  1683. unsigned char flags = 0;
  1684. if (!rightPolyRef)
  1685. flags = DT_STRAIGHTPATH_END;
  1686. else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
  1687. flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
  1688. dtPolyRef ref = rightPolyRef;
  1689. // Append or update vertex
  1690. stat = appendVertex(portalApex, flags, ref,
  1691. straightPath, straightPathFlags, straightPathRefs,
  1692. straightPathCount, maxStraightPath);
  1693. if (stat != DT_IN_PROGRESS)
  1694. return stat;
  1695. dtVcopy(portalLeft, portalApex);
  1696. dtVcopy(portalRight, portalApex);
  1697. leftIndex = apexIndex;
  1698. rightIndex = apexIndex;
  1699. // Restart
  1700. i = apexIndex;
  1701. continue;
  1702. }
  1703. }
  1704. }
  1705. // Append portals along the current straight path segment.
  1706. if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
  1707. {
  1708. stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
  1709. straightPath, straightPathFlags, straightPathRefs,
  1710. straightPathCount, maxStraightPath, options);
  1711. if (stat != DT_IN_PROGRESS)
  1712. return stat;
  1713. }
  1714. }
  1715. // Ignore status return value as we're just about to return anyway.
  1716. appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
  1717. straightPath, straightPathFlags, straightPathRefs,
  1718. straightPathCount, maxStraightPath);
  1719. return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
  1720. }
  1721. /// @par
  1722. ///
  1723. /// This method is optimized for small delta movement and a small number of
  1724. /// polygons. If used for too great a distance, the result set will form an
  1725. /// incomplete path.
  1726. ///
  1727. /// @p resultPos will equal the @p endPos if the end is reached.
  1728. /// Otherwise the closest reachable position will be returned.
  1729. ///
  1730. /// @p resultPos is not projected onto the surface of the navigation
  1731. /// mesh. Use #getPolyHeight if this is needed.
  1732. ///
  1733. /// This method treats the end position in the same manner as
  1734. /// the #raycast method. (As a 2D point.) See that method's documentation
  1735. /// for details.
  1736. ///
  1737. /// If the @p visited array is too small to hold the entire result set, it will
  1738. /// be filled as far as possible from the start position toward the end
  1739. /// position.
  1740. ///
  1741. dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
  1742. const dtQueryFilter* filter,
  1743. float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const
  1744. {
  1745. dtAssert(m_nav);
  1746. dtAssert(m_tinyNodePool);
  1747. if (!visitedCount)
  1748. return DT_FAILURE | DT_INVALID_PARAM;
  1749. *visitedCount = 0;
  1750. if (!m_nav->isValidPolyRef(startRef) ||
  1751. !startPos || !dtVisfinite(startPos) ||
  1752. !endPos || !dtVisfinite(endPos) ||
  1753. !filter || !resultPos || !visited ||
  1754. maxVisitedSize <= 0)
  1755. {
  1756. return DT_FAILURE | DT_INVALID_PARAM;
  1757. }
  1758. dtStatus status = DT_SUCCESS;
  1759. static const int MAX_STACK = 48;
  1760. dtNode* stack[MAX_STACK];
  1761. int nstack = 0;
  1762. m_tinyNodePool->clear();
  1763. dtNode* startNode = m_tinyNodePool->getNode(startRef);
  1764. startNode->pidx = 0;
  1765. startNode->cost = 0;
  1766. startNode->total = 0;
  1767. startNode->id = startRef;
  1768. startNode->flags = DT_NODE_CLOSED;
  1769. stack[nstack++] = startNode;
  1770. float bestPos[3];
  1771. float bestDist = FLT_MAX;
  1772. dtNode* bestNode = 0;
  1773. dtVcopy(bestPos, startPos);
  1774. // Search constraints
  1775. float searchPos[3], searchRadSqr;
  1776. dtVlerp(searchPos, startPos, endPos, 0.5f);
  1777. searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);
  1778. float verts[DT_VERTS_PER_POLYGON*3];
  1779. while (nstack)
  1780. {
  1781. // Pop front.
  1782. dtNode* curNode = stack[0];
  1783. for (int i = 0; i < nstack-1; ++i)
  1784. stack[i] = stack[i+1];
  1785. nstack--;
  1786. // Get poly and tile.
  1787. // The API input has been cheked already, skip checking internal data.
  1788. const dtPolyRef curRef = curNode->id;
  1789. const dtMeshTile* curTile = 0;
  1790. const dtPoly* curPoly = 0;
  1791. m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
  1792. // Collect vertices.
  1793. const int nverts = curPoly->vertCount;
  1794. for (int i = 0; i < nverts; ++i)
  1795. dtVcopy(&verts[i*3], &curTile->verts[curPoly->verts[i]*3]);
  1796. // If target is inside the poly, stop search.
  1797. if (dtPointInPolygon(endPos, verts, nverts))
  1798. {
  1799. bestNode = curNode;
  1800. dtVcopy(bestPos, endPos);
  1801. break;
  1802. }
  1803. // Find wall edges and find nearest point inside the walls.
  1804. for (int i = 0, j = (int)curPoly->vertCount-1; i < (int)curPoly->vertCount; j = i++)
  1805. {
  1806. // Find links to neighbours.
  1807. static const int MAX_NEIS = 8;
  1808. int nneis = 0;
  1809. dtPolyRef neis[MAX_NEIS];
  1810. if (curPoly->neis[j] & DT_EXT_LINK)
  1811. {
  1812. // Tile border.
  1813. for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
  1814. {
  1815. const dtLink* link = &curTile->links[k];
  1816. if (link->edge == j)
  1817. {
  1818. if (link->ref != 0)
  1819. {
  1820. const dtMeshTile* neiTile = 0;
  1821. const dtPoly* neiPoly = 0;
  1822. m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
  1823. if (filter->passFilter(link->ref, neiTile, neiPoly))
  1824. {
  1825. if (nneis < MAX_NEIS)
  1826. neis[nneis++] = link->ref;
  1827. }
  1828. }
  1829. }
  1830. }
  1831. }
  1832. else if (curPoly->neis[j])
  1833. {
  1834. const unsigned int idx = (unsigned int)(curPoly->neis[j]-1);
  1835. const dtPolyRef ref = m_nav->getPolyRefBase(curTile) | idx;
  1836. if (filter->passFilter(ref, curTile, &curTile->polys[idx]))
  1837. {
  1838. // Internal edge, encode id.
  1839. neis[nneis++] = ref;
  1840. }
  1841. }
  1842. if (!nneis)
  1843. {
  1844. // Wall edge, calc distance.
  1845. const float* vj = &verts[j*3];
  1846. const float* vi = &verts[i*3];
  1847. float tseg;
  1848. const float distSqr = dtDistancePtSegSqr2D(endPos, vj, vi, tseg);
  1849. if (distSqr < bestDist)
  1850. {
  1851. // Update nearest distance.
  1852. dtVlerp(bestPos, vj,vi, tseg);
  1853. bestDist = distSqr;
  1854. bestNode = curNode;
  1855. }
  1856. }
  1857. else
  1858. {
  1859. for (int k = 0; k < nneis; ++k)
  1860. {
  1861. // Skip if no node can be allocated.
  1862. dtNode* neighbourNode = m_tinyNodePool->getNode(neis[k]);
  1863. if (!neighbourNode)
  1864. continue;
  1865. // Skip if already visited.
  1866. if (neighbourNode->flags & DT_NODE_CLOSED)
  1867. continue;
  1868. // Skip the link if it is too far from search constraint.
  1869. // TODO: Maybe should use getPortalPoints(), but this one is way faster.
  1870. const float* vj = &verts[j*3];
  1871. const float* vi = &verts[i*3];
  1872. float tseg;
  1873. float distSqr = dtDistancePtSegSqr2D(searchPos, vj, vi, tseg);
  1874. if (distSqr > searchRadSqr)
  1875. continue;
  1876. // Mark as the node as visited and push to queue.
  1877. if (nstack < MAX_STACK)
  1878. {
  1879. neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
  1880. neighbourNode->flags |= DT_NODE_CLOSED;
  1881. stack[nstack++] = neighbourNode;
  1882. }
  1883. }
  1884. }
  1885. }
  1886. }
  1887. int n = 0;
  1888. if (bestNode)
  1889. {
  1890. // Reverse the path.
  1891. dtNode* prev = 0;
  1892. dtNode* node = bestNode;
  1893. do
  1894. {
  1895. dtNode* next = m_tinyNodePool->getNodeAtIdx(node->pidx);
  1896. node->pidx = m_tinyNodePool->getNodeIdx(prev);
  1897. prev = node;
  1898. node = next;
  1899. }
  1900. while (node);
  1901. // Store result
  1902. node = prev;
  1903. do
  1904. {
  1905. visited[n++] = node->id;
  1906. if (n >= maxVisitedSize)
  1907. {
  1908. status |= DT_BUFFER_TOO_SMALL;
  1909. break;
  1910. }
  1911. node = m_tinyNodePool->getNodeAtIdx(node->pidx);
  1912. }
  1913. while (node);
  1914. }
  1915. dtVcopy(resultPos, bestPos);
  1916. *visitedCount = n;
  1917. return status;
  1918. }
  1919. dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
  1920. unsigned char& fromType, unsigned char& toType) const
  1921. {
  1922. dtAssert(m_nav);
  1923. const dtMeshTile* fromTile = 0;
  1924. const dtPoly* fromPoly = 0;
  1925. if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
  1926. return DT_FAILURE | DT_INVALID_PARAM;
  1927. fromType = fromPoly->getType();
  1928. const dtMeshTile* toTile = 0;
  1929. const dtPoly* toPoly = 0;
  1930. if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
  1931. return DT_FAILURE | DT_INVALID_PARAM;
  1932. toType = toPoly->getType();
  1933. return getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right);
  1934. }
  1935. // Returns portal points between two polygons.
  1936. dtStatus dtNavMeshQuery::getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
  1937. dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
  1938. float* left, float* right) const
  1939. {
  1940. // Find the link that points to the 'to' polygon.
  1941. const dtLink* link = 0;
  1942. for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
  1943. {
  1944. if (fromTile->links[i].ref == to)
  1945. {
  1946. link = &fromTile->links[i];
  1947. break;
  1948. }
  1949. }
  1950. if (!link)
  1951. return DT_FAILURE | DT_INVALID_PARAM;
  1952. // Handle off-mesh connections.
  1953. if (fromPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  1954. {
  1955. // Find link that points to first vertex.
  1956. for (unsigned int i = fromPoly->firstLink; i != DT_NULL_LINK; i = fromTile->links[i].next)
  1957. {
  1958. if (fromTile->links[i].ref == to)
  1959. {
  1960. const int v = fromTile->links[i].edge;
  1961. dtVcopy(left, &fromTile->verts[fromPoly->verts[v]*3]);
  1962. dtVcopy(right, &fromTile->verts[fromPoly->verts[v]*3]);
  1963. return DT_SUCCESS;
  1964. }
  1965. }
  1966. return DT_FAILURE | DT_INVALID_PARAM;
  1967. }
  1968. if (toPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  1969. {
  1970. for (unsigned int i = toPoly->firstLink; i != DT_NULL_LINK; i = toTile->links[i].next)
  1971. {
  1972. if (toTile->links[i].ref == from)
  1973. {
  1974. const int v = toTile->links[i].edge;
  1975. dtVcopy(left, &toTile->verts[toPoly->verts[v]*3]);
  1976. dtVcopy(right, &toTile->verts[toPoly->verts[v]*3]);
  1977. return DT_SUCCESS;
  1978. }
  1979. }
  1980. return DT_FAILURE | DT_INVALID_PARAM;
  1981. }
  1982. // Find portal vertices.
  1983. const int v0 = fromPoly->verts[link->edge];
  1984. const int v1 = fromPoly->verts[(link->edge+1) % (int)fromPoly->vertCount];
  1985. dtVcopy(left, &fromTile->verts[v0*3]);
  1986. dtVcopy(right, &fromTile->verts[v1*3]);
  1987. // If the link is at tile boundary, dtClamp the vertices to
  1988. // the link width.
  1989. if (link->side != 0xff)
  1990. {
  1991. // Unpack portal limits.
  1992. if (link->bmin != 0 || link->bmax != 255)
  1993. {
  1994. const float s = 1.0f/255.0f;
  1995. const float tmin = link->bmin*s;
  1996. const float tmax = link->bmax*s;
  1997. dtVlerp(left, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmin);
  1998. dtVlerp(right, &fromTile->verts[v0*3], &fromTile->verts[v1*3], tmax);
  1999. }
  2000. }
  2001. return DT_SUCCESS;
  2002. }
  2003. // Returns edge mid point between two polygons.
  2004. dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const
  2005. {
  2006. float left[3], right[3];
  2007. unsigned char fromType, toType;
  2008. if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
  2009. return DT_FAILURE | DT_INVALID_PARAM;
  2010. mid[0] = (left[0]+right[0])*0.5f;
  2011. mid[1] = (left[1]+right[1])*0.5f;
  2012. mid[2] = (left[2]+right[2])*0.5f;
  2013. return DT_SUCCESS;
  2014. }
  2015. dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
  2016. dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
  2017. float* mid) const
  2018. {
  2019. float left[3], right[3];
  2020. if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
  2021. return DT_FAILURE | DT_INVALID_PARAM;
  2022. mid[0] = (left[0]+right[0])*0.5f;
  2023. mid[1] = (left[1]+right[1])*0.5f;
  2024. mid[2] = (left[2]+right[2])*0.5f;
  2025. return DT_SUCCESS;
  2026. }
  2027. /// @par
  2028. ///
  2029. /// This method is meant to be used for quick, short distance checks.
  2030. ///
  2031. /// If the path array is too small to hold the result, it will be filled as
  2032. /// far as possible from the start postion toward the end position.
  2033. ///
  2034. /// <b>Using the Hit Parameter (t)</b>
  2035. ///
  2036. /// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
  2037. /// the end position. In this case the path represents a valid corridor to the
  2038. /// end position and the value of @p hitNormal is undefined.
  2039. ///
  2040. /// If the hit parameter is zero, then the start position is on the wall that
  2041. /// was hit and the value of @p hitNormal is undefined.
  2042. ///
  2043. /// If 0 < t < 1.0 then the following applies:
  2044. ///
  2045. /// @code
  2046. /// distanceToHitBorder = distanceToEndPosition * t
  2047. /// hitPoint = startPos + (endPos - startPos) * t
  2048. /// @endcode
  2049. ///
  2050. /// <b>Use Case Restriction</b>
  2051. ///
  2052. /// The raycast ignores the y-value of the end position. (2D check.) This
  2053. /// places significant limits on how it can be used. For example:
  2054. ///
  2055. /// Consider a scene where there is a main floor with a second floor balcony
  2056. /// that hangs over the main floor. So the first floor mesh extends below the
  2057. /// balcony mesh. The start position is somewhere on the first floor. The end
  2058. /// position is on the balcony.
  2059. ///
  2060. /// The raycast will search toward the end position along the first floor mesh.
  2061. /// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
  2062. /// (no wall hit), meaning it reached the end position. This is one example of why
  2063. /// this method is meant for short distance checks.
  2064. ///
  2065. dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
  2066. const dtQueryFilter* filter,
  2067. float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const
  2068. {
  2069. dtRaycastHit hit;
  2070. hit.path = path;
  2071. hit.maxPath = maxPath;
  2072. dtStatus status = raycast(startRef, startPos, endPos, filter, 0, &hit);
  2073. *t = hit.t;
  2074. if (hitNormal)
  2075. dtVcopy(hitNormal, hit.hitNormal);
  2076. if (pathCount)
  2077. *pathCount = hit.pathCount;
  2078. return status;
  2079. }
  2080. /// @par
  2081. ///
  2082. /// This method is meant to be used for quick, short distance checks.
  2083. ///
  2084. /// If the path array is too small to hold the result, it will be filled as
  2085. /// far as possible from the start postion toward the end position.
  2086. ///
  2087. /// <b>Using the Hit Parameter t of RaycastHit</b>
  2088. ///
  2089. /// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
  2090. /// the end position. In this case the path represents a valid corridor to the
  2091. /// end position and the value of @p hitNormal is undefined.
  2092. ///
  2093. /// If the hit parameter is zero, then the start position is on the wall that
  2094. /// was hit and the value of @p hitNormal is undefined.
  2095. ///
  2096. /// If 0 < t < 1.0 then the following applies:
  2097. ///
  2098. /// @code
  2099. /// distanceToHitBorder = distanceToEndPosition * t
  2100. /// hitPoint = startPos + (endPos - startPos) * t
  2101. /// @endcode
  2102. ///
  2103. /// <b>Use Case Restriction</b>
  2104. ///
  2105. /// The raycast ignores the y-value of the end position. (2D check.) This
  2106. /// places significant limits on how it can be used. For example:
  2107. ///
  2108. /// Consider a scene where there is a main floor with a second floor balcony
  2109. /// that hangs over the main floor. So the first floor mesh extends below the
  2110. /// balcony mesh. The start position is somewhere on the first floor. The end
  2111. /// position is on the balcony.
  2112. ///
  2113. /// The raycast will search toward the end position along the first floor mesh.
  2114. /// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
  2115. /// (no wall hit), meaning it reached the end position. This is one example of why
  2116. /// this method is meant for short distance checks.
  2117. ///
  2118. dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
  2119. const dtQueryFilter* filter, const unsigned int options,
  2120. dtRaycastHit* hit, dtPolyRef prevRef) const
  2121. {
  2122. dtAssert(m_nav);
  2123. if (!hit)
  2124. return DT_FAILURE | DT_INVALID_PARAM;
  2125. hit->t = 0;
  2126. hit->pathCount = 0;
  2127. hit->pathCost = 0;
  2128. // Validate input
  2129. if (!m_nav->isValidPolyRef(startRef) ||
  2130. !startPos || !dtVisfinite(startPos) ||
  2131. !endPos || !dtVisfinite(endPos) ||
  2132. !filter ||
  2133. (prevRef && !m_nav->isValidPolyRef(prevRef)))
  2134. {
  2135. return DT_FAILURE | DT_INVALID_PARAM;
  2136. }
  2137. float dir[3], curPos[3], lastPos[3];
  2138. float verts[DT_VERTS_PER_POLYGON*3+3];
  2139. int n = 0;
  2140. dtVcopy(curPos, startPos);
  2141. dtVsub(dir, endPos, startPos);
  2142. dtVset(hit->hitNormal, 0, 0, 0);
  2143. dtStatus status = DT_SUCCESS;
  2144. const dtMeshTile* prevTile, *tile, *nextTile;
  2145. const dtPoly* prevPoly, *poly, *nextPoly;
  2146. dtPolyRef curRef;
  2147. // The API input has been checked already, skip checking internal data.
  2148. curRef = startRef;
  2149. tile = 0;
  2150. poly = 0;
  2151. m_nav->getTileAndPolyByRefUnsafe(curRef, &tile, &poly);
  2152. nextTile = prevTile = tile;
  2153. nextPoly = prevPoly = poly;
  2154. if (prevRef)
  2155. m_nav->getTileAndPolyByRefUnsafe(prevRef, &prevTile, &prevPoly);
  2156. while (curRef)
  2157. {
  2158. // Cast ray against current polygon.
  2159. // Collect vertices.
  2160. int nv = 0;
  2161. for (int i = 0; i < (int)poly->vertCount; ++i)
  2162. {
  2163. dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
  2164. nv++;
  2165. }
  2166. float tmin, tmax;
  2167. int segMin, segMax;
  2168. if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
  2169. {
  2170. // Could not hit the polygon, keep the old t and report hit.
  2171. hit->pathCount = n;
  2172. return status;
  2173. }
  2174. hit->hitEdgeIndex = segMax;
  2175. // Keep track of furthest t so far.
  2176. if (tmax > hit->t)
  2177. hit->t = tmax;
  2178. // Store visited polygons.
  2179. if (n < hit->maxPath)
  2180. hit->path[n++] = curRef;
  2181. else
  2182. status |= DT_BUFFER_TOO_SMALL;
  2183. // Ray end is completely inside the polygon.
  2184. if (segMax == -1)
  2185. {
  2186. hit->t = FLT_MAX;
  2187. hit->pathCount = n;
  2188. // add the cost
  2189. if (options & DT_RAYCAST_USE_COSTS)
  2190. hit->pathCost += filter->getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
  2191. return status;
  2192. }
  2193. // Follow neighbours.
  2194. dtPolyRef nextRef = 0;
  2195. for (unsigned int i = poly->firstLink; i != DT_NULL_LINK; i = tile->links[i].next)
  2196. {
  2197. const dtLink* link = &tile->links[i];
  2198. // Find link which contains this edge.
  2199. if ((int)link->edge != segMax)
  2200. continue;
  2201. // Get pointer to the next polygon.
  2202. nextTile = 0;
  2203. nextPoly = 0;
  2204. m_nav->getTileAndPolyByRefUnsafe(link->ref, &nextTile, &nextPoly);
  2205. // Skip off-mesh connections.
  2206. if (nextPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  2207. continue;
  2208. // Skip links based on filter.
  2209. if (!filter->passFilter(link->ref, nextTile, nextPoly))
  2210. continue;
  2211. // If the link is internal, just return the ref.
  2212. if (link->side == 0xff)
  2213. {
  2214. nextRef = link->ref;
  2215. break;
  2216. }
  2217. // If the link is at tile boundary,
  2218. // Check if the link spans the whole edge, and accept.
  2219. if (link->bmin == 0 && link->bmax == 255)
  2220. {
  2221. nextRef = link->ref;
  2222. break;
  2223. }
  2224. // Check for partial edge links.
  2225. const int v0 = poly->verts[link->edge];
  2226. const int v1 = poly->verts[(link->edge+1) % poly->vertCount];
  2227. const float* left = &tile->verts[v0*3];
  2228. const float* right = &tile->verts[v1*3];
  2229. // Check that the intersection lies inside the link portal.
  2230. if (link->side == 0 || link->side == 4)
  2231. {
  2232. // Calculate link size.
  2233. const float s = 1.0f/255.0f;
  2234. float lmin = left[2] + (right[2] - left[2])*(link->bmin*s);
  2235. float lmax = left[2] + (right[2] - left[2])*(link->bmax*s);
  2236. if (lmin > lmax) dtSwap(lmin, lmax);
  2237. // Find Z intersection.
  2238. float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
  2239. if (z >= lmin && z <= lmax)
  2240. {
  2241. nextRef = link->ref;
  2242. break;
  2243. }
  2244. }
  2245. else if (link->side == 2 || link->side == 6)
  2246. {
  2247. // Calculate link size.
  2248. const float s = 1.0f/255.0f;
  2249. float lmin = left[0] + (right[0] - left[0])*(link->bmin*s);
  2250. float lmax = left[0] + (right[0] - left[0])*(link->bmax*s);
  2251. if (lmin > lmax) dtSwap(lmin, lmax);
  2252. // Find X intersection.
  2253. float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
  2254. if (x >= lmin && x <= lmax)
  2255. {
  2256. nextRef = link->ref;
  2257. break;
  2258. }
  2259. }
  2260. }
  2261. // add the cost
  2262. if (options & DT_RAYCAST_USE_COSTS)
  2263. {
  2264. // compute the intersection point at the furthest end of the polygon
  2265. // and correct the height (since the raycast moves in 2d)
  2266. dtVcopy(lastPos, curPos);
  2267. dtVmad(curPos, startPos, dir, hit->t);
  2268. float* e1 = &verts[segMax*3];
  2269. float* e2 = &verts[((segMax+1)%nv)*3];
  2270. float eDir[3], diff[3];
  2271. dtVsub(eDir, e2, e1);
  2272. dtVsub(diff, curPos, e1);
  2273. float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
  2274. curPos[1] = e1[1] + eDir[1] * s;
  2275. hit->pathCost += filter->getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
  2276. }
  2277. if (!nextRef)
  2278. {
  2279. // No neighbour, we hit a wall.
  2280. // Calculate hit normal.
  2281. const int a = segMax;
  2282. const int b = segMax+1 < nv ? segMax+1 : 0;
  2283. const float* va = &verts[a*3];
  2284. const float* vb = &verts[b*3];
  2285. const float dx = vb[0] - va[0];
  2286. const float dz = vb[2] - va[2];
  2287. hit->hitNormal[0] = dz;
  2288. hit->hitNormal[1] = 0;
  2289. hit->hitNormal[2] = -dx;
  2290. dtVnormalize(hit->hitNormal);
  2291. hit->pathCount = n;
  2292. return status;
  2293. }
  2294. // No hit, advance to neighbour polygon.
  2295. prevRef = curRef;
  2296. curRef = nextRef;
  2297. prevTile = tile;
  2298. tile = nextTile;
  2299. prevPoly = poly;
  2300. poly = nextPoly;
  2301. }
  2302. hit->pathCount = n;
  2303. return status;
  2304. }
  2305. /// @par
  2306. ///
  2307. /// At least one result array must be provided.
  2308. ///
  2309. /// The order of the result set is from least to highest cost to reach the polygon.
  2310. ///
  2311. /// A common use case for this method is to perform Dijkstra searches.
  2312. /// Candidate polygons are found by searching the graph beginning at the start polygon.
  2313. ///
  2314. /// If a polygon is not found via the graph search, even if it intersects the
  2315. /// search circle, it will not be included in the result set. For example:
  2316. ///
  2317. /// polyA is the start polygon.
  2318. /// polyB shares an edge with polyA. (Is adjacent.)
  2319. /// polyC shares an edge with polyB, but not with polyA
  2320. /// Even if the search circle overlaps polyC, it will not be included in the
  2321. /// result set unless polyB is also in the set.
  2322. ///
  2323. /// The value of the center point is used as the start position for cost
  2324. /// calculations. It is not projected onto the surface of the mesh, so its
  2325. /// y-value will effect the costs.
  2326. ///
  2327. /// Intersection tests occur in 2D. All polygons and the search circle are
  2328. /// projected onto the xz-plane. So the y-value of the center point does not
  2329. /// effect intersection tests.
  2330. ///
  2331. /// If the result arrays are to small to hold the entire result set, they will be
  2332. /// filled to capacity.
  2333. ///
  2334. dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
  2335. const dtQueryFilter* filter,
  2336. dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
  2337. int* resultCount, const int maxResult) const
  2338. {
  2339. dtAssert(m_nav);
  2340. dtAssert(m_nodePool);
  2341. dtAssert(m_openList);
  2342. if (!resultCount)
  2343. return DT_FAILURE | DT_INVALID_PARAM;
  2344. *resultCount = 0;
  2345. if (!m_nav->isValidPolyRef(startRef) ||
  2346. !centerPos || !dtVisfinite(centerPos) ||
  2347. radius < 0 || !dtMathIsfinite(radius) ||
  2348. !filter || maxResult < 0)
  2349. {
  2350. return DT_FAILURE | DT_INVALID_PARAM;
  2351. }
  2352. m_nodePool->clear();
  2353. m_openList->clear();
  2354. dtNode* startNode = m_nodePool->getNode(startRef);
  2355. dtVcopy(startNode->pos, centerPos);
  2356. startNode->pidx = 0;
  2357. startNode->cost = 0;
  2358. startNode->total = 0;
  2359. startNode->id = startRef;
  2360. startNode->flags = DT_NODE_OPEN;
  2361. m_openList->push(startNode);
  2362. dtStatus status = DT_SUCCESS;
  2363. int n = 0;
  2364. const float radiusSqr = dtSqr(radius);
  2365. while (!m_openList->empty())
  2366. {
  2367. dtNode* bestNode = m_openList->pop();
  2368. bestNode->flags &= ~DT_NODE_OPEN;
  2369. bestNode->flags |= DT_NODE_CLOSED;
  2370. // Get poly and tile.
  2371. // The API input has been cheked already, skip checking internal data.
  2372. const dtPolyRef bestRef = bestNode->id;
  2373. const dtMeshTile* bestTile = 0;
  2374. const dtPoly* bestPoly = 0;
  2375. m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
  2376. // Get parent poly and tile.
  2377. dtPolyRef parentRef = 0;
  2378. const dtMeshTile* parentTile = 0;
  2379. const dtPoly* parentPoly = 0;
  2380. if (bestNode->pidx)
  2381. parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
  2382. if (parentRef)
  2383. m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
  2384. if (n < maxResult)
  2385. {
  2386. if (resultRef)
  2387. resultRef[n] = bestRef;
  2388. if (resultParent)
  2389. resultParent[n] = parentRef;
  2390. if (resultCost)
  2391. resultCost[n] = bestNode->total;
  2392. ++n;
  2393. }
  2394. else
  2395. {
  2396. status |= DT_BUFFER_TOO_SMALL;
  2397. }
  2398. for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
  2399. {
  2400. const dtLink* link = &bestTile->links[i];
  2401. dtPolyRef neighbourRef = link->ref;
  2402. // Skip invalid neighbours and do not follow back to parent.
  2403. if (!neighbourRef || neighbourRef == parentRef)
  2404. continue;
  2405. // Expand to neighbour
  2406. const dtMeshTile* neighbourTile = 0;
  2407. const dtPoly* neighbourPoly = 0;
  2408. m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
  2409. // Do not advance if the polygon is excluded by the filter.
  2410. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  2411. continue;
  2412. // Find edge and calc distance to the edge.
  2413. float va[3], vb[3];
  2414. if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
  2415. continue;
  2416. // If the circle is not touching the next polygon, skip it.
  2417. float tseg;
  2418. float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
  2419. if (distSqr > radiusSqr)
  2420. continue;
  2421. dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
  2422. if (!neighbourNode)
  2423. {
  2424. status |= DT_OUT_OF_NODES;
  2425. continue;
  2426. }
  2427. if (neighbourNode->flags & DT_NODE_CLOSED)
  2428. continue;
  2429. // Cost
  2430. if (neighbourNode->flags == 0)
  2431. dtVlerp(neighbourNode->pos, va, vb, 0.5f);
  2432. float cost = filter->getCost(
  2433. bestNode->pos, neighbourNode->pos,
  2434. parentRef, parentTile, parentPoly,
  2435. bestRef, bestTile, bestPoly,
  2436. neighbourRef, neighbourTile, neighbourPoly);
  2437. const float total = bestNode->total + cost;
  2438. // The node is already in open list and the new result is worse, skip.
  2439. if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
  2440. continue;
  2441. neighbourNode->id = neighbourRef;
  2442. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
  2443. neighbourNode->total = total;
  2444. if (neighbourNode->flags & DT_NODE_OPEN)
  2445. {
  2446. m_openList->modify(neighbourNode);
  2447. }
  2448. else
  2449. {
  2450. neighbourNode->flags = DT_NODE_OPEN;
  2451. m_openList->push(neighbourNode);
  2452. }
  2453. }
  2454. }
  2455. *resultCount = n;
  2456. return status;
  2457. }
  2458. /// @par
  2459. ///
  2460. /// The order of the result set is from least to highest cost.
  2461. ///
  2462. /// At least one result array must be provided.
  2463. ///
  2464. /// A common use case for this method is to perform Dijkstra searches.
  2465. /// Candidate polygons are found by searching the graph beginning at the start
  2466. /// polygon.
  2467. ///
  2468. /// The same intersection test restrictions that apply to findPolysAroundCircle()
  2469. /// method apply to this method.
  2470. ///
  2471. /// The 3D centroid of the search polygon is used as the start position for cost
  2472. /// calculations.
  2473. ///
  2474. /// Intersection tests occur in 2D. All polygons are projected onto the
  2475. /// xz-plane. So the y-values of the vertices do not effect intersection tests.
  2476. ///
  2477. /// If the result arrays are is too small to hold the entire result set, they will
  2478. /// be filled to capacity.
  2479. ///
  2480. dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
  2481. const dtQueryFilter* filter,
  2482. dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
  2483. int* resultCount, const int maxResult) const
  2484. {
  2485. dtAssert(m_nav);
  2486. dtAssert(m_nodePool);
  2487. dtAssert(m_openList);
  2488. if (!resultCount)
  2489. return DT_FAILURE | DT_INVALID_PARAM;
  2490. *resultCount = 0;
  2491. if (!m_nav->isValidPolyRef(startRef) ||
  2492. !verts || nverts < 3 ||
  2493. !filter || maxResult < 0)
  2494. {
  2495. return DT_FAILURE | DT_INVALID_PARAM;
  2496. }
  2497. // Validate input
  2498. if (!startRef || !m_nav->isValidPolyRef(startRef))
  2499. return DT_FAILURE | DT_INVALID_PARAM;
  2500. m_nodePool->clear();
  2501. m_openList->clear();
  2502. float centerPos[3] = {0,0,0};
  2503. for (int i = 0; i < nverts; ++i)
  2504. dtVadd(centerPos,centerPos,&verts[i*3]);
  2505. dtVscale(centerPos,centerPos,1.0f/nverts);
  2506. dtNode* startNode = m_nodePool->getNode(startRef);
  2507. dtVcopy(startNode->pos, centerPos);
  2508. startNode->pidx = 0;
  2509. startNode->cost = 0;
  2510. startNode->total = 0;
  2511. startNode->id = startRef;
  2512. startNode->flags = DT_NODE_OPEN;
  2513. m_openList->push(startNode);
  2514. dtStatus status = DT_SUCCESS;
  2515. int n = 0;
  2516. while (!m_openList->empty())
  2517. {
  2518. dtNode* bestNode = m_openList->pop();
  2519. bestNode->flags &= ~DT_NODE_OPEN;
  2520. bestNode->flags |= DT_NODE_CLOSED;
  2521. // Get poly and tile.
  2522. // The API input has been cheked already, skip checking internal data.
  2523. const dtPolyRef bestRef = bestNode->id;
  2524. const dtMeshTile* bestTile = 0;
  2525. const dtPoly* bestPoly = 0;
  2526. m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
  2527. // Get parent poly and tile.
  2528. dtPolyRef parentRef = 0;
  2529. const dtMeshTile* parentTile = 0;
  2530. const dtPoly* parentPoly = 0;
  2531. if (bestNode->pidx)
  2532. parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
  2533. if (parentRef)
  2534. m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
  2535. if (n < maxResult)
  2536. {
  2537. if (resultRef)
  2538. resultRef[n] = bestRef;
  2539. if (resultParent)
  2540. resultParent[n] = parentRef;
  2541. if (resultCost)
  2542. resultCost[n] = bestNode->total;
  2543. ++n;
  2544. }
  2545. else
  2546. {
  2547. status |= DT_BUFFER_TOO_SMALL;
  2548. }
  2549. for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
  2550. {
  2551. const dtLink* link = &bestTile->links[i];
  2552. dtPolyRef neighbourRef = link->ref;
  2553. // Skip invalid neighbours and do not follow back to parent.
  2554. if (!neighbourRef || neighbourRef == parentRef)
  2555. continue;
  2556. // Expand to neighbour
  2557. const dtMeshTile* neighbourTile = 0;
  2558. const dtPoly* neighbourPoly = 0;
  2559. m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
  2560. // Do not advance if the polygon is excluded by the filter.
  2561. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  2562. continue;
  2563. // Find edge and calc distance to the edge.
  2564. float va[3], vb[3];
  2565. if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
  2566. continue;
  2567. // If the poly is not touching the edge to the next polygon, skip the connection it.
  2568. float tmin, tmax;
  2569. int segMin, segMax;
  2570. if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax))
  2571. continue;
  2572. if (tmin > 1.0f || tmax < 0.0f)
  2573. continue;
  2574. dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
  2575. if (!neighbourNode)
  2576. {
  2577. status |= DT_OUT_OF_NODES;
  2578. continue;
  2579. }
  2580. if (neighbourNode->flags & DT_NODE_CLOSED)
  2581. continue;
  2582. // Cost
  2583. if (neighbourNode->flags == 0)
  2584. dtVlerp(neighbourNode->pos, va, vb, 0.5f);
  2585. float cost = filter->getCost(
  2586. bestNode->pos, neighbourNode->pos,
  2587. parentRef, parentTile, parentPoly,
  2588. bestRef, bestTile, bestPoly,
  2589. neighbourRef, neighbourTile, neighbourPoly);
  2590. const float total = bestNode->total + cost;
  2591. // The node is already in open list and the new result is worse, skip.
  2592. if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
  2593. continue;
  2594. neighbourNode->id = neighbourRef;
  2595. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
  2596. neighbourNode->total = total;
  2597. if (neighbourNode->flags & DT_NODE_OPEN)
  2598. {
  2599. m_openList->modify(neighbourNode);
  2600. }
  2601. else
  2602. {
  2603. neighbourNode->flags = DT_NODE_OPEN;
  2604. m_openList->push(neighbourNode);
  2605. }
  2606. }
  2607. }
  2608. *resultCount = n;
  2609. return status;
  2610. }
  2611. dtStatus dtNavMeshQuery::getPathFromDijkstraSearch(dtPolyRef endRef, dtPolyRef* path, int* pathCount, int maxPath) const
  2612. {
  2613. if (!m_nav->isValidPolyRef(endRef) || !path || !pathCount || maxPath < 0)
  2614. return DT_FAILURE | DT_INVALID_PARAM;
  2615. *pathCount = 0;
  2616. dtNode* endNode;
  2617. if (m_nodePool->findNodes(endRef, &endNode, 1) != 1 ||
  2618. (endNode->flags & DT_NODE_CLOSED) == 0)
  2619. return DT_FAILURE | DT_INVALID_PARAM;
  2620. return getPathToNode(endNode, path, pathCount, maxPath);
  2621. }
  2622. /// @par
  2623. ///
  2624. /// This method is optimized for a small search radius and small number of result
  2625. /// polygons.
  2626. ///
  2627. /// Candidate polygons are found by searching the navigation graph beginning at
  2628. /// the start polygon.
  2629. ///
  2630. /// The same intersection test restrictions that apply to the findPolysAroundCircle
  2631. /// mehtod applies to this method.
  2632. ///
  2633. /// The value of the center point is used as the start point for cost calculations.
  2634. /// It is not projected onto the surface of the mesh, so its y-value will effect
  2635. /// the costs.
  2636. ///
  2637. /// Intersection tests occur in 2D. All polygons and the search circle are
  2638. /// projected onto the xz-plane. So the y-value of the center point does not
  2639. /// effect intersection tests.
  2640. ///
  2641. /// If the result arrays are is too small to hold the entire result set, they will
  2642. /// be filled to capacity.
  2643. ///
  2644. dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
  2645. const dtQueryFilter* filter,
  2646. dtPolyRef* resultRef, dtPolyRef* resultParent,
  2647. int* resultCount, const int maxResult) const
  2648. {
  2649. dtAssert(m_nav);
  2650. dtAssert(m_tinyNodePool);
  2651. if (!resultCount)
  2652. return DT_FAILURE | DT_INVALID_PARAM;
  2653. *resultCount = 0;
  2654. if (!m_nav->isValidPolyRef(startRef) ||
  2655. !centerPos || !dtVisfinite(centerPos) ||
  2656. radius < 0 || !dtMathIsfinite(radius) ||
  2657. !filter || maxResult < 0)
  2658. {
  2659. return DT_FAILURE | DT_INVALID_PARAM;
  2660. }
  2661. static const int MAX_STACK = 48;
  2662. dtNode* stack[MAX_STACK];
  2663. int nstack = 0;
  2664. m_tinyNodePool->clear();
  2665. dtNode* startNode = m_tinyNodePool->getNode(startRef);
  2666. startNode->pidx = 0;
  2667. startNode->id = startRef;
  2668. startNode->flags = DT_NODE_CLOSED;
  2669. stack[nstack++] = startNode;
  2670. const float radiusSqr = dtSqr(radius);
  2671. float pa[DT_VERTS_PER_POLYGON*3];
  2672. float pb[DT_VERTS_PER_POLYGON*3];
  2673. dtStatus status = DT_SUCCESS;
  2674. int n = 0;
  2675. if (n < maxResult)
  2676. {
  2677. resultRef[n] = startNode->id;
  2678. if (resultParent)
  2679. resultParent[n] = 0;
  2680. ++n;
  2681. }
  2682. else
  2683. {
  2684. status |= DT_BUFFER_TOO_SMALL;
  2685. }
  2686. while (nstack)
  2687. {
  2688. // Pop front.
  2689. dtNode* curNode = stack[0];
  2690. for (int i = 0; i < nstack-1; ++i)
  2691. stack[i] = stack[i+1];
  2692. nstack--;
  2693. // Get poly and tile.
  2694. // The API input has been cheked already, skip checking internal data.
  2695. const dtPolyRef curRef = curNode->id;
  2696. const dtMeshTile* curTile = 0;
  2697. const dtPoly* curPoly = 0;
  2698. m_nav->getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);
  2699. for (unsigned int i = curPoly->firstLink; i != DT_NULL_LINK; i = curTile->links[i].next)
  2700. {
  2701. const dtLink* link = &curTile->links[i];
  2702. dtPolyRef neighbourRef = link->ref;
  2703. // Skip invalid neighbours.
  2704. if (!neighbourRef)
  2705. continue;
  2706. // Skip if cannot alloca more nodes.
  2707. dtNode* neighbourNode = m_tinyNodePool->getNode(neighbourRef);
  2708. if (!neighbourNode)
  2709. continue;
  2710. // Skip visited.
  2711. if (neighbourNode->flags & DT_NODE_CLOSED)
  2712. continue;
  2713. // Expand to neighbour
  2714. const dtMeshTile* neighbourTile = 0;
  2715. const dtPoly* neighbourPoly = 0;
  2716. m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
  2717. // Skip off-mesh connections.
  2718. if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  2719. continue;
  2720. // Do not advance if the polygon is excluded by the filter.
  2721. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  2722. continue;
  2723. // Find edge and calc distance to the edge.
  2724. float va[3], vb[3];
  2725. if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
  2726. continue;
  2727. // If the circle is not touching the next polygon, skip it.
  2728. float tseg;
  2729. float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
  2730. if (distSqr > radiusSqr)
  2731. continue;
  2732. // Mark node visited, this is done before the overlap test so that
  2733. // we will not visit the poly again if the test fails.
  2734. neighbourNode->flags |= DT_NODE_CLOSED;
  2735. neighbourNode->pidx = m_tinyNodePool->getNodeIdx(curNode);
  2736. // Check that the polygon does not collide with existing polygons.
  2737. // Collect vertices of the neighbour poly.
  2738. const int npa = neighbourPoly->vertCount;
  2739. for (int k = 0; k < npa; ++k)
  2740. dtVcopy(&pa[k*3], &neighbourTile->verts[neighbourPoly->verts[k]*3]);
  2741. bool overlap = false;
  2742. for (int j = 0; j < n; ++j)
  2743. {
  2744. dtPolyRef pastRef = resultRef[j];
  2745. // Connected polys do not overlap.
  2746. bool connected = false;
  2747. for (unsigned int k = curPoly->firstLink; k != DT_NULL_LINK; k = curTile->links[k].next)
  2748. {
  2749. if (curTile->links[k].ref == pastRef)
  2750. {
  2751. connected = true;
  2752. break;
  2753. }
  2754. }
  2755. if (connected)
  2756. continue;
  2757. // Potentially overlapping.
  2758. const dtMeshTile* pastTile = 0;
  2759. const dtPoly* pastPoly = 0;
  2760. m_nav->getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);
  2761. // Get vertices and test overlap
  2762. const int npb = pastPoly->vertCount;
  2763. for (int k = 0; k < npb; ++k)
  2764. dtVcopy(&pb[k*3], &pastTile->verts[pastPoly->verts[k]*3]);
  2765. if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
  2766. {
  2767. overlap = true;
  2768. break;
  2769. }
  2770. }
  2771. if (overlap)
  2772. continue;
  2773. // This poly is fine, store and advance to the poly.
  2774. if (n < maxResult)
  2775. {
  2776. resultRef[n] = neighbourRef;
  2777. if (resultParent)
  2778. resultParent[n] = curRef;
  2779. ++n;
  2780. }
  2781. else
  2782. {
  2783. status |= DT_BUFFER_TOO_SMALL;
  2784. }
  2785. if (nstack < MAX_STACK)
  2786. {
  2787. stack[nstack++] = neighbourNode;
  2788. }
  2789. }
  2790. }
  2791. *resultCount = n;
  2792. return status;
  2793. }
  2794. struct dtSegInterval
  2795. {
  2796. dtPolyRef ref;
  2797. short tmin, tmax;
  2798. };
  2799. static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
  2800. const short tmin, const short tmax, const dtPolyRef ref)
  2801. {
  2802. if (nints+1 > maxInts) return;
  2803. // Find insertion point.
  2804. int idx = 0;
  2805. while (idx < nints)
  2806. {
  2807. if (tmax <= ints[idx].tmin)
  2808. break;
  2809. idx++;
  2810. }
  2811. // Move current results.
  2812. if (nints-idx)
  2813. memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
  2814. // Store
  2815. ints[idx].ref = ref;
  2816. ints[idx].tmin = tmin;
  2817. ints[idx].tmax = tmax;
  2818. nints++;
  2819. }
  2820. /// @par
  2821. ///
  2822. /// If the @p segmentRefs parameter is provided, then all polygon segments will be returned.
  2823. /// Otherwise only the wall segments are returned.
  2824. ///
  2825. /// A segment that is normally a portal will be included in the result set as a
  2826. /// wall if the @p filter results in the neighbor polygon becoomming impassable.
  2827. ///
  2828. /// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the
  2829. /// maximum segments per polygon of the source navigation mesh.
  2830. ///
  2831. dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
  2832. float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
  2833. const int maxSegments) const
  2834. {
  2835. dtAssert(m_nav);
  2836. if (!segmentCount)
  2837. return DT_FAILURE | DT_INVALID_PARAM;
  2838. *segmentCount = 0;
  2839. const dtMeshTile* tile = 0;
  2840. const dtPoly* poly = 0;
  2841. if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
  2842. return DT_FAILURE | DT_INVALID_PARAM;
  2843. if (!filter || !segmentVerts || maxSegments < 0)
  2844. return DT_FAILURE | DT_INVALID_PARAM;
  2845. int n = 0;
  2846. static const int MAX_INTERVAL = 16;
  2847. dtSegInterval ints[MAX_INTERVAL];
  2848. int nints;
  2849. const bool storePortals = segmentRefs != 0;
  2850. dtStatus status = DT_SUCCESS;
  2851. for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
  2852. {
  2853. // Skip non-solid edges.
  2854. nints = 0;
  2855. if (poly->neis[j] & DT_EXT_LINK)
  2856. {
  2857. // Tile border.
  2858. for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
  2859. {
  2860. const dtLink* link = &tile->links[k];
  2861. if (link->edge == j)
  2862. {
  2863. if (link->ref != 0)
  2864. {
  2865. const dtMeshTile* neiTile = 0;
  2866. const dtPoly* neiPoly = 0;
  2867. m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
  2868. if (filter->passFilter(link->ref, neiTile, neiPoly))
  2869. {
  2870. insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref);
  2871. }
  2872. }
  2873. }
  2874. }
  2875. }
  2876. else
  2877. {
  2878. // Internal edge
  2879. dtPolyRef neiRef = 0;
  2880. if (poly->neis[j])
  2881. {
  2882. const unsigned int idx = (unsigned int)(poly->neis[j]-1);
  2883. neiRef = m_nav->getPolyRefBase(tile) | idx;
  2884. if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
  2885. neiRef = 0;
  2886. }
  2887. // If the edge leads to another polygon and portals are not stored, skip.
  2888. if (neiRef != 0 && !storePortals)
  2889. continue;
  2890. if (n < maxSegments)
  2891. {
  2892. const float* vj = &tile->verts[poly->verts[j]*3];
  2893. const float* vi = &tile->verts[poly->verts[i]*3];
  2894. float* seg = &segmentVerts[n*6];
  2895. dtVcopy(seg+0, vj);
  2896. dtVcopy(seg+3, vi);
  2897. if (segmentRefs)
  2898. segmentRefs[n] = neiRef;
  2899. n++;
  2900. }
  2901. else
  2902. {
  2903. status |= DT_BUFFER_TOO_SMALL;
  2904. }
  2905. continue;
  2906. }
  2907. // Add sentinels
  2908. insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
  2909. insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
  2910. // Store segments.
  2911. const float* vj = &tile->verts[poly->verts[j]*3];
  2912. const float* vi = &tile->verts[poly->verts[i]*3];
  2913. for (int k = 1; k < nints; ++k)
  2914. {
  2915. // Portal segment.
  2916. if (storePortals && ints[k].ref)
  2917. {
  2918. const float tmin = ints[k].tmin/255.0f;
  2919. const float tmax = ints[k].tmax/255.0f;
  2920. if (n < maxSegments)
  2921. {
  2922. float* seg = &segmentVerts[n*6];
  2923. dtVlerp(seg+0, vj,vi, tmin);
  2924. dtVlerp(seg+3, vj,vi, tmax);
  2925. if (segmentRefs)
  2926. segmentRefs[n] = ints[k].ref;
  2927. n++;
  2928. }
  2929. else
  2930. {
  2931. status |= DT_BUFFER_TOO_SMALL;
  2932. }
  2933. }
  2934. // Wall segment.
  2935. const int imin = ints[k-1].tmax;
  2936. const int imax = ints[k].tmin;
  2937. if (imin != imax)
  2938. {
  2939. const float tmin = imin/255.0f;
  2940. const float tmax = imax/255.0f;
  2941. if (n < maxSegments)
  2942. {
  2943. float* seg = &segmentVerts[n*6];
  2944. dtVlerp(seg+0, vj,vi, tmin);
  2945. dtVlerp(seg+3, vj,vi, tmax);
  2946. if (segmentRefs)
  2947. segmentRefs[n] = 0;
  2948. n++;
  2949. }
  2950. else
  2951. {
  2952. status |= DT_BUFFER_TOO_SMALL;
  2953. }
  2954. }
  2955. }
  2956. }
  2957. *segmentCount = n;
  2958. return status;
  2959. }
  2960. /// @par
  2961. ///
  2962. /// @p hitPos is not adjusted using the height detail data.
  2963. ///
  2964. /// @p hitDist will equal the search radius if there is no wall within the
  2965. /// radius. In this case the values of @p hitPos and @p hitNormal are
  2966. /// undefined.
  2967. ///
  2968. /// The normal will become unpredicable if @p hitDist is a very small number.
  2969. ///
  2970. dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
  2971. const dtQueryFilter* filter,
  2972. float* hitDist, float* hitPos, float* hitNormal) const
  2973. {
  2974. dtAssert(m_nav);
  2975. dtAssert(m_nodePool);
  2976. dtAssert(m_openList);
  2977. // Validate input
  2978. if (!m_nav->isValidPolyRef(startRef) ||
  2979. !centerPos || !dtVisfinite(centerPos) ||
  2980. maxRadius < 0 || !dtMathIsfinite(maxRadius) ||
  2981. !filter || !hitDist || !hitPos || !hitNormal)
  2982. {
  2983. return DT_FAILURE | DT_INVALID_PARAM;
  2984. }
  2985. m_nodePool->clear();
  2986. m_openList->clear();
  2987. dtNode* startNode = m_nodePool->getNode(startRef);
  2988. dtVcopy(startNode->pos, centerPos);
  2989. startNode->pidx = 0;
  2990. startNode->cost = 0;
  2991. startNode->total = 0;
  2992. startNode->id = startRef;
  2993. startNode->flags = DT_NODE_OPEN;
  2994. m_openList->push(startNode);
  2995. float radiusSqr = dtSqr(maxRadius);
  2996. dtStatus status = DT_SUCCESS;
  2997. while (!m_openList->empty())
  2998. {
  2999. dtNode* bestNode = m_openList->pop();
  3000. bestNode->flags &= ~DT_NODE_OPEN;
  3001. bestNode->flags |= DT_NODE_CLOSED;
  3002. // Get poly and tile.
  3003. // The API input has been cheked already, skip checking internal data.
  3004. const dtPolyRef bestRef = bestNode->id;
  3005. const dtMeshTile* bestTile = 0;
  3006. const dtPoly* bestPoly = 0;
  3007. m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
  3008. // Get parent poly and tile.
  3009. dtPolyRef parentRef = 0;
  3010. const dtMeshTile* parentTile = 0;
  3011. const dtPoly* parentPoly = 0;
  3012. if (bestNode->pidx)
  3013. parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
  3014. if (parentRef)
  3015. m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
  3016. // Hit test walls.
  3017. for (int i = 0, j = (int)bestPoly->vertCount-1; i < (int)bestPoly->vertCount; j = i++)
  3018. {
  3019. // Skip non-solid edges.
  3020. if (bestPoly->neis[j] & DT_EXT_LINK)
  3021. {
  3022. // Tile border.
  3023. bool solid = true;
  3024. for (unsigned int k = bestPoly->firstLink; k != DT_NULL_LINK; k = bestTile->links[k].next)
  3025. {
  3026. const dtLink* link = &bestTile->links[k];
  3027. if (link->edge == j)
  3028. {
  3029. if (link->ref != 0)
  3030. {
  3031. const dtMeshTile* neiTile = 0;
  3032. const dtPoly* neiPoly = 0;
  3033. m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
  3034. if (filter->passFilter(link->ref, neiTile, neiPoly))
  3035. solid = false;
  3036. }
  3037. break;
  3038. }
  3039. }
  3040. if (!solid) continue;
  3041. }
  3042. else if (bestPoly->neis[j])
  3043. {
  3044. // Internal edge
  3045. const unsigned int idx = (unsigned int)(bestPoly->neis[j]-1);
  3046. const dtPolyRef ref = m_nav->getPolyRefBase(bestTile) | idx;
  3047. if (filter->passFilter(ref, bestTile, &bestTile->polys[idx]))
  3048. continue;
  3049. }
  3050. // Calc distance to the edge.
  3051. const float* vj = &bestTile->verts[bestPoly->verts[j]*3];
  3052. const float* vi = &bestTile->verts[bestPoly->verts[i]*3];
  3053. float tseg;
  3054. float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);
  3055. // Edge is too far, skip.
  3056. if (distSqr > radiusSqr)
  3057. continue;
  3058. // Hit wall, update radius.
  3059. radiusSqr = distSqr;
  3060. // Calculate hit pos.
  3061. hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
  3062. hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
  3063. hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
  3064. }
  3065. for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
  3066. {
  3067. const dtLink* link = &bestTile->links[i];
  3068. dtPolyRef neighbourRef = link->ref;
  3069. // Skip invalid neighbours and do not follow back to parent.
  3070. if (!neighbourRef || neighbourRef == parentRef)
  3071. continue;
  3072. // Expand to neighbour.
  3073. const dtMeshTile* neighbourTile = 0;
  3074. const dtPoly* neighbourPoly = 0;
  3075. m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
  3076. // Skip off-mesh connections.
  3077. if (neighbourPoly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
  3078. continue;
  3079. // Calc distance to the edge.
  3080. const float* va = &bestTile->verts[bestPoly->verts[link->edge]*3];
  3081. const float* vb = &bestTile->verts[bestPoly->verts[(link->edge+1) % bestPoly->vertCount]*3];
  3082. float tseg;
  3083. float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
  3084. // If the circle is not touching the next polygon, skip it.
  3085. if (distSqr > radiusSqr)
  3086. continue;
  3087. if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
  3088. continue;
  3089. dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
  3090. if (!neighbourNode)
  3091. {
  3092. status |= DT_OUT_OF_NODES;
  3093. continue;
  3094. }
  3095. if (neighbourNode->flags & DT_NODE_CLOSED)
  3096. continue;
  3097. // Cost
  3098. if (neighbourNode->flags == 0)
  3099. {
  3100. getEdgeMidPoint(bestRef, bestPoly, bestTile,
  3101. neighbourRef, neighbourPoly, neighbourTile, neighbourNode->pos);
  3102. }
  3103. const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
  3104. // The node is already in open list and the new result is worse, skip.
  3105. if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
  3106. continue;
  3107. neighbourNode->id = neighbourRef;
  3108. neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
  3109. neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
  3110. neighbourNode->total = total;
  3111. if (neighbourNode->flags & DT_NODE_OPEN)
  3112. {
  3113. m_openList->modify(neighbourNode);
  3114. }
  3115. else
  3116. {
  3117. neighbourNode->flags |= DT_NODE_OPEN;
  3118. m_openList->push(neighbourNode);
  3119. }
  3120. }
  3121. }
  3122. // Calc hit normal.
  3123. dtVsub(hitNormal, centerPos, hitPos);
  3124. dtVnormalize(hitNormal);
  3125. *hitDist = dtMathSqrtf(radiusSqr);
  3126. return status;
  3127. }
  3128. bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const
  3129. {
  3130. const dtMeshTile* tile = 0;
  3131. const dtPoly* poly = 0;
  3132. dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly);
  3133. // If cannot get polygon, assume it does not exists and boundary is invalid.
  3134. if (dtStatusFailed(status))
  3135. return false;
  3136. // If cannot pass filter, assume flags has changed and boundary is invalid.
  3137. if (!filter->passFilter(ref, tile, poly))
  3138. return false;
  3139. return true;
  3140. }
  3141. /// @par
  3142. ///
  3143. /// The closed list is the list of polygons that were fully evaluated during
  3144. /// the last navigation graph search. (A* or Dijkstra)
  3145. ///
  3146. bool dtNavMeshQuery::isInClosedList(dtPolyRef ref) const
  3147. {
  3148. if (!m_nodePool) return false;
  3149. dtNode* nodes[DT_MAX_STATES_PER_NODE];
  3150. int n= m_nodePool->findNodes(ref, nodes, DT_MAX_STATES_PER_NODE);
  3151. for (int i=0; i<n; i++)
  3152. {
  3153. if (nodes[i]->flags & DT_NODE_CLOSED)
  3154. return true;
  3155. }
  3156. return false;
  3157. }