server_common/recastnavigation_v1.6.0
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- dtCrowd API Changed!

Browse Source 
- finalizeSlicedFindPathPartial() returns best if no existing match found
- refactored crowd move requests
- removed adjustMoveTarget()
- added requestMoveVelocity() and resetMoveTarget()
- improved path replanning robustness
- added move visualization to crowd tool
This commit is contained in:
Mikko Mononen 2012-05-31 09:07:54 +00:00
parent 6aa8b1d989
commit 00edec6ffb
6 changed files with 505 additions and 530 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -1491,7 +1491,9 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
if (!node) if (!node)
{ {
return DT_FAILURE; m_query.status |= DT_PARTIAL_RESULT;
dtAssert(m_query.lastBestNode);
node = m_query.lastBestNode;
} }
// Reverse the path. // Reverse the path.

66
DetourCrowd/Include/DetourCrowd.h
View File

@ -91,6 +91,17 @@ struct dtCrowdAgentParams
void* userData; void* userData;
}; };
enum MoveRequestState
{
DT_CROWDAGENT_TARGET_NONE = 0,
DT_CROWDAGENT_TARGET_FAILED,
DT_CROWDAGENT_TARGET_VALID,
DT_CROWDAGENT_TARGET_REQUESTING,
DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE,
DT_CROWDAGENT_TARGET_WAITING_FOR_PATH,
DT_CROWDAGENT_TARGET_VELOCITY,
};
/// Represents an agent managed by a #dtCrowd object. /// Represents an agent managed by a #dtCrowd object.
/// @ingroup crowd /// @ingroup crowd
struct dtCrowdAgent struct dtCrowdAgent
@ -107,10 +118,7 @@ struct dtCrowdAgent
/// The local boundary data for the agent. /// The local boundary data for the agent.
dtLocalBoundary boundary; dtLocalBoundary boundary;
float t; /// Time since the agent's path corridor was optimized.
float var;
/// The last time the agent's path corridor was optimized.
float topologyOptTime; float topologyOptTime;
/// The known neighbors of the agent. /// The known neighbors of the agent.
@ -142,6 +150,13 @@ struct dtCrowdAgent
/// The number of corners. /// The number of corners.
int ncorners; int ncorners;
unsigned char targetState; ///< State of the movement request.
dtPolyRef targetRef; ///< Target polyref of the movement request.
float targetPos[3]; ///< Target position of the movement request (or velocity in case of DT_CROWDAGENT_TARGET_VELOCITY).
dtPathQueueRef targetPathqRef; ///< Path finder ref.
bool targetReplan; ///< Flag indicating that the current path is being replanned.
float targetReplanTime; /// <Time since the agent's target was replanned.
}; };
struct dtCrowdAgentAnimation struct dtCrowdAgentAnimation
@ -197,42 +212,13 @@ class dtCrowd
int m_velocitySampleCount; int m_velocitySampleCount;
enum MoveRequestState
{
MR_TARGET_NONE,
MR_TARGET_FAILED,
MR_TARGET_VALID,
MR_TARGET_REQUESTING,
MR_TARGET_WAITING_FOR_PATH,
MR_TARGET_ADJUST,
};
static const int MAX_TEMP_PATH = 32;
struct MoveRequest
{
unsigned char state; ///< State of the request
int idx; ///< Agent index
dtPolyRef ref; ///< Goal ref
float pos[3]; ///< Goal position
dtPathQueueRef pathqRef; ///< Path find query ref
dtPolyRef aref; ///< Goal adjustment ref
float apos[3]; ///< Goal adjustment pos
dtPolyRef temp[MAX_TEMP_PATH]; ///< Adjusted path to the goal
int ntemp;
bool replan;
};
MoveRequest* m_moveRequests;
int m_moveRequestCount;
dtNavMeshQuery* m_navquery; dtNavMeshQuery* m_navquery;
void updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt); void updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt);
void updateMoveRequest(const float dt); void updateMoveRequest(const float dt);
void checkPathValidty(dtCrowdAgent** agents, const int nagents, const float dt); void checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt);
inline int getAgentIndex(const dtCrowdAgent* agent) const { return agent - m_agents; } inline int getAgentIndex(const dtCrowdAgent* agent) const { return agent - m_agents; }
const MoveRequest* getActiveMoveTarget(const int idx) const;
bool requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos); bool requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos);
@ -291,12 +277,16 @@ public:
/// @return True if the request was successfully submitted. /// @return True if the request was successfully submitted.
bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos); bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos);
/// Sumbits a request to adjust the target position of the specified agent. /// Submits a new move request for the specified agent.
/// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()]
/// @param[in] ref The position's polygon reference. /// @param[in] vel The movement velocity. [(x, y, z)]
/// @param[in] pos The position within the polygon. [(x, y, z)]
/// @return True if the request was successfully submitted. /// @return True if the request was successfully submitted.
bool adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos); bool requestMoveVelocity(const int idx, const float* vel);
/// Resets any request for the specified agent.
/// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()]
/// @return True if the request was successfully reseted.
bool resetMoveTarget(const int idx);
/// Gets the active agents int the agent pool. /// Gets the active agents int the agent pool.
/// @param[out] agents An array of agent pointers. [(#dtCrowdAgent *) * maxAgents] /// @param[out] agents An array of agent pointers. [(#dtCrowdAgent *) * maxAgents]

514
DetourCrowd/Source/DetourCrowd.cpp
View File

@ -236,7 +236,7 @@ A common process for managing the crowd is as follows:
-# Call #update() to allow the crowd to manage its agents. -# Call #update() to allow the crowd to manage its agents.
-# Retrieve agent information using #getActiveAgents(). -# Retrieve agent information using #getActiveAgents().
-# Make movement requests using #requestMoveTarget() and #adjustMoveTarget(). -# Make movement requests using #requestMoveTarget() when movement goal changes.
-# Repeat every frame. -# Repeat every frame.
Some agent configuration settings can be updated using #updateAgentParameters(). But the crowd owns the Some agent configuration settings can be updated using #updateAgentParameters(). But the crowd owns the
@ -267,8 +267,6 @@ dtCrowd::dtCrowd() :
m_maxPathResult(0), m_maxPathResult(0),
m_maxAgentRadius(0), m_maxAgentRadius(0),
m_velocitySampleCount(0), m_velocitySampleCount(0),
m_moveRequests(0),
m_moveRequestCount(0),
m_navquery(0) m_navquery(0)
{ {
} }
@ -295,10 +293,6 @@ void dtCrowd::purge()
dtFree(m_pathResult); dtFree(m_pathResult);
m_pathResult = 0; m_pathResult = 0;
dtFree(m_moveRequests);
m_moveRequests = 0;
m_moveRequestCount = 0;
dtFreeProximityGrid(m_grid); dtFreeProximityGrid(m_grid);
m_grid = 0; m_grid = 0;
@ -356,11 +350,6 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
if (!m_pathResult) if (!m_pathResult)
return false; return false;
m_moveRequests = (MoveRequest*)dtAlloc(sizeof(MoveRequest)*m_maxAgents, DT_ALLOC_PERM);
if (!m_moveRequests)
return false;
m_moveRequestCount = 0;
if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav)) if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav))
return false; return false;
@ -463,6 +452,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
updateAgentParameters(idx, params); updateAgentParameters(idx, params);
ag->topologyOptTime = 0; ag->topologyOptTime = 0;
ag->targetReplanTime = 0;
ag->nneis = 0; ag->nneis = 0;
dtVset(ag->dvel, 0,0,0); dtVset(ag->dvel, 0,0,0);
@ -471,14 +461,14 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
dtVcopy(ag->npos, nearest); dtVcopy(ag->npos, nearest);
ag->desiredSpeed = 0; ag->desiredSpeed = 0;
ag->t = 0;
ag->var = (rand() % 10) / 9.0f;
if (ref) if (ref)
ag->state = DT_CROWDAGENT_STATE_WALKING; ag->state = DT_CROWDAGENT_STATE_WALKING;
else else
ag->state = DT_CROWDAGENT_STATE_INVALID; ag->state = DT_CROWDAGENT_STATE_INVALID;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
ag->active = 1; ag->active = 1;
return idx; return idx;
@ -496,69 +486,29 @@ void dtCrowd::removeAgent(const int idx)
} }
} }
const dtCrowd::MoveRequest* dtCrowd::getActiveMoveTarget(const int idx) const
{
if (idx < 0 || idx > m_maxAgents)
return 0;
MoveRequest* req = 0;
// Check if there is existing request and update that instead.
for (int i = 0; i < m_moveRequestCount; ++i)
{
if (m_moveRequests[i].idx == idx)
{
req = &m_moveRequests[i];
break;
}
}
return req;
}
bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos) bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos)
{ {
if (idx < 0 || idx > m_maxAgents) if (idx < 0 || idx > m_maxAgents)
return false; return false;
if (!ref)
return false;
MoveRequest* req = 0; dtCrowdAgent* ag = &m_agents[idx];
// Check if there is existing request and update that instead.
for (int i = 0; i < m_moveRequestCount; ++i)
{
if (m_moveRequests[i].idx == idx)
{
req = &m_moveRequests[i];
break;
}
}
if (!req)
{
if (m_moveRequestCount >= m_maxAgents)
return false;
req = &m_moveRequests[m_moveRequestCount++];
memset(req, 0, sizeof(MoveRequest));
}
// Initialize request. // Initialize request.
req->idx = idx; ag->targetRef = ref;
req->ref = ref; dtVcopy(ag->targetPos, pos);
dtVcopy(req->pos, pos); ag->targetPathqRef = DT_PATHQ_INVALID;
req->pathqRef = DT_PATHQ_INVALID; ag->targetReplan = true;
req->state = MR_TARGET_REQUESTING; if (ag->targetRef)
req->replan = true; ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
else
req->temp[0] = ref; ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
req->ntemp = 1;
return true; return true;
} }
/// @par /// @par
/// ///
/// This method is used when a new target is set. Use #adjustMoveTarget() when /// This method is used when a new target is set.
/// only small local adjustments are needed. (Such as happens when following a
/// moving target.)
/// ///
/// The position will be constrained to the surface of the navigation mesh. /// The position will be constrained to the surface of the navigation mesh.
/// ///
@ -570,79 +520,51 @@ bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
if (!ref) if (!ref)
return false; return false;
MoveRequest* req = 0; dtCrowdAgent* ag = &m_agents[idx];
// Check if there is existing request and update that instead.
for (int i = 0; i < m_moveRequestCount; ++i)
{
if (m_moveRequests[i].idx == idx)
{
req = &m_moveRequests[i];
break;
}
}
if (!req)
{
if (m_moveRequestCount >= m_maxAgents)
return false;
req = &m_moveRequests[m_moveRequestCount++];
memset(req, 0, sizeof(MoveRequest));
}
// Initialize request. // Initialize request.
req->idx = idx; ag->targetRef = ref;
req->ref = ref; dtVcopy(ag->targetPos, pos);
dtVcopy(req->pos, pos); ag->targetPathqRef = DT_PATHQ_INVALID;
req->pathqRef = DT_PATHQ_INVALID; ag->targetReplan = false;
req->state = MR_TARGET_REQUESTING; if (ag->targetRef)
req->replan = false; ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
else
req->temp[0] = ref; ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
req->ntemp = 1;
return true; return true;
} }
/// @par bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
///
/// This method is used when to make small local adjustments to the current
/// target. (Such as happens when following a moving target.) Use
/// #requestMoveTarget() when a new target is needed.
///
/// The position will be constrained to the surface of the navigation mesh.
///
/// The request will be processed during the next #update().
bool dtCrowd::adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos)
{ {
if (idx < 0 || idx > m_maxAgents) if (idx < 0 || idx > m_maxAgents)
return false; return false;
if (!ref)
dtCrowdAgent* ag = &m_agents[idx];
// Initialize request.
ag->targetRef = 0;
dtVcopy(ag->targetPos, vel);
ag->targetPathqRef = DT_PATHQ_INVALID;
ag->targetReplan = false;
ag->targetState = DT_CROWDAGENT_TARGET_VELOCITY;
return true;
}
bool dtCrowd::resetMoveTarget(const int idx)
{
if (idx < 0 || idx > m_maxAgents)
return false; return false;
MoveRequest* req = 0; dtCrowdAgent* ag = &m_agents[idx];
// Check if there is existing request and update that instead.
for (int i = 0; i < m_moveRequestCount; ++i)
{
if (m_moveRequests[i].idx == idx)
{
req = &m_moveRequests[i];
break;
}
}
if (!req)
{
if (m_moveRequestCount >= m_maxAgents)
return false;
req = &m_moveRequests[m_moveRequestCount++];
memset(req, 0, sizeof(MoveRequest));
// New adjust request // Initialize request.
req->state = MR_TARGET_ADJUST; ag->targetRef = 0;
req->idx = idx; dtVset(ag->targetPos, 0,0,0);
} ag->targetPathqRef = DT_PATHQ_INVALID;
ag->targetReplan = false;
// Set adjustment request. ag->targetState = DT_CROWDAGENT_TARGET_NONE;
req->aref = ref;
dtVcopy(req->apos, pos);
return true; return true;
} }
@ -663,108 +585,91 @@ int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents)
void dtCrowd::updateMoveRequest(const float /*dt*/) void dtCrowd::updateMoveRequest(const float /*dt*/)
{ {
// Fire off new requests. // Fire off new requests.
for (int i = 0; i < m_moveRequestCount; ++i) for (int i = 0; i < m_maxAgents; ++i)
{ {
MoveRequest* req = &m_moveRequests[i]; dtCrowdAgent* ag = &m_agents[i];
dtCrowdAgent* ag = &m_agents[req->idx];
// Agent not active anymore, kill request.
if (!ag->active) 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)
{
// If agent location is invalid, try to recover.
if (ag->state == DT_CROWDAGENT_STATE_INVALID)
{
float agentPos[3];
dtVcopy(agentPos, ag->npos);
dtPolyRef agentRef = ag->corridor.getFirstPoly();
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)
{
// Current location still outside navmesh, fail the request.
req->state = MR_TARGET_FAILED;
continue; continue;
} if (ag->state == DT_CROWDAGENT_STATE_INVALID)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Could relocation agent on navmesh again. if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING)
ag->state = DT_CROWDAGENT_STATE_WALKING;
ag->corridor.reset(agentRef, agentPos);
}
// Calculate request position.
if (req->replan)
{ {
// Replan from the end of the current path.
dtPolyRef reqRef = ag->corridor.getLastPoly();
float reqPos[3];
dtVcopy(reqPos, ag->corridor.getTarget());
req->pathqRef = m_pathq.request(reqRef, req->ref, reqPos, req->pos, &m_filter);
if (req->pathqRef != DT_PATHQ_INVALID)
{
req->state = MR_TARGET_WAITING_FOR_PATH;
}
}
else
{
// 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 dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount(); const int npath = ag->corridor.getPathCount();
dtAssert(npath); 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); static const int MAX_RES = 32;
if (req->pathqRef != DT_PATHQ_INVALID) float reqPos[3];
dtPolyRef reqPath[MAX_RES]; // The path to the request location
int reqPathCount = 0;
// Quick seach towards the goal.
static const int MAX_ITER = 20;
m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter);
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);
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->corridor.setCorridor(reqPos, reqPath, reqPathCount);
req->state = MR_TARGET_WAITING_FOR_PATH; ag->boundary.reset();
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)
{
ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef, ag->corridor.getTarget(), ag->targetPos, &m_filter);
if (ag->targetPathqRef != DT_PATHQ_INVALID)
ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
} }
} }
@ -775,19 +680,27 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
dtStatus status; dtStatus status;
// Process path results. // Process path results.
for (int i = 0; i < m_moveRequestCount; ++i) for (int i = 0; i < m_maxAgents; ++i)
{ {
MoveRequest* req = &m_moveRequests[i]; dtCrowdAgent* ag = &m_agents[i];
dtCrowdAgent* ag = &m_agents[req->idx]; if (!ag->active)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
if (req->state == MR_TARGET_WAITING_FOR_PATH) if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
{ {
// Poll path queue. // Poll path queue.
status = m_pathq.getRequestStatus(req->pathqRef); status = m_pathq.getRequestStatus(ag->targetPathqRef);
if (dtStatusFailed(status)) if (dtStatusFailed(status))
{ {
req->pathqRef = DT_PATHQ_INVALID; // Path find failed, retry if the target location is still valid.
req->state = MR_TARGET_FAILED; 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)) else if (dtStatusSucceed(status))
{ {
@ -797,21 +710,15 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
// Apply results. // Apply results.
float targetPos[3]; float targetPos[3];
dtVcopy(targetPos, req->pos); dtVcopy(targetPos, ag->targetPos);
dtPolyRef* res = m_pathResult; dtPolyRef* res = m_pathResult;
bool valid = true; bool valid = true;
int nres = 0; int nres = 0;
status = m_pathq.getPathResult(req->pathqRef, res, &nres, m_maxPathResult); status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
if (dtStatusFailed(status) || !nres) if (dtStatusFailed(status) || !nres)
valid = false; 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. // Merge result and existing path.
// The agent might have moved whilst the request is // The agent might have moved whilst the request is
// being processed, so the path may have changed. // being processed, so the path may have changed.
@ -854,11 +761,12 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
} }
// Check for partial path. // Check for partial path.
if (res[nres-1] != req->ref) if (res[nres-1] != ag->targetRef)
{ {
// Partial path, constrain target position inside the last polygon. // Partial path, constrain target position inside the last polygon.
float nearest[3]; float nearest[3];
if (m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS) status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest);
if (dtStatusSucceed(status))
dtVcopy(targetPos, nearest); dtVcopy(targetPos, nearest);
else else
valid = false; valid = false;
@ -871,23 +779,16 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
ag->corridor.setCorridor(targetPos, res, nres); ag->corridor.setCorridor(targetPos, res, nres);
// Force to update boundary. // Force to update boundary.
ag->boundary.reset(); ag->boundary.reset();
req->state = MR_TARGET_VALID; ag->targetState = DT_CROWDAGENT_TARGET_VALID;
} }
else else
{ {
// Something went wrong. // Something went wrong.
req->state = MR_TARGET_FAILED; ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
}
}
} }
// Remove request when done with it. ag->targetReplanTime = 0.0;
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;
} }
} }
@ -946,6 +847,8 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
dtCrowdAgent* ag = agents[i]; dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING) if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue; continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0) if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
continue; continue;
ag->topologyOptTime += dt; ag->topologyOptTime += dt;
@ -962,9 +865,10 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
} }
void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const float dt) void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt)
{ {
static const int CHECK_LOOKAHEAD = 10; static const int CHECK_LOOKAHEAD = 10;
static const float TARGET_REPLAN_DELAY = 1.0; // seconds
for (int i = 0; i < nagents; ++i) for (int i = 0; i < nagents; ++i)
{ {
@ -973,42 +877,18 @@ void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const f
if (ag->state != DT_CROWDAGENT_STATE_WALKING) if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue; continue;
// Skip if the corridor is valid if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
if (ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
continue; continue;
// The current path is bad, try to recover. ag->targetReplanTime += dt;
// Store current state. bool replan = false;
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. // 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)) if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
{ {
// Current location is not valid, try to reposition. // Current location is not valid, try to reposition.
@ -1017,43 +897,67 @@ void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const f
agentRef = 0; agentRef = 0;
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest); m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
dtVcopy(agentPos, nearest); dtVcopy(agentPos, nearest);
}
if (!agentRef) if (!agentRef)
{ {
// Count not find location in navmesh, set state to invalid. // Could not find location in navmesh, set state to invalid.
ag->corridor.reset(0, agentPos); ag->corridor.reset(0, agentPos);
ag->boundary.reset(); ag->boundary.reset();
ag->state = DT_CROWDAGENT_STATE_INVALID; ag->state = DT_CROWDAGENT_STATE_INVALID;
continue; continue;
} }
// TODO: make temp path by raycasting towards current velocity.
ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
ag->boundary.reset(); ag->boundary.reset();
dtVcopy(ag->npos, agentPos); dtVcopy(ag->npos, agentPos);
replan = true;
}
// Check that target is still reachable. // Try to recover move request position.
if (!m_navquery->isValidPolyRef(targetRef, &m_filter)) 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. // Current target is not valid, try to reposition.
float nearest[3]; float nearest[3];
targetRef = 0; m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
m_navquery->findNearestPoly(targetPos, m_ext, &m_filter, &targetRef, nearest); dtVcopy(ag->targetPos, nearest);
dtVcopy(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 (!targetRef) // If nearby corridor is not valid, replan.
if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
{ {
// Target not reachable anymore, cannot replan. // Fix current path.
// TODO: indicate failure! ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
continue; 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. // Try to replan path to goal.
requestMoveTargetReplan(idx, targetRef, targetPos); if (replan)
{
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE)
{
requestMoveTargetReplan(idx, ag->targetRef, ag->targetPos);
}
}
} }
} }
@ -1067,7 +971,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
int nagents = getActiveAgents(agents, m_maxAgents); int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths. // Check that all agents still have valid paths.
checkPathValidty(agents, nagents, dt); checkPathValidity(agents, nagents, dt);
// Update async move request and path finder. // Update async move request and path finder.
updateMoveRequest(dt); updateMoveRequest(dt);
@ -1105,6 +1009,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange, ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS, ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
agents, nagents, m_grid); 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. // Find next corner to steer to.
@ -1114,6 +1020,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (ag->state != DT_CROWDAGENT_STATE_WALKING) if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue; continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering // Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys, ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
@ -1151,6 +1059,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (ag->state != DT_CROWDAGENT_STATE_WALKING) if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue; continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check // Check
const float triggerRadius = ag->params.radius*2.25f; const float triggerRadius = ag->params.radius*2.25f;
@ -1178,9 +1088,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
} }
else else
{ {
// TODO: Off-mesh connection failed, replan. // Path validity check will ensure that bad/blocked connections will be replanned.
} }
} }
} }
@ -1191,9 +1100,18 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (ag->state != DT_CROWDAGENT_STATE_WALKING) if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue; continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0}; 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. // Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS) if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel); calcSmoothSteerDirection(ag, dvel);
@ -1206,6 +1124,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
ag->desiredSpeed = ag->params.maxSpeed; ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale); dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
}
// Separation // Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION) if (ag->params.updateFlags & DT_CROWD_SEPARATION)
@ -1268,7 +1187,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
// Add neighbours as obstacles. // Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j) for (int j = 0; j < ag->nneis; ++j)
{ {
const dtCrowdAgent* nei = agents[ag->neis[j].idx]; const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel); m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
} }
@ -1338,7 +1257,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
for (int j = 0; j < ag->nneis; ++j) for (int j = 0; j < ag->nneis; ++j)
{ {
const dtCrowdAgent* nei = agents[ag->neis[j].idx]; const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei); const int idx1 = getAgentIndex(nei);
float diff[3]; float diff[3];
@ -1396,6 +1315,13 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter); ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
// Get valid constrained position back. // Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos()); 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. // Update agents using off-mesh connection.

2
DetourCrowd/Source/DetourPathCorridor.cpp
View File

@ -368,7 +368,7 @@ bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQuer
navquery->updateSlicedFindPath(MAX_ITER, 0); navquery->updateSlicedFindPath(MAX_ITER, 0);
dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES); dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES);
if (status == DT_SUCCESS && nres > 0) if (dtStatusSucceed(status) && nres > 0)
{ {
m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres); m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
return true; return true;

1
RecastDemo/Include/CrowdTool.h
View File

@ -42,6 +42,7 @@ struct CrowdToolParams
bool m_showGrid; bool m_showGrid;
bool m_showNodes; bool m_showNodes;
bool m_showPerfGraph; bool m_showPerfGraph;
bool m_showDetailAll;
bool m_expandOptions; bool m_expandOptions;
bool m_anticipateTurns; bool m_anticipateTurns;

162
RecastDemo/Source/CrowdTool.cpp
View File

@ -31,6 +31,7 @@
#include "DetourDebugDraw.h" #include "DetourDebugDraw.h"
#include "DetourObstacleAvoidance.h" #include "DetourObstacleAvoidance.h"
#include "DetourCommon.h" #include "DetourCommon.h"
#include "DetourNode.h"
#include "SampleInterfaces.h" #include "SampleInterfaces.h"
#ifdef WIN32 #ifdef WIN32
@ -109,6 +110,7 @@ CrowdToolState::CrowdToolState() :
m_toolParams.m_showGrid = false; m_toolParams.m_showGrid = false;
m_toolParams.m_showNodes = false; m_toolParams.m_showNodes = false;
m_toolParams.m_showPerfGraph = false; m_toolParams.m_showPerfGraph = false;
m_toolParams.m_showDetailAll = false;
m_toolParams.m_expandOptions = true; m_toolParams.m_expandOptions = true;
m_toolParams.m_anticipateTurns = true; m_toolParams.m_anticipateTurns = true;
m_toolParams.m_optimizeVis = true; m_toolParams.m_optimizeVis = true;
@ -217,17 +219,17 @@ void CrowdToolState::handleRender()
// Draw paths // Draw paths
if (m_toolParams.m_showPath) if (m_toolParams.m_showPath)
{ {
if (m_agentDebug.idx != -1) for (int i = 0; i < crowd->getAgentCount(); i++)
{ {
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx); if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
if (ag->active) continue;
{ const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
const dtPolyRef* path = ag->corridor.getPath(); const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount(); const int npath = ag->corridor.getPathCount();
for (int i = 0; i < npath; ++i) for (int j = 0; j < npath; ++j)
duDebugDrawNavMeshPoly(&dd, *nav, path[i], duRGBA(0,0,0,16)); duDebugDrawNavMeshPoly(&dd, *nav, path[j], duRGBA(255,255,255,24));
}
} }
} }
@ -294,11 +296,13 @@ void CrowdToolState::handleRender()
} }
// Corners & co // Corners & co
if (m_agentDebug.idx != -1) for (int i = 0; i < crowd->getAgentCount(); i++)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag->active)
{ {
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
const float radius = ag->params.radius; const float radius = ag->params.radius;
const float* pos = ag->npos; const float* pos = ag->npos;
@ -380,19 +384,7 @@ void CrowdToolState::handleRender()
{ {
// Get 'n'th active agent. // Get 'n'th active agent.
// TODO: fix this properly. // TODO: fix this properly.
int n = ag->neis[j].idx; const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
const dtCrowdAgent* nei = 0;
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* nag = crowd->getAgent(i);
if (!nag->active) continue;
if (n == 0)
{
nei = nag;
break;
}
n--;
}
if (nei) if (nei)
{ {
dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128)); dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
@ -410,7 +402,6 @@ void CrowdToolState::handleRender()
dd.end(); dd.end();
} }
} }
}
// Agent cylinders. // Agent cylinders.
for (int i = 0; i < crowd->getAgentCount(); ++i) for (int i = 0; i < crowd->getAgentCount(); ++i)
@ -421,7 +412,11 @@ void CrowdToolState::handleRender()
const float radius = ag->params.radius; const float radius = ag->params.radius;
const float* pos = ag->npos; const float* pos = ag->npos;
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f); unsigned int col = duRGBA(0,0,0,32);
if (m_agentDebug.idx == i)
col = duRGBA(255,0,0,128);
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, col, 2.0f);
} }
for (int i = 0; i < crowd->getAgentCount(); ++i) for (int i = 0; i < crowd->getAgentCount(); ++i)
@ -434,21 +429,30 @@ void CrowdToolState::handleRender()
const float* pos = ag->npos; const float* pos = ag->npos;
unsigned int col = duRGBA(220,220,220,128); unsigned int col = duRGBA(220,220,220,128);
if (m_agentDebug.idx == i) if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = duRGBA(255,192,0,128); col = duLerpCol(col, duRGBA(128,0,255,128), 32);
else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = duLerpCol(col, duRGBA(128,0,255,128), 128);
else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
col = duRGBA(255,32,16,128);
else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
col = duLerpCol(col, duRGBA(64,255,0,128), 128);
duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius, duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
pos[0]+radius, pos[1]+height, pos[2]+radius, col); pos[0]+radius, pos[1]+height, pos[2]+radius, col);
} }
if (m_agentDebug.idx != -1)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag->active)
{
if (m_toolParams.m_showVO) if (m_toolParams.m_showVO)
{ {
for (int i = 0; i < crowd->getAgentCount(); i++)
{
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
// Draw detail about agent sela // Draw detail about agent sela
const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod; const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
@ -459,12 +463,12 @@ void CrowdToolState::handleRender()
duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f); duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
dd.begin(DU_DRAW_QUADS); dd.begin(DU_DRAW_QUADS);
for (int i = 0; i < vod->getSampleCount(); ++i) for (int j = 0; j < vod->getSampleCount(); ++j)
{ {
const float* p = vod->getSampleVelocity(i); const float* p = vod->getSampleVelocity(j);
const float sr = vod->getSampleSize(i); const float sr = vod->getSampleSize(j);
const float pen = vod->getSamplePenalty(i); const float pen = vod->getSamplePenalty(j);
const float pen2 = vod->getSamplePreferredSidePenalty(i); const float pen2 = vod->getSamplePreferredSidePenalty(j);
unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255)); unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255));
col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128)); col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128));
dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col); dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
@ -475,7 +479,6 @@ void CrowdToolState::handleRender()
dd.end(); dd.end();
} }
} }
}
// Velocity stuff. // Velocity stuff.
for (int i = 0; i < crowd->getAgentCount(); ++i) for (int i = 0; i < crowd->getAgentCount(); ++i)
@ -490,8 +493,14 @@ void CrowdToolState::handleRender()
const float* dvel = ag->dvel; const float* dvel = ag->dvel;
unsigned int col = duRGBA(220,220,220,192); unsigned int col = duRGBA(220,220,220,192);
if (m_agentDebug.idx == i) if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
col = duRGBA(255,192,0,192); col = duLerpCol(col, duRGBA(128,0,255,192), 32);
else if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
col = duLerpCol(col, duRGBA(128,0,255,192), 128);
else if (ag->targetState == DT_CROWDAGENT_TARGET_FAILED)
col = duRGBA(255,32,16,192);
else if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
col = duLerpCol(col, duRGBA(64,255,0,192), 128);
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f); duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
@ -521,6 +530,36 @@ void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
char label[32]; char label[32];
if (m_toolParams.m_showNodes)
{
dtCrowd* crowd = m_sample->getCrowd();
if (crowd && crowd->getPathQueue())
{
const dtNavMeshQuery* navquery = crowd->getPathQueue()->getNavQuery();
const dtNodePool* pool = navquery->getNodePool();
if (pool)
{
const float off = 0.5f;
for (int i = 0; i < pool->getHashSize(); ++i)
{
for (dtNodeIndex j = pool->getFirst(i); j != DT_NULL_IDX; j = pool->getNext(j))
{
const dtNode* node = pool->getNodeAtIdx(j+1);
if (!node) continue;
if (gluProject((GLdouble)node->pos[0],(GLdouble)node->pos[1]+off,(GLdouble)node->pos[2],
model, proj, view, &x, &y, &z))
{
const float heuristic = node->total;// - node->cost;
snprintf(label, 32, "%.2f", heuristic);
imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(0,0,0,220));
}
}
}
}
}
}
if (m_toolParams.m_showLabels) if (m_toolParams.m_showLabels)
{ {
dtCrowd* crowd = m_sample->getCrowd(); dtCrowd* crowd = m_sample->getCrowd();
@ -546,11 +585,14 @@ void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
dtCrowd* crowd = m_sample->getCrowd(); dtCrowd* crowd = m_sample->getCrowd();
if (crowd) if (crowd)
{ {
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx); for (int i = 0; i < crowd->getAgentCount(); i++)
if (ag->active)
{ {
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
const float radius = ag->params.radius; const float radius = ag->params.radius;
if (m_toolParams.m_showNeis) if (m_toolParams.m_showNeis)
{ {
for (int j = 0; j < ag->nneis; ++j) for (int j = 0; j < ag->nneis; ++j)
@ -650,6 +692,14 @@ void CrowdToolState::hilightAgent(const int idx)
m_agentDebug.idx = idx; m_agentDebug.idx = idx;
} }
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);
}
void CrowdToolState::setMoveTarget(const float* p, bool adjust) void CrowdToolState::setMoveTarget(const float* p, bool adjust)
{ {
if (!m_sample) return; if (!m_sample) return;
@ -660,16 +710,18 @@ void CrowdToolState::setMoveTarget(const float* p, bool adjust)
const dtQueryFilter* filter = crowd->getFilter(); const dtQueryFilter* filter = crowd->getFilter();
const float* ext = crowd->getQueryExtents(); const float* ext = crowd->getQueryExtents();
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
if (adjust) if (adjust)
{ {
// Adjust target using tiny local search. float vel[3];
// Request velocity
if (m_agentDebug.idx != -1) if (m_agentDebug.idx != -1)
{ {
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx); const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag && ag->active) if (ag && ag->active)
crowd->adjustMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos); {
calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(m_agentDebug.idx, vel);
}
} }
else else
{ {
@ -677,13 +729,15 @@ void CrowdToolState::setMoveTarget(const float* p, bool adjust)
{ {
const dtCrowdAgent* ag = crowd->getAgent(i); const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue; if (!ag->active) continue;
crowd->adjustMoveTarget(i, m_targetRef, m_targetPos); calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(i, vel);
} }
} }
} }
else else
{ {
// Move target using path finder navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
if (m_agentDebug.idx != -1) if (m_agentDebug.idx != -1)
{ {
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx); const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
@ -931,6 +985,8 @@ void CrowdTool::handleMenu()
params->m_showNodes = !params->m_showNodes; params->m_showNodes = !params->m_showNodes;
if (imguiCheck("Show Perf Graph", params->m_showPerfGraph)) if (imguiCheck("Show Perf Graph", params->m_showPerfGraph))
params->m_showPerfGraph = !params->m_showPerfGraph; params->m_showPerfGraph = !params->m_showPerfGraph;
if (imguiCheck("Show Detail All", params->m_showDetailAll))
params->m_showDetailAll = !params->m_showDetailAll;
imguiUnindent(); imguiUnindent();
} }
} }
@ -1030,9 +1086,9 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
} }
else if (m_mode == TOOLMODE_MOVE_TARGET) else if (m_mode == TOOLMODE_MOVE_TARGET)
{ {
imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: set move target. Shift+LMB: adjust target.", imguiRGBA(255,255,255,192)); imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "LMB: set move target. Shift+LMB: adjust set velocity.", imguiRGBA(255,255,255,192));
ty -= 20; ty -= 20;
imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "Adjusting allows to change the target location in short range without pathfinder.", imguiRGBA(255,255,255,192)); imguiDrawText(280, ty, IMGUI_ALIGN_LEFT, "Setting velocity will move the agents without pathfinder.", imguiRGBA(255,255,255,192));
} }
else if (m_mode == TOOLMODE_SELECT) else if (m_mode == TOOLMODE_SELECT)
{ {