server_common/recastnavigation_v1.6.0
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Collect neighbours once and use them in local avoidance and collision detection. Moved movement request update to separate function.

Browse Source
This commit is contained in:
Mikko Mononen 2010-10-10 10:00:14 +00:00
parent 347071a563
commit 7e69ce1296
1 changed files with 87 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
RecastDemo/Source/CrowdManager.cpp
View File

@ -749,6 +749,7 @@ int CrowdManager::addAgent(const float* pos, const float radius, const float hei
ag->height = height;
ag->collisionQueryRange = radius * 8;
ag->pathOptimizationRange = radius * 30;
ag->nneis = 0;
dtVset(ag->dvel, 0,0,0);
dtVset(ag->nvel, 0,0,0);
@ -822,8 +823,49 @@ int CrowdManager::getActiveAgents(Agent** agents, const int maxAgents)
return n;
}
static int addNeighbour(const int idx, const float dist,
Neighbour* neis, const int nneis, const int maxNeis)
{
// Insert neighbour based on the distance.
Neighbour* nei = 0;
if (!nneis)
{
nei = &neis[nneis];
}
else if (dist >= neis[nneis-1].dist)
{
if (nneis >= maxNeis)
return nneis;
nei = &neis[nneis];
}
else
{
int i;
for (i = 0; i < nneis; ++i)
if (dist <= neis[i].dist)
break;
const int tgt = i+1;
const int n = dtMin(nneis-i, maxNeis-tgt);
dtAssert(tgt+n <= maxNeis);
if (n > 0)
memmove(&neis[tgt], &neis[i], sizeof(Neighbour)*n);
nei = &neis[i];
}
memset(nei, 0, sizeof(Neighbour));
nei->idx = idx;
nei->dist = dist;
return dtMin(nneis+1, maxNeis);
}
int CrowdManager::getNeighbours(const float* pos, const float height, const float range,
const Agent* skip, Agent** result, const int maxResult)
const Agent* skip, Neighbour* result, const int maxResult)
{
int n = 0;
@ -838,6 +880,7 @@ int CrowdManager::getNeighbours(const float* pos, const float height, const floa
if (ag == skip) continue;
// Check for overlap.
float diff[3];
dtVsub(diff, pos, ag->npos);
if (fabsf(diff[1]) >= (height+ag->height)/2.0f)
@ -847,36 +890,20 @@ int CrowdManager::getNeighbours(const float* pos, const float height, const floa
if (distSqr > dtSqr(range))
continue;
if (n < maxResult)
result[n++] = ag;
n = addNeighbour(ids[i], distSqr, result, n, maxResult);
}
return n;
}
void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* navquery)
void CrowdManager::updateMoveRequest(const float dt, dtNavMeshQuery* navquery)
{
m_sampleCount = 0;
m_totalTime = 0;
m_rvoTime = 0;
if (!navquery)
return;
TimeVal startTime = getPerfTime();
Agent* agents[MAX_AGENTS];
Agent* neis[MAX_AGENTS];
int nagents = getActiveAgents(agents, MAX_AGENTS);
static const float MAX_ACC = 8.0f;
static const float MAX_SPEED = 3.5f;
// Update move requests.
for (int i = 0; i < m_moveRequestCount; ++i)
{
MoveRequest* req = &m_moveRequests[i];
Agent* ag = &m_agents[req->idx];
// Agent not active anymore, kill request.
if (!ag->active)
req->state = MR_TARGET_FAILED;
@ -990,7 +1017,27 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
}
m_pathq.update(navquery);
}
void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* navquery)
{
m_sampleCount = 0;
m_totalTime = 0;
m_rvoTime = 0;
if (!navquery)
return;
TimeVal startTime = getPerfTime();
Agent* agents[MAX_AGENTS];
int nagents = getActiveAgents(agents, MAX_AGENTS);
static const float MAX_ACC = 8.0f;
static const float MAX_SPEED = 3.5f;
// Update async move request and path finder.
updateMoveRequest(dt, navquery);
// Register agents to proximity grid.
m_grid.clear();
@ -999,19 +1046,17 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
Agent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->radius;
const float minx = p[0] - r;
const float miny = p[2] - r;
const float maxx = p[0] + r;
const float maxy = p[2] + r;
m_grid.addItem((unsigned short)i, minx, miny, maxx, maxy);
m_grid.addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
}
// Get nearby navmesh segments to collide with.
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
Agent* ag = agents[i];
// Update collision segments
ag->corridor.updateLocalNeighbourhood(ag->collisionQueryRange, navquery, &m_filter);
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, ag->neis, MAX_NEIGHBOURS);
}
// Find next corner to steer to.
@ -1034,8 +1079,6 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
else
ag->corridor.calcStraightSteerDirection(dvel);
// Calculate steering speed.
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->radius*2; // TODO: make less hacky.
const float speedScale = ag->corridor.getDistanceToGoal(slowDownRadius) / slowDownRadius;
@ -1080,11 +1123,10 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
{
m_obstacleQuery->reset();
// Find neighbours and add them as obstacles.
const int nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, neis, MAX_AGENTS);
for (int j = 0; j < nneis; ++j)
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const Agent* nei = neis[j];
const Agent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel,
dtVdist2DSqr(ag->npos, nei->npos));
}
@ -1100,7 +1142,7 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
m_obstacleQuery->addSegment(s, s+3, distSqr);
}
// Sample new safe velocity.
bool adaptive = true;
if (adaptive)
@ -1144,10 +1186,9 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
float w = 0;
for (int j = 0; j < nagents; ++j)
for (int j = 0; j < ag->nneis; ++j)
{
if (i == j) continue;
Agent* nei = agents[j];
const Agent* nei = &m_agents[ag->neis[j].idx];
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
@ -1193,9 +1234,9 @@ void CrowdManager::update(const float dt, unsigned int flags, dtNavMeshQuery* na
dtVcopy(ag->npos, ag->corridor.getPos());
}
TimeVal endTime = getPerfTime();
// Debug/demo book keeping
int ns = 0;
for (int i = 0; i < nagents; ++i)
{