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

Merge pull request #42 from axelrodR/master

Browse Source 
Detour crowd optmizations
This commit is contained in:
Mikko Mononen 2014-11-09 19:51:36 +02:00
commit 2d474bc595
2 changed files with 89 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
DetourCrowd/Include/DetourObstacleAvoidance.h
View File

@ -128,6 +128,7 @@ private:
float processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float* vel, const float* dvel,
const float minPenalty,
dtObstacleAvoidanceDebugData* debug);
dtObstacleCircle* insertCircle(const float dist);

101
DetourCrowd/Source/DetourObstacleAvoidance.cpp
View File

@ -311,11 +311,30 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
}
}
/* Calculate the collision penalty for a given velocity vector
*
* @param vcand sampled velocity
* @param dvel desired velocity
* @param minPenalty threshold penalty for early out
*/
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float* vel, const float* dvel,
const float minPenalty,
dtObstacleAvoidanceDebugData* debug)
{
// penalty for straying away from the desired and current velocities
const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
// find the threshold hit time to bail out based on the early out penalty
// (see how the penalty is calculated below to understnad)
float minPen = minPenalty - vpen - vcpen;
float tThresold = ((double)m_params.weightToi/(double)minPen - 0.1) * (double)m_params.horizTime;
if (tThresold - m_params.horizTime > -FLT_EPSILON)
return minPenalty; // already too much
// Find min time of impact and exit amongst all obstacles.
float tmin = m_params.horizTime;
float side = 0;
@ -350,7 +369,11 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
}
@ -383,15 +406,17 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
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));
@ -414,7 +439,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
@ -440,7 +465,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,vel,dvel, debug);
const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
@ -454,6 +479,28 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
}
// vector normalization that ignores the y-component.
inline void dtNormalize2D(float* v)
{
float d = dtSqrt(v[0]*v[0]+v[2]*v[2]);
if (d==0)
return;
d = 1.0f / d;
v[0] *= d;
v[2] *= d;
}
// vector normalization that ignores the y-component.
inline void dtRorate2D(float* dest, const float* v, float ang)
{
float c = cosf(ang);
float s = sinf(ang);
dest[0] = v[0]*c - v[2]*s;
dest[2] = v[0]*s + v[2]*c;
dest[1] = v[1];
}
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel,
const dtObstacleAvoidanceParams* params,
@ -464,7 +511,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
@ -481,8 +528,16 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
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 dang = dtMathAtan2f(dvel[2], dvel[0]);
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;
@ -492,16 +547,35 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
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;
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++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float res[3];
@ -523,7 +597,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,vel,dvel, debug);
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
@ -541,4 +615,3 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
return ns;
}