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)
{
return DT_FAILURE;
m_query.status |= DT_PARTIAL_RESULT;
dtAssert(m_query.lastBestNode);
node = m_query.lastBestNode;
}
// Reverse the path.

66
DetourCrowd/Include/DetourCrowd.h
View File

@ -91,6 +91,17 @@ struct dtCrowdAgentParams
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.
/// @ingroup crowd
struct dtCrowdAgent
@ -107,10 +118,7 @@ struct dtCrowdAgent
/// The local boundary data for the agent.
dtLocalBoundary boundary;
float t;
float var;
/// The last time the agent's path corridor was optimized.
/// Time since the agent's path corridor was optimized.
float topologyOptTime;
/// The known neighbors of the agent.
@ -142,6 +150,13 @@ struct dtCrowdAgent
/// The number of corners.
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
@ -197,42 +212,13 @@ class dtCrowd
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;
void updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, 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; }
const MoveRequest* getActiveMoveTarget(const int idx) const;
bool requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos);
@ -291,12 +277,16 @@ public:
/// @return True if the request was successfully submitted.
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] ref The position's polygon reference.
/// @param[in] pos The position within the polygon. [(x, y, z)]
/// @param[in] vel The movement velocity. [(x, y, z)]
/// @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.
/// @param[out] agents An array of agent pointers. [(#dtCrowdAgent *) * maxAgents]

574
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.
-# Retrieve agent information using #getActiveAgents().
-# Make movement requests using #requestMoveTarget() and #adjustMoveTarget().
-# Make movement requests using #requestMoveTarget() when movement goal changes.
-# Repeat every frame.
Some agent configuration settings can be updated using #updateAgentParameters(). But the crowd owns the
@ -267,8 +267,6 @@ dtCrowd::dtCrowd() :
m_maxPathResult(0),
m_maxAgentRadius(0),
m_velocitySampleCount(0),
m_moveRequests(0),
m_moveRequestCount(0),
m_navquery(0)
{
}
@ -295,10 +293,6 @@ void dtCrowd::purge()
dtFree(m_pathResult);
m_pathResult = 0;
dtFree(m_moveRequests);
m_moveRequests = 0;
m_moveRequestCount = 0;
dtFreeProximityGrid(m_grid);
m_grid = 0;
@ -356,11 +350,6 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
if (!m_pathResult)
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))
return false;
@ -463,6 +452,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
updateAgentParameters(idx, params);
ag->topologyOptTime = 0;
ag->targetReplanTime = 0;
ag->nneis = 0;
dtVset(ag->dvel, 0,0,0);
@ -471,14 +461,14 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
dtVcopy(ag->npos, nearest);
ag->desiredSpeed = 0;
ag->t = 0;
ag->var = (rand() % 10) / 9.0f;
if (ref)
ag->state = DT_CROWDAGENT_STATE_WALKING;
else
ag->state = DT_CROWDAGENT_STATE_INVALID;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
ag->active = 1;
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)
{
if (idx < 0 || idx > m_maxAgents)
return false;
if (!ref)
return false;
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;
}
}
if (!req)
{
if (m_moveRequestCount >= m_maxAgents)
return false;
req = &m_moveRequests[m_moveRequestCount++];
memset(req, 0, sizeof(MoveRequest));
}
dtCrowdAgent* ag = &m_agents[idx];
// Initialize request.
req->idx = idx;
req->ref = ref;
dtVcopy(req->pos, pos);
req->pathqRef = DT_PATHQ_INVALID;
req->state = MR_TARGET_REQUESTING;
req->replan = true;
ag->targetRef = ref;
dtVcopy(ag->targetPos, pos);
ag->targetPathqRef = DT_PATHQ_INVALID;
ag->targetReplan = true;
if (ag->targetRef)
ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
else
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
req->temp[0] = ref;
req->ntemp = 1;
return true;
}
/// @par
///
/// This method is used when a new target is set. Use #adjustMoveTarget() when
/// only small local adjustments are needed. (Such as happens when following a
/// moving target.)
/// This method is used when a new target is set.
///
/// The position will be constrained to the surface of the navigation mesh.
///
@ -569,81 +519,53 @@ bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
return false;
if (!ref)
return false;
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;
}
}
if (!req)
{
if (m_moveRequestCount >= m_maxAgents)
return false;
req = &m_moveRequests[m_moveRequestCount++];
memset(req, 0, sizeof(MoveRequest));
}
dtCrowdAgent* ag = &m_agents[idx];
// Initialize request.
req->idx = idx;
req->ref = ref;
dtVcopy(req->pos, pos);
req->pathqRef = DT_PATHQ_INVALID;
req->state = MR_TARGET_REQUESTING;
req->replan = false;
req->temp[0] = ref;
req->ntemp = 1;
ag->targetRef = ref;
dtVcopy(ag->targetPos, pos);
ag->targetPathqRef = DT_PATHQ_INVALID;
ag->targetReplan = false;
if (ag->targetRef)
ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
else
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
return true;
}
/// @par
///
/// 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)
bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
{
if (idx < 0 || idx > m_maxAgents)
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;
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;
}
}
if (!req)
{
if (m_moveRequestCount >= m_maxAgents)
return false;
req = &m_moveRequests[m_moveRequestCount++];
memset(req, 0, sizeof(MoveRequest));
// New adjust request
req->state = MR_TARGET_ADJUST;
req->idx = idx;
}
// Set adjustment request.
req->aref = ref;
dtVcopy(req->apos, pos);
dtCrowdAgent* ag = &m_agents[idx];
// Initialize request.
ag->targetRef = 0;
dtVset(ag->targetPos, 0,0,0);
ag->targetPathqRef = DT_PATHQ_INVALID;
ag->targetReplan = false;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
return true;
}
@ -663,108 +585,91 @@ int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents)
void dtCrowd::updateMoveRequest(const float /*dt*/)
{
// 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[req->idx];
// Agent not active anymore, kill request.
dtCrowdAgent* ag = &m_agents[i];
if (!ag->active)
req->state = MR_TARGET_FAILED;
// Adjust target
if (req->aref)
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)
{
if (req->state == MR_TARGET_ADJUST)
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 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)
{
// Adjust existing path.
ag->corridor.moveTargetPosition(req->apos, m_navquery, &m_filter);
req->state = MR_TARGET_VALID;
// Try to use existing steady path during replan if possible.
status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
}
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);
// 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;
}
// Reset adjustment.
dtVset(req->apos, 0,0,0);
req->aref = 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();
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 (req->state == MR_TARGET_REQUESTING)
if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
{
// 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;
}
// Could relocation agent on navmesh again.
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 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;
}
}
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;
// 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[req->idx];
dtCrowdAgent* ag = &m_agents[i];
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.
status = m_pathq.getRequestStatus(req->pathqRef);
status = m_pathq.getRequestStatus(ag->targetPathqRef);
if (dtStatusFailed(status))
{
req->pathqRef = DT_PATHQ_INVALID;
req->state = MR_TARGET_FAILED;
// 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))
{
@ -797,21 +710,15 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
// Apply results.
float targetPos[3];
dtVcopy(targetPos, req->pos);
dtVcopy(targetPos, ag->targetPos);
dtPolyRef* res = m_pathResult;
bool valid = true;
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)
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.
@ -854,11 +761,12 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
}
// 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.
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);
else
valid = false;
@ -871,24 +779,17 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
ag->corridor.setCorridor(targetPos, res, nres);
// Force to update boundary.
ag->boundary.reset();
req->state = MR_TARGET_VALID;
ag->targetState = DT_CROWDAGENT_TARGET_VALID;
}
else
{
// Something went wrong.
req->state = MR_TARGET_FAILED;
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
}
ag->targetReplanTime = 0.0;
}
}
// 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;
}
}
}
@ -946,6 +847,8 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
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;
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
continue;
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 float TARGET_REPLAN_DELAY = 1.0; // seconds
for (int i = 0; i < nagents; ++i)
{
@ -972,43 +876,19 @@ void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const f
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Skip if the corridor is valid
if (ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
ag->targetReplanTime += dt;
// The current path is bad, try to recover.
bool replan = false;
// 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.
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.
@ -1017,43 +897,67 @@ void dtCrowd::checkPathValidty(dtCrowdAgent** agents, const int nagents, const f
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;
}
ag->boundary.reset();
dtVcopy(ag->npos, agentPos);
replan = true;
}
if (!agentRef)
// Try to recover move request position.
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
{
// Count not find location in navmesh, set state to invalid.
ag->corridor.reset(0, agentPos);
if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
{
// Current target is not valid, try to reposition.
float nearest[3];
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();
ag->state = DT_CROWDAGENT_STATE_INVALID;
continue;
replan = true;
}
// 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))
// If the end of the path is near and it is not the requested location, replan.
if (ag->targetState == DT_CROWDAGENT_TARGET_VALID)
{
// 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 (ag->targetReplanTime > TARGET_REPLAN_DELAY &&
ag->corridor.getPathCount() < CHECK_LOOKAHEAD &&
ag->corridor.getLastPoly() != ag->targetRef)
replan = true;
}
if (!targetRef)
{
// Target not reachable anymore, cannot replan.
// TODO: indicate failure!
continue;
}
// 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);
// Check that all agents still have valid paths.
checkPathValidty(agents, nagents, dt);
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
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, 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.
@ -1114,6 +1020,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
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,
@ -1151,6 +1059,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
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;
@ -1178,9 +1088,8 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
}
else
{
// TODO: Off-mesh connection failed, replan.
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
@ -1191,21 +1100,31 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
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;
{
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
// 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)
@ -1268,7 +1187,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
// Add neighbours as obstacles.
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);
}
@ -1338,7 +1257,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
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);
float diff[3];
@ -1396,6 +1315,13 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
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.

2
DetourCrowd/Source/DetourPathCorridor.cpp
View File

@ -368,7 +368,7 @@ bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQuer
navquery->updateSlicedFindPath(MAX_ITER, 0);
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);
return true;

1
RecastDemo/Include/CrowdTool.h
View File

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

388
RecastDemo/Source/CrowdTool.cpp
View File

@ -31,6 +31,7 @@
#include "DetourDebugDraw.h"
#include "DetourObstacleAvoidance.h"
#include "DetourCommon.h"
#include "DetourNode.h"
#include "SampleInterfaces.h"
#ifdef WIN32
@ -109,6 +110,7 @@ CrowdToolState::CrowdToolState() :
m_toolParams.m_showGrid = false;
m_toolParams.m_showNodes = false;
m_toolParams.m_showPerfGraph = false;
m_toolParams.m_showDetailAll = false;
m_toolParams.m_expandOptions = true;
m_toolParams.m_anticipateTurns = true;
m_toolParams.m_optimizeVis = true;
@ -211,23 +213,23 @@ void CrowdToolState::handleRender()
if (navquery)
duDebugDrawNavMeshNodes(&dd, *navquery);
}
dd.depthMask(false);
// Draw paths
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 (ag->active)
{
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
for (int i = 0; i < npath; ++i)
duDebugDrawNavMeshPoly(&dd, *nav, path[i], duRGBA(0,0,0,16));
}
if (m_toolParams.m_showDetailAll == false && i != m_agentDebug.idx)
continue;
const dtCrowdAgent* ag =crowd->getAgent(i);
if (!ag->active)
continue;
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
for (int j = 0; j < npath; ++j)
duDebugDrawNavMeshPoly(&dd, *nav, path[j], duRGBA(255,255,255,24));
}
}
@ -294,122 +296,111 @@ void CrowdToolState::handleRender()
}
// 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* pos = ag->npos;
if (m_toolParams.m_showCorners)
{
const float radius = ag->params.radius;
const float* pos = ag->npos;
if (m_toolParams.m_showCorners)
{
if (ag->ncorners)
{
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->ncorners; ++j)
{
const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
const float* vb = &ag->cornerVerts[j*3];
dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192));
dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192));
}
if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
{
const float* v = &ag->cornerVerts[(ag->ncorners-1)*3];
dd.vertex(v[0],v[1],v[2], duRGBA(192,0,0,192));
dd.vertex(v[0],v[1]+radius*2,v[2], duRGBA(192,0,0,192));
}
dd.end();
if (m_toolParams.m_anticipateTurns)
{
/* float dvel[3], pos[3];
calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel);
pos[0] = ag->pos[0] + dvel[0];
pos[1] = ag->pos[1] + dvel[1];
pos[2] = ag->pos[2] + dvel[2];
const float off = ag->radius+0.1f;
const float* tgt = &ag->cornerVerts[0];
const float y = ag->pos[1]+off;
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
dd.end();*/
}
}
}
if (m_toolParams.m_showCollisionSegments)
{
const float* center = ag->boundary.getCenter();
duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f);
duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange,
duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
unsigned int col = duRGBA(192,0,128,192);
if (dtTriArea2D(pos, s, s+3) < 0.0f)
col = duDarkenCol(col);
duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
}
dd.end();
}
if (m_toolParams.m_showNeis)
{
duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
duRGBA(0,192,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->nneis; ++j)
{
// Get 'n'th active agent.
// TODO: fix this properly.
int n = 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)
{
dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
dd.vertex(nei->npos[0],nei->npos[1]+radius,nei->npos[2], duRGBA(0,192,128,128));
}
}
dd.end();
}
if (m_toolParams.m_showOpt)
if (ag->ncorners)
{
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192));
dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192));
for (int j = 0; j < ag->ncorners; ++j)
{
const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
const float* vb = &ag->cornerVerts[j*3];
dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192));
dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192));
}
if (ag->ncorners && ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
{
const float* v = &ag->cornerVerts[(ag->ncorners-1)*3];
dd.vertex(v[0],v[1],v[2], duRGBA(192,0,0,192));
dd.vertex(v[0],v[1]+radius*2,v[2], duRGBA(192,0,0,192));
}
dd.end();
if (m_toolParams.m_anticipateTurns)
{
/* float dvel[3], pos[3];
calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel);
pos[0] = ag->pos[0] + dvel[0];
pos[1] = ag->pos[1] + dvel[1];
pos[2] = ag->pos[2] + dvel[2];
const float off = ag->radius+0.1f;
const float* tgt = &ag->cornerVerts[0];
const float y = ag->pos[1]+off;
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
dd.end();*/
}
}
}
if (m_toolParams.m_showCollisionSegments)
{
const float* center = ag->boundary.getCenter();
duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f);
duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange,
duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
unsigned int col = duRGBA(192,0,128,192);
if (dtTriArea2D(pos, s, s+3) < 0.0f)
col = duDarkenCol(col);
duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
}
dd.end();
}
if (m_toolParams.m_showNeis)
{
duDebugDrawCircle(&dd, pos[0],pos[1]+radius,pos[2], ag->params.collisionQueryRange,
duRGBA(0,192,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->nneis; ++j)
{
// Get 'n'th active agent.
// TODO: fix this properly.
const dtCrowdAgent* nei = crowd->getAgent(ag->neis[j].idx);
if (nei)
{
dd.vertex(pos[0],pos[1]+radius,pos[2], duRGBA(0,192,128,128));
dd.vertex(nei->npos[0],nei->npos[1]+radius,nei->npos[2], duRGBA(0,192,128,128));
}
}
dd.end();
}
if (m_toolParams.m_showOpt)
{
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(m_agentDebug.optStart[0],m_agentDebug.optStart[1]+0.3f,m_agentDebug.optStart[2], duRGBA(0,128,0,192));
dd.vertex(m_agentDebug.optEnd[0],m_agentDebug.optEnd[1]+0.3f,m_agentDebug.optEnd[2], duRGBA(0,128,0,192));
dd.end();
}
}
// Agent cylinders.
@ -421,7 +412,11 @@ void CrowdToolState::handleRender()
const float radius = ag->params.radius;
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)
@ -434,46 +429,54 @@ void CrowdToolState::handleRender()
const float* pos = ag->npos;
unsigned int col = duRGBA(220,220,220,128);
if (m_agentDebug.idx == i)
col = duRGBA(255,192,0,128);
if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
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,
pos[0]+radius, pos[1]+height, pos[2]+radius, col);
}
if (m_agentDebug.idx != -1)
if (m_toolParams.m_showVO)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag->active)
for (int i = 0; i < crowd->getAgentCount(); i++)
{
if (m_toolParams.m_showVO)
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
const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
const float dx = ag->npos[0];
const float dy = ag->npos[1]+ag->params.height;
const float dz = ag->npos[2];
duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
dd.begin(DU_DRAW_QUADS);
for (int j = 0; j < vod->getSampleCount(); ++j)
{
// Draw detail about agent sela
const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
const float dx = ag->npos[0];
const float dy = ag->npos[1]+ag->params.height;
const float dz = ag->npos[2];
duDebugDrawCircle(&dd, dx,dy,dz, ag->params.maxSpeed, duRGBA(255,255,255,64), 2.0f);
dd.begin(DU_DRAW_QUADS);
for (int i = 0; i < vod->getSampleCount(); ++i)
{
const float* p = vod->getSampleVelocity(i);
const float sr = vod->getSampleSize(i);
const float pen = vod->getSamplePenalty(i);
const float pen2 = vod->getSamplePreferredSidePenalty(i);
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));
dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
}
dd.end();
}
const float* p = vod->getSampleVelocity(j);
const float sr = vod->getSampleSize(j);
const float pen = vod->getSamplePenalty(j);
const float pen2 = vod->getSamplePreferredSidePenalty(j);
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));
dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
}
dd.end();
}
}
@ -490,8 +493,14 @@ void CrowdToolState::handleRender()
const float* dvel = ag->dvel;
unsigned int col = duRGBA(220,220,220,192);
if (m_agentDebug.idx == i)
col = duRGBA(255,192,0,192);
if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING || ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
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);
@ -521,6 +530,36 @@ void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
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)
{
dtCrowd* crowd = m_sample->getCrowd();
@ -546,11 +585,14 @@ void CrowdToolState::handleRenderOverlay(double* proj, double* model, int* view)
dtCrowd* crowd = m_sample->getCrowd();
if (crowd)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag->active)
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;
const float radius = ag->params.radius;
if (m_toolParams.m_showNeis)
{
for (int j = 0; j < ag->nneis; ++j)
@ -650,6 +692,14 @@ void CrowdToolState::hilightAgent(const int 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)
{
if (!m_sample) return;
@ -659,17 +709,19 @@ void CrowdToolState::setMoveTarget(const float* p, bool adjust)
dtCrowd* crowd = m_sample->getCrowd();
const dtQueryFilter* filter = crowd->getFilter();
const float* ext = crowd->getQueryExtents();
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
if (adjust)
{
// Adjust target using tiny local search.
float vel[3];
// Request velocity
if (m_agentDebug.idx != -1)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
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
{
@ -677,13 +729,15 @@ void CrowdToolState::setMoveTarget(const float* p, bool adjust)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
crowd->adjustMoveTarget(i, m_targetRef, m_targetPos);
calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(i, vel);
}
}
}
else
{
// Move target using path finder
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
if (m_agentDebug.idx != -1)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
@ -931,6 +985,8 @@ void CrowdTool::handleMenu()
params->m_showNodes = !params->m_showNodes;
if (imguiCheck("Show Perf Graph", params->m_showPerfGraph))
params->m_showPerfGraph = !params->m_showPerfGraph;
if (imguiCheck("Show Detail All", params->m_showDetailAll))
params->m_showDetailAll = !params->m_showDetailAll;
imguiUnindent();
}
}
@ -1030,9 +1086,9 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
}
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;
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)
{