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

Allow different obstacle avoidance parameters. Changed update flags to be per agent, not per crowd. Added optional separation to steering. Added UI to change obstacle avoidance quality.

Browse Source
This commit is contained in:
Mikko Mononen 2011-02-06 12:52:08 +00:00
parent 9bb9abad33
commit 5b4f8b6047
8 changed files with 405 additions and 200 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
DetourCrowd/Include/DetourCrowd.h
View File

@ -29,7 +29,7 @@
static const int DT_CROWDAGENT_MAX_NEIGHBOURS = 6; static const int DT_CROWDAGENT_MAX_NEIGHBOURS = 6;
static const int DT_CROWDAGENT_MAX_CORNERS = 4; static const int DT_CROWDAGENT_MAX_CORNERS = 4;
static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8;
struct dtCrowdNeighbour struct dtCrowdNeighbour
{ {
int idx; int idx;
@ -42,10 +42,22 @@ enum CrowdAgentState
DT_CROWDAGENT_STATE_OFFMESH, DT_CROWDAGENT_STATE_OFFMESH,
}; };
struct dtCrowdAgentParams
{
float radius;
float height;
float maxAcceleration;
float maxSpeed;
float collisionQueryRange;
float pathOptimizationRange;
float separationWeight;
unsigned char updateFlags;
unsigned char obstacleAvoidanceType;
};
struct dtCrowdAgent struct dtCrowdAgent
{ {
unsigned char active; unsigned char active;
unsigned char state; unsigned char state;
dtPathCorridor corridor; dtPathCorridor corridor;
@ -59,12 +71,6 @@ struct dtCrowdAgent
dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS]; dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS];
int nneis; int nneis;
float radius, height;
float maxAcceleration;
float maxSpeed;
float collisionQueryRange;
float pathOptimizationRange;
float desiredSpeed; float desiredSpeed;
float npos[3]; float npos[3];
@ -73,6 +79,8 @@ struct dtCrowdAgent
float nvel[3]; float nvel[3];
float vel[3]; float vel[3];
dtCrowdAgentParams params;
float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3]; float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3];
unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS]; unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS];
dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS]; dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS];
@ -90,22 +98,12 @@ struct dtCrowdAgentAnimation
enum UpdateFlags enum UpdateFlags
{ {
DT_CROWD_ANTICIPATE_TURNS = 1, DT_CROWD_ANTICIPATE_TURNS = 1,
DT_CROWD_USE_VO = 2, DT_CROWD_OBSTACLE_AVOIDANCE = 2,
// DT_CROWD_DRUNK = 4, DT_CROWD_SEPARATION = 4,
DT_CROWD_OPTIMIZE_VIS = 8, DT_CROWD_OPTIMIZE_VIS = 8,
DT_CROWD_OPTIMIZE_TOPO = 16, DT_CROWD_OPTIMIZE_TOPO = 16,
}; };
struct dtCrowdAgentParams
{
float radius;
float height;
float maxAcceleration;
float maxSpeed;
float collisionQueryRange;
float pathOptimizationRange;
};
struct dtCrowdAgentDebugInfo struct dtCrowdAgentDebugInfo
{ {
int idx; int idx;
@ -122,7 +120,9 @@ class dtCrowd
dtPathQueue m_pathq; dtPathQueue m_pathq;
dtObstacleAvoidanceParams m_obstacleQueryParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS];
dtObstacleAvoidanceQuery* m_obstacleQuery; dtObstacleAvoidanceQuery* m_obstacleQuery;
dtProximityGrid* m_grid; dtProximityGrid* m_grid;
dtPolyRef* m_pathResult; dtPolyRef* m_pathResult;
@ -176,17 +176,21 @@ public:
bool init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav); bool init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav);
void setObstacleAvoidanceParams(const int idx, const dtObstacleAvoidanceParams* params);
const dtObstacleAvoidanceParams* getObstacleAvoidanceParams(const int idx) const;
const dtCrowdAgent* getAgent(const int idx); const dtCrowdAgent* getAgent(const int idx);
const int getAgentCount() const; const int getAgentCount() const;
int addAgent(const float* pos, const dtCrowdAgentParams* params); int addAgent(const float* pos, const dtCrowdAgentParams* params);
void updateAgentParameters(const int idx, const dtCrowdAgentParams* params);
void removeAgent(const int idx); void removeAgent(const int idx);
bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos); bool requestMoveTarget(const int idx, dtPolyRef ref, const float* pos);
bool adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos); bool adjustMoveTarget(const int idx, dtPolyRef ref, const float* pos);
int getActiveAgents(dtCrowdAgent** agents, const int maxAgents); int getActiveAgents(dtCrowdAgent** agents, const int maxAgents);
void update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* debug); void update(const float dt, dtCrowdAgentDebugInfo* debug);
const dtQueryFilter* getFilter() const { return &m_filter; } const dtQueryFilter* getFilter() const { return &m_filter; }
const float* getQueryExtents() const { return m_ext; } const float* getQueryExtents() const { return m_ext; }
@ -195,6 +199,7 @@ public:
const dtProximityGrid* getGrid() const { return m_grid; } const dtProximityGrid* getGrid() const { return m_grid; }
const dtPathQueue* getPathQueue() const { return &m_pathq; } const dtPathQueue* getPathQueue() const { return &m_pathq; }
const dtNavMeshQuery* getNavMeshQuery() const { return m_navquery; }
}; };

42
DetourCrowd/Include/DetourObstacleAvoidance.h
View File

@ -34,8 +34,6 @@ struct dtObstacleSegment
bool touch; bool touch;
}; };
static const int RVO_SAMPLE_RAD = 15;
static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100;
class dtObstacleAvoidanceDebugData class dtObstacleAvoidanceDebugData
{ {
@ -75,6 +73,23 @@ dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData();
void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr); void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr);
static const int DT_MAX_PATTERN_DIVS = 32; // Max numver of adaptive divs.
static const int DT_MAX_PATTERN_RINGS = 4; // Max number of adaptive rings.
struct dtObstacleAvoidanceParams
{
float velBias;
float weightDesVel;
float weightCurVel;
float weightSide;
float weightToi;
float horizTime;
unsigned char gridSize; // grid
unsigned char adaptiveDivs; // adaptive
unsigned char adaptiveRings; // adaptive
unsigned char adaptiveDepth; // adaptive
};
class dtObstacleAvoidanceQuery class dtObstacleAvoidanceQuery
{ {
public: public:
@ -90,21 +105,14 @@ public:
void addSegment(const float* p, const float* q); void addSegment(const float* p, const float* q);
inline void setVelocitySelectionBias(float v) { m_velBias = v; }
inline void setDesiredVelocityWeight(float w) { m_weightDesVel = w; }
inline void setCurrentVelocityWeight(float w) { m_weightCurVel = w; }
inline void setPreferredSideWeight(float w) { m_weightSide = w; }
inline void setCollisionTimeWeight(float w) { m_weightToi = w; }
inline void setTimeHorizon(float t) { m_horizTime = t; }
int sampleVelocityGrid(const float* pos, const float rad, const float vmax, int sampleVelocityGrid(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel, const float* vel, const float* dvel, float* nvel,
const int gsize, const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug = 0); dtObstacleAvoidanceDebugData* debug = 0);
int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel, const float* vel, const float* dvel, float* nvel,
const int ndivs, const int nrings, const int depth, const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug = 0); dtObstacleAvoidanceDebugData* debug = 0);
inline int getObstacleCircleCount() const { return m_ncircles; } inline int getObstacleCircleCount() const { return m_ncircles; }
@ -119,18 +127,16 @@ private:
float processSample(const float* vcand, const float cs, float processSample(const float* vcand, const float cs,
const float* pos, const float rad, const float* pos, const float rad,
const float vmax, const float* vel, const float* dvel, const float* vel, const float* dvel,
dtObstacleAvoidanceDebugData* debug); dtObstacleAvoidanceDebugData* debug);
dtObstacleCircle* insertCircle(const float dist); dtObstacleCircle* insertCircle(const float dist);
dtObstacleSegment* insertSegment(const float dist); dtObstacleSegment* insertSegment(const float dist);
float m_velBias; dtObstacleAvoidanceParams m_params;
float m_weightDesVel; float m_invHorizTime;
float m_weightCurVel; float m_vmax;
float m_weightSide; float m_invVmax;
float m_weightToi;
float m_horizTime;
int m_maxCircles; int m_maxCircles;
dtObstacleCircle* m_circles; dtObstacleCircle* m_circles;

174
DetourCrowd/Source/DetourCrowd.cpp
View File

@ -30,11 +30,6 @@
#include "DetourAssert.h" #include "DetourAssert.h"
#include "DetourAlloc.h" #include "DetourAlloc.h"
static const int VO_ADAPTIVE_DIVS = 7;
static const int VO_ADAPTIVE_RINGS = 2;
static const int VO_ADAPTIVE_DEPTH = 5;
static const int VO_GRID_SIZE = 33;
static const int MAX_ITERS_PER_UPDATE = 10; static const int MAX_ITERS_PER_UPDATE = 10;
@ -49,7 +44,7 @@ inline float between(const float t, const float t0, const float t1)
static void integrate(dtCrowdAgent* ag, const float dt) static void integrate(dtCrowdAgent* ag, const float dt)
{ {
// Fake dynamic constraint. // Fake dynamic constraint.
const float maxDelta = ag->maxAcceleration * dt; const float maxDelta = ag->params.maxAcceleration * dt;
float dv[3]; float dv[3];
dtVsub(dv, ag->nvel, ag->vel); dtVsub(dv, ag->nvel, ag->vel);
float ds = dtVlen(dv); float ds = dtVlen(dv);
@ -251,12 +246,22 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
if (!m_obstacleQuery->init(6, 8)) if (!m_obstacleQuery->init(6, 8))
return false; return false;
m_obstacleQuery->setDesiredVelocityWeight(2.0f); // Init obstacle query params.
m_obstacleQuery->setCurrentVelocityWeight(0.75f); memset(m_obstacleQueryParams, 0, sizeof(m_obstacleQueryParams));
m_obstacleQuery->setPreferredSideWeight(0.75f); for (int i = 0; i < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS; ++i)
m_obstacleQuery->setCollisionTimeWeight(2.5f); {
m_obstacleQuery->setTimeHorizon(2.5f); dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[i];
m_obstacleQuery->setVelocitySelectionBias(0.4f); params->velBias = 0.4f;
params->weightDesVel = 2.0f;
params->weightCurVel = 0.75f;
params->weightSide = 0.75f;
params->weightToi = 2.5f;
params->horizTime = 2.5f;
params->gridSize = 33;
params->adaptiveDivs = 7;
params->adaptiveRings = 2;
params->adaptiveDepth = 5;
}
// Allocate temp buffer for merging paths. // Allocate temp buffer for merging paths.
m_maxPathResult = 256; m_maxPathResult = 256;
@ -307,6 +312,19 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
return true; return true;
} }
void dtCrowd::setObstacleAvoidanceParams(const int idx, const dtObstacleAvoidanceParams* params)
{
if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS)
memcpy(&m_obstacleQueryParams[idx], params, sizeof(dtObstacleAvoidanceParams));
}
const dtObstacleAvoidanceParams* dtCrowd::getObstacleAvoidanceParams(const int idx) const
{
if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS)
return &m_obstacleQueryParams[idx];
return 0;
}
const int dtCrowd::getAgentCount() const const int dtCrowd::getAgentCount() const
{ {
return m_maxAgents; return m_maxAgents;
@ -317,6 +335,13 @@ const dtCrowdAgent* dtCrowd::getAgent(const int idx)
return &m_agents[idx]; return &m_agents[idx];
} }
void dtCrowd::updateAgentParameters(const int idx, const dtCrowdAgentParams* params)
{
if (idx < 0 || idx > m_maxAgents)
return;
memcpy(&m_agents[idx].params, params, sizeof(dtCrowdAgentParams));
}
int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params) int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
{ {
// Find empty slot. // Find empty slot.
@ -347,12 +372,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
ag->corridor.reset(ref, nearest); ag->corridor.reset(ref, nearest);
ag->boundary.reset(); ag->boundary.reset();
ag->radius = params->radius; updateAgentParameters(idx, params);
ag->height = params->height;
ag->maxAcceleration = params->maxAcceleration;
ag->maxSpeed = params->maxSpeed;
ag->collisionQueryRange = params->collisionQueryRange;
ag->pathOptimizationRange = params->pathOptimizationRange;
ag->topologyOptTime = 0; ag->topologyOptTime = 0;
ag->nneis = 0; ag->nneis = 0;
@ -488,7 +508,7 @@ int dtCrowd::getNeighbours(const float* pos, const float height, const float ran
// Check for overlap. // Check for overlap.
float diff[3]; float diff[3];
dtVsub(diff, pos, ag->npos); dtVsub(diff, pos, ag->npos);
if (fabsf(diff[1]) >= (height+ag->height)/2.0f) if (fabsf(diff[1]) >= (height+ag->params.height)/2.0f)
continue; continue;
diff[1] = 0; diff[1] = 0;
const float distSqr = dtVlenSqr(diff); const float distSqr = dtVlenSqr(diff);
@ -723,6 +743,8 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
dtCrowdAgent* ag = agents[i]; dtCrowdAgent* ag = agents[i];
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH) if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
continue; continue;
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
continue;
ag->topologyOptTime += dt; ag->topologyOptTime += dt;
if (ag->topologyOptTime >= OPT_TIME_THR) if (ag->topologyOptTime >= OPT_TIME_THR)
nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS); nqueue = addToOptQueue(ag, queue, nqueue, OPT_MAX_AGENTS);
@ -737,7 +759,7 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
} }
void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo* debug) void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{ {
m_velocitySampleCount = 0; m_velocitySampleCount = 0;
@ -750,8 +772,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
updateMoveRequest(dt); updateMoveRequest(dt);
// Optimize path topology. // Optimize path topology.
if (flags & DT_CROWD_OPTIMIZE_TOPO) updateTopologyOptimization(agents, nagents, dt);
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid. // Register agents to proximity grid.
m_grid->clear(); m_grid->clear();
@ -759,7 +780,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
{ {
dtCrowdAgent* ag = agents[i]; dtCrowdAgent* ag = agents[i];
const float* p = ag->npos; const float* p = ag->npos;
const float r = ag->radius; const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r); m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
} }
@ -771,10 +792,15 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
continue; continue;
// Only update the collision boundary after certain distance has been passed. // Only update the collision boundary after certain distance has been passed.
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(ag->collisionQueryRange*0.25f)) const float updateThr = ag->params.collisionQueryRange*0.25f;
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->collisionQueryRange, m_navquery, &m_filter); if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents // Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->height, ag->collisionQueryRange, ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS); ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS);
} }
// Find next corner to steer to. // Find next corner to steer to.
@ -791,10 +817,10 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
// Check to see if the corner after the next corner is directly visible, // Check to see if the corner after the next corner is directly visible,
// and short cut to there. // and short cut to there.
if ((flags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0) if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{ {
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3]; const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->pathOptimizationRange, m_navquery, &m_filter); ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes. // Copy data for debug purposes.
if (debugIdx == i) if (debugIdx == i)
@ -824,7 +850,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
continue; continue;
// Check // Check
const float triggerRadius = ag->radius*2.25f; const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius)) if (overOffmeshConnection(ag, triggerRadius))
{ {
// Prepare to off-mesh connection. // Prepare to off-mesh connection.
@ -840,7 +866,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
anim->polyRef = refs[1]; anim->polyRef = refs[1];
anim->active = 1; anim->active = 1;
anim->t = 0.0f; anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->maxSpeed) * 0.5f; anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH; ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0; ag->ncorners = 0;
@ -866,39 +892,58 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
float dvel[3] = {0,0,0}; float dvel[3] = {0,0,0};
// Calculate steering direction. // Calculate steering direction.
if (flags & DT_CROWD_ANTICIPATE_TURNS) if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel); calcSmoothSteerDirection(ag, dvel);
else else
calcStraightSteerDirection(ag, dvel); calcStraightSteerDirection(ag, dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path. // Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->radius*2; // TODO: make less hacky. const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius; const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
// Apply style. ag->desiredSpeed = ag->params.maxSpeed;
// TODO: find way to express custom movement styles. dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
/* if (flags & DT_CROWD_DRUNK)
// Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION)
{ {
// Drunken steering const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
// Pulsating speed. float w = 0;
ag->t += dt * (1.0f - ag->var*0.25f); float disp[3] = {0,0,0};
ag->desiredSpeed = ag->maxSpeed * (1 + dtSqr(cosf(ag->t*2.0f))*0.3f);
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale); for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
// Slightly wandering steering. float diff[3];
const float amp = cosf(ag->var*13.69f+ag->t*3.123f) * 0.2f; dtVsub(diff, ag->npos, nei->npos);
const float nx = -dvel[2]; diff[1] = 0;
const float nz = dvel[0];
dvel[0] += nx*amp; const float distSqr = dtVlenSqr(diff);
dvel[2] += nz*amp; if (distSqr < 0.00001f)
} continue;
else*/ if (distSqr > dtSqr(separationDist))
{ continue;
// Normal steering. const float dist = sqrtf(distSqr);
ag->desiredSpeed = ag->maxSpeed; const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
dtVmad(disp, disp, diff, weight/dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dtVmad(dvel, dvel, disp, 1.0f/w);
// Clamp desired velocity to desired speed.
const float speedSqr = dtVlenSqr(dvel);
const float desiredSqr = dtSqr(ag->desiredSpeed);
if (speedSqr > desiredSqr)
dtVscale(dvel, dvel, desiredSqr/speedSqr);
}
} }
// Set the desired velocity. // Set the desired velocity.
@ -913,7 +958,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
if (ag->state != DT_CROWDAGENT_STATE_WALKING) if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue; continue;
if (flags & DT_CROWD_USE_VO) if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{ {
m_obstacleQuery->reset(); m_obstacleQuery->reset();
@ -921,7 +966,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
for (int j = 0; j < ag->nneis; ++j) for (int j = 0; j < ag->nneis; ++j)
{ {
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx]; const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->radius, nei->vel, nei->dvel); m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
} }
// Append neighbour segments as obstacles. // Append neighbour segments as obstacles.
@ -941,18 +986,17 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
bool adaptive = true; bool adaptive = true;
int ns = 0; int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive) if (adaptive)
{ {
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->desiredSpeed, ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, ag->vel, ag->dvel, ag->nvel, params, vod);
VO_ADAPTIVE_DIVS, VO_ADAPTIVE_RINGS, VO_ADAPTIVE_DEPTH,
vod);
} }
else else
{ {
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->radius, ag->desiredSpeed, ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, ag->vel, ag->dvel, ag->nvel, params, vod);
VO_GRID_SIZE, vod);
} }
m_velocitySampleCount += ns; m_velocitySampleCount += ns;
} }
@ -996,17 +1040,13 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
float diff[3]; float diff[3];
dtVsub(diff, ag->npos, nei->npos); dtVsub(diff, ag->npos, nei->npos);
if (fabsf(diff[1]) >= (ag->height+ nei->height)/2.0f)
continue;
diff[1] = 0; diff[1] = 0;
float dist = dtVlenSqr(diff); float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->radius + nei->radius)) if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue; continue;
dist = sqrtf(dist); dist = sqrtf(dist);
float pen = (ag->radius + nei->radius) - dist; float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f) if (dist < 0.0001f)
{ {
// Agents on top of each other, try to choose diverging separation directions. // Agents on top of each other, try to choose diverging separation directions.

70
DetourCrowd/Source/DetourObstacleAvoidance.cpp
View File

@ -25,6 +25,7 @@
#include <float.h> #include <float.h>
#include <new> #include <new>
static const float DT_PI = 3.14159265f;
static int sweepCircleCircle(const float* c0, const float r0, const float* v, static int sweepCircleCircle(const float* c0, const float r0, const float* v,
const float* c1, const float r1, const float* c1, const float r1,
@ -206,12 +207,6 @@ void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr)
dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() : dtObstacleAvoidanceQuery::dtObstacleAvoidanceQuery() :
m_velBias(0.0f),
m_weightDesVel(0.0f),
m_weightCurVel(0.0f),
m_weightSide(0.0f),
m_weightToi(0.0f),
m_horizTime(0.0f),
m_maxCircles(0), m_maxCircles(0),
m_circles(0), m_circles(0),
m_ncircles(0), m_ncircles(0),
@ -318,11 +313,11 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs, float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
const float* pos, const float rad, const float* pos, const float rad,
const float vmax, const float* vel, const float* dvel, const float* vel, const float* dvel,
dtObstacleAvoidanceDebugData* debug) dtObstacleAvoidanceDebugData* debug)
{ {
// Find min time of impact and exit amongst all obstacles. // Find min time of impact and exit amongst all obstacles.
float tmin = m_horizTime; float tmin = m_params.horizTime;
float side = 0; float side = 0;
int nside = 0; int nside = 0;
@ -395,11 +390,10 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
if (nside) if (nside)
side /= nside; side /= nside;
const float ivmax = 1.0f / vmax; const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax); const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax); const float spen = m_params.weightSide * side;
const float spen = m_weightSide * side; const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime));
const float penalty = vpen + vcpen + spen + tpen; const float penalty = vpen + vcpen + spen + tpen;
@ -411,28 +405,33 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
} }
int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax, int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, const float* vel, const float* dvel, float* nvel,
float* nvel, const int gsize, const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug) dtObstacleAvoidanceDebugData* debug)
{ {
prepare(pos, dvel); prepare(pos, dvel);
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
dtVset(nvel, 0,0,0); dtVset(nvel, 0,0,0);
if (debug) if (debug)
debug->reset(); debug->reset();
const float cvx = dvel[0] * m_velBias; const float cvx = dvel[0] * m_params.velBias;
const float cvz = dvel[2] * m_velBias; const float cvz = dvel[2] * m_params.velBias;
const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1); const float cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1);
const float half = (gsize-1)*cs*0.5f; const float half = (m_params.gridSize-1)*cs*0.5f;
float minPenalty = FLT_MAX; float minPenalty = FLT_MAX;
int ns = 0; int ns = 0;
for (int y = 0; y < gsize; ++y) for (int y = 0; y < m_params.gridSize; ++y)
{ {
for (int x = 0; x < gsize; ++x) for (int x = 0; x < m_params.gridSize; ++x)
{ {
float vcand[3]; float vcand[3];
vcand[0] = cvx + x*cs - half; vcand[0] = cvx + x*cs - half;
@ -441,7 +440,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue; if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug); const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug);
ns++; ns++;
if (penalty < minPenalty) if (penalty < minPenalty)
{ {
@ -455,28 +454,33 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
} }
static const float DT_PI = 3.14159265f;
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax, int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel, const float* vel, const float* dvel, float* nvel,
const int ndivs, const int nrings, const int depth, const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug) dtObstacleAvoidanceDebugData* debug)
{ {
prepare(pos, dvel); prepare(pos, dvel);
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
dtVset(nvel, 0,0,0); dtVset(nvel, 0,0,0);
if (debug) if (debug)
debug->reset(); debug->reset();
// Build sampling pattern aligned to desired velocity. // Build sampling pattern aligned to desired velocity.
static const int MAX_PATTERN_DIVS = 32; float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
static const int MAX_PATTERN_RINGS = 4;
float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2];
int npat = 0; int npat = 0;
const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS); const int ndivs = (int)m_params.adaptiveDivs;
const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS); const int nrings= (int)m_params.adaptiveRings;
const int depth = (int)m_params.adaptiveDepth;
const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
const float da = (1.0f/nd) * DT_PI*2; const float da = (1.0f/nd) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]); const float dang = atan2f(dvel[2], dvel[0]);
@ -499,9 +503,9 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
} }
// Start sampling. // Start sampling.
float cr = vmax * (1.0f-m_velBias); float cr = vmax * (1.0f - m_params.velBias);
float res[3]; float res[3];
dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias); dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0; int ns = 0;
for (int k = 0; k < depth; ++k) for (int k = 0; k < depth; ++k)
@ -519,7 +523,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue; if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug); const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
ns++; ns++;
if (penalty < minPenalty) if (penalty < minPenalty)
{ {

BIN
RecastDemo/Bin/Recast.app/Contents/MacOS/Recast
View File

Binary file not shown.

8
RecastDemo/Include/CrowdTool.h
View File

@ -42,6 +42,7 @@ class CrowdTool : public SampleTool
bool m_showPath; bool m_showPath;
bool m_showVO; bool m_showVO;
bool m_showOpt; bool m_showOpt;
bool m_showNeis;
bool m_expandDebugDraw; bool m_expandDebugDraw;
bool m_showLabels; bool m_showLabels;
@ -53,8 +54,10 @@ class CrowdTool : public SampleTool
bool m_anticipateTurns; bool m_anticipateTurns;
bool m_optimizeVis; bool m_optimizeVis;
bool m_optimizeTopo; bool m_optimizeTopo;
bool m_useVO; bool m_obstacleAvoidance;
bool m_drunkMove; float m_obstacleAvoidanceType;
bool m_separation;
float m_separationWeight;
bool m_run; bool m_run;
@ -83,6 +86,7 @@ class CrowdTool : public SampleTool
}; };
ToolMode m_mode; ToolMode m_mode;
void updateAgentParams();
void updateTick(const float dt); void updateTick(const float dt);
public: public:

252
RecastDemo/Source/CrowdTool.cpp
View File

@ -80,8 +80,8 @@ static bool isectSegAABB(const float* sp, const float* sq,
static void getAgentBounds(const dtCrowdAgent* ag, float* bmin, float* bmax) static void getAgentBounds(const dtCrowdAgent* ag, float* bmin, float* bmax)
{ {
const float* p = ag->npos; const float* p = ag->npos;
const float r = ag->radius; const float r = ag->params.radius;
const float h = ag->height; const float h = ag->params.height;
bmin[0] = p[0] - r; bmin[0] = p[0] - r;
bmin[1] = p[1]; bmin[1] = p[1];
bmin[2] = p[2] - r; bmin[2] = p[2] - r;
@ -100,17 +100,20 @@ CrowdTool::CrowdTool() :
m_showPath(false), m_showPath(false),
m_showVO(false), m_showVO(false),
m_showOpt(false), m_showOpt(false),
m_showNeis(false),
m_expandDebugDraw(false), m_expandDebugDraw(false),
m_showLabels(false), m_showLabels(false),
m_showGrid(false), m_showGrid(false),
m_showNodes(false), m_showNodes(false),
m_showPerfGraph(false), m_showPerfGraph(false),
m_expandOptions(true), m_expandOptions(true),
m_anticipateTurns(true), m_anticipateTurns(true),
m_optimizeVis(true), m_optimizeVis(true),
m_optimizeTopo(true), m_optimizeTopo(true),
m_useVO(true), m_obstacleAvoidance(true),
m_obstacleAvoidanceType(3.0f),
m_separation(false),
m_separationWeight(2.0f),
m_run(true), m_run(true),
m_mode(TOOLMODE_CREATE) m_mode(TOOLMODE_CREATE)
{ {
@ -145,8 +148,43 @@ void CrowdTool::init(Sample* sample)
dtNavMesh* nav = m_sample->getNavMesh(); dtNavMesh* nav = m_sample->getNavMesh();
if (nav) if (nav)
{
m_crowd.init(MAX_AGENTS, m_sample->getAgentRadius(), nav); m_crowd.init(MAX_AGENTS, m_sample->getAgentRadius(), nav);
// Setup local avoidance params to different qualities.
dtObstacleAvoidanceParams params;
// Use mostly default settings, copy from dtCrowd.
memcpy(&params, m_crowd.getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
// Low (11)
params.velBias = 0.5f;
params.adaptiveDivs = 5;
params.adaptiveRings = 2;
params.adaptiveDepth = 1;
m_crowd.setObstacleAvoidanceParams(0, &params);
// Medium (22)
params.velBias = 0.5f;
params.adaptiveDivs = 5;
params.adaptiveRings = 2;
params.adaptiveDepth = 2;
m_crowd.setObstacleAvoidanceParams(1, &params);
// Good (45)
params.velBias = 0.5f;
params.adaptiveDivs = 7;
params.adaptiveRings = 2;
params.adaptiveDepth = 3;
m_crowd.setObstacleAvoidanceParams(2, &params);
// High (66)
params.velBias = 0.5f;
params.adaptiveDivs = 7;
params.adaptiveRings = 3;
params.adaptiveDepth = 3;
m_crowd.setObstacleAvoidanceParams(3, &params);
}
} }
void CrowdTool::reset() void CrowdTool::reset()
@ -173,13 +211,39 @@ void CrowdTool::handleMenu()
{ {
imguiIndent(); imguiIndent();
if (imguiCheck("Optimize Visibility", m_optimizeVis)) if (imguiCheck("Optimize Visibility", m_optimizeVis))
{
m_optimizeVis = !m_optimizeVis; m_optimizeVis = !m_optimizeVis;
updateAgentParams();
}
if (imguiCheck("Optimize Topology", m_optimizeTopo)) if (imguiCheck("Optimize Topology", m_optimizeTopo))
{
m_optimizeTopo = !m_optimizeTopo; m_optimizeTopo = !m_optimizeTopo;
updateAgentParams();
}
if (imguiCheck("Anticipate Turns", m_anticipateTurns)) if (imguiCheck("Anticipate Turns", m_anticipateTurns))
{
m_anticipateTurns = !m_anticipateTurns; m_anticipateTurns = !m_anticipateTurns;
if (imguiCheck("Use VO", m_useVO)) updateAgentParams();
m_useVO = !m_useVO; }
if (imguiCheck("Obstacle Avoidance", m_obstacleAvoidance))
{
m_obstacleAvoidance = !m_obstacleAvoidance;
updateAgentParams();
}
if (imguiSlider("Avoidance Quality", &m_obstacleAvoidanceType, 0.0f, 3.0f, 1.0f))
{
updateAgentParams();
}
if (imguiCheck("Separation", m_separation))
{
m_separation = !m_separation;
updateAgentParams();
}
if (imguiSlider("Separation Weight", &m_separationWeight, 0.0f, 20.0f, 0.01f))
{
updateAgentParams();
}
imguiUnindent(); imguiUnindent();
} }
@ -199,6 +263,8 @@ void CrowdTool::handleMenu()
m_showVO = !m_showVO; m_showVO = !m_showVO;
if (imguiCheck("Show Path Optimization", m_showOpt)) if (imguiCheck("Show Path Optimization", m_showOpt))
m_showOpt = !m_showOpt; m_showOpt = !m_showOpt;
if (imguiCheck("Show Neighbours", m_showNeis))
m_showNeis = !m_showNeis;
imguiUnindent(); imguiUnindent();
} }
@ -265,6 +331,19 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
ap.maxSpeed = 3.5f; ap.maxSpeed = 3.5f;
ap.collisionQueryRange = ap.radius * 8.0f; ap.collisionQueryRange = ap.radius * 8.0f;
ap.pathOptimizationRange = ap.radius * 30.0f; ap.pathOptimizationRange = ap.radius * 30.0f;
ap.updateFlags = 0;
if (m_anticipateTurns)
ap.updateFlags |= DT_CROWD_ANTICIPATE_TURNS;
if (m_optimizeVis)
ap.updateFlags |= DT_CROWD_OPTIMIZE_VIS;
if (m_optimizeTopo)
ap.updateFlags |= DT_CROWD_OPTIMIZE_TOPO;
if (m_obstacleAvoidance)
ap.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
if (m_separation)
ap.updateFlags |= DT_CROWD_SEPARATION;
ap.obstacleAvoidanceType = (unsigned char)m_obstacleAvoidanceType;
ap.separationWeight = m_separationWeight;
int idx = m_crowd.addAgent(p, &ap); int idx = m_crowd.addAgent(p, &ap);
if (idx != -1) if (idx != -1)
@ -335,26 +414,49 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
} }
} }
void CrowdTool::updateAgentParams()
{
unsigned char updateFlags = 0;
unsigned char obstacleAvoidanceType = 0;
if (m_anticipateTurns)
updateFlags |= DT_CROWD_ANTICIPATE_TURNS;
if (m_optimizeVis)
updateFlags |= DT_CROWD_OPTIMIZE_VIS;
if (m_optimizeTopo)
updateFlags |= DT_CROWD_OPTIMIZE_TOPO;
if (m_obstacleAvoidance)
updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
if (m_obstacleAvoidance)
updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE;
if (m_separation)
updateFlags |= DT_CROWD_SEPARATION;
obstacleAvoidanceType = (unsigned char)m_obstacleAvoidanceType;
dtCrowdAgentParams params;
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
memcpy(&params, &ag->params, sizeof(dtCrowdAgentParams));
params.updateFlags = updateFlags;
params.obstacleAvoidanceType = obstacleAvoidanceType;
params.separationWeight = m_separationWeight;
m_crowd.updateAgentParameters(i, &params);
}
}
void CrowdTool::updateTick(const float dt) void CrowdTool::updateTick(const float dt)
{ {
dtNavMesh* nav = m_sample->getNavMesh(); dtNavMesh* nav = m_sample->getNavMesh();
if (!nav) if (!nav)
return; return;
unsigned int flags = 0;
if (m_anticipateTurns)
flags |= DT_CROWD_ANTICIPATE_TURNS;
if (m_useVO)
flags |= DT_CROWD_USE_VO;
if (m_optimizeVis)
flags |= DT_CROWD_OPTIMIZE_VIS;
if (m_optimizeTopo)
flags |= DT_CROWD_OPTIMIZE_TOPO;
TimeVal startTime = getPerfTime(); TimeVal startTime = getPerfTime();
m_crowd.update(dt, flags, &m_agentDebug); m_crowd.update(dt, &m_agentDebug);
TimeVal endTime = getPerfTime(); TimeVal endTime = getPerfTime();
@ -499,7 +601,7 @@ void CrowdTool::handleRender()
if (ag->active) if (ag->active)
{ {
const float radius = ag->radius; const float radius = ag->params.radius;
const float* pos = ag->npos; const float* pos = ag->npos;
if (m_showCorners) if (m_showCorners)
@ -553,7 +655,7 @@ void CrowdTool::handleRender()
{ {
const float* center = ag->boundary.getCenter(); const float* center = ag->boundary.getCenter();
duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f); 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->collisionQueryRange, duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->params.collisionQueryRange,
duRGBA(192,0,128,128), 2.0f); duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f); dd.begin(DU_DRAW_LINES, 3.0f);
@ -569,6 +671,22 @@ void CrowdTool::handleRender()
dd.end(); dd.end();
} }
if (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)
{
const dtCrowdAgent* nei = m_crowd.getAgent(ag->neis[j].idx);
if (!nei->active) continue;
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_showOpt) if (m_showOpt)
{ {
dd.begin(DU_DRAW_LINES, 2.0f); dd.begin(DU_DRAW_LINES, 2.0f);
@ -585,7 +703,7 @@ void CrowdTool::handleRender()
const dtCrowdAgent* ag = m_crowd.getAgent(i); const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue; if (!ag->active) continue;
const float radius = ag->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); duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f);
@ -596,8 +714,8 @@ void CrowdTool::handleRender()
const dtCrowdAgent* ag = m_crowd.getAgent(i); const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue; if (!ag->active) continue;
const float height = ag->height; const float height = ag->params.height;
const float radius = ag->radius; const float radius = ag->params.radius;
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);
@ -609,33 +727,6 @@ void CrowdTool::handleRender()
} }
// Velocity stuff.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float height = ag->height;
const float* pos = ag->npos;
const float* vel = ag->vel;
const float* dvel = ag->dvel;
unsigned int col = duRGBA(220,220,220,192);
if (m_agentDebug.idx == i)
col = duRGBA(255,192,0,192);
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+dvel[2],
0.0f, 0.4f, duRGBA(0,192,255,192), 1.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,192), 2.0f);
}
if (m_agentDebug.idx != -1) if (m_agentDebug.idx != -1)
{ {
const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx); const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
@ -647,9 +738,11 @@ void CrowdTool::handleRender()
const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod; const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
const float dx = ag->npos[0]; const float dx = ag->npos[0];
const float dy = ag->npos[1]+ag->height; const float dy = ag->npos[1]+ag->params.height;
const float dz = ag->npos[2]; 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); dd.begin(DU_DRAW_QUADS);
for (int i = 0; i < vod->getSampleCount(); ++i) for (int i = 0; i < vod->getSampleCount(); ++i)
{ {
@ -669,6 +762,33 @@ void CrowdTool::handleRender()
} }
} }
// Velocity stuff.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->params.radius;
const float height = ag->params.height;
const float* pos = ag->npos;
const float* vel = ag->vel;
const float* dvel = ag->dvel;
unsigned int col = duRGBA(220,220,220,192);
if (m_agentDebug.idx == i)
col = duRGBA(255,192,0,192);
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, col, 2.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+dvel[2],
0.0f, 0.4f, duRGBA(0,192,255,192), (m_agentDebug.idx == i) ? 2.0f : 1.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,160), 2.0f);
}
dd.depthMask(true); dd.depthMask(true);
} }
@ -683,15 +803,16 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
imguiDrawText((int)x, (int)(y+25), IMGUI_ALIGN_CENTER, "TARGET", imguiRGBA(0,0,0,220)); imguiDrawText((int)x, (int)(y+25), IMGUI_ALIGN_CENTER, "TARGET", imguiRGBA(0,0,0,220));
} }
char label[32];
if (m_showLabels) if (m_showLabels)
{ {
char label[32];
for (int i = 0; i < m_crowd.getAgentCount(); ++i) for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{ {
const dtCrowdAgent* ag = m_crowd.getAgent(i); const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue; if (!ag->active) continue;
const float* pos = ag->npos; const float* pos = ag->npos;
const float h = ag->height; const float h = ag->params.height;
if (gluProject((GLdouble)pos[0], (GLdouble)pos[1]+h, (GLdouble)pos[2], if (gluProject((GLdouble)pos[0], (GLdouble)pos[1]+h, (GLdouble)pos[2],
model, proj, view, &x, &y, &z)) model, proj, view, &x, &y, &z))
{ {
@ -701,6 +822,31 @@ void CrowdTool::handleRenderOverlay(double* proj, double* model, int* view)
} }
} }
if (m_agentDebug.idx != -1)
{
const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
if (ag->active)
{
const float radius = ag->params.radius;
if (m_showNeis)
{
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = m_crowd.getAgent(ag->neis[j].idx);
if (!nei->active) continue;
if (gluProject((GLdouble)nei->npos[0], (GLdouble)nei->npos[1]+radius, (GLdouble)nei->npos[2],
model, proj, view, &x, &y, &z))
{
snprintf(label, 32, "%.3f", ag->neis[j].dist);
imguiDrawText((int)x, (int)y+15, IMGUI_ALIGN_CENTER, label, imguiRGBA(255,255,255,220));
}
}
}
}
}
if (m_showPerfGraph) if (m_showPerfGraph)
{ {

2
RecastDemo/Source/imgui.cpp
View File

@ -593,7 +593,7 @@ bool imguiSlider(const char* text, float* val, float vmin, float vmax, float vin
if (u < 0) u = 0; if (u < 0) u = 0;
if (u > 1) u = 1; if (u > 1) u = 1;
*val = vmin + u*(vmax-vmin); *val = vmin + u*(vmax-vmin);
*val = floorf(*val / vinc)*vinc; // Snap to vinc *val = floorf(*val/vinc+0.5f)*vinc; // Snap to vinc
m = (int)(u * range); m = (int)(u * range);
valChanged = true; valChanged = true;
} }