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