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