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);
}
public virtual int sampleVelocityAdaptive(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();
}
// Build sampling pattern aligned to desired velocity.
//float[] pat = new float[(DT_MAX_PATTERN_DIVS * DT_MAX_PATTERN_RINGS + 1) * 2];
DetourCommon.vResetArray(svapat);
int npat = 0;
int ndivs = m_params.adaptiveDivs;
int nrings = m_params.adaptiveRings;
int depth = m_params.adaptiveDepth;
int nd = DetourCommon.clamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
int nr = DetourCommon.clamp(nrings, 1, DT_MAX_PATTERN_RINGS);
//int nd2 = nd / 2;
float da = (1.0f / nd) * DT_PI * 2;
float ca = (float)Math.Cos(da);
float sa = (float)Math.Sin(da);
// desired direction
//float[] ddir = new float[6];
DetourCommon.vResetArray(svaddir);
DetourCommon.vCopy(svaddir, dvel);
dtNormalize2D(svaddir);
//float[] rotated = dtRotate2D(ddir, da * 0.5f); // rotated by da/2
dtRotate2D(svarotated, svaddir, da * 0.5f);
svaddir[3] = svarotated[0];
svaddir[4] = svarotated[1];
svaddir[5] = svarotated[2];
// Always add sample at zero
svapat[npat * 2 + 0] = 0;
svapat[npat * 2 + 1] = 0;
npat++;
float r;
int last1, last2;
for (int j = 0; j < nr; ++j)
{
r = (float)(nr - j) / (float)nr;
svapat[npat * 2 + 0] = svaddir[(j % 2) * 3] * r;
svapat[npat * 2 + 1] = svaddir[(j % 2) * 3 + 2] * r;
last1 = npat * 2;
last2 = last1;
npat++;
for (int i = 1; i < nd - 1; i += 2)
{
// get next point on the "right" (rotate CW)
svapat[npat * 2 + 0] = svapat[last1] * ca + svapat[last1 + 1] * sa;
svapat[npat * 2 + 1] = -svapat[last1] * sa + svapat[last1 + 1] * ca;
// get next point on the "left" (rotate CCW)
svapat[npat * 2 + 2] = svapat[last2] * ca - svapat[last2 + 1] * sa;
svapat[npat * 2 + 3] = svapat[last2] * sa + svapat[last2 + 1] * ca;
last1 = npat * 2;
last2 = last1 + 2;
npat += 2;
}
if ((nd & 1) == 0)
{
svapat[npat * 2 + 2] = svapat[last2] * ca - svapat[last2 + 1] * sa;
svapat[npat * 2 + 3] = svapat[last2] * sa + svapat[last2 + 1] * ca;
npat++;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
//float[] res = new float[3];
DetourCommon.vSet(svares, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
float minPenalty;
float penalty;
for (int k = 0; k < depth; ++k)
{
minPenalty = float.MaxValue;
//float[] bvel = new float[3];
DetourCommon.vSet(svabvel, 0, 0, 0);
for (int i = 0; i < npat; ++i)
{
//float[] vcand = new float[3];
svavcand[0] = svares[0] + svapat[i * 2 + 0] * cr;
svavcand[1] = 0f;
svavcand[2] = svares[2] + svapat[i * 2 + 1] * cr;
if (DetourCommon.sqr(svavcand[0]) + DetourCommon.sqr(svavcand[2]) > DetourCommon.sqr(vmax + 0.001f))
{
continue;
}
penalty = processSample(svavcand, cr / 10, pos, rad, vel, dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
DetourCommon.vCopy(svabvel, svavcand);
}
}
DetourCommon.vCopy(svares, svabvel);
cr *= 0.5f;
}
DetourCommon.vCopy(nvel, svares);
return(ns);
}
/// <summary>
/// Calculate the collision penalty for a given velocity vector
/// </summary>
/// <param name="vcand"> sampled velocity </param>
/// <param name="dvel"> desired velocity </param>
/// <param name="minPenalty"> threshold penalty for early out </param>
public virtual float processSample(float[] vcand, float cs, float[] pos, float rad, float[] vel, float[] dvel, float minPenalty, ObstacleAvoidanceDebugData debug)
{
// penalty for straying away from the desired and current velocities
float vpen = m_params.weightDesVel * (DetourCommon.vDist2D(vcand, dvel) * m_invVmax);
float vcpen = m_params.weightCurVel * (DetourCommon.vDist2D(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 = (float)(((double)m_params.weightToi / (double)minPen - 0.1) * m_params.horizTime);
if (tThresold - m_params.horizTime > -float.Epsilon)
{
return(minPenalty); // already too much
}
// Find min time of impact and exit amongst all obstacles.
float tmin = m_params.horizTime;
float side = 0;
int nside = 0;
ObstacleCircle cir;
//RefFloat htmin = new RefFloat();
//RefFloat htmax = new RefFloat();
for (int i = 0; i < m_ncircles; ++i)
{
cir = m_circles[i];
// RVO
//float[] vab = vScale(vcand, 2);
//vab = vSub(vab, vel);
//vab = vSub(vab, cir.vel);
DetourCommon.vScale(psvab, vcand, 2);
DetourCommon.vSub(psvab, psvab, vel);
DetourCommon.vSub(psvab, psvab, cir.vel);
// Side
side += DetourCommon.clamp(Math.Min(DetourCommon.vDot2D(cir.dp, psvab) * 0.5f + 0.5f, DetourCommon.vDot2D(cir.np, psvab) * 2), 0.0f, 1.0f);
nside++;
float htmin = 0;
float htmax = 0;
if (!sweepCircleCircle(pos, rad, psvab, cir.p, cir.rad, ref htmin, ref htmax))
{
continue;
}
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
{
return(minPenalty);
}
}
}
}
ObstacleSegment seg;
for (int i = 0; i < m_nsegments; ++i)
{
seg = m_segments[i];
float htmin = 0;
if (seg.touch)
{
// Special case when the agent is very close to the segment.
//float[] sdir = vSub(seg.q, seg.p);
//float[] snorm = new float[3];
DetourCommon.vSub(pssdir, seg.q, seg.p);
DetourCommon.vSet(pssnorm, 0, 0, 0);
pssnorm[0] = -pssdir[2];
pssnorm[2] = pssdir[0];
// If the velocity is pointing towards the segment, no collision.
if (DetourCommon.vDot2D(pssnorm, vcand) < 0.0001f)
{
continue;
}
// Else immediate collision.
htmin = 0.0f;
}
else
{
if (!isectRaySeg(pos, vcand, seg.p, seg.q, ref htmin))
{
continue;
}
}
// Avoid less when facing walls.
htmin = htmin * 2.0f;
// 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 != 0)
{
side /= nside;
}
float spen = m_params.weightSide * side;
float tpen = m_params.weightToi * (1.0f / (0.1f + tmin * m_invHorizTime));
float penalty = vpen + vcpen + spen + tpen;
// Store different penalties for debug viewing
if (debug != null)
{
debug.addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
}
return(penalty);
}