/**
@par
Behavior:
- The movement is constrained to the surface of the navigation mesh.
- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid.
- The new target will be located in the adjusted corridor's last polygon.
The expected use case is that the desired target will be 'near' the current corridor. What is considered 'near' depends on local polygon density, query search extents, etc.
The resulting target will differ from the desired target if the desired target is not on the navigation mesh, or it can't be reached using a local search.
*/
bool dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_path);
dtAssert(m_npath);
// Move along navmesh and update new position.
float result[3];
static const int MAX_VISITED = 16;
dtPolyRef visited[MAX_VISITED];
int nvisited = 0;
dtStatus status = navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter,
result, visited, &nvisited, MAX_VISITED);
if (dtStatusSucceed(status))
{
m_npath = dtMergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// TODO: should we do that?
// Adjust the position to stay on top of the navmesh.
/* float h = m_target[1];
navquery->getPolyHeight(m_path[m_npath-1], result, &h);
result[1] = h;*/
dtVcopy(m_target, result);
return true;
}
return false;
}
void dtCrowd::updateMoveRequest(const float /*dt*/)
{
// Fire off new requests.
for (int i = 0; i < m_moveRequestCount; ++i)
{
MoveRequest* req = &m_moveRequests[i];
dtCrowdAgent* ag = &m_agents[req->idx];
// Agent not active anymore, kill request.
if (!ag->active)
req->state = MR_TARGET_FAILED;
// Adjust target
if (req->aref)
{
if (req->state == MR_TARGET_ADJUST)
{
// Adjust existing path.
ag->corridor.moveTargetPosition(req->apos, m_navquery, &m_filter);
req->state = MR_TARGET_VALID;
}
else
{
// Adjust on the flight request.
float result[3];
static const int MAX_VISITED = 16;
dtPolyRef visited[MAX_VISITED];
int nvisited = 0;
m_navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, &m_filter,
result, visited, &nvisited, MAX_VISITED);
req->ntemp = dtMergeCorridorEndMoved(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited);
dtVcopy(req->pos, result);
// Reset adjustment.
dtVset(req->apos, 0,0,0);
req->aref = 0;
}
}
if (req->state == MR_TARGET_REQUESTING)
{
// Calculate request position.
// If there is a lot of latency between requests, it is possible to
// project the current position ahead and use raycast to find the actual
// location and path.
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
dtAssert(npath);
// Here we take the simple approach and set the path to be just the current location.
float reqPos[3];
dtVcopy(reqPos, ag->corridor.getPos()); // The location of the request
dtPolyRef reqPath[8]; // The path to the request location
reqPath[0] = path[0];
int reqPathCount = 1;
req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter);
if (req->pathqRef != DT_PATHQ_INVALID)
{
ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
req->state = MR_TARGET_WAITING_FOR_PATH;
}
}
}
// Update requests.
m_pathq.update(MAX_ITERS_PER_UPDATE);
// Process path results.
for (int i = 0; i < m_moveRequestCount; ++i)
{
MoveRequest* req = &m_moveRequests[i];
dtCrowdAgent* ag = &m_agents[req->idx];
if (req->state == MR_TARGET_WAITING_FOR_PATH)
{
// Poll path queue.
dtStatus status = m_pathq.getRequestStatus(req->pathqRef);
if (dtStatusFailed(status))
{
req->pathqRef = DT_PATHQ_INVALID;
req->state = MR_TARGET_FAILED;
}
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, req->pos);
dtPolyRef* res = m_pathResult;
bool valid = true;
int nres = 0;
dtStatus status = m_pathq.getPathResult(req->pathqRef, res, &nres, m_maxPathResult);
if (dtStatusFailed(status) || !nres)
valid = false;
// Merge with any target adjustment that happened during the search.
if (req->ntemp > 1)
{
nres = dtMergeCorridorEndMoved(res, nres, m_maxPathResult, req->temp, req->ntemp);
}
// 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;
}
// Check for partial path.
if (res[nres-1] != req->ref)
{
// Partial path, constrain target position inside the last polygon.
float nearest[3];
if (m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS)
dtVcopy(targetPos, nearest);
else
valid = false;
}
}
if (valid)
{
ag->corridor.setCorridor(targetPos, res, nres);
req->state = MR_TARGET_VALID;
}
else
{
// Something went wrong.
req->state = MR_TARGET_FAILED;
}
}
}
// Remove request when done with it.
if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED)
{
m_moveRequestCount--;
if (i != m_moveRequestCount)
memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest));
--i;
}
}
}
