| 254 | } |
| 255 | |
| 256 | static int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents) |
| 257 | { |
| 258 | // Insert neighbour based on greatest time. |
| 259 | int slot = 0; |
| 260 | if (!nagents) |
| 261 | { |
| 262 | slot = nagents; |
| 263 | } |
| 264 | else if (newag->targetReplanTime <= agents[nagents-1]->targetReplanTime) |
| 265 | { |
| 266 | if (nagents >= maxAgents) |
| 267 | return nagents; |
| 268 | slot = nagents; |
| 269 | } |
| 270 | else |
| 271 | { |
| 272 | int i; |
| 273 | for (i = 0; i < nagents; ++i) |
| 274 | if (newag->targetReplanTime >= agents[i]->targetReplanTime) |
| 275 | break; |
| 276 | |
| 277 | const int tgt = i+1; |
| 278 | const int n = dtMin(nagents-i, maxAgents-tgt); |
| 279 | |
| 280 | dtAssert(tgt+n <= maxAgents); |
| 281 | |
| 282 | if (n > 0) |
| 283 | memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n); |
| 284 | slot = i; |
| 285 | } |
| 286 | |
| 287 | agents[slot] = newag; |
| 288 | |
| 289 | return dtMin(nagents+1, maxAgents); |
| 290 | } |
| 291 | |
| 292 | |
| 293 | /** |
no test coverage detected