static void calcVel(float* vel, const float* pos, const float* tgt, const float speed)
{
dtVsub(vel, tgt, pos);
vel[1] = 0.0;
dtVnormalize(vel);
dtVscale(vel, vel, speed);
}
static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
{
if (!ag->ncorners)
{
dtVset(dir, 0,0,0);
return;
}
const int ip0 = 0;
const int ip1 = dtMin(1, ag->ncorners-1);
const float* p0 = &ag->cornerVerts[ip0*3];
const float* p1 = &ag->cornerVerts[ip1*3];
float dir0[3], dir1[3];
dtVsub(dir0, p0, ag->npos);
dtVsub(dir1, p1, ag->npos);
dir0[1] = 0;
dir1[1] = 0;
float len0 = dtVlen(dir0);
float len1 = dtVlen(dir1);
if (len1 > 0.001f)
dtVscale(dir1,dir1,1.0f/len1);
dir[0] = dir0[0] - dir1[0]*len0*0.5f;
dir[1] = 0;
dir[2] = dir0[2] - dir1[2]*len0*0.5f;
dtVnormalize(dir);
}
void OgreDetourCrowd::calcVel(float* velocity, const float* position, const float* target, const float speed)
{
dtVsub(velocity, target, position);
velocity[1] = 0.0;
dtVnormalize(velocity);
dtVscale(velocity, velocity, speed);
}
void Agent::Force(const float* p)
{
float vel[3];
if (m_CrowdAgent && m_CrowdAgent->active)
{
// calculate velocity
dtVsub(vel, p, m_CrowdAgent->npos);
vel[1] = 0.0;
dtVnormalize(vel);
dtVscale(vel, vel, m_CrowdAgent->params.maxSpeed);
m_NavMesh->Crowd()->requestMoveVelocity(m_AgentID, vel);
}
}
void dtSeekBehavior::applyForce(const dtCrowdAgent* oldAgent, dtCrowdAgent* newAgent, float* force, float dt)
{
float tmpForce[] = {0, 0, 0};
float newVelocity[] = {0, 0, 0};
const dtCrowdAgent* target = m_agentParams[oldAgent->id]->targetID;
const float distance = m_agentParams[oldAgent->id]->seekDistance;
// Adapting the force to the dt and the previous velocity
dtVscale(tmpForce, force, dt);
dtVadd(newVelocity, oldAgent->vel, tmpForce);
float currentSpeed = dtVlen(oldAgent->vel);
// Required distance to reach nil speed according to the acceleration and current speed
float slowDist = currentSpeed * (currentSpeed - 0) / oldAgent->params.maxAcceleration;
float distToObj = dtVdist(oldAgent->npos, target->npos) - oldAgent->params.radius - target->params.radius - distance;
// If we have reached the target, we stop
if (distToObj <= EPSILON)
{
dtVset(newVelocity, 0, 0, 0);
newAgent->desiredSpeed = 0.f;
}
// If the have to slow down
else if (distToObj < slowDist)
{
float slowDownRadius = distToObj / slowDist;
dtVscale(newVelocity, newVelocity, slowDownRadius);
newAgent->desiredSpeed = dtVlen(newVelocity);
}
// Else, we move as fast as possible
else
newAgent->desiredSpeed = oldAgent->params.maxSpeed;
// Check for maximal speed
dtVclamp(newVelocity, dtVlen(newVelocity), oldAgent->params.maxSpeed);
dtVcopy(newAgent->dvel, newVelocity);
}
static void integrate(dtCrowdAgent* ag, const float dt)
{
// Fake dynamic constraint.
const float maxDelta = ag->params.maxAcceleration * dt;
float dv[3];
dtVsub(dv, ag->nvel, ag->vel);
float ds = dtVlen(dv);
if (ds > maxDelta)
dtVscale(dv, dv, maxDelta/ds);
dtVadd(ag->vel, ag->vel, dv);
// Integrate
if (dtVlen(ag->vel) > 0.0001f)
dtVmad(ag->npos, ag->npos, ag->vel, dt);
else
dtVset(ag->vel,0,0,0);
}
void dtCrowd::updateVelocity(const float dt, unsigned* agentsIdx, unsigned nbIdx)
{
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// If we want to update every agent
if (agentsIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->behavior)
ag->behavior->update(*m_crowdQuery, *ag, *ag, dt);
}
// Fake dynamic constraint
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
const float maxDelta = ag->maxAcceleration * dt;
float dv[3];
dtVsub(dv, ag->desiredVelocity, ag->velocity);
float ds = dtVlen(dv);
if (ds > maxDelta)
dtVscale(dv, dv, maxDelta/ds);
dtVadd(ag->velocity, ag->velocity, dv);
}
}
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 dtSeekBehavior::computeForce(const dtCrowdAgent* ag, float* force)
{
const dtCrowdAgent* target = m_agentParams[ag->id]->targetID;
const float predictionFactor = m_agentParams[ag->id]->seekPredictionFactor;
// Required force in order to reach the target
dtVsub(force, target->npos, ag->npos);
// We take into account the prediction factor
float scaledVelocity[3] = {0, 0, 0};
dtVscale(scaledVelocity, target->vel, predictionFactor);
dtVadd(force, force, scaledVelocity);
// Set the force according to the maximum acceleration
dtVclamp(force, dtVlen(force), ag->params.maxAcceleration);
force[1] = 0;
}
/* Calculate the collision penalty for a given velocity vector
*
* @param vcand sampled velocity
* @param dvel desired velocity
* @param minPenalty threshold penalty for early out
*/
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float* vel, const float* dvel,
const float minPenalty,
dtObstacleAvoidanceDebugData* debug)
{
// penalty for straying away from the desired and current velocities
const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
// find the threshold hit time to bail out based on the early out penalty
// (see how the penalty is calculated below to understnad)
float minPen = minPenalty - vpen - vcpen;
float tThresold = ((double)m_params.weightToi/(double)minPen - 0.1) * (double)m_params.horizTime;
if (tThresold - m_params.horizTime > -FLT_EPSILON)
return minPenalty; // already too much
// Find min time of impact and exit amongst all obstacles.
float tmin = m_params.horizTime;
float side = 0;
int nside = 0;
for (int i = 0; i < m_ncircles; ++i)
{
const dtObstacleCircle* cir = &m_circles[i];
// RVO
float vab[3];
dtVscale(vab, vcand, 2);
dtVsub(vab, vab, vel);
dtVsub(vab, vab, cir->vel);
// Side
side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f);
nside++;
float htmin = 0, htmax = 0;
if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
}
for (int i = 0; i < m_nsegments; ++i)
{
const dtObstacleSegment* seg = &m_segments[i];
float htmin = 0;
if (seg->touch)
{
// Special case when the agent is very close to the segment.
float sdir[3], snorm[3];
dtVsub(sdir, seg->q, seg->p);
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (dtVdot2D(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
}
else
{
if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float spen = m_params.weightSide * side;
const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
const float penalty = vpen + vcpen + spen + tpen;
// Store different penalties for debug viewing
if (debug)
debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
return penalty;
}
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float vmax, const float* vel, const float* dvel,
dtObstacleAvoidanceDebugData* debug)
{
// Find min time of impact and exit amongst all obstacles.
float tmin = m_horizTime;
float side = 0;
int nside = 0;
for (int i = 0; i < m_ncircles; ++i)
{
const dtObstacleCircle* cir = &m_circles[i];
// RVO
float vab[3];
dtVscale(vab, vcand, 2);
dtVsub(vab, vab, vel);
dtVsub(vab, vab, cir->vel);
// Side
side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f);
nside++;
float htmin = 0, htmax = 0;
if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
}
for (int i = 0; i < m_nsegments; ++i)
{
const dtObstacleSegment* seg = &m_segments[i];
float htmin = 0;
if (seg->touch)
{
// Special case when the agent is very close to the segment.
float sdir[3], snorm[3];
dtVsub(sdir, seg->q, seg->p);
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (dtVdot2D(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
}
else
{
if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float ivmax = 1.0f / vmax;
const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax);
const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax);
const float spen = m_weightSide * side;
const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime));
const float penalty = vpen + vcpen + spen + tpen;
// Store different penalties for debug viewing
if (debug)
debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
return penalty;
}
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::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);
}
}
bool Visualization::update()
{
if (!m_initialized)
{
printf("Visualization has not been yet initialized.");
return false;
}
// Compute the time since last update (in seconds).
Uint32 time = SDL_GetTicks();
float dt = (time - m_lastTickCount) / 1000.0f;
m_lastTickCount = time;
bool singleSimulationStep = false;
bool scrollForward = false;
bool scrollBackward = false;
int mscroll = 0;
SDL_Event event;
while (SDL_PollEvent(&event))
{
switch (event.type)
{
case SDL_KEYDOWN:
// Handle any key presses here.
if (event.key.keysym.sym == SDLK_ESCAPE) //Escape.
{
m_stopRequested = true;
}
else if (event.key.keysym.sym == SDLK_SPACE)
{
m_paused = !m_paused;
}
else if (event.key.keysym.sym == SDLK_TAB)
{
singleSimulationStep = true;
}
else if (event.key.keysym.sym == SDLK_r)
{
float pos[3] = {-15, 0, 15};
m_crowd->pushAgentPosition(0, pos);
}
else if (event.key.keysym.sym == SDLK_KP7)
{
float pos[3] = {-19, 0, -19};
dtVnormalize(pos);
dtVscale(pos, pos, 2.f);
dtCrowdAgent ag;
m_crowd->fetchAgent(ag, 0);
dtVcopy(ag.velocity, pos);
m_crowd->pushAgent(ag);
}
else if (event.key.keysym.sym == SDLK_KP9)
{
float pos[] = {19, 0, -19};
dtVnormalize(pos);
dtVscale(pos, pos, 2.f);
dtCrowdAgent ag;
m_crowd->fetchAgent(ag, 0);
dtVcopy(ag.velocity, pos);
m_crowd->pushAgent(ag);
}
else if (event.key.keysym.sym == SDLK_KP3)
{
float pos[3] = {19, 0, 19};
dtVnormalize(pos);
dtVscale(pos, pos, 2.f);
dtCrowdAgent ag;
m_crowd->fetchAgent(ag, 0);
dtVcopy(ag.velocity, pos);
m_crowd->pushAgent(ag);
}
else if (event.key.keysym.sym == SDLK_KP1)
{
float pos[3] = {-19, 0, 19};
dtVnormalize(pos);
dtVscale(pos, pos, 2.f);
dtCrowdAgent ag;
m_crowd->fetchAgent(ag, 0);
dtVcopy(ag.velocity, pos);
m_crowd->pushAgent(ag);
}
else if (event.key.keysym.sym == SDLK_KP5)
{
float pos[3] = {0, 0, 0};
dtVnormalize(pos);
dtVscale(pos, pos, 2.f);
dtCrowdAgent ag;
m_crowd->fetchAgent(ag, 0);
dtVcopy(ag.velocity, pos);
m_crowd->pushAgent(ag);
}
break;
case SDL_MOUSEBUTTONDOWN:
if (event.button.button == SDL_BUTTON_RIGHT)
{
// Rotate view
m_rotating = true;
m_initialMousePosition[0] = m_mousePosition[0];
m_initialMousePosition[1] = m_mousePosition[1];
m_intialCameraOrientation[0] = m_cameraOrientation[0];
m_intialCameraOrientation[1] = m_cameraOrientation[1];
m_intialCameraOrientation[2] = m_cameraOrientation[2];
}
else if (event.button.button == SDL_BUTTON_WHEELUP)
{
scrollForward = true;
}
else if (event.button.button == SDL_BUTTON_WHEELDOWN)
{
scrollBackward = true;
}
break;
case SDL_MOUSEBUTTONUP:
if (event.button.button == SDL_BUTTON_RIGHT)
{
m_rotating = false;
}
else if (event.button.button == SDL_BUTTON_LEFT)
{
float pickedPosition[3];
int index = 0;
pick(pickedPosition, &index);
}
break;
case SDL_MOUSEMOTION:
m_mousePosition[0] = event.motion.x;
m_mousePosition[1] = m_winHeight-1 - event.motion.y;
if (m_rotating)
{
int dx = m_mousePosition[0] - m_initialMousePosition[0];
int dy = m_mousePosition[1] - m_initialMousePosition[1];
m_cameraOrientation[0] = m_intialCameraOrientation[0] - dy*0.25f;
m_cameraOrientation[1] = m_intialCameraOrientation[1] + dx*0.25f;
}
break;
case SDL_QUIT:
m_stopRequested = true;
break;
default:
break;
}
}
unsigned char mbut = 0;
if (SDL_GetMouseState(0,0) & SDL_BUTTON_LMASK)
mbut |= IMGUI_MBUT_LEFT;
if (SDL_GetMouseState(0,0) & SDL_BUTTON_RMASK)
mbut |= IMGUI_MBUT_RIGHT;
// Update the camera velocity from keyboard state.
Uint8* keystate = SDL_GetKeyState(NULL);
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable:4800)
#endif
updateCameraVelocity(
dt,
keystate[SDLK_w] || keystate[SDLK_UP] || scrollForward,
keystate[SDLK_s] || keystate[SDLK_DOWN] || scrollBackward,
keystate[SDLK_a] || keystate[SDLK_LEFT],
keystate[SDLK_d] || keystate[SDLK_RIGHT],
SDL_GetModState() & KMOD_SHIFT);
#ifdef _MSC_VER
#pragma warning(pop)
#endif
//Update the camera position
updateCameraPosition(dt);
//Update the crowd
if (m_crowd)
{
if (singleSimulationStep)
{
m_paused = true;
m_debugInfo->startUpdate();
m_crowd->update(m_crowdDt);
m_debugInfo->endUpdate(m_crowdDt);
m_crowdAvailableDt = 0.f;
}
else if (!m_paused)
{
m_crowdAvailableDt += dt;
while(m_crowdAvailableDt > m_crowdDt)
{
m_debugInfo->startUpdate();
m_crowd->update(m_crowdDt);
m_debugInfo->endUpdate(m_crowdDt);
m_crowdAvailableDt -= m_crowdDt;
}
}
else
{
m_crowdAvailableDt = 0.f;
}
}
// Set rendering context
glViewport(0, 0, m_winWidth, m_winHeight);
glClearColor(0.3f, 0.3f, 0.32f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDisable(GL_TEXTURE_2D);
// Render 3D
glEnable(GL_DEPTH_TEST);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(50.0f, (float)m_winWidth/(float)m_winHeight, m_zNear, m_zFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glRotatef(m_cameraOrientation[0],1,0,0);
glRotatef(m_cameraOrientation[1],0,1,0);
glRotatef(m_cameraOrientation[2],0,0,1);
glTranslatef(-m_cameraPosition[0], -m_cameraPosition[1], -m_cameraPosition[2]);
// Extract OpenGL view properties
glGetDoublev(GL_PROJECTION_MATRIX, m_projection);
glGetDoublev(GL_MODELVIEW_MATRIX, m_modelView);
glGetIntegerv(GL_VIEWPORT, m_viewport);
renderScene();
renderCrowd();
// Render 2D Overlay
glDisable(GL_DEPTH_TEST);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0, m_winWidth, 0, m_winHeight);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
imguiBeginFrame(m_mousePosition[0], m_mousePosition[1], mbut,mscroll);
renderDebugInfoOverlay();
imguiEndFrame();
imguiRenderGLDraw();
glEnable(GL_DEPTH_TEST);
SDL_GL_SwapBuffers();
return true;
}
