void dtCrowd::updateStepSteering(const float dt, dtCrowdAgentDebugInfo*) { // Calculate steering. 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) 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); float speedScale = 1.0f; if (ag->params.updateFlags & DT_CROWD_SLOWDOWN_AT_GOAL) { // 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. 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 = sqrtf(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); } }
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); } }
bool OgreDetourCrowd::destinationReached(const dtCrowdAgent* agent, const float maxDistanceFromTarget) { return getDistanceToGoal(agent, maxDistanceFromTarget) < maxDistanceFromTarget; }