public int SampleVelocityGrid(float[] pos, float rad, float vmax, float[] vel, float[] dvel, ref float[] nvel,
ObstacleAvoidanceParams param, ObstacleAvoidanceDebugData debug = null)
{
Prepare(pos, dvel);
_params = new ObstacleAvoidanceParams(param);
_invHorizTime = 1.0f / _params.horizTime;
_vmax = vmax;
_invVmax = 1.0f / vmax;
Helper.VSet(ref nvel, 0, 0, 0);
if (debug != null)
{
debug.Reset();
}
float cvx = dvel[0] * _params.velBias;
float cvz = dvel[2] * _params.velBias;
float cs = vmax * 2 * (1 - _params.velBias) / (float)(_params.gridSize - 1);
float half = (_params.gridSize - 1) * cs * 0.5f;
float minPenalty = float.MaxValue;
int ns = 0;
for (int y = 0; y < _params.gridSize; y++)
{
for (int x = 0; x < _params.gridSize; x++)
{
float[] vcand = new float[3];
vcand[0] = cvx + x * cs - half;
vcand[1] = 0;
vcand[2] = cvz + y * cs - half;
if (vcand[0] * vcand[0] + vcand[2] * vcand[2] > (vmax + cs / 2) * (vmax + cs / 2))
{
continue;
}
float penalty = ProcessSample(vcand, cs, pos, rad, vel, dvel, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
Helper.VCopy(ref nvel, vcand);
}
}
}
return(ns);
}
public int SampleVelocityAdaptive(float[] pos, float rad, float vmax, float[] vel, float[] dvel,
ref float[] nvel, ObstacleAvoidanceParams param,
ObstacleAvoidanceDebugData debug = null)
{
Prepare(pos, dvel);
_params = new ObstacleAvoidanceParams(param);
_invHorizTime = 1.0f / _params.horizTime;
_vmax = vmax;
_invVmax = 1.0f / vmax;
Helper.VSet(ref nvel, 0, 0, 0);
if (debug != null)
{
debug.Reset();
}
float[] pat = new float[(MaxPatternDivs * MaxPatternRings + 1) * 2];
int npat = 0;
int ndivs = _params.adaptiveDivs;
int nrings = _params.adaptiveRings;
int depth = _params.adaptiveDepth;
int nd = Math.Max(1, Math.Min(MaxPatternDivs, ndivs));
int nr = Math.Max(1, Math.Min(MaxPatternRings, nrings));
float da = (1.0f / nd) * (float)Math.PI * 2f;
float dang = (float)Math.Atan2(dvel[2], dvel[0]);
pat[npat * 2 + 0] = 0;
pat[npat * 2 + 1] = 0;
npat++;
for (int j = 0; j < nr; j++)
{
float r = (nr - j) / (float)nr;
float a = dang + (j & 1) * 0.5f * da;
for (int i = 0; i < nd; i++)
{
pat[npat * 2 + 0] = (float)Math.Cos(a) * r;
pat[npat * 2 + 1] = (float)Math.Sin(a) * r;
npat++;
a += da;
}
}
float cr = vmax * (1.0f - _params.velBias);
float[] res = new float[3];
Helper.VSet(ref res, dvel[0] * _params.velBias, 0, dvel[2] * _params.velBias);
int ns = 0;
for (int k = 0; k < depth; k++)
{
float minPenalty = float.MaxValue;
float[] bvel = new float[3];
Helper.VSet(ref bvel, 0, 0, 0);
for (int i = 0; i < npat; i++)
{
float[] vcand = { res[0] + pat[i * 2 + 0] * cr, 0, res[2] + pat[i * 2 + 1] * cr };
if (vcand[0] * vcand[0] + vcand[2] * vcand[2] > (vmax + 0.001f) * (vmax + 0.001f))
{
continue;
}
float penalty = ProcessSample(vcand, cr / 10f, pos, rad, vel, dvel, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
Helper.VCopy(ref bvel, vcand);
}
}
Helper.VCopy(ref res, bvel);
cr *= 0.5f;
}
Helper.VCopy(ref nvel, res);
return(ns);
}
