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); } }