9
3

DetourCrowd.cpp 37 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448
  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. #define _USE_MATH_DEFINES
  19. #include <string.h>
  20. #include <float.h>
  21. #include <stdlib.h>
  22. #include <new>
  23. #include "DetourCrowd.h"
  24. #include "DetourNavMesh.h"
  25. #include "DetourNavMeshQuery.h"
  26. #include "DetourObstacleAvoidance.h"
  27. #include "DetourCommon.h"
  28. #include "DetourMath.h"
  29. #include "DetourAssert.h"
  30. #include "DetourAlloc.h"
  31. dtCrowd* dtAllocCrowd()
  32. {
  33. void* mem = dtAlloc(sizeof(dtCrowd), DT_ALLOC_PERM);
  34. if (!mem) return 0;
  35. return new(mem) dtCrowd;
  36. }
  37. void dtFreeCrowd(dtCrowd* ptr)
  38. {
  39. if (!ptr) return;
  40. ptr->~dtCrowd();
  41. dtFree(ptr);
  42. }
  43. static const int MAX_ITERS_PER_UPDATE = 100;
  44. static const int MAX_PATHQUEUE_NODES = 4096;
  45. static const int MAX_COMMON_NODES = 512;
  46. inline float tween(const float t, const float t0, const float t1)
  47. {
  48. return dtClamp((t-t0) / (t1-t0), 0.0f, 1.0f);
  49. }
  50. static void integrate(dtCrowdAgent* ag, const float dt)
  51. {
  52. // Fake dynamic constraint.
  53. const float maxDelta = ag->params.maxAcceleration * dt;
  54. float dv[3];
  55. dtVsub(dv, ag->nvel, ag->vel);
  56. float ds = dtVlen(dv);
  57. if (ds > maxDelta)
  58. dtVscale(dv, dv, maxDelta/ds);
  59. dtVadd(ag->vel, ag->vel, dv);
  60. // Integrate
  61. if (dtVlen(ag->vel) > 0.0001f)
  62. dtVmad(ag->npos, ag->npos, ag->vel, dt);
  63. else
  64. dtVset(ag->vel,0,0,0);
  65. }
  66. static bool overOffmeshConnection(const dtCrowdAgent* ag, const float radius)
  67. {
  68. if (!ag->ncorners)
  69. return false;
  70. const bool offMeshConnection = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
  71. if (offMeshConnection)
  72. {
  73. const float distSq = dtVdist2DSqr(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]);
  74. if (distSq < radius*radius)
  75. return true;
  76. }
  77. return false;
  78. }
  79. static float getDistanceToGoal(const dtCrowdAgent* ag, const float range)
  80. {
  81. if (!ag->ncorners)
  82. return range;
  83. const bool endOfPath = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_END) ? true : false;
  84. if (endOfPath)
  85. return dtMin(dtVdist2D(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]), range);
  86. return range;
  87. }
  88. static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
  89. {
  90. if (!ag->ncorners)
  91. {
  92. dtVset(dir, 0,0,0);
  93. return;
  94. }
  95. const int ip0 = 0;
  96. const int ip1 = dtMin(1, ag->ncorners-1);
  97. const float* p0 = &ag->cornerVerts[ip0*3];
  98. const float* p1 = &ag->cornerVerts[ip1*3];
  99. float dir0[3], dir1[3];
  100. dtVsub(dir0, p0, ag->npos);
  101. dtVsub(dir1, p1, ag->npos);
  102. dir0[1] = 0;
  103. dir1[1] = 0;
  104. float len0 = dtVlen(dir0);
  105. float len1 = dtVlen(dir1);
  106. if (len1 > 0.001f)
  107. dtVscale(dir1,dir1,1.0f/len1);
  108. dir[0] = dir0[0] - dir1[0]*len0*0.5f;
  109. dir[1] = 0;
  110. dir[2] = dir0[2] - dir1[2]*len0*0.5f;
  111. dtVnormalize(dir);
  112. }
  113. static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir)
  114. {
  115. if (!ag->ncorners)
  116. {
  117. dtVset(dir, 0,0,0);
  118. return;
  119. }
  120. dtVsub(dir, &ag->cornerVerts[0], ag->npos);
  121. dir[1] = 0;
  122. dtVnormalize(dir);
  123. }
  124. static int addNeighbour(const int idx, const float dist,
  125. dtCrowdNeighbour* neis, const int nneis, const int maxNeis)
  126. {
  127. // Insert neighbour based on the distance.
  128. dtCrowdNeighbour* nei = 0;
  129. if (!nneis)
  130. {
  131. nei = &neis[nneis];
  132. }
  133. else if (dist >= neis[nneis-1].dist)
  134. {
  135. if (nneis >= maxNeis)
  136. return nneis;
  137. nei = &neis[nneis];
  138. }
  139. else
  140. {
  141. int i;
  142. for (i = 0; i < nneis; ++i)
  143. if (dist <= neis[i].dist)
  144. break;
  145. const int tgt = i+1;
  146. const int n = dtMin(nneis-i, maxNeis-tgt);
  147. dtAssert(tgt+n <= maxNeis);
  148. if (n > 0)
  149. memmove(&neis[tgt], &neis[i], sizeof(dtCrowdNeighbour)*n);
  150. nei = &neis[i];
  151. }
  152. memset(nei, 0, sizeof(dtCrowdNeighbour));
  153. nei->idx = idx;
  154. nei->dist = dist;
  155. return dtMin(nneis+1, maxNeis);
  156. }
  157. static int getNeighbours(const float* pos, const float height, const float range,
  158. const dtCrowdAgent* skip, dtCrowdNeighbour* result, const int maxResult,
  159. dtCrowdAgent** agents, const int /*nagents*/, dtProximityGrid* grid)
  160. {
  161. int n = 0;
  162. static const int MAX_NEIS = 32;
  163. unsigned short ids[MAX_NEIS];
  164. int nids = grid->queryItems(pos[0]-range, pos[2]-range,
  165. pos[0]+range, pos[2]+range,
  166. ids, MAX_NEIS);
  167. for (int i = 0; i < nids; ++i)
  168. {
  169. const dtCrowdAgent* ag = agents[ids[i]];
  170. if (ag == skip) continue;
  171. // Check for overlap.
  172. float diff[3];
  173. dtVsub(diff, pos, ag->npos);
  174. if (dtMathFabsf(diff[1]) >= (height+ag->params.height)/2.0f)
  175. continue;
  176. diff[1] = 0;
  177. const float distSqr = dtVlenSqr(diff);
  178. if (distSqr > dtSqr(range))
  179. continue;
  180. n = addNeighbour(ids[i], distSqr, result, n, maxResult);
  181. }
  182. return n;
  183. }
  184. static int addToOptQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents)
  185. {
  186. // Insert neighbour based on greatest time.
  187. int slot = 0;
  188. if (!nagents)
  189. {
  190. slot = nagents;
  191. }
  192. else if (newag->topologyOptTime <= agents[nagents-1]->topologyOptTime)
  193. {
  194. if (nagents >= maxAgents)
  195. return nagents;
  196. slot = nagents;
  197. }
  198. else
  199. {
  200. int i;
  201. for (i = 0; i < nagents; ++i)
  202. if (newag->topologyOptTime >= agents[i]->topologyOptTime)
  203. break;
  204. const int tgt = i+1;
  205. const int n = dtMin(nagents-i, maxAgents-tgt);
  206. dtAssert(tgt+n <= maxAgents);
  207. if (n > 0)
  208. memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n);
  209. slot = i;
  210. }
  211. agents[slot] = newag;
  212. return dtMin(nagents+1, maxAgents);
  213. }
  214. static int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents)
  215. {
  216. // Insert neighbour based on greatest time.
  217. int slot = 0;
  218. if (!nagents)
  219. {
  220. slot = nagents;
  221. }
  222. else if (newag->targetReplanTime <= agents[nagents-1]->targetReplanTime)
  223. {
  224. if (nagents >= maxAgents)
  225. return nagents;
  226. slot = nagents;
  227. }
  228. else
  229. {
  230. int i;
  231. for (i = 0; i < nagents; ++i)
  232. if (newag->targetReplanTime >= agents[i]->targetReplanTime)
  233. break;
  234. const int tgt = i+1;
  235. const int n = dtMin(nagents-i, maxAgents-tgt);
  236. dtAssert(tgt+n <= maxAgents);
  237. if (n > 0)
  238. memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n);
  239. slot = i;
  240. }
  241. agents[slot] = newag;
  242. return dtMin(nagents+1, maxAgents);
  243. }
  244. /**
  245. @class dtCrowd
  246. @par
  247. This is the core class of the @ref crowd module. See the @ref crowd documentation for a summary
  248. of the crowd features.
  249. A common method for setting up the crowd is as follows:
  250. -# Allocate the crowd using #dtAllocCrowd.
  251. -# Initialize the crowd using #init().
  252. -# Set the avoidance configurations using #setObstacleAvoidanceParams().
  253. -# Add agents using #addAgent() and make an initial movement request using #requestMoveTarget().
  254. A common process for managing the crowd is as follows:
  255. -# Call #update() to allow the crowd to manage its agents.
  256. -# Retrieve agent information using #getActiveAgents().
  257. -# Make movement requests using #requestMoveTarget() when movement goal changes.
  258. -# Repeat every frame.
  259. Some agent configuration settings can be updated using #updateAgentParameters(). But the crowd owns the
  260. agent position. So it is not possible to update an active agent's position. If agent position
  261. must be fed back into the crowd, the agent must be removed and re-added.
  262. Notes:
  263. - Path related information is available for newly added agents only after an #update() has been
  264. performed.
  265. - Agent objects are kept in a pool and re-used. So it is important when using agent objects to check the value of
  266. #dtCrowdAgent::active to determine if the agent is actually in use or not.
  267. - This class is meant to provide 'local' movement. There is a limit of 256 polygons in the path corridor.
  268. So it is not meant to provide automatic pathfinding services over long distances.
  269. @see dtAllocCrowd(), dtFreeCrowd(), init(), dtCrowdAgent
  270. */
  271. dtCrowd::dtCrowd() :
  272. m_maxAgents(0),
  273. m_agents(0),
  274. m_activeAgents(0),
  275. m_agentAnims(0),
  276. m_obstacleQuery(0),
  277. m_grid(0),
  278. m_pathResult(0),
  279. m_maxPathResult(0),
  280. m_maxAgentRadius(0),
  281. m_velocitySampleCount(0),
  282. m_navquery(0)
  283. {
  284. }
  285. dtCrowd::~dtCrowd()
  286. {
  287. purge();
  288. }
  289. void dtCrowd::purge()
  290. {
  291. for (int i = 0; i < m_maxAgents; ++i)
  292. m_agents[i].~dtCrowdAgent();
  293. dtFree(m_agents);
  294. m_agents = 0;
  295. m_maxAgents = 0;
  296. dtFree(m_activeAgents);
  297. m_activeAgents = 0;
  298. dtFree(m_agentAnims);
  299. m_agentAnims = 0;
  300. dtFree(m_pathResult);
  301. m_pathResult = 0;
  302. dtFreeProximityGrid(m_grid);
  303. m_grid = 0;
  304. dtFreeObstacleAvoidanceQuery(m_obstacleQuery);
  305. m_obstacleQuery = 0;
  306. dtFreeNavMeshQuery(m_navquery);
  307. m_navquery = 0;
  308. }
  309. /// @par
  310. ///
  311. /// May be called more than once to purge and re-initialize the crowd.
  312. bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav)
  313. {
  314. purge();
  315. m_maxAgents = maxAgents;
  316. m_maxAgentRadius = maxAgentRadius;
  317. // Larger than agent radius because it is also used for agent recovery.
  318. dtVset(m_agentPlacementHalfExtents, m_maxAgentRadius*2.0f, m_maxAgentRadius*1.5f, m_maxAgentRadius*2.0f);
  319. m_grid = dtAllocProximityGrid();
  320. if (!m_grid)
  321. return false;
  322. if (!m_grid->init(m_maxAgents*4, maxAgentRadius*3))
  323. return false;
  324. m_obstacleQuery = dtAllocObstacleAvoidanceQuery();
  325. if (!m_obstacleQuery)
  326. return false;
  327. if (!m_obstacleQuery->init(6, 8))
  328. return false;
  329. // Init obstacle query params.
  330. memset(m_obstacleQueryParams, 0, sizeof(m_obstacleQueryParams));
  331. for (int i = 0; i < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS; ++i)
  332. {
  333. dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[i];
  334. params->velBias = 0.4f;
  335. params->weightDesVel = 2.0f;
  336. params->weightCurVel = 0.75f;
  337. params->weightSide = 0.75f;
  338. params->weightToi = 2.5f;
  339. params->horizTime = 2.5f;
  340. params->gridSize = 33;
  341. params->adaptiveDivs = 7;
  342. params->adaptiveRings = 2;
  343. params->adaptiveDepth = 5;
  344. }
  345. // Allocate temp buffer for merging paths.
  346. m_maxPathResult = 256;
  347. m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM);
  348. if (!m_pathResult)
  349. return false;
  350. if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav))
  351. return false;
  352. m_agents = (dtCrowdAgent*)dtAlloc(sizeof(dtCrowdAgent)*m_maxAgents, DT_ALLOC_PERM);
  353. if (!m_agents)
  354. return false;
  355. m_activeAgents = (dtCrowdAgent**)dtAlloc(sizeof(dtCrowdAgent*)*m_maxAgents, DT_ALLOC_PERM);
  356. if (!m_activeAgents)
  357. return false;
  358. m_agentAnims = (dtCrowdAgentAnimation*)dtAlloc(sizeof(dtCrowdAgentAnimation)*m_maxAgents, DT_ALLOC_PERM);
  359. if (!m_agentAnims)
  360. return false;
  361. for (int i = 0; i < m_maxAgents; ++i)
  362. {
  363. new(&m_agents[i]) dtCrowdAgent();
  364. m_agents[i].active = false;
  365. if (!m_agents[i].corridor.init(m_maxPathResult))
  366. return false;
  367. }
  368. for (int i = 0; i < m_maxAgents; ++i)
  369. {
  370. m_agentAnims[i].active = false;
  371. }
  372. // The navquery is mostly used for local searches, no need for large node pool.
  373. m_navquery = dtAllocNavMeshQuery();
  374. if (!m_navquery)
  375. return false;
  376. if (dtStatusFailed(m_navquery->init(nav, MAX_COMMON_NODES)))
  377. return false;
  378. return true;
  379. }
  380. void dtCrowd::setObstacleAvoidanceParams(const int idx, const dtObstacleAvoidanceParams* params)
  381. {
  382. if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS)
  383. memcpy(&m_obstacleQueryParams[idx], params, sizeof(dtObstacleAvoidanceParams));
  384. }
  385. const dtObstacleAvoidanceParams* dtCrowd::getObstacleAvoidanceParams(const int idx) const
  386. {
  387. if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS)
  388. return &m_obstacleQueryParams[idx];
  389. return 0;
  390. }
  391. int dtCrowd::getAgentCount() const
  392. {
  393. return m_maxAgents;
  394. }
  395. /// @par
  396. ///
  397. /// Agents in the pool may not be in use. Check #dtCrowdAgent.active before using the returned object.
  398. const dtCrowdAgent* dtCrowd::getAgent(const int idx)
  399. {
  400. if (idx < 0 || idx >= m_maxAgents)
  401. return 0;
  402. return &m_agents[idx];
  403. }
  404. ///
  405. /// Agents in the pool may not be in use. Check #dtCrowdAgent.active before using the returned object.
  406. dtCrowdAgent* dtCrowd::getEditableAgent(const int idx)
  407. {
  408. if (idx < 0 || idx >= m_maxAgents)
  409. return 0;
  410. return &m_agents[idx];
  411. }
  412. void dtCrowd::updateAgentParameters(const int idx, const dtCrowdAgentParams* params)
  413. {
  414. if (idx < 0 || idx >= m_maxAgents)
  415. return;
  416. memcpy(&m_agents[idx].params, params, sizeof(dtCrowdAgentParams));
  417. }
  418. /// @par
  419. ///
  420. /// The agent's position will be constrained to the surface of the navigation mesh.
  421. int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
  422. {
  423. // Find empty slot.
  424. int idx = -1;
  425. for (int i = 0; i < m_maxAgents; ++i)
  426. {
  427. if (!m_agents[i].active)
  428. {
  429. idx = i;
  430. break;
  431. }
  432. }
  433. if (idx == -1)
  434. return -1;
  435. dtCrowdAgent* ag = &m_agents[idx];
  436. updateAgentParameters(idx, params);
  437. // Find nearest position on navmesh and place the agent there.
  438. float nearest[3];
  439. dtPolyRef ref = 0;
  440. dtVcopy(nearest, pos);
  441. dtStatus status = m_navquery->findNearestPoly(pos, m_agentPlacementHalfExtents, &m_filters[ag->params.queryFilterType], &ref, nearest);
  442. if (dtStatusFailed(status))
  443. {
  444. dtVcopy(nearest, pos);
  445. ref = 0;
  446. }
  447. ag->corridor.reset(ref, nearest);
  448. ag->boundary.reset();
  449. ag->partial = false;
  450. ag->topologyOptTime = 0;
  451. ag->targetReplanTime = 0;
  452. ag->nneis = 0;
  453. dtVset(ag->dvel, 0,0,0);
  454. dtVset(ag->nvel, 0,0,0);
  455. dtVset(ag->vel, 0,0,0);
  456. dtVcopy(ag->npos, nearest);
  457. ag->desiredSpeed = 0;
  458. if (ref)
  459. ag->state = DT_CROWDAGENT_STATE_WALKING;
  460. else
  461. ag->state = DT_CROWDAGENT_STATE_INVALID;
  462. ag->targetState = DT_CROWDAGENT_TARGET_NONE;
  463. ag->active = true;
  464. return idx;
  465. }
  466. /// @par
  467. ///
  468. /// The agent is deactivated and will no longer be processed. Its #dtCrowdAgent object
  469. /// is not removed from the pool. It is marked as inactive so that it is available for reuse.
  470. void dtCrowd::removeAgent(const int idx)
  471. {
  472. if (idx >= 0 && idx < m_maxAgents)
  473. {
  474. m_agents[idx].active = false;
  475. }
  476. }
  477. bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos)
  478. {
  479. if (idx < 0 || idx >= m_maxAgents)
  480. return false;
  481. dtCrowdAgent* ag = &m_agents[idx];
  482. // Initialize request.
  483. ag->targetRef = ref;
  484. dtVcopy(ag->targetPos, pos);
  485. ag->targetPathqRef = DT_PATHQ_INVALID;
  486. ag->targetReplan = true;
  487. if (ag->targetRef)
  488. ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
  489. else
  490. ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
  491. return true;
  492. }
  493. /// @par
  494. ///
  495. /// This method is used when a new target is set.
  496. ///
  497. /// The position will be constrained to the surface of the navigation mesh.
  498. ///
  499. /// The request will be processed during the next #update().
  500. bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
  501. {
  502. if (idx < 0 || idx >= m_maxAgents)
  503. return false;
  504. if (!ref)
  505. return false;
  506. dtCrowdAgent* ag = &m_agents[idx];
  507. // Initialize request.
  508. ag->targetRef = ref;
  509. dtVcopy(ag->targetPos, pos);
  510. ag->targetPathqRef = DT_PATHQ_INVALID;
  511. ag->targetReplan = false;
  512. if (ag->targetRef)
  513. ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
  514. else
  515. ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
  516. return true;
  517. }
  518. bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
  519. {
  520. if (idx < 0 || idx >= m_maxAgents)
  521. return false;
  522. dtCrowdAgent* ag = &m_agents[idx];
  523. // Initialize request.
  524. ag->targetRef = 0;
  525. dtVcopy(ag->targetPos, vel);
  526. ag->targetPathqRef = DT_PATHQ_INVALID;
  527. ag->targetReplan = false;
  528. ag->targetState = DT_CROWDAGENT_TARGET_VELOCITY;
  529. return true;
  530. }
  531. bool dtCrowd::resetMoveTarget(const int idx)
  532. {
  533. if (idx < 0 || idx >= m_maxAgents)
  534. return false;
  535. dtCrowdAgent* ag = &m_agents[idx];
  536. // Initialize request.
  537. ag->targetRef = 0;
  538. dtVset(ag->targetPos, 0,0,0);
  539. dtVset(ag->dvel, 0,0,0);
  540. ag->targetPathqRef = DT_PATHQ_INVALID;
  541. ag->targetReplan = false;
  542. ag->targetState = DT_CROWDAGENT_TARGET_NONE;
  543. return true;
  544. }
  545. int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents)
  546. {
  547. int n = 0;
  548. for (int i = 0; i < m_maxAgents; ++i)
  549. {
  550. if (!m_agents[i].active) continue;
  551. if (n < maxAgents)
  552. agents[n++] = &m_agents[i];
  553. }
  554. return n;
  555. }
  556. void dtCrowd::updateMoveRequest(const float /*dt*/)
  557. {
  558. const int PATH_MAX_AGENTS = 8;
  559. dtCrowdAgent* queue[PATH_MAX_AGENTS];
  560. int nqueue = 0;
  561. // Fire off new requests.
  562. for (int i = 0; i < m_maxAgents; ++i)
  563. {
  564. dtCrowdAgent* ag = &m_agents[i];
  565. if (!ag->active)
  566. continue;
  567. if (ag->state == DT_CROWDAGENT_STATE_INVALID)
  568. continue;
  569. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  570. continue;
  571. if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING)
  572. {
  573. const dtPolyRef* path = ag->corridor.getPath();
  574. const int npath = ag->corridor.getPathCount();
  575. dtAssert(npath);
  576. static const int MAX_RES = 32;
  577. float reqPos[3];
  578. dtPolyRef reqPath[MAX_RES]; // The path to the request location
  579. int reqPathCount = 0;
  580. // Quick search towards the goal.
  581. static const int MAX_ITER = 20;
  582. m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
  583. m_navquery->updateSlicedFindPath(MAX_ITER, 0);
  584. dtStatus status = 0;
  585. if (ag->targetReplan) // && npath > 10)
  586. {
  587. // Try to use existing steady path during replan if possible.
  588. status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
  589. }
  590. else
  591. {
  592. // Try to move towards target when goal changes.
  593. status = m_navquery->finalizeSlicedFindPath(reqPath, &reqPathCount, MAX_RES);
  594. }
  595. if (!dtStatusFailed(status) && reqPathCount > 0)
  596. {
  597. // In progress or succeed.
  598. if (reqPath[reqPathCount-1] != ag->targetRef)
  599. {
  600. // Partial path, constrain target position inside the last polygon.
  601. status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
  602. if (dtStatusFailed(status))
  603. reqPathCount = 0;
  604. }
  605. else
  606. {
  607. dtVcopy(reqPos, ag->targetPos);
  608. }
  609. }
  610. else
  611. {
  612. reqPathCount = 0;
  613. }
  614. if (!reqPathCount)
  615. {
  616. // Could not find path, start the request from current location.
  617. dtVcopy(reqPos, ag->npos);
  618. reqPath[0] = path[0];
  619. reqPathCount = 1;
  620. }
  621. ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
  622. ag->boundary.reset();
  623. ag->partial = false;
  624. if (reqPath[reqPathCount-1] == ag->targetRef)
  625. {
  626. ag->targetState = DT_CROWDAGENT_TARGET_VALID;
  627. ag->targetReplanTime = 0.0;
  628. }
  629. else
  630. {
  631. // The path is longer or potentially unreachable, full plan.
  632. ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE;
  633. }
  634. }
  635. if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
  636. {
  637. nqueue = addToPathQueue(ag, queue, nqueue, PATH_MAX_AGENTS);
  638. }
  639. }
  640. for (int i = 0; i < nqueue; ++i)
  641. {
  642. dtCrowdAgent* ag = queue[i];
  643. ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
  644. ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->params.queryFilterType]);
  645. if (ag->targetPathqRef != DT_PATHQ_INVALID)
  646. ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
  647. }
  648. // Update requests.
  649. m_pathq.update(MAX_ITERS_PER_UPDATE);
  650. dtStatus status;
  651. // Process path results.
  652. for (int i = 0; i < m_maxAgents; ++i)
  653. {
  654. dtCrowdAgent* ag = &m_agents[i];
  655. if (!ag->active)
  656. continue;
  657. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  658. continue;
  659. if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
  660. {
  661. // Poll path queue.
  662. status = m_pathq.getRequestStatus(ag->targetPathqRef);
  663. if (dtStatusFailed(status))
  664. {
  665. // Path find failed, retry if the target location is still valid.
  666. ag->targetPathqRef = DT_PATHQ_INVALID;
  667. if (ag->targetRef)
  668. ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
  669. else
  670. ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
  671. ag->targetReplanTime = 0.0;
  672. }
  673. else if (dtStatusSucceed(status))
  674. {
  675. const dtPolyRef* path = ag->corridor.getPath();
  676. const int npath = ag->corridor.getPathCount();
  677. dtAssert(npath);
  678. // Apply results.
  679. float targetPos[3];
  680. dtVcopy(targetPos, ag->targetPos);
  681. dtPolyRef* res = m_pathResult;
  682. bool valid = true;
  683. int nres = 0;
  684. status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
  685. if (dtStatusFailed(status) || !nres)
  686. valid = false;
  687. if (dtStatusDetail(status, DT_PARTIAL_RESULT))
  688. ag->partial = true;
  689. else
  690. ag->partial = false;
  691. // Merge result and existing path.
  692. // The agent might have moved whilst the request is
  693. // being processed, so the path may have changed.
  694. // We assume that the end of the path is at the same location
  695. // where the request was issued.
  696. // The last ref in the old path should be the same as
  697. // the location where the request was issued..
  698. if (valid && path[npath-1] != res[0])
  699. valid = false;
  700. if (valid)
  701. {
  702. // Put the old path infront of the old path.
  703. if (npath > 1)
  704. {
  705. // Make space for the old path.
  706. if ((npath-1)+nres > m_maxPathResult)
  707. nres = m_maxPathResult - (npath-1);
  708. memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
  709. // Copy old path in the beginning.
  710. memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
  711. nres += npath-1;
  712. // Remove trackbacks
  713. for (int j = 0; j < nres; ++j)
  714. {
  715. if (j-1 >= 0 && j+1 < nres)
  716. {
  717. if (res[j-1] == res[j+1])
  718. {
  719. memmove(res+(j-1), res+(j+1), sizeof(dtPolyRef)*(nres-(j+1)));
  720. nres -= 2;
  721. j -= 2;
  722. }
  723. }
  724. }
  725. }
  726. // Check for partial path.
  727. if (res[nres-1] != ag->targetRef)
  728. {
  729. // Partial path, constrain target position inside the last polygon.
  730. float nearest[3];
  731. status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
  732. if (dtStatusSucceed(status))
  733. dtVcopy(targetPos, nearest);
  734. else
  735. valid = false;
  736. }
  737. }
  738. if (valid)
  739. {
  740. // Set current corridor.
  741. ag->corridor.setCorridor(targetPos, res, nres);
  742. // Force to update boundary.
  743. ag->boundary.reset();
  744. ag->targetState = DT_CROWDAGENT_TARGET_VALID;
  745. }
  746. else
  747. {
  748. // Something went wrong.
  749. ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
  750. }
  751. ag->targetReplanTime = 0.0;
  752. }
  753. }
  754. }
  755. }
  756. void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt)
  757. {
  758. if (!nagents)
  759. return;
  760. const float OPT_TIME_THR = 0.5f; // seconds
  761. const int OPT_MAX_AGENTS = 1;
  762. dtCrowdAgent* queue[OPT_MAX_AGENTS];
  763. int nqueue = 0;
  764. for (int i = 0; i < nagents; ++i)
  765. {
  766. dtCrowdAgent* ag = agents[i];
  767. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  768. continue;
  769. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  770. continue;
  771. if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
  772. continue;
  773. ag->topologyOptTime += dt;
  774. if (ag->topologyOptTime >= OPT_TIME_THR)
  775. nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS);
  776. }
  777. for (int i = 0; i < nqueue; ++i)
  778. {
  779. dtCrowdAgent* ag = queue[i];
  780. ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
  781. ag->topologyOptTime = 0;
  782. }
  783. }
  784. void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt)
  785. {
  786. static const int CHECK_LOOKAHEAD = 10;
  787. static const float TARGET_REPLAN_DELAY = 1.0; // seconds
  788. for (int i = 0; i < nagents; ++i)
  789. {
  790. dtCrowdAgent* ag = agents[i];
  791. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  792. continue;
  793. ag->targetReplanTime += dt;
  794. bool replan = false;
  795. // First check that the current location is valid.
  796. const int idx = getAgentIndex(ag);
  797. float agentPos[3];
  798. dtPolyRef agentRef = ag->corridor.getFirstPoly();
  799. dtVcopy(agentPos, ag->npos);
  800. if (!m_navquery->isValidPolyRef(agentRef, &m_filters[ag->params.queryFilterType]))
  801. {
  802. // Current location is not valid, try to reposition.
  803. // TODO: this can snap agents, how to handle that?
  804. float nearest[3];
  805. dtVcopy(nearest, agentPos);
  806. agentRef = 0;
  807. m_navquery->findNearestPoly(ag->npos, m_agentPlacementHalfExtents, &m_filters[ag->params.queryFilterType], &agentRef, nearest);
  808. dtVcopy(agentPos, nearest);
  809. if (!agentRef)
  810. {
  811. // Could not find location in navmesh, set state to invalid.
  812. ag->corridor.reset(0, agentPos);
  813. ag->partial = false;
  814. ag->boundary.reset();
  815. ag->state = DT_CROWDAGENT_STATE_INVALID;
  816. continue;
  817. }
  818. // Make sure the first polygon is valid, but leave other valid
  819. // polygons in the path so that replanner can adjust the path better.
  820. ag->corridor.fixPathStart(agentRef, agentPos);
  821. // ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
  822. ag->boundary.reset();
  823. dtVcopy(ag->npos, agentPos);
  824. replan = true;
  825. }
  826. // If the agent does not have move target or is controlled by velocity, no need to recover the target nor replan.
  827. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  828. continue;
  829. // Try to recover move request position.
  830. if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
  831. {
  832. if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filters[ag->params.queryFilterType]))
  833. {
  834. // Current target is not valid, try to reposition.
  835. float nearest[3];
  836. dtVcopy(nearest, ag->targetPos);
  837. ag->targetRef = 0;
  838. m_navquery->findNearestPoly(ag->targetPos, m_agentPlacementHalfExtents, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest);
  839. dtVcopy(ag->targetPos, nearest);
  840. replan = true;
  841. }
  842. if (!ag->targetRef)
  843. {
  844. // Failed to reposition target, fail moverequest.
  845. ag->corridor.reset(agentRef, agentPos);
  846. ag->partial = false;
  847. ag->targetState = DT_CROWDAGENT_TARGET_NONE;
  848. }
  849. }
  850. // If nearby corridor is not valid, replan.
  851. if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->params.queryFilterType]))
  852. {
  853. // Fix current path.
  854. // ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
  855. // ag->boundary.reset();
  856. replan = true;
  857. }
  858. // If the end of the path is near and it is not the requested location, replan.
  859. if (ag->targetState == DT_CROWDAGENT_TARGET_VALID)
  860. {
  861. if (ag->targetReplanTime > TARGET_REPLAN_DELAY &&
  862. ag->corridor.getPathCount() < CHECK_LOOKAHEAD &&
  863. ag->corridor.getLastPoly() != ag->targetRef)
  864. replan = true;
  865. }
  866. // Try to replan path to goal.
  867. if (replan)
  868. {
  869. if (ag->targetState != DT_CROWDAGENT_TARGET_NONE)
  870. {
  871. requestMoveTargetReplan(idx, ag->targetRef, ag->targetPos);
  872. }
  873. }
  874. }
  875. }
  876. void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
  877. {
  878. m_velocitySampleCount = 0;
  879. const int debugIdx = debug ? debug->idx : -1;
  880. dtCrowdAgent** agents = m_activeAgents;
  881. int nagents = getActiveAgents(agents, m_maxAgents);
  882. // Check that all agents still have valid paths.
  883. checkPathValidity(agents, nagents, dt);
  884. // Update async move request and path finder.
  885. updateMoveRequest(dt);
  886. // Optimize path topology.
  887. updateTopologyOptimization(agents, nagents, dt);
  888. // Register agents to proximity grid.
  889. m_grid->clear();
  890. for (int i = 0; i < nagents; ++i)
  891. {
  892. dtCrowdAgent* ag = agents[i];
  893. const float* p = ag->npos;
  894. const float r = ag->params.radius;
  895. m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
  896. }
  897. // Get nearby navmesh segments and agents to collide with.
  898. for (int i = 0; i < nagents; ++i)
  899. {
  900. dtCrowdAgent* ag = agents[i];
  901. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  902. continue;
  903. // Update the collision boundary after certain distance has been passed or
  904. // if it has become invalid.
  905. const float updateThr = ag->params.collisionQueryRange*0.25f;
  906. if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
  907. !ag->boundary.isValid(m_navquery, &m_filters[ag->params.queryFilterType]))
  908. {
  909. ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
  910. m_navquery, &m_filters[ag->params.queryFilterType]);
  911. }
  912. // Query neighbour agents
  913. ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
  914. ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
  915. agents, nagents, m_grid);
  916. for (int j = 0; j < ag->nneis; j++)
  917. ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
  918. }
  919. // Find next corner to steer to.
  920. for (int i = 0; i < nagents; ++i)
  921. {
  922. dtCrowdAgent* ag = agents[i];
  923. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  924. continue;
  925. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  926. continue;
  927. // Find corners for steering
  928. ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
  929. DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.queryFilterType]);
  930. // Check to see if the corner after the next corner is directly visible,
  931. // and short cut to there.
  932. if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
  933. {
  934. const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
  935. ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->params.queryFilterType]);
  936. // Copy data for debug purposes.
  937. if (debugIdx == i)
  938. {
  939. dtVcopy(debug->optStart, ag->corridor.getPos());
  940. dtVcopy(debug->optEnd, target);
  941. }
  942. }
  943. else
  944. {
  945. // Copy data for debug purposes.
  946. if (debugIdx == i)
  947. {
  948. dtVset(debug->optStart, 0,0,0);
  949. dtVset(debug->optEnd, 0,0,0);
  950. }
  951. }
  952. }
  953. // Trigger off-mesh connections (depends on corners).
  954. for (int i = 0; i < nagents; ++i)
  955. {
  956. dtCrowdAgent* ag = agents[i];
  957. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  958. continue;
  959. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  960. continue;
  961. // Check
  962. const float triggerRadius = ag->params.radius*2.25f;
  963. if (overOffmeshConnection(ag, triggerRadius))
  964. {
  965. // Prepare to off-mesh connection.
  966. const int idx = (int)(ag - m_agents);
  967. dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
  968. // Adjust the path over the off-mesh connection.
  969. dtPolyRef refs[2];
  970. if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
  971. anim->startPos, anim->endPos, m_navquery))
  972. {
  973. dtVcopy(anim->initPos, ag->npos);
  974. anim->polyRef = refs[1];
  975. anim->active = true;
  976. anim->t = 0.0f;
  977. anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
  978. ag->state = DT_CROWDAGENT_STATE_OFFMESH;
  979. ag->ncorners = 0;
  980. ag->nneis = 0;
  981. continue;
  982. }
  983. else
  984. {
  985. // Path validity check will ensure that bad/blocked connections will be replanned.
  986. }
  987. }
  988. }
  989. // Calculate steering.
  990. for (int i = 0; i < nagents; ++i)
  991. {
  992. dtCrowdAgent* ag = agents[i];
  993. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  994. continue;
  995. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
  996. continue;
  997. float dvel[3] = {0,0,0};
  998. if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  999. {
  1000. dtVcopy(dvel, ag->targetPos);
  1001. ag->desiredSpeed = dtVlen(ag->targetPos);
  1002. }
  1003. else
  1004. {
  1005. // Calculate steering direction.
  1006. if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
  1007. calcSmoothSteerDirection(ag, dvel);
  1008. else
  1009. calcStraightSteerDirection(ag, dvel);
  1010. // Calculate speed scale, which tells the agent to slowdown at the end of the path.
  1011. const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
  1012. const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
  1013. ag->desiredSpeed = ag->params.maxSpeed;
  1014. dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
  1015. }
  1016. // Separation
  1017. if (ag->params.updateFlags & DT_CROWD_SEPARATION)
  1018. {
  1019. const float separationDist = ag->params.collisionQueryRange;
  1020. const float invSeparationDist = 1.0f / separationDist;
  1021. const float separationWeight = ag->params.separationWeight;
  1022. float w = 0;
  1023. float disp[3] = {0,0,0};
  1024. for (int j = 0; j < ag->nneis; ++j)
  1025. {
  1026. const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
  1027. float diff[3];
  1028. dtVsub(diff, ag->npos, nei->npos);
  1029. diff[1] = 0;
  1030. const float distSqr = dtVlenSqr(diff);
  1031. if (distSqr < 0.00001f)
  1032. continue;
  1033. if (distSqr > dtSqr(separationDist))
  1034. continue;
  1035. const float dist = dtMathSqrtf(distSqr);
  1036. const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
  1037. dtVmad(disp, disp, diff, weight/dist);
  1038. w += 1.0f;
  1039. }
  1040. if (w > 0.0001f)
  1041. {
  1042. // Adjust desired velocity.
  1043. dtVmad(dvel, dvel, disp, 1.0f/w);
  1044. // Clamp desired velocity to desired speed.
  1045. const float speedSqr = dtVlenSqr(dvel);
  1046. const float desiredSqr = dtSqr(ag->desiredSpeed);
  1047. if (speedSqr > desiredSqr)
  1048. dtVscale(dvel, dvel, desiredSqr/speedSqr);
  1049. }
  1050. }
  1051. // Set the desired velocity.
  1052. dtVcopy(ag->dvel, dvel);
  1053. }
  1054. // Velocity planning.
  1055. for (int i = 0; i < nagents; ++i)
  1056. {
  1057. dtCrowdAgent* ag = agents[i];
  1058. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  1059. continue;
  1060. if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
  1061. {
  1062. m_obstacleQuery->reset();
  1063. // Add neighbours as obstacles.
  1064. for (int j = 0; j < ag->nneis; ++j)
  1065. {
  1066. const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
  1067. m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
  1068. }
  1069. // Append neighbour segments as obstacles.
  1070. for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
  1071. {
  1072. const float* s = ag->boundary.getSegment(j);
  1073. if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
  1074. continue;
  1075. m_obstacleQuery->addSegment(s, s+3);
  1076. }
  1077. dtObstacleAvoidanceDebugData* vod = 0;
  1078. if (debugIdx == i)
  1079. vod = debug->vod;
  1080. // Sample new safe velocity.
  1081. bool adaptive = true;
  1082. int ns = 0;
  1083. const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
  1084. if (adaptive)
  1085. {
  1086. ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
  1087. ag->vel, ag->dvel, ag->nvel, params, vod);
  1088. }
  1089. else
  1090. {
  1091. ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
  1092. ag->vel, ag->dvel, ag->nvel, params, vod);
  1093. }
  1094. m_velocitySampleCount += ns;
  1095. }
  1096. else
  1097. {
  1098. // If not using velocity planning, new velocity is directly the desired velocity.
  1099. dtVcopy(ag->nvel, ag->dvel);
  1100. }
  1101. }
  1102. // Integrate.
  1103. for (int i = 0; i < nagents; ++i)
  1104. {
  1105. dtCrowdAgent* ag = agents[i];
  1106. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  1107. continue;
  1108. integrate(ag, dt);
  1109. }
  1110. // Handle collisions.
  1111. static const float COLLISION_RESOLVE_FACTOR = 0.7f;
  1112. for (int iter = 0; iter < 4; ++iter)
  1113. {
  1114. for (int i = 0; i < nagents; ++i)
  1115. {
  1116. dtCrowdAgent* ag = agents[i];
  1117. const int idx0 = getAgentIndex(ag);
  1118. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  1119. continue;
  1120. dtVset(ag->disp, 0,0,0);
  1121. float w = 0;
  1122. for (int j = 0; j < ag->nneis; ++j)
  1123. {
  1124. const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
  1125. const int idx1 = getAgentIndex(nei);
  1126. float diff[3];
  1127. dtVsub(diff, ag->npos, nei->npos);
  1128. diff[1] = 0;
  1129. float dist = dtVlenSqr(diff);
  1130. if (dist > dtSqr(ag->params.radius + nei->params.radius))
  1131. continue;
  1132. dist = dtMathSqrtf(dist);
  1133. float pen = (ag->params.radius + nei->params.radius) - dist;
  1134. if (dist < 0.0001f)
  1135. {
  1136. // Agents on top of each other, try to choose diverging separation directions.
  1137. if (idx0 > idx1)
  1138. dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
  1139. else
  1140. dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
  1141. pen = 0.01f;
  1142. }
  1143. else
  1144. {
  1145. pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
  1146. }
  1147. dtVmad(ag->disp, ag->disp, diff, pen);
  1148. w += 1.0f;
  1149. }
  1150. if (w > 0.0001f)
  1151. {
  1152. const float iw = 1.0f / w;
  1153. dtVscale(ag->disp, ag->disp, iw);
  1154. }
  1155. }
  1156. for (int i = 0; i < nagents; ++i)
  1157. {
  1158. dtCrowdAgent* ag = agents[i];
  1159. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  1160. continue;
  1161. dtVadd(ag->npos, ag->npos, ag->disp);
  1162. }
  1163. }
  1164. for (int i = 0; i < nagents; ++i)
  1165. {
  1166. dtCrowdAgent* ag = agents[i];
  1167. if (ag->state != DT_CROWDAGENT_STATE_WALKING)
  1168. continue;
  1169. // Move along navmesh.
  1170. ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
  1171. // Get valid constrained position back.
  1172. dtVcopy(ag->npos, ag->corridor.getPos());
  1173. // If not using path, truncate the corridor to just one poly.
  1174. if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
  1175. {
  1176. ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
  1177. ag->partial = false;
  1178. }
  1179. }
  1180. // Update agents using off-mesh connection.
  1181. for (int i = 0; i < m_maxAgents; ++i)
  1182. {
  1183. dtCrowdAgentAnimation* anim = &m_agentAnims[i];
  1184. if (!anim->active)
  1185. continue;
  1186. dtCrowdAgent* ag = agents[i];
  1187. anim->t += dt;
  1188. if (anim->t > anim->tmax)
  1189. {
  1190. // Reset animation
  1191. anim->active = false;
  1192. // Prepare agent for walking.
  1193. ag->state = DT_CROWDAGENT_STATE_WALKING;
  1194. continue;
  1195. }
  1196. // Update position
  1197. const float ta = anim->tmax*0.15f;
  1198. const float tb = anim->tmax;
  1199. if (anim->t < ta)
  1200. {
  1201. const float u = tween(anim->t, 0.0, ta);
  1202. dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
  1203. }
  1204. else
  1205. {
  1206. const float u = tween(anim->t, ta, tb);
  1207. dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
  1208. }
  1209. // Update velocity.
  1210. dtVset(ag->vel, 0,0,0);
  1211. dtVset(ag->dvel, 0,0,0);
  1212. }
  1213. }