void dtCrowd::updateStepAvoidance(const float dt, dtCrowdAgentDebugInfo* debug)
{
const int debugIdx = debug ? debug->idx : -1;
m_velocitySampleCount = 0;
// Velocity planning.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s + 3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s + 3);
}
dtObstacleAvoidanceDebugData* vod = 0;
const int agIndex = getAgentIndex(ag);
if (debugIdx == agIndex)
vod = debug->vod;
// Sample new safe velocity.
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
const int ns = m_obstacleQuery->sampleVelocity(
ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
}
void dtCrowd::updateStepOffMeshAnim(const float dt, dtCrowdAgentDebugInfo*)
{
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int agentIndex = getAgentIndex(ag);
dtCrowdAgentAnimation* anim = &m_agentAnims[agentIndex];
if (!anim->active)
continue;
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
// UE4: m_keepOffmeshConnections support
if (m_keepOffmeshConnections)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0, 0, 0);
dtVset(ag->dvel, 0, 0, 0);
}
}
void dtCrowd::updateEnvironment(unsigned* agentsIdx, unsigned nbIdx)
{
// If we want to update every agent
if (agentsIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// Get nearby navmesh segments and agents to collide with.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->state == DT_CROWDAGENT_STATE_INVALID)
m_agentsEnv[ag->id].boundary.reset();
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->perceptionDistance * 0.25f;
if (dtVdist2DSqr(ag->position, m_agentsEnv[ag->id].boundary.getCenter()) > dtSqr(updateThr) ||
!m_agentsEnv[ag->id].boundary.isValid(m_crowdQuery->getNavMeshQuery(), m_crowdQuery->getQueryFilter()))
{
dtPolyRef ref;
float nearest[3];
m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), &ref, nearest);
m_agentsEnv[ag->id].boundary.update(ref, ag->position, ag->perceptionDistance,
m_crowdQuery->getNavMeshQuery(), m_crowdQuery->getQueryFilter());
}
// Query neighbour agents
m_agentsEnv[ag->id].nbNeighbors = this->computeNeighbors(ag->id);
for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; j++)
m_agentsEnv[ag->id].neighbors[j].idx = getAgentIndex(&m_agents[m_agentsEnv[ag->id].neighbors[j].idx]);
}
}
void dtCrowd::updateStepOffMeshVelocity(const float dt, dtCrowdAgentDebugInfo*)
{
// UE4 version of offmesh anims, updates velocity and checks distance instead of fixed time
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int agentIndex = getAgentIndex(ag);
dtCrowdAgentAnimation* anim = &m_agentAnims[agentIndex];
if (!anim->active)
continue;
anim->t += dt;
const float dist = dtVdistSqr(ag->npos, anim->endPos);
const float distThres = dtSqr(5.0f);
if (dist < distThres)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
// UE4: m_keepOffmeshConnections support
if (m_keepOffmeshConnections)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
}
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
{
float dir[3] = { 0 };
dtVsub(dir, anim->endPos, anim->initPos);
dir[1] = 0.0f;
dtVnormalize(dir);
dtVscale(ag->nvel, dir, ag->params.maxSpeed);
dtVcopy(ag->vel, ag->nvel);
dtVset(ag->dvel, 0, 0, 0);
}
}
}
void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt)
{
static const int CHECK_LOOKAHEAD = 10;
static const float TARGET_REPLAN_DELAY = 1.0; // seconds
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
ag->targetReplanTime += dt;
bool replan = false;
// First check that the current location is valid.
const int idx = getAgentIndex(ag);
float agentPos[3];
dtPolyRef agentRef = ag->corridor.getFirstPoly();
dtVcopy(agentPos, ag->npos);
if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
{
// Current location is not valid, try to reposition.
// TODO: this can snap agents, how to handle that?
float nearest[3];
dtVcopy(nearest, agentPos);
agentRef = 0;
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
dtVcopy(agentPos, nearest);
if (!agentRef)
{
// Could not find location in navmesh, set state to invalid.
ag->corridor.reset(0, agentPos);
ag->boundary.reset();
ag->state = DT_CROWDAGENT_STATE_INVALID;
continue;
}
// Make sure the first polygon is valid, but leave other valid
// polygons in the path so that replanner can adjust the path better.
ag->corridor.fixPathStart(agentRef, agentPos);
// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
ag->boundary.reset();
dtVcopy(ag->npos, agentPos);
replan = true;
}
// Try to recover move request position.
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
{
if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
{
// Current target is not valid, try to reposition.
float nearest[3];
dtVcopy(nearest, ag->targetPos);
ag->targetRef = 0;
m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
dtVcopy(ag->targetPos, nearest);
replan = true;
}
if (!ag->targetRef)
{
// Failed to reposition target, fail moverequest.
ag->corridor.reset(agentRef, agentPos);
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
}
}
// If nearby corridor is not valid, replan.
if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
{
// Fix current path.
// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
// ag->boundary.reset();
replan = true;
}
// If the end of the path is near and it is not the requested location, replan.
if (ag->targetState == DT_CROWDAGENT_TARGET_VALID)
{
if (ag->targetReplanTime > TARGET_REPLAN_DELAY &&
ag->corridor.getPathCount() < CHECK_LOOKAHEAD &&
ag->corridor.getLastPoly() != ag->targetRef)
replan = true;
}
// Try to replan path to goal.
if (replan)
{
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE)
{
requestMoveTargetReplan(idx, ag->targetRef, ag->targetPos);
}
}
}
}
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
const int debugIdx = debug ? debug->idx : -1;
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
updateMoveRequest(dt);
// Optimize path topology.
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
agents, nagents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
}
// Find next corner to steer to.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
}
else
{
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVset(debug->optStart, 0,0,0);
dtVset(debug->optEnd, 0,0,0);
}
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = (int)(ag - m_agents);
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
anim->startPos, anim->endPos, m_navquery))
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
// Calculate steering.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
else
{
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
}
// Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION)
{
const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
float w = 0;
float disp[3] = {0,0,0};
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = dtMathSqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVmad(disp, disp, diff, weight/dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dtVmad(dvel, dvel, disp, 1.0f/w);
// Clamp desired velocity to desired speed.
const float speedSqr = dtVlenSqr(dvel);
const float desiredSqr = dtSqr(ag->desiredSpeed);
if (speedSqr > desiredSqr)
dtVscale(dvel, dvel, desiredSqr/speedSqr);
}
}
// Set the desired velocity.
dtVcopy(ag->dvel, dvel);
}
// Velocity planning.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s+3);
}
dtObstacleAvoidanceDebugData* vod = 0;
if (debugIdx == i)
vod = debug->vod;
// Sample new safe velocity.
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
else
{
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
// Integrate.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0,0,0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = dtMathSqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
// Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos());
// If not using path, truncate the corridor to just one poly.
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
}
}
// Update agents using off-mesh connection.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgentAnimation* anim = &m_agentAnims[i];
if (!anim->active)
continue;
dtCrowdAgent* ag = agents[i];
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}
void dtCrowd::updatePosition(const float dt, unsigned* agentsIdx, unsigned nbIdx)
{
// If we want to update every agent
if (nbIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// The current start position of the agent (not yet modified)
dtPolyRef* currentPosPoly = (dtPolyRef*) dtAlloc(sizeof(dtPolyRef) * nbIdx, DT_ALLOC_TEMP);
float* currentPos = (float*) dtAlloc(sizeof(float) * 3 * nbIdx, DT_ALLOC_TEMP);
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), currentPosPoly + i, currentPos + (i * 3));
}
// Integrate.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (unsigned iter = 0; iter < 4; ++iter)
{
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVset(disp, 0, 0, 0);
float w = 0;
for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; ++j)
{
const dtCrowdAgent* nei = &m_agents[m_agentsEnv[ag->id].neighbors[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->position, nei->position);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->radius + nei->radius) + EPSILON)
continue;
dist = sqrtf(dist);
float pen = (ag->radius + nei->radius) - dist;
if (dist < EPSILON)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->desiredVelocity[2], 0, ag->desiredVelocity[0]);
else
dtVset(diff, ag->desiredVelocity[2], 0, -ag->desiredVelocity[0]);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(disp, disp, diff, pen);
w += 1.0f;
}
if (w > EPSILON)
{
const float iw = 1.0f / w;
dtVscale(disp, disp, iw);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVadd(ag->position, ag->position, disp);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
float newPos[3];
dtPolyRef visited[dtPathCorridor::MAX_VISITED];
int visitedCount;
m_crowdQuery->getNavMeshQuery()->moveAlongSurface(currentPosPoly[i], currentPos + (i * 3), ag->position, m_crowdQuery->getQueryFilter(), newPos,
visited, &visitedCount, dtPathCorridor::MAX_VISITED);
// Get valid constrained position back.
float newHeight = *(currentPos + (i * 3) + 1);
m_crowdQuery->getNavMeshQuery()->getPolyHeight(currentPosPoly[i], newPos, &newHeight);
newPos[1] = newHeight;
dtVcopy(ag->position, newPos);
}
// Update agents using off-mesh connection.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
float offmeshTotalTime = ag->offmeshInitToStartTime + ag->offmeshStartToEndTime;
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH && offmeshTotalTime > EPSILON)
{
ag->offmeshElaspedTime += dt;
if (ag->offmeshElaspedTime > offmeshTotalTime)
{
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
if (ag->offmeshElaspedTime < ag->offmeshInitToStartTime)
{
const float u = tween(ag->offmeshElaspedTime, 0.0, ag->offmeshInitToStartTime);
dtVlerp(ag->position, ag->offmeshInitPos, ag->offmeshStartPos, u);
}
else
{
const float u = tween(ag->offmeshElaspedTime, ag->offmeshInitToStartTime, offmeshTotalTime);
dtVlerp(ag->position, ag->offmeshStartPos, ag->offmeshEndPos, u);
}
// Update velocity.
dtVset(ag->velocity, 0,0,0);
dtVset(ag->desiredVelocity, 0,0,0);
}
}
dtFree(currentPosPoly);
dtFree(currentPos);
}
void dtCrowd::updateStepMove(const float dt, dtCrowdAgentDebugInfo*)
{
// Integrate.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0, 0, 0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = sqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// m_activeAgents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2], 0, ag->dvel[0]);
else
dtVset(diff, ag->dvel[2], 0, -ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
}
void dtCrowd::updateStepNextMovePoint(const float dt, dtCrowdAgentDebugInfo* debug)
{
const int debugIdx = debug ? debug->idx : -1;
// Find next corner to steer to.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering
m_navquery->updateLinkFilter(ag->params.linkFilter);
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.filter], ag->params.radius);
const int agIndex = getAgentIndex(ag);
if (debugIdx == agIndex)
{
dtVset(debug->optStart, 0, 0, 0);
dtVset(debug->optEnd, 0, 0, 0);
}
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 1)
{
unsigned char allowedArea = DT_WALKABLE_AREA;
if (m_raycastSingleArea)
{
m_navquery->getAttachedNavMesh()->getPolyArea(ag->corridor.getFirstPoly(), &allowedArea);
m_raycastFilter.setAreaCost(allowedArea, 1.0f);
}
const int firstCheckedIdx = ag->ncorners - 1;
const int lastCheckedIdx = (ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS_MULTI) ? 1 : firstCheckedIdx;
for (int cornerIdx = firstCheckedIdx; cornerIdx >= lastCheckedIdx; cornerIdx--)
{
float* target = &ag->cornerVerts[cornerIdx * 3];
const bool bOptimized = ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery,
m_raycastSingleArea ? &m_raycastFilter : &m_filters[ag->params.filter]);
if (bOptimized)
{
// Copy data for debug purposes.
if (debugIdx == agIndex)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
break;
}
}
m_raycastFilter.setAreaCost(allowedArea, DT_UNWALKABLE_POLY_COST);
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = ag - m_agents;
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
m_navquery->updateLinkFilter(ag->params.linkFilter);
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
// UE4: m_keepOffmeshConnections support
const bool bCanStart = m_keepOffmeshConnections ?
ag->corridor.canMoveOverOffmeshConnection(ag->cornerPolys[ag->ncorners - 1], refs, ag->npos, anim->startPos, anim->endPos, m_navquery) :
ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners - 1], refs, ag->npos, anim->startPos, anim->endPos, m_navquery);
if (bCanStart)
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
}
void dtCrowd::updateStepProximityData(const float dt, dtCrowdAgentDebugInfo*)
{
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0] - r, p[2] - r, p[0] + r, p[2] + r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
m_navquery->updateLinkFilter(ag->params.linkFilter);
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filters[ag->params.filter]))
{
// UE4: force removing segments too close to offmesh links
const bool useForcedRemove = m_linkRemovalRadius > 0.0f && ag->ncorners &&
(ag->cornerFlags[ag->ncorners - 1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
const float* removePos = useForcedRemove ? &ag->cornerVerts[(ag->ncorners - 1) * 3] : 0;
const float removeRadius = m_linkRemovalRadius;
// UE4: prepare filter containing only current area
// boundary update will take all corner polys and include them in local neighborhood
unsigned char allowedArea = DT_WALKABLE_AREA;
if (m_raycastSingleArea)
{
m_navquery->getAttachedNavMesh()->getPolyArea(ag->corridor.getFirstPoly(), &allowedArea);
m_raycastFilter.setAreaCost(allowedArea, 1.0f);
}
// UE4: move dir for segment scoring
float moveDir[3] = { 0.0f };
if (ag->ncorners)
{
dtVsub(moveDir, &ag->cornerVerts[2], &ag->cornerVerts[0]);
}
else
{
dtVcopy(moveDir, ag->vel);
}
dtVnormalize(moveDir);
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
removePos, removeRadius, useForcedRemove,
ag->corridor.getPath(), ag->corridor.getPathCount(),
moveDir,
m_navquery, m_raycastSingleArea ? &m_raycastFilter : &m_filters[ag->params.filter]);
m_raycastFilter.setAreaCost(allowedArea, DT_UNWALKABLE_POLY_COST);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
m_activeAgents, m_numActiveAgents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(m_activeAgents[ag->neis[j].idx]);
}
}
void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const float dt)
{
static const int CHECK_LOOKAHEAD = 10;
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Skip if the corridor is valid
if (ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
continue;
// The current path is bad, try to recover.
// Store current state.
float agentPos[3];
dtPolyRef agentRef = 0;
dtVcopy(agentPos, ag->npos);
agentRef = ag->corridor.getFirstPoly();
float targetPos[3];
dtPolyRef targetRef = 0;
dtVcopy(targetPos, ag->corridor.getTarget());
targetRef = ag->corridor.getLastPoly();
// If has active move target, get target location from that instead.
const int idx = getAgentIndex(ag);
const MoveRequest* req = getActiveMoveTarget(idx);
if (req)
{
if (req->state == MR_TARGET_REQUESTING ||
req->state == MR_TARGET_WAITING_FOR_PATH ||
req->state == MR_TARGET_VALID)
{
targetRef = req->ref;
dtVcopy(targetPos, req->pos);
}
else if (req->state == MR_TARGET_ADJUST)
{
targetRef = req->aref;
dtVcopy(targetPos, req->apos);
}
}
// First check that the current location is valid.
if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
{
// Current location is not valid, try to reposition.
// TODO: this can snap agents, how to handle that?
float nearest[3];
agentRef = 0;
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
dtVcopy(agentPos, nearest);
}
if (!agentRef)
{
// Count not find location in navmesh, set state to invalid.
ag->corridor.reset(0, agentPos);
ag->boundary.reset();
ag->state = DT_CROWDAGENT_STATE_INVALID;
continue;
}
// TODO: make temp path by raycasting towards current velocity.
ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
ag->boundary.reset();
dtVcopy(ag->npos, agentPos);
// Check that target is still reachable.
if (!m_navquery->isValidPolyRef(targetRef, &m_filter))
{
// Current target is not valid, try to reposition.
float nearest[3];
targetRef = 0;
m_navquery->findNearestPoly(targetPos, m_ext, &m_filter, &targetRef, nearest);
dtVcopy(targetPos, nearest);
}
if (!targetRef)
{
// Target not reachable anymore, cannot replan.
// TODO: indicate failure!
continue;
}
// Try to replan path to goal.
requestMoveTargetReplan(idx, targetRef, targetPos);
}
}
