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_CORNERS = 4;
static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8;
struct dtCrowdNeighbour
{
int idx;
@ -42,10 +42,22 @@ enum CrowdAgentState
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
{
unsigned char active;
unsigned char state;
dtPathCorridor corridor;
@ -59,12 +71,6 @@ struct dtCrowdAgent
dtCrowdNeighbour neis[DT_CROWDAGENT_MAX_NEIGHBOURS];
int nneis;
float radius, height;
float maxAcceleration;
float maxSpeed;
float collisionQueryRange;
float pathOptimizationRange;
float desiredSpeed;
float npos[3];
@ -73,6 +79,8 @@ struct dtCrowdAgent
float nvel[3];
float vel[3];
dtCrowdAgentParams params;
float cornerVerts[DT_CROWDAGENT_MAX_CORNERS*3];
unsigned char cornerFlags[DT_CROWDAGENT_MAX_CORNERS];
dtPolyRef cornerPolys[DT_CROWDAGENT_MAX_CORNERS];
@ -90,22 +98,12 @@ struct dtCrowdAgentAnimation
enum UpdateFlags
{
DT_CROWD_ANTICIPATE_TURNS = 1,
DT_CROWD_USE_VO = 2,
// DT_CROWD_DRUNK = 4,
DT_CROWD_OBSTACLE_AVOIDANCE = 2,
DT_CROWD_SEPARATION = 4,
DT_CROWD_OPTIMIZE_VIS = 8,
DT_CROWD_OPTIMIZE_TOPO = 16,
};
struct dtCrowdAgentParams
{
float radius;
float height;
float maxAcceleration;
float maxSpeed;
float collisionQueryRange;
float pathOptimizationRange;
};
struct dtCrowdAgentDebugInfo
{
int idx;
@ -122,7 +120,9 @@ class dtCrowd
dtPathQueue m_pathq;
dtObstacleAvoidanceParams m_obstacleQueryParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS];
dtObstacleAvoidanceQuery* m_obstacleQuery;
dtProximityGrid* m_grid;
dtPolyRef* m_pathResult;
@ -176,17 +176,21 @@ public:
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 int getAgentCount() const;
int addAgent(const float* pos, const dtCrowdAgentParams* params);
void updateAgentParameters(const int idx, const dtCrowdAgentParams* params);
void removeAgent(const int idx);
bool requestMoveTarget(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);
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 float* getQueryExtents() const { return m_ext; }
@ -195,6 +199,7 @@ public:
const dtProximityGrid* getGrid() const { return m_grid; }
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;
};
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
{
@ -75,6 +73,23 @@ dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData();
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
{
public:
@ -90,21 +105,14 @@ public:
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,
const float* vel, const float* dvel, float* nvel,
const int gsize,
const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug = 0);
int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel,
const int ndivs, const int nrings, const int depth,
const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug = 0);
inline int getObstacleCircleCount() const { return m_ncircles; }
@ -119,18 +127,16 @@ private:
float processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float vmax, const float* vel, const float* dvel,
const float* vel, const float* dvel,
dtObstacleAvoidanceDebugData* debug);
dtObstacleCircle* insertCircle(const float dist);
dtObstacleSegment* insertSegment(const float dist);
float m_velBias;
float m_weightDesVel;
float m_weightCurVel;
float m_weightSide;
float m_weightToi;
float m_horizTime;
dtObstacleAvoidanceParams m_params;
float m_invHorizTime;
float m_vmax;
float m_invVmax;
int m_maxCircles;
dtObstacleCircle* m_circles;

174
DetourCrowd/Source/DetourCrowd.cpp
View File

@ -30,11 +30,6 @@
#include "DetourAssert.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;
@ -49,7 +44,7 @@ inline float between(const float t, const float t0, const float t1)
static void integrate(dtCrowdAgent* ag, const float dt)
{
// Fake dynamic constraint.
const float maxDelta = ag->maxAcceleration * dt;
const float maxDelta = ag->params.maxAcceleration * dt;
float dv[3];
dtVsub(dv, ag->nvel, ag->vel);
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))
return false;
m_obstacleQuery->setDesiredVelocityWeight(2.0f);
m_obstacleQuery->setCurrentVelocityWeight(0.75f);
m_obstacleQuery->setPreferredSideWeight(0.75f);
m_obstacleQuery->setCollisionTimeWeight(2.5f);
m_obstacleQuery->setTimeHorizon(2.5f);
m_obstacleQuery->setVelocitySelectionBias(0.4f);
// Init obstacle query params.
memset(m_obstacleQueryParams, 0, sizeof(m_obstacleQueryParams));
for (int i = 0; i < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS; ++i)
{
dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[i];
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.
m_maxPathResult = 256;
@ -307,6 +312,19 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
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
{
return m_maxAgents;
@ -317,6 +335,13 @@ const dtCrowdAgent* dtCrowd::getAgent(const int 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)
{
// Find empty slot.
@ -347,12 +372,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
ag->radius = params->radius;
ag->height = params->height;
ag->maxAcceleration = params->maxAcceleration;
ag->maxSpeed = params->maxSpeed;
ag->collisionQueryRange = params->collisionQueryRange;
ag->pathOptimizationRange = params->pathOptimizationRange;
updateAgentParameters(idx, params);
ag->topologyOptTime = 0;
ag->nneis = 0;
@ -488,7 +508,7 @@ int dtCrowd::getNeighbours(const float* pos, const float height, const float ran
// Check for overlap.
float diff[3];
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;
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
@ -723,6 +743,8 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
dtCrowdAgent* ag = agents[i];
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
continue;
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_TOPO) == 0)
continue;
ag->topologyOptTime += dt;
if (ag->topologyOptTime >= OPT_TIME_THR)
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;
@ -750,8 +772,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
updateMoveRequest(dt);
// Optimize path topology.
if (flags & DT_CROWD_OPTIMIZE_TOPO)
updateTopologyOptimization(agents, nagents, dt);
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
@ -759,7 +780,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
{
dtCrowdAgent* ag = agents[i];
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);
}
@ -771,10 +792,15 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
continue;
// Only update the collision boundary after certain distance has been passed.
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(ag->collisionQueryRange*0.25f))
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->collisionQueryRange, m_navquery, &m_filter);
const float updateThr = ag->params.collisionQueryRange*0.25f;
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
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.
@ -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,
// 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];
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.
if (debugIdx == i)
@ -824,7 +850,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
continue;
// Check
const float triggerRadius = ag->radius*2.25f;
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
@ -840,7 +866,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
anim->polyRef = refs[1];
anim->active = 1;
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->ncorners = 0;
@ -866,39 +892,58 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
float dvel[3] = {0,0,0};
// Calculate steering direction.
if (flags & DT_CROWD_ANTICIPATE_TURNS)
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
// 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;
// Apply style.
// TODO: find way to express custom movement styles.
/* if (flags & DT_CROWD_DRUNK)
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
// 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.
ag->t += dt * (1.0f - ag->var*0.25f);
ag->desiredSpeed = ag->maxSpeed * (1 + dtSqr(cosf(ag->t*2.0f))*0.3f);
float w = 0;
float disp[3] = {0,0,0};
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.
const float amp = cosf(ag->var*13.69f+ag->t*3.123f) * 0.2f;
const float nx = -dvel[2];
const float nz = dvel[0];
dvel[0] += nx*amp;
dvel[2] += nz*amp;
}
else*/
{
// Normal steering.
ag->desiredSpeed = ag->maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = sqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
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.
@ -913,7 +958,7 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (flags & DT_CROWD_USE_VO)
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
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)
{
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.
@ -941,18 +986,17 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel,
VO_ADAPTIVE_DIVS, VO_ADAPTIVE_RINGS, VO_ADAPTIVE_DEPTH,
vod);
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
else
{
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel,
VO_GRID_SIZE, vod);
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
@ -996,17 +1040,13 @@ void dtCrowd::update(const float dt, unsigned int flags, dtCrowdAgentDebugInfo*
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
if (fabsf(diff[1]) >= (ag->height+ nei->height)/2.0f)
continue;
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->radius + nei->radius))
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = sqrtf(dist);
float pen = (ag->radius + nei->radius) - dist;
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// 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 <new>
static const float DT_PI = 3.14159265f;
static int sweepCircleCircle(const float* c0, const float r0, const float* v,
const float* c1, const float r1,
@ -206,12 +207,6 @@ void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr)
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_circles(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,
const float* pos, const float rad,
const float vmax, const float* vel, const float* dvel,
const float* vel, const float* dvel,
dtObstacleAvoidanceDebugData* debug)
{
// Find min time of impact and exit amongst all obstacles.
float tmin = m_horizTime;
float tmin = m_params.horizTime;
float side = 0;
int nside = 0;
@ -395,11 +390,10 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
if (nside)
side /= nside;
const float ivmax = 1.0f / vmax;
const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax);
const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax);
const float spen = m_weightSide * side;
const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime));
const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
const float spen = m_params.weightSide * side;
const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
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,
const float* vel, const float* dvel,
float* nvel, const int gsize,
const float* vel, const float* dvel, float* nvel,
const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug)
{
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);
if (debug)
debug->reset();
const float cvx = dvel[0] * m_velBias;
const float cvz = dvel[2] * m_velBias;
const float cs = vmax * 2 * (1 - m_velBias) / (float)(gsize-1);
const float half = (gsize-1)*cs*0.5f;
const float cvx = dvel[0] * m_params.velBias;
const float cvz = dvel[2] * m_params.velBias;
const float cs = vmax * 2 * (1 - m_params.velBias) / (float)(m_params.gridSize-1);
const float half = (m_params.gridSize-1)*cs*0.5f;
float minPenalty = FLT_MAX;
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];
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;
const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug);
const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug);
ns++;
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,
const float* vel, const float* dvel, float* nvel,
const int ndivs, const int nrings, const int depth,
const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug)
{
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);
if (debug)
debug->reset();
// Build sampling pattern aligned to desired velocity.
static const int MAX_PATTERN_DIVS = 32;
static const int MAX_PATTERN_RINGS = 4;
float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2];
float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
int npat = 0;
const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS);
const int ndivs = (int)m_params.adaptiveDivs;
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 dang = atan2f(dvel[2], dvel[0]);
@ -499,9 +503,9 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
}
// Start sampling.
float cr = vmax * (1.0f-m_velBias);
float cr = vmax * (1.0f - m_params.velBias);
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;
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;
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++;
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_showVO;
bool m_showOpt;
bool m_showNeis;
bool m_expandDebugDraw;
bool m_showLabels;
@ -53,8 +54,10 @@ class CrowdTool : public SampleTool
bool m_anticipateTurns;
bool m_optimizeVis;
bool m_optimizeTopo;
bool m_useVO;
bool m_drunkMove;
bool m_obstacleAvoidance;
float m_obstacleAvoidanceType;
bool m_separation;
float m_separationWeight;
bool m_run;
@ -83,6 +86,7 @@ class CrowdTool : public SampleTool
};
ToolMode m_mode;
void updateAgentParams();
void updateTick(const float dt);
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)
{
const float* p = ag->npos;
const float r = ag->radius;
const float h = ag->height;
const float r = ag->params.radius;
const float h = ag->params.height;
bmin[0] = p[0] - r;
bmin[1] = p[1];
bmin[2] = p[2] - r;
@ -100,17 +100,20 @@ CrowdTool::CrowdTool() :
m_showPath(false),
m_showVO(false),
m_showOpt(false),
m_showNeis(false),
m_expandDebugDraw(false),
m_showLabels(false),
m_showGrid(false),
m_showNodes(false),
m_showPerfGraph(false),
m_expandOptions(true),
m_anticipateTurns(true),
m_optimizeVis(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_mode(TOOLMODE_CREATE)
{
@ -145,8 +148,43 @@ void CrowdTool::init(Sample* sample)
dtNavMesh* nav = m_sample->getNavMesh();
if (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()
@ -173,13 +211,39 @@ void CrowdTool::handleMenu()
{
imguiIndent();
if (imguiCheck("Optimize Visibility", m_optimizeVis))
{
m_optimizeVis = !m_optimizeVis;
updateAgentParams();
}
if (imguiCheck("Optimize Topology", m_optimizeTopo))
{
m_optimizeTopo = !m_optimizeTopo;
updateAgentParams();
}
if (imguiCheck("Anticipate Turns", m_anticipateTurns))
{
m_anticipateTurns = !m_anticipateTurns;
if (imguiCheck("Use VO", m_useVO))
m_useVO = !m_useVO;
updateAgentParams();
}
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();
}
@ -199,6 +263,8 @@ void CrowdTool::handleMenu()
m_showVO = !m_showVO;
if (imguiCheck("Show Path Optimization", m_showOpt))
m_showOpt = !m_showOpt;
if (imguiCheck("Show Neighbours", m_showNeis))
m_showNeis = !m_showNeis;
imguiUnindent();
}
@ -265,6 +331,19 @@ void CrowdTool::handleClick(const float* s, const float* p, bool shift)
ap.maxSpeed = 3.5f;
ap.collisionQueryRange = ap.radius * 8.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);
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)
{
dtNavMesh* nav = m_sample->getNavMesh();
if (!nav)
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();
m_crowd.update(dt, flags, &m_agentDebug);
m_crowd.update(dt, &m_agentDebug);
TimeVal endTime = getPerfTime();
@ -499,7 +601,7 @@ void CrowdTool::handleRender()
if (ag->active)
{
const float radius = ag->radius;
const float radius = ag->params.radius;
const float* pos = ag->npos;
if (m_showCorners)
@ -553,7 +655,7 @@ void CrowdTool::handleRender()
{
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->collisionQueryRange,
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);
@ -569,6 +671,22 @@ void CrowdTool::handleRender()
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)
{
dd.begin(DU_DRAW_LINES, 2.0f);
@ -585,7 +703,7 @@ void CrowdTool::handleRender()
const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
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);
@ -596,8 +714,8 @@ void CrowdTool::handleRender()
const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float height = ag->height;
const float radius = ag->radius;
const float height = ag->params.height;
const float radius = ag->params.radius;
const float* pos = ag->npos;
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)
{
const dtCrowdAgent* ag = m_crowd.getAgent(m_agentDebug.idx);
@ -647,9 +738,11 @@ void CrowdTool::handleRender()
const dtObstacleAvoidanceDebugData* vod = m_agentDebug.vod;
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];
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)
{
@ -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);
}
@ -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));
}
char label[32];
if (m_showLabels)
{
char label[32];
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const dtCrowdAgent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
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],
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)
{

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 > 1) u = 1;
*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);
valChanged = true;
}