bool dtCrowd::updateAgentPosition(unsigned id, const float* position)
{
if (id < m_maxAgents)
{
dtCrowdAgent& ag = m_agents[id];
dtPolyRef ref = 0;
float nearestPosition[] = {0, 0, 0};
if (dtStatusFailed(m_crowdQuery->getNavMeshQuery()->findNearestPoly(position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), &ref, nearestPosition)))
return false;
// If no polygons have been found, it's a failure
if (ref == 0)
return false;
dtVset(ag.desiredVelocity, 0, 0, 0);
dtVset(ag.velocity, 0, 0, 0);
dtVcopy(ag.position, nearestPosition);
ag.state = DT_CROWDAGENT_STATE_WALKING;
return true;
}
return false;
}
/// @par
///
/// The agent's position will be constrained to the surface of the navigation mesh.
int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
{
// Find empty slot.
int idx = -1;
for (int i = 0; i < m_maxAgents; ++i)
{
if (!m_agents[i].active)
{
idx = i;
break;
}
}
if (idx == -1)
return -1;
dtCrowdAgent* ag = &m_agents[idx];
updateAgentParameters(idx, params);
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref = 0;
dtVcopy(nearest, pos);
dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
if (dtStatusFailed(status))
{
dtVcopy(nearest, pos);
ref = 0;
}
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
ag->partial = false;
ag->topologyOptTime = 0;
ag->targetReplanTime = 0;
ag->nneis = 0;
dtVset(ag->dvel, 0,0,0);
dtVset(ag->nvel, 0,0,0);
dtVset(ag->vel, 0,0,0);
dtVcopy(ag->npos, nearest);
ag->desiredSpeed = 0;
if (ref)
ag->state = DT_CROWDAGENT_STATE_WALKING;
else
ag->state = DT_CROWDAGENT_STATE_INVALID;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
ag->active = true;
// Clockwork: added to fix illegal memory access when ncorners is queried before the agent has updated
ag->ncorners = 0;
return idx;
}
/// @par
///
/// The agent's position will be constrained to the surface of the navigation mesh.
int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params, const dtQueryFilter* filter)
{
// Find empty slot.
int idx = -1;
for (int i = 0; i < m_maxAgents; ++i)
{
if (!m_agents[i].active)
{
idx = i;
break;
}
}
if (idx == -1)
return -1;
dtCrowdAgent* ag = &m_agents[idx];
// [UE4] multiple filter support
const bool bStoredFilter = updateAgentFilter(idx, filter);
if (!bStoredFilter)
{
return -1;
}
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref;
m_navquery->updateLinkFilter(params->linkFilter);
m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.filter], &ref, nearest);
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
updateAgentParameters(idx, params);
ag->topologyOptTime = 0;
ag->targetReplanTime = 0;
ag->nneis = 0;
ag->ncorners = 0;
dtVset(ag->dvel, 0,0,0);
dtVset(ag->nvel, 0,0,0);
dtVset(ag->vel, 0,0,0);
dtVcopy(ag->npos, nearest);
ag->desiredSpeed = 0;
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;
}
bool dtCrowd::resetAgentVelocity(const int idx)
{
if (idx < 0 || idx > m_maxAgents)
return false;
dtCrowdAgent* ag = &m_agents[idx];
dtVset(ag->nvel, 0, 0, 0);
dtVset(ag->vel, 0, 0, 0);
dtVset(ag->dvel, 0, 0, 0);
return true;
}
int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
{
// Find empty slot.
int idx = -1;
for (int i = 0; i < m_maxAgents; ++i)
{
if (!m_agents[i].active)
{
idx = i;
break;
}
}
if (idx == -1)
return -1;
dtCrowdAgent* ag = &m_agents[idx];
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref;
m_navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest);
if (!ref)
{
// Could not find a location on navmesh.
return -1;
}
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
updateAgentParameters(idx, params);
ag->topologyOptTime = 0;
ag->nneis = 0;
dtVset(ag->dvel, 0,0,0);
dtVset(ag->nvel, 0,0,0);
dtVset(ag->vel, 0,0,0);
dtVcopy(ag->npos, nearest);
ag->desiredSpeed = 0;
ag->t = 0;
ag->var = (rand() % 10) / 9.0f;
ag->state = DT_CROWDAGENT_STATE_WALKING;
ag->active = 1;
return idx;
}
static void integrate(dtCrowdAgent* ag, const float dt)
{
if (dtVlen(ag->velocity) > EPSILON)
dtVmad(ag->position, ag->position, ag->velocity, dt);
else
dtVset(ag->velocity,0,0,0);
}
static void calcSmoothSteerDirection(const dtCrowdAgent* ag, float* dir)
{
if (!ag->ncorners)
{
dtVset(dir, 0,0,0);
return;
}
const int ip0 = 0;
const int ip1 = dtMin(1, ag->ncorners-1);
const float* p0 = &ag->cornerVerts[ip0*3];
const float* p1 = &ag->cornerVerts[ip1*3];
float dir0[3], dir1[3];
dtVsub(dir0, p0, ag->npos);
dtVsub(dir1, p1, ag->npos);
dir0[1] = 0;
dir1[1] = 0;
float len0 = dtVlen(dir0);
float len1 = dtVlen(dir1);
if (len1 > 0.001f)
dtVscale(dir1,dir1,1.0f/len1);
dir[0] = dir0[0] - dir1[0]*len0*0.5f;
dir[1] = 0;
dir[2] = dir0[2] - dir1[2]*len0*0.5f;
dtVnormalize(dir);
}
void dtCrowd::updateStepOffMeshAnim(const float dt, dtCrowdAgentDebugInfo*)
{
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int agentIndex = getAgentIndex(ag);
dtCrowdAgentAnimation* anim = &m_agentAnims[agentIndex];
if (!anim->active)
continue;
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
// UE4: m_keepOffmeshConnections support
if (m_keepOffmeshConnections)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0, 0, 0);
dtVset(ag->dvel, 0, 0, 0);
}
}
dtLocalBoundary::dtLocalBoundary() :
m_nsegs(0),
m_npolys(0)
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
// Urho3D: initialize all class members
memset(&m_segs, 0, sizeof(m_segs));
memset(&m_polys, 0, sizeof(m_polys));
}
static void calcStraightSteerDirection(const dtCrowdAgent* ag, float* dir)
{
if (!ag->ncorners)
{
dtVset(dir, 0,0,0);
return;
}
dtVsub(dir, &ag->cornerVerts[0], ag->npos);
dir[1] = 0;
dtVnormalize(dir);
}
/// @par
///
/// The agent's position will be constrained to the surface of the navigation mesh.
bool dtCrowd::addAgent(dtCrowdAgent& agent, const float* pos)
{
// Find empty slot.
int idx = -1;
for (unsigned i = 0; i < m_maxAgents; ++i)
{
if (!m_agents[i].active)
{
idx = i;
break;
}
}
if (idx == -1)
return false;
dtCrowdAgent* ag = &m_agents[idx];
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref;
m_crowdQuery->getNavMeshQuery()->findNearestPoly(pos, m_crowdQuery->getQueryExtents(),
m_crowdQuery->getQueryFilter(), &ref, nearest);
dtVset(ag->desiredVelocity, 0, 0, 0);
dtVset(ag->velocity, 0, 0, 0);
dtVcopy(ag->position, nearest);
if (ref)
ag->state = DT_CROWDAGENT_STATE_WALKING;
else
ag->state = DT_CROWDAGENT_STATE_INVALID;
ag->active = 1;
agent = *ag;
return true;
}
int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax,
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 = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
if (debug)
debug->reset();
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 < m_params.gridSize; ++y)
{
for (int x = 0; x < m_params.gridSize; ++x)
{
float vcand[3];
vcand[0] = cvx + x*cs - half;
vcand[1] = 0;
vcand[2] = cvz + y*cs - half;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(nvel, vcand);
}
}
}
return ns;
}
bool dtCrowd::resetMoveTarget(const int idx)
{
if (idx < 0 || idx >= m_maxAgents)
return false;
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;
}
static void integrate(dtCrowdAgent* ag, const float dt)
{
// Fake dynamic constraint.
const float maxDelta = ag->params.maxAcceleration * dt;
float dv[3];
dtVsub(dv, ag->nvel, ag->vel);
float ds = dtVlen(dv);
if (ds > maxDelta)
dtVscale(dv, dv, maxDelta/ds);
dtVadd(ag->vel, ag->vel, dv);
// Integrate
if (dtVlen(ag->vel) > 0.0001f)
dtVmad(ag->npos, ag->npos, ag->vel, dt);
else
dtVset(ag->vel,0,0,0);
}
void dtCrowd::updateStepOffMeshVelocity(const float dt, dtCrowdAgentDebugInfo*)
{
// UE4 version of offmesh anims, updates velocity and checks distance instead of fixed time
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int agentIndex = getAgentIndex(ag);
dtCrowdAgentAnimation* anim = &m_agentAnims[agentIndex];
if (!anim->active)
continue;
anim->t += dt;
const float dist = dtVdistSqr(ag->npos, anim->endPos);
const float distThres = dtSqr(5.0f);
if (dist < distThres)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
// UE4: m_keepOffmeshConnections support
if (m_keepOffmeshConnections)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
}
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
{
float dir[3] = { 0 };
dtVsub(dir, anim->endPos, anim->initPos);
dir[1] = 0.0f;
dtVnormalize(dir);
dtVscale(ag->nvel, dir, ag->params.maxSpeed);
dtVcopy(ag->vel, ag->nvel);
dtVset(ag->dvel, 0, 0, 0);
}
}
}
void dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel,
float* nvel, const int gsize,
dtObstacleAvoidanceDebugData* debug)
{
prepare(pos, dvel);
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;
float minPenalty = FLT_MAX;
for (int y = 0; y < gsize; ++y)
{
for (int x = 0; x < gsize; ++x)
{
float vcand[3];
vcand[0] = cvx + x*cs - half;
vcand[1] = 0;
vcand[2] = cvz + y*cs - half;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
const float penalty = processSample(vcand, cs, pos,rad,vmax,vel,dvel, debug);
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(nvel, vcand);
}
}
}
}
void dtSeekBehavior::applyForce(const dtCrowdAgent* oldAgent, dtCrowdAgent* newAgent, float* force, float dt)
{
float tmpForce[] = {0, 0, 0};
float newVelocity[] = {0, 0, 0};
const dtCrowdAgent* target = m_agentParams[oldAgent->id]->targetID;
const float distance = m_agentParams[oldAgent->id]->seekDistance;
// Adapting the force to the dt and the previous velocity
dtVscale(tmpForce, force, dt);
dtVadd(newVelocity, oldAgent->vel, tmpForce);
float currentSpeed = dtVlen(oldAgent->vel);
// Required distance to reach nil speed according to the acceleration and current speed
float slowDist = currentSpeed * (currentSpeed - 0) / oldAgent->params.maxAcceleration;
float distToObj = dtVdist(oldAgent->npos, target->npos) - oldAgent->params.radius - target->params.radius - distance;
// If we have reached the target, we stop
if (distToObj <= EPSILON)
{
dtVset(newVelocity, 0, 0, 0);
newAgent->desiredSpeed = 0.f;
}
// If the have to slow down
else if (distToObj < slowDist)
{
float slowDownRadius = distToObj / slowDist;
dtVscale(newVelocity, newVelocity, slowDownRadius);
newAgent->desiredSpeed = dtVlen(newVelocity);
}
// Else, we move as fast as possible
else
newAgent->desiredSpeed = oldAgent->params.maxSpeed;
// Check for maximal speed
dtVclamp(newVelocity, dtVlen(newVelocity), oldAgent->params.maxSpeed);
dtVcopy(newAgent->dvel, newVelocity);
}
void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*3;
if (!ref)
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
m_nsegs = 0;
m_npolys = 0;
return;
}
dtVcopy(m_center, pos);
// First query non-overlapping polygons.
navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange,
filter, m_polys, 0, &m_npolys, MAX_LOCAL_POLYS);
// Secondly, store all polygon edges.
m_nsegs = 0;
float segs[MAX_SEGS_PER_POLY*6];
int nsegs = 0;
for (int j = 0; j < m_npolys; ++j)
{
navquery->getPolyWallSegments(m_polys[j], filter, segs, 0, &nsegs, MAX_SEGS_PER_POLY);
for (int k = 0; k < nsegs; ++k)
{
const float* s = &segs[k*6];
// Skip too distant segments.
float tseg;
const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg);
if (distSqr > dtSqr(collisionQueryRange))
continue;
addSegment(distSqr, s);
}
}
}
void 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,
dtObstacleAvoidanceDebugData* debug)
{
prepare(pos, dvel);
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];
int npat = 0;
const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS);
const float da = (1.0f/nd) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]);
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
for (int j = 0; j < nr; ++j)
{
const float rad = (float)(nr-j)/(float)nr;
float a = dang + (j&1)*0.5f*da;
for (int i = 0; i < nd; ++i)
{
pat[npat*2+0] = cosf(a)*rad;
pat[npat*2+1] = sinf(a)*rad;
npat++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f-m_velBias);
float res[3];
dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias);
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];
dtVset(bvel, 0,0,0);
for (int i = 0; i < npat; ++i)
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
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);
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(bvel, vcand);
}
}
dtVcopy(res, bvel);
cr *= 0.5f;
}
dtVcopy(nvel, res);
}
void dtCrowd::updateStepNextMovePoint(const float dt, dtCrowdAgentDebugInfo* debug)
{
const int debugIdx = debug ? debug->idx : -1;
// Find next corner to steer to.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
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
m_navquery->updateLinkFilter(ag->params.linkFilter);
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.filter], ag->params.radius);
const int agIndex = getAgentIndex(ag);
if (debugIdx == agIndex)
{
dtVset(debug->optStart, 0, 0, 0);
dtVset(debug->optEnd, 0, 0, 0);
}
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 1)
{
unsigned char allowedArea = DT_WALKABLE_AREA;
if (m_raycastSingleArea)
{
m_navquery->getAttachedNavMesh()->getPolyArea(ag->corridor.getFirstPoly(), &allowedArea);
m_raycastFilter.setAreaCost(allowedArea, 1.0f);
}
const int firstCheckedIdx = ag->ncorners - 1;
const int lastCheckedIdx = (ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS_MULTI) ? 1 : firstCheckedIdx;
for (int cornerIdx = firstCheckedIdx; cornerIdx >= lastCheckedIdx; cornerIdx--)
{
float* target = &ag->cornerVerts[cornerIdx * 3];
const bool bOptimized = ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery,
m_raycastSingleArea ? &m_raycastFilter : &m_filters[ag->params.filter]);
if (bOptimized)
{
// Copy data for debug purposes.
if (debugIdx == agIndex)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
break;
}
}
m_raycastFilter.setAreaCost(allowedArea, DT_UNWALKABLE_POLY_COST);
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
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;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = ag - m_agents;
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
m_navquery->updateLinkFilter(ag->params.linkFilter);
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
// UE4: m_keepOffmeshConnections support
const bool bCanStart = m_keepOffmeshConnections ?
ag->corridor.canMoveOverOffmeshConnection(ag->cornerPolys[ag->ncorners - 1], refs, ag->npos, anim->startPos, anim->endPos, m_navquery) :
ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners - 1], refs, ag->npos, anim->startPos, anim->endPos, m_navquery);
if (bCanStart)
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
}
void dtLocalBoundary::reset()
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
m_npolys = 0;
m_nsegs = 0;
}
/// @par
///
/// The output data array is allocated using the detour allocator (dtAlloc()). The method
/// used to free the memory will be determined by how the tile is added to the navigation
/// mesh.
///
/// @see dtNavMesh, dtNavMesh::addTile()
bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize)
{
if (params->nvp > DT_VERTS_PER_POLYGON)
return false;
if (params->vertCount >= 0xffff)
return false;
if (!params->vertCount || !params->verts)
return false;
if (!params->polyCount || !params->polys)
return false;
const int nvp = params->nvp;
// Classify off-mesh connection points. We store only the connections
// whose start point is inside the tile.
unsigned char* offMeshConClass = 0;
int storedOffMeshConCount = 0;
int offMeshConLinkCount = 0;
int storedOffMeshSegCount = 0;
if (params->offMeshConCount > 0)
{
offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP);
if (!offMeshConClass)
return false;
memset(offMeshConClass, 0, sizeof(unsigned char)*params->offMeshConCount*2);
// Find tight heigh bounds, used for culling out off-mesh start locations.
float hmin = FLT_MAX;
float hmax = -FLT_MAX;
for (int i = 0; i < params->vertCount; ++i)
{
const unsigned short* iv = ¶ms->verts[i*3];
const float h = params->bmin[1] + iv[1] * params->ch;
hmin = dtMin(hmin,h);
hmax = dtMax(hmax,h);
}
if (params->detailVerts && params->detailVertsCount)
{
for (int i = 0; i < params->detailVertsCount; ++i)
{
const float h = params->detailVerts[i*3+1];
hmin = dtMin(hmin,h);
hmax = dtMax(hmax,h);
}
}
hmin -= params->walkableClimb;
hmax += params->walkableClimb;
float bmin[3], bmax[3];
dtVcopy(bmin, params->bmin);
dtVcopy(bmax, params->bmax);
bmin[1] = hmin;
bmax[1] = hmax;
float bverts[3*4];
bverts[ 0] = bmin[0]; bverts[ 2] = bmin[2];
bverts[ 3] = bmax[0]; bverts[ 5] = bmin[2];
bverts[ 6] = bmax[0]; bverts[ 8] = bmax[2];
bverts[ 9] = bmin[0]; bverts[11] = bmax[2];
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
if (offMeshCon.type & DT_OFFMESH_CON_POINT)
{
offMeshConClass[i*2+0] = classifyOffMeshPoint(offMeshCon.vertsA0, bmin, bmax);
offMeshConClass[i*2+1] = classifyOffMeshPoint(offMeshCon.vertsB0, bmin, bmax);
// Zero out off-mesh start positions which are not even potentially touching the mesh.
if (offMeshConClass[i*2+0] == 0xff)
{
if ((offMeshCon.vertsA0[1] - offMeshCon.snapHeight) < bmin[1] &&
(offMeshCon.vertsA0[1] + offMeshCon.snapHeight) > bmax[1])
{
offMeshConClass[i * 2 + 0] = 0;
}
}
// Count how many links should be allocated for off-mesh connections.
if (offMeshConClass[i*2+0] == 0xff)
offMeshConLinkCount++;
if (offMeshConClass[i*2+1] == 0xff)
offMeshConLinkCount++;
if (offMeshConClass[i*2+0] == 0xff)
storedOffMeshConCount++;
}
else if (offMeshCon.type & DT_OFFMESH_CON_SEGMENT)
{
int smin, smax;
float tmin, tmax;
if ((offMeshCon.vertsA0[1] >= bmin[1] && offMeshCon.vertsA0[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsA0, bmin, bmax) == 0xff) ||
(offMeshCon.vertsA1[1] >= bmin[1] && offMeshCon.vertsA1[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsA1, bmin, bmax) == 0xff) ||
(offMeshCon.vertsB0[1] >= bmin[1] && offMeshCon.vertsB0[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsB0, bmin, bmax) == 0xff) ||
(offMeshCon.vertsB1[1] >= bmin[1] && offMeshCon.vertsB1[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsB1, bmin, bmax) == 0xff) ||
dtIntersectSegmentPoly2D(offMeshCon.vertsA0, offMeshCon.vertsA1, bverts, 4, tmin, tmax, smin, smax) ||
dtIntersectSegmentPoly2D(offMeshCon.vertsB0, offMeshCon.vertsB1, bverts, 4, tmin, tmax, smin, smax))
{
offMeshConClass[i*2] = 0xff;
storedOffMeshSegCount++;
}
}
}
}
// Off-mesh connections are stored as polygons, adjust values.
const int firstSegVert = params->vertCount + storedOffMeshConCount*2;
const int firstSegPoly = params->polyCount + storedOffMeshConCount;
const int totPolyCount = firstSegPoly + storedOffMeshSegCount*DT_MAX_OFFMESH_SEGMENT_PARTS;
const int totVertCount = firstSegVert + storedOffMeshSegCount*DT_MAX_OFFMESH_SEGMENT_PARTS*4;
// Find portal edges which are at tile borders.
int edgeCount = 0;
int portalCount = 0;
for (int i = 0; i < params->polyCount; ++i)
{
const unsigned short* p = ¶ms->polys[i*2*nvp];
for (int j = 0; j < nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
edgeCount++;
if (p[nvp+j] & 0x8000)
{
unsigned short dir = p[nvp+j] & 0xf;
if (dir != 0xf)
portalCount++;
}
}
}
// offmesh links will be added in dynamic array
const int maxLinkCount = edgeCount + portalCount*2;
// Find unique detail vertices.
int uniqueDetailVertCount = 0;
int detailTriCount = 0;
if (params->detailMeshes)
{
// Has detail mesh, count unique detail vertex count and use input detail tri count.
detailTriCount = params->detailTriCount;
for (int i = 0; i < params->polyCount; ++i)
{
const unsigned short* p = ¶ms->polys[i*nvp*2];
int ndv = params->detailMeshes[i*4+1];
int nv = 0;
for (int j = 0; j < nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
nv++;
}
ndv -= nv;
uniqueDetailVertCount += ndv;
}
}
else
{
// No input detail mesh, build detail mesh from nav polys.
uniqueDetailVertCount = 0; // No extra detail verts.
detailTriCount = 0;
for (int i = 0; i < params->polyCount; ++i)
{
const unsigned short* p = ¶ms->polys[i*nvp*2];
int nv = 0;
for (int j = 0; j < nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
nv++;
}
detailTriCount += nv-2;
}
}
// Calculate data size
const int headerSize = dtAlign4(sizeof(dtMeshHeader));
const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount);
const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount);
const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount);
const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount);
const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount);
const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount);
const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0;
const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount);
const int offMeshSegSize = dtAlign4(sizeof(dtOffMeshSegmentConnection)*storedOffMeshSegCount);
const int clustersSize = dtAlign4(sizeof(dtCluster)*params->clusterCount);
const int polyClustersSize = dtAlign4(sizeof(unsigned short)*params->polyCount);
const int dataSize = headerSize + vertsSize + polysSize + linksSize +
detailMeshesSize + detailVertsSize + detailTrisSize +
bvTreeSize + offMeshConsSize + offMeshSegSize +
clustersSize + polyClustersSize;
unsigned char* data = (unsigned char*)dtAlloc(sizeof(unsigned char)*dataSize, DT_ALLOC_PERM);
if (!data)
{
dtFree(offMeshConClass);
return false;
}
memset(data, 0, dataSize);
unsigned char* d = data;
dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize;
float* navVerts = (float*)d; d += vertsSize;
dtPoly* navPolys = (dtPoly*)d; d += polysSize;
d += linksSize;
dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
float* navDVerts = (float*)d; d += detailVertsSize;
unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize;
dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize;
dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize;
dtOffMeshSegmentConnection* offMeshSegs = (dtOffMeshSegmentConnection*)d; d += offMeshSegSize;
dtCluster* clusters = (dtCluster*)d; d += clustersSize;
unsigned short* polyClusters = (unsigned short*)d; d += polyClustersSize;
// Store header
header->magic = DT_NAVMESH_MAGIC;
header->version = DT_NAVMESH_VERSION;
header->x = params->tileX;
header->y = params->tileY;
header->layer = params->tileLayer;
header->userId = params->userId;
header->polyCount = totPolyCount;
header->vertCount = totVertCount;
header->maxLinkCount = maxLinkCount;
dtVcopy(header->bmin, params->bmin);
dtVcopy(header->bmax, params->bmax);
header->detailMeshCount = params->polyCount;
header->detailVertCount = uniqueDetailVertCount;
header->detailTriCount = detailTriCount;
header->bvQuantFactor = 1.0f / params->cs;
header->offMeshBase = params->polyCount;
header->offMeshSegPolyBase = firstSegPoly;
header->offMeshSegVertBase = firstSegVert;
header->walkableHeight = params->walkableHeight;
header->walkableRadius = params->walkableRadius;
header->walkableClimb = params->walkableClimb;
header->offMeshConCount = storedOffMeshConCount;
header->offMeshSegConCount = storedOffMeshSegCount;
header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0;
header->clusterCount = params->clusterCount;
const int offMeshVertsBase = params->vertCount;
const int offMeshPolyBase = params->polyCount;
// Store vertices
// Mesh vertices
for (int i = 0; i < params->vertCount; ++i)
{
const unsigned short* iv = ¶ms->verts[i*3];
float* v = &navVerts[i*3];
v[0] = params->bmin[0] + iv[0] * params->cs;
v[1] = params->bmin[1] + iv[1] * params->ch;
v[2] = params->bmin[2] + iv[2] * params->cs;
}
// Off-mesh point link vertices.
int n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
// Only store connections which start from this tile.
if ((offMeshConClass[i*2+0] == 0xff) && (offMeshCon.type & DT_OFFMESH_CON_POINT))
{
float* v = &navVerts[(offMeshVertsBase + n*2)*3];
dtVcopy(&v[0], &offMeshCon.vertsA0[0]);
dtVcopy(&v[3], &offMeshCon.vertsB0[0]);
n++;
}
}
// Store polygons
// Mesh polys
const unsigned short* src = params->polys;
for (int i = 0; i < params->polyCount; ++i)
{
dtPoly* p = &navPolys[i];
p->vertCount = 0;
p->flags = params->polyFlags[i];
p->setArea(params->polyAreas[i]);
p->setType(DT_POLYTYPE_GROUND);
for (int j = 0; j < nvp; ++j)
{
if (src[j] == MESH_NULL_IDX) break;
p->verts[j] = src[j];
if (src[nvp+j] & 0x8000)
{
// Border or portal edge.
unsigned short dir = src[nvp+j] & 0xf;
if (dir == 0xf) // Border
p->neis[j] = 0;
else if (dir == 0) // Portal x-
p->neis[j] = DT_EXT_LINK | 4;
else if (dir == 1) // Portal z+
p->neis[j] = DT_EXT_LINK | 2;
else if (dir == 2) // Portal x+
p->neis[j] = DT_EXT_LINK | 0;
else if (dir == 3) // Portal z-
p->neis[j] = DT_EXT_LINK | 6;
}
else
{
// Normal connection
p->neis[j] = src[nvp+j]+1;
}
p->vertCount++;
}
src += nvp*2;
}
// Off-mesh point connection polygons.
n = 0;
int nseg = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
if (offMeshCon.type & DT_OFFMESH_CON_POINT)
{
dtPoly* p = &navPolys[offMeshPolyBase+n];
p->vertCount = 2;
p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0);
p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1);
p->flags = offMeshCon.polyFlag;
p->setArea(offMeshCon.area);
p->setType(DT_POLYTYPE_OFFMESH_POINT);
n++;
}
else
{
for (int j = 0; j < DT_MAX_OFFMESH_SEGMENT_PARTS; j++)
{
dtPoly* p = &navPolys[firstSegPoly+nseg];
p->vertCount = 0;
p->flags = offMeshCon.polyFlag;
p->setArea(offMeshCon.area);
p->setType(DT_POLYTYPE_OFFMESH_SEGMENT);
nseg++;
}
}
}
}
// Store detail meshes and vertices.
// The nav polygon vertices are stored as the first vertices on each mesh.
// We compress the mesh data by skipping them and using the navmesh coordinates.
if (params->detailMeshes)
{
unsigned short vbase = 0;
for (int i = 0; i < params->polyCount; ++i)
{
dtPolyDetail& dtl = navDMeshes[i];
const int vb = (int)params->detailMeshes[i*4+0];
const int ndv = (int)params->detailMeshes[i*4+1];
const int nv = navPolys[i].vertCount;
dtl.vertBase = (unsigned int)vbase;
dtl.vertCount = (unsigned char)(ndv-nv);
dtl.triBase = (unsigned int)params->detailMeshes[i*4+2];
dtl.triCount = (unsigned char)params->detailMeshes[i*4+3];
// Copy vertices except the first 'nv' verts which are equal to nav poly verts.
if (ndv-nv)
{
memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv));
vbase += (unsigned short)(ndv-nv);
}
}
// Store triangles.
memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount);
}
else
{
// Create dummy detail mesh by triangulating polys.
int tbase = 0;
for (int i = 0; i < params->polyCount; ++i)
{
dtPolyDetail& dtl = navDMeshes[i];
const int nv = navPolys[i].vertCount;
dtl.vertBase = 0;
dtl.vertCount = 0;
dtl.triBase = (unsigned int)tbase;
dtl.triCount = (unsigned char)(nv-2);
// Triangulate polygon (local indices).
for (int j = 2; j < nv; ++j)
{
unsigned char* t = &navDTris[tbase*4];
t[0] = 0;
t[1] = (unsigned char)(j-1);
t[2] = (unsigned char)j;
// Bit for each edge that belongs to poly boundary.
t[3] = (1<<2);
if (j == 2) t[3] |= (1<<0);
if (j == nv-1) t[3] |= (1<<4);
tbase++;
}
}
}
// Store and create BVtree.
if (params->buildBvTree)
{
createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, nvp,
navDMeshes, navDVerts, navDTris, params->bmin,
params->cs, params->ch, params->polyCount*2, navBvtree);
}
// Store Off-Mesh connections.
n = 0;
nseg = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
if (offMeshCon.type & DT_OFFMESH_CON_POINT)
{
dtOffMeshConnection* con = &offMeshCons[n];
con->poly = (unsigned short)(offMeshPolyBase + n);
// Copy connection end-points.
dtVcopy(&con->pos[0], &offMeshCon.vertsA0[0]);
dtVcopy(&con->pos[3], &offMeshCon.vertsB0[0]);
con->rad = offMeshCon.snapRadius;
con->height = offMeshCon.snapHeight;
con->setFlags(offMeshCon.type);
con->side = offMeshConClass[i*2+1] == 0xff ? DT_CONNECTION_INTERNAL : offMeshConClass[i*2+1];
if (offMeshCon.userID)
con->userId = offMeshCon.userID;
n++;
}
else
{
dtOffMeshSegmentConnection* con = &offMeshSegs[nseg];
dtVcopy(con->startA, &offMeshCon.vertsA0[0]);
dtVcopy(con->endA, &offMeshCon.vertsA1[0]);
dtVcopy(con->startB, &offMeshCon.vertsB0[0]);
dtVcopy(con->endB, &offMeshCon.vertsB1[0]);
con->rad = offMeshCon.snapRadius;
con->height = offMeshCon.snapHeight;
con->setFlags(offMeshCon.type);
if (offMeshCon.userID)
con->userId = offMeshCon.userID;
nseg++;
}
}
}
dtFree(offMeshConClass);
// Store clusters
if (params->polyClusters)
{
memcpy(polyClusters, params->polyClusters, sizeof(unsigned short)*params->polyCount);
}
for (int i = 0; i < params->clusterCount; i++)
{
dtCluster& cluster = clusters[i];
cluster.firstLink = DT_NULL_LINK;
cluster.numLinks = 0;
dtVset(cluster.center, 0.f, 0.f, 0.f);
// calculate center point: take from first poly
for (int j = 0; j < params->polyCount; j++)
{
if (polyClusters[j] != i)
{
continue;
}
const dtPoly* poly = &navPolys[j];
float c[3] = { 0.0f, 0.0f, 0.0f };
for (int iv = 0; iv < poly->vertCount; iv++)
{
dtVadd(c, c, &navVerts[poly->verts[iv] * 3]);
}
dtVmad(cluster.center, cluster.center, c, 1.0f / poly->vertCount);
break;
}
}
*outData = data;
*outDataSize = dataSize;
return true;
}
dtLocalBoundary::dtLocalBoundary() :
m_nsegs(0),
m_npolys(0)
{
dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
}
void dtCrowd::updateStepMove(const float dt, dtCrowdAgentDebugInfo*)
{
// Integrate.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0, 0, 0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = sqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// m_activeAgents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2], 0, ag->dvel[0]);
else
dtVset(diff, ag->dvel[2], 0, -ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
}
/// @par
///
/// May be called more than once to purge and re-initialize the crowd.
bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav)
{
purge();
m_maxAgents = maxAgents;
m_maxAgentRadius = maxAgentRadius;
m_numActiveAgents = 0;
dtVset(m_ext, m_maxAgentRadius*2.0f,m_maxAgentRadius*1.5f,m_maxAgentRadius*2.0f);
m_grid = dtAllocProximityGrid();
if (!m_grid)
return false;
if (!m_grid->init(m_maxAgents*4, maxAgentRadius*3))
return false;
// [UE4] moved avoidance query init to separate function
// Allocate temp buffer for merging paths.
m_maxPathResult = 256;
m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM);
if (!m_pathResult)
return false;
if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav))
return false;
m_agents = (dtCrowdAgent*)dtAlloc(sizeof(dtCrowdAgent)*m_maxAgents, DT_ALLOC_PERM);
if (!m_agents)
return false;
m_activeAgents = (dtCrowdAgent**)dtAlloc(sizeof(dtCrowdAgent*)*m_maxAgents, DT_ALLOC_PERM);
if (!m_activeAgents)
return false;
m_agentAnims = (dtCrowdAgentAnimation*)dtAlloc(sizeof(dtCrowdAgentAnimation)*m_maxAgents, DT_ALLOC_PERM);
if (!m_agentAnims)
return false;
for (int i = 0; i < m_maxAgents; ++i)
{
new(&m_agents[i]) dtCrowdAgent();
m_agents[i].active = 0;
if (!m_agents[i].corridor.init(m_maxPathResult))
return false;
}
for (int i = 0; i < m_maxAgents; ++i)
{
m_agentAnims[i].active = 0;
}
for (int i = 0; i < DT_MAX_AREAS; i++)
{
m_raycastFilter.setAreaCost(i, DT_UNWALKABLE_POLY_COST);
}
// The navquery is mostly used for local searches, no need for large node pool.
m_navquery = dtAllocNavMeshQuery();
if (!m_navquery)
return false;
if (dtStatusFailed(m_navquery->init(nav, MAX_COMMON_NODES)))
return false;
return true;
}
/// @par
///
/// May be called more than once to purge and re-initialize the crowd.
bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* nav)
{
purge();
m_maxAgents = maxAgents;
m_maxAgentRadius = maxAgentRadius;
dtVset(m_ext, m_maxAgentRadius*2.0f,m_maxAgentRadius*1.5f,m_maxAgentRadius*2.0f);
m_grid = dtAllocProximityGrid();
if (!m_grid)
return false;
if (!m_grid->init(m_maxAgents*4, maxAgentRadius*3))
return false;
m_obstacleQuery = dtAllocObstacleAvoidanceQuery();
if (!m_obstacleQuery)
return false;
if (!m_obstacleQuery->init(6, 8))
return false;
// 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;
m_pathResult = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathResult, DT_ALLOC_PERM);
if (!m_pathResult)
return false;
if (!m_pathq.init(m_maxPathResult, MAX_PATHQUEUE_NODES, nav))
return false;
m_agents = (dtCrowdAgent*)dtAlloc(sizeof(dtCrowdAgent)*m_maxAgents, DT_ALLOC_PERM);
if (!m_agents)
return false;
m_activeAgents = (dtCrowdAgent**)dtAlloc(sizeof(dtCrowdAgent*)*m_maxAgents, DT_ALLOC_PERM);
if (!m_activeAgents)
return false;
m_agentAnims = (dtCrowdAgentAnimation*)dtAlloc(sizeof(dtCrowdAgentAnimation)*m_maxAgents, DT_ALLOC_PERM);
if (!m_agentAnims)
return false;
for (int i = 0; i < m_maxAgents; ++i)
{
new(&m_agents[i]) dtCrowdAgent();
m_agents[i].active = 0;
if (!m_agents[i].corridor.init(m_maxPathResult))
return false;
}
for (int i = 0; i < m_maxAgents; ++i)
{
m_agentAnims[i].active = 0;
}
// The navquery is mostly used for local searches, no need for large node pool.
m_navquery = dtAllocNavMeshQuery();
if (!m_navquery)
return false;
if (dtStatusFailed(m_navquery->init(nav, MAX_COMMON_NODES)))
return false;
return true;
}
void dtCrowd::updatePosition(const float dt, unsigned* agentsIdx, unsigned nbIdx)
{
// If we want to update every agent
if (nbIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// The current start position of the agent (not yet modified)
dtPolyRef* currentPosPoly = (dtPolyRef*) dtAlloc(sizeof(dtPolyRef) * nbIdx, DT_ALLOC_TEMP);
float* currentPos = (float*) dtAlloc(sizeof(float) * 3 * nbIdx, DT_ALLOC_TEMP);
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), currentPosPoly + i, currentPos + (i * 3));
}
// Integrate.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (unsigned iter = 0; iter < 4; ++iter)
{
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVset(disp, 0, 0, 0);
float w = 0;
for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; ++j)
{
const dtCrowdAgent* nei = &m_agents[m_agentsEnv[ag->id].neighbors[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->position, nei->position);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->radius + nei->radius) + EPSILON)
continue;
dist = sqrtf(dist);
float pen = (ag->radius + nei->radius) - dist;
if (dist < EPSILON)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->desiredVelocity[2], 0, ag->desiredVelocity[0]);
else
dtVset(diff, ag->desiredVelocity[2], 0, -ag->desiredVelocity[0]);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(disp, disp, diff, pen);
w += 1.0f;
}
if (w > EPSILON)
{
const float iw = 1.0f / w;
dtVscale(disp, disp, iw);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVadd(ag->position, ag->position, disp);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
float newPos[3];
dtPolyRef visited[dtPathCorridor::MAX_VISITED];
int visitedCount;
m_crowdQuery->getNavMeshQuery()->moveAlongSurface(currentPosPoly[i], currentPos + (i * 3), ag->position, m_crowdQuery->getQueryFilter(), newPos,
visited, &visitedCount, dtPathCorridor::MAX_VISITED);
// Get valid constrained position back.
float newHeight = *(currentPos + (i * 3) + 1);
m_crowdQuery->getNavMeshQuery()->getPolyHeight(currentPosPoly[i], newPos, &newHeight);
newPos[1] = newHeight;
dtVcopy(ag->position, newPos);
}
// Update agents using off-mesh connection.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
float offmeshTotalTime = ag->offmeshInitToStartTime + ag->offmeshStartToEndTime;
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH && offmeshTotalTime > EPSILON)
{
ag->offmeshElaspedTime += dt;
if (ag->offmeshElaspedTime > offmeshTotalTime)
{
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
if (ag->offmeshElaspedTime < ag->offmeshInitToStartTime)
{
const float u = tween(ag->offmeshElaspedTime, 0.0, ag->offmeshInitToStartTime);
dtVlerp(ag->position, ag->offmeshInitPos, ag->offmeshStartPos, u);
}
else
{
const float u = tween(ag->offmeshElaspedTime, ag->offmeshInitToStartTime, offmeshTotalTime);
dtVlerp(ag->position, ag->offmeshStartPos, ag->offmeshEndPos, u);
}
// Update velocity.
dtVset(ag->velocity, 0,0,0);
dtVset(ag->desiredVelocity, 0,0,0);
}
}
dtFree(currentPosPoly);
dtFree(currentPos);
}
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
const int debugIdx = debug ? debug->idx : -1;
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
updateMoveRequest(dt);
// Optimize path topology.
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents
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.
for (int i = 0; i < nagents; ++i)
{
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;
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
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->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
}
else
{
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVset(debug->optStart, 0,0,0);
dtVset(debug->optEnd, 0,0,0);
}
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < nagents; ++i)
{
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;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = (int)(ag - m_agents);
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
anim->startPos, anim->endPos, m_navquery))
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
// Calculate steering.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
else
{
// Calculate steering direction.
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->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)
{
const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
float w = 0;
float disp[3] = {0,0,0};
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
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 = dtMathSqrtf(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.
dtVcopy(ag->dvel, dvel);
}
// Velocity planning.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s+3);
}
dtObstacleAvoidanceDebugData* vod = 0;
if (debugIdx == i)
vod = debug->vod;
// Sample new safe velocity.
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
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->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
// Integrate.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0,0,0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = dtMathSqrtf(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.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
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.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgentAnimation* anim = &m_agentAnims[i];
if (!anim->active)
continue;
dtCrowdAgent* ag = agents[i];
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
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 = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
if (debug)
debug->reset();
// Build sampling pattern aligned to desired velocity.
float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
int npat = 0;
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 int nd2 = nd / 2;
const float da = (1.0f/nd) * DT_PI*2;
const float ca = cosf(da);
const float sa = sinf(da);
// desired direction
float ddir[6];
dtVcopy(ddir, dvel);
dtNormalize2D(ddir);
dtRorate2D (ddir+3, ddir, da*0.5f); // rotated by da/2
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
for (int j = 0; j < nr; ++j)
{
const float r = (float)(nr-j)/(float)nr;
pat[npat*2+0] = ddir[(j%1)*3] * r;
pat[npat*2+1] = ddir[(j%1)*3+2] * r;
float* last1 = pat + npat*2;
float* last2 = last1;
npat++;
for (int i = 1; i < nd-1; i+=2)
{
// get next point on the "right" (rotate CW)
pat[npat*2+0] = last1[0]*ca + last1[1]*sa;
pat[npat*2+1] = -last1[0]*sa + last1[1]*ca;
// get next point on the "left" (rotate CCW)
pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
last1 = pat + npat*2;
last2 = last1 + 2;
npat += 2;
}
if ((nd&1) == 0)
{
pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
npat++;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float res[3];
dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];
dtVset(bvel, 0,0,0);
for (int i = 0; i < npat; ++i)
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(bvel, vcand);
}
}
dtVcopy(res, bvel);
cr *= 0.5f;
}
dtVcopy(nvel, res);
return ns;
}
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
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();
// Build sampling pattern aligned to desired velocity.
float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
int npat = 0;
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]);
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
for (int j = 0; j < nr; ++j)
{
const float r = (float)(nr-j)/(float)nr;
float a = dang + (j&1)*0.5f*da;
for (int i = 0; i < nd; ++i)
{
pat[npat*2+0] = cosf(a)*r;
pat[npat*2+1] = sinf(a)*r;
npat++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float res[3];
dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];
dtVset(bvel, 0,0,0);
for (int i = 0; i < npat; ++i)
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(bvel, vcand);
}
}
dtVcopy(res, bvel);
cr *= 0.5f;
}
dtVcopy(nvel, res);
return ns;
}
