public virtual int sampleVelocityGrid(float[] pos, float rad, float vmax, float[] vel, float[] dvel, float[] nvel, ObstacleAvoidanceParams @params, ObstacleAvoidanceDebugData debug)
{
prepare(pos, dvel, rad);
m_params = @params;
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue;
//float[] nvel = new float[3];
DetourCommon.vSet(nvel, 0f, 0f, 0f);
if (debug != null)
{
debug.reset();
}
float cvx = dvel[0] * m_params.velBias;
float cvz = dvel[2] * m_params.velBias;
float cs = vmax * 2 * (1 - m_params.velBias) / (m_params.gridSize - 1);
float half = (m_params.gridSize - 1) * cs * 0.5f;
float minPenalty = float.MaxValue;
int ns = 0;
for (int y = 0; y < m_params.gridSize; ++y)
{
for (int x = 0; x < m_params.gridSize; ++x)
{
//float[] vcand = new float[3];
svgvcand[0] = cvx + x * cs - half;
svgvcand[1] = 0f;
svgvcand[2] = cvz + y * cs - half;
if (DetourCommon.sqr(svgvcand[0]) + DetourCommon.sqr(svgvcand[2]) > DetourCommon.sqr(vmax + cs / 2))
{
continue;
}
float penalty = processSample(svgvcand, cs, pos, rad, vel, dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
DetourCommon.vCopy(nvel, svgvcand);
}
}
}
return(ns);
}