static void AddRouteResult( Bot_t *bot, dtPolyRef start, dtPolyRef end, dtStatus status )
{
// only store route failures or partial results
if ( !dtStatusFailed( status ) && !dtStatusDetail( status, DT_PARTIAL_RESULT ) )
{
return;
}
dtRouteResult *bestPos = nullptr;
for ( int i = 0; i < MAX_ROUTE_CACHE; i++ )
{
dtRouteResult &res = bot->routeResults[ i ];
if ( !bestPos || res.time < bestPos->time || res.invalid )
{
bestPos = &res;
}
}
// add result
bestPos->endRef = end;
bestPos->startRef = start;
bestPos->invalid = false;
bestPos->time = svs.time;
bestPos->status = status;
}
bool FindRoute( Bot_t *bot, rVec s, botRouteTargetInternal rtarget, bool allowPartial )
{
rVec start;
rVec end;
dtPolyRef startRef, endRef = 1;
dtPolyRef pathPolys[ MAX_BOT_PATH ];
dtStatus status;
int pathNumPolys;
InvalidateRouteResults( bot );
if ( !BotFindNearestPoly( bot, s, &startRef, start ) )
{
return false;
}
status = bot->nav->query->findNearestPoly( rtarget.pos, rtarget.polyExtents,
&bot->nav->filter, &endRef, end );
if ( dtStatusFailed( status ) || !endRef )
{
return false;
}
// cache failed results
dtRouteResult *res = FindRouteResult( bot, startRef, endRef );
if ( res )
{
if ( dtStatusFailed( res->status ) )
{
return false;
}
if ( dtStatusDetail( res->status, DT_PARTIAL_RESULT ) && !allowPartial )
{
return false;
}
}
status = bot->nav->query->findPath( startRef, endRef, start, end, &bot->nav->filter, pathPolys, &pathNumPolys, MAX_BOT_PATH );
AddRouteResult( bot, startRef, endRef, status );
if ( dtStatusFailed( status ) )
{
return false;
}
if ( dtStatusDetail( status, DT_PARTIAL_RESULT ) && !allowPartial )
{
return false;
}
bot->corridor.reset( startRef, start );
bot->corridor.setCorridor( end, pathPolys, pathNumPolys );
bot->needReplan = false;
bot->offMesh = false;
return true;
}
void dtCrowd::updateMoveRequest(const float /*dt*/)
{
const int PATH_MAX_AGENTS = 8;
dtCrowdAgent* queue[PATH_MAX_AGENTS];
int nqueue = 0;
// Fire off new requests.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgent* ag = &m_agents[i];
if (!ag->active)
continue;
if (ag->state == DT_CROWDAGENT_STATE_INVALID)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING)
{
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
dtAssert(npath);
static const int MAX_RES = 32;
float reqPos[3];
dtPolyRef reqPath[MAX_RES]; // The path to the request location
int reqPathCount = 0;
// Quick search towards the goal.
static const int MAX_ITER = 20;
m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
m_navquery->updateSlicedFindPath(MAX_ITER, 0);
dtStatus status = 0;
if (ag->targetReplan) // && npath > 10)
{
// Try to use existing steady path during replan if possible.
status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
}
else
{
// Try to move towards target when goal changes.
status = m_navquery->finalizeSlicedFindPath(reqPath, &reqPathCount, MAX_RES);
}
if (!dtStatusFailed(status) && reqPathCount > 0)
{
// In progress or succeed.
if (reqPath[reqPathCount-1] != ag->targetRef)
{
// Partial path, constrain target position inside the last polygon.
status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
if (dtStatusFailed(status))
reqPathCount = 0;
}
else
{
dtVcopy(reqPos, ag->targetPos);
}
}
else
{
reqPathCount = 0;
}
if (!reqPathCount)
{
// Could not find path, start the request from current location.
dtVcopy(reqPos, ag->npos);
reqPath[0] = path[0];
reqPathCount = 1;
}
ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
ag->boundary.reset();
ag->partial = false;
if (reqPath[reqPathCount-1] == ag->targetRef)
{
ag->targetState = DT_CROWDAGENT_TARGET_VALID;
ag->targetReplanTime = 0.0;
}
else
{
// The path is longer or potentially unreachable, full plan.
ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE;
}
}
if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
{
nqueue = addToPathQueue(ag, queue, nqueue, PATH_MAX_AGENTS);
}
}
for (int i = 0; i < nqueue; ++i)
{
dtCrowdAgent* ag = queue[i];
ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->params.queryFilterType]);
if (ag->targetPathqRef != DT_PATHQ_INVALID)
ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
}
// Update requests.
m_pathq.update(MAX_ITERS_PER_UPDATE);
dtStatus status;
// Process path results.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgent* ag = &m_agents[i];
if (!ag->active)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
{
// Poll path queue.
status = m_pathq.getRequestStatus(ag->targetPathqRef);
if (dtStatusFailed(status))
{
// Path find failed, retry if the target location is still valid.
ag->targetPathqRef = DT_PATHQ_INVALID;
if (ag->targetRef)
ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
else
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
ag->targetReplanTime = 0.0;
}
else if (dtStatusSucceed(status))
{
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
dtAssert(npath);
// Apply results.
float targetPos[3];
dtVcopy(targetPos, ag->targetPos);
dtPolyRef* res = m_pathResult;
bool valid = true;
int nres = 0;
status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
if (dtStatusFailed(status) || !nres)
valid = false;
if (dtStatusDetail(status, DT_PARTIAL_RESULT))
ag->partial = true;
else
ag->partial = false;
// Merge result and existing path.
// The agent might have moved whilst the request is
// being processed, so the path may have changed.
// We assume that the end of the path is at the same location
// where the request was issued.
// The last ref in the old path should be the same as
// the location where the request was issued..
if (valid && path[npath-1] != res[0])
valid = false;
if (valid)
{
// Put the old path infront of the old path.
if (npath > 1)
{
// Make space for the old path.
if ((npath-1)+nres > m_maxPathResult)
nres = m_maxPathResult - (npath-1);
memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
// Copy old path in the beginning.
memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
nres += npath-1;
// Remove trackbacks
for (int j = 0; j < nres; ++j)
{
if (j-1 >= 0 && j+1 < nres)
{
if (res[j-1] == res[j+1])
{
memmove(res+(j-1), res+(j+1), sizeof(dtPolyRef)*(nres-(j+1)));
nres -= 2;
j -= 2;
}
}
}
}
// Check for partial path.
if (res[nres-1] != ag->targetRef)
{
// Partial path, constrain target position inside the last polygon.
float nearest[3];
status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
if (dtStatusSucceed(status))
dtVcopy(targetPos, nearest);
else
valid = false;
}
}
if (valid)
{
// Set current corridor.
ag->corridor.setCorridor(targetPos, res, nres);
// Force to update boundary.
ag->boundary.reset();
ag->targetState = DT_CROWDAGENT_TARGET_VALID;
}
else
{
// Something went wrong.
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
}
ag->targetReplanTime = 0.0;
}
}
}
}
