public void AddSegment(float[] p, float[] q)
{
if (_nSegments >= _maxSegments)
{
return;
}
ObstacleSegment seg = _segments[_nSegments++];
Helper.VCopy(ref seg.p, p);
Helper.VCopy(ref seg.q, q);
}
public bool Init(int maxCircles, int maxSegments)
{
_maxCircles = maxCircles;
_nCircles = 0;
_circles = new ObstacleCircle[_maxCircles];
for (int i = 0; i < _maxCircles; i++)
{
_circles[i] = new ObstacleCircle();
}
_maxSegments = maxSegments;
_nSegments = 0;
_segments = new ObstacleSegment[_maxSegments];
for (int i = 0; i < _maxSegments; i++)
{
_segments[i] = new ObstacleSegment();
}
return(true);
}
private void Prepare(float[] pos, float[] dvel)
{
for (int i = 0; i < _nCircles; i++)
{
ObstacleCircle cir = _circles[i];
float[] pa = pos;
float[] pb = cir.p;
float[] orig = new float[3];
orig[0] = 0;
orig[1] = 0;
float[] dv = new float[3];
cir.dp = Helper.VSub(pb[0], pb[1], pb[2], pa[0], pa[1], pa[2]);
Helper.VNormalize(ref cir.dp);
dv = Helper.VSub(cir.dvel[0], cir.dvel[1], cir.dvel[2], dvel[0], dvel[1], dvel[2]);
float a = Helper.TriArea2D(orig, cir.dp, dv);
if (a < 0.01f)
{
cir.np[0] = -cir.dp[2];
cir.np[2] = cir.dp[0];
}
else
{
cir.np[0] = cir.dp[2];
cir.np[2] = -cir.dp[0];
}
}
for (int i = 0; i < _nSegments; i++)
{
ObstacleSegment seg = _segments[i];
float r = 0.01f;
float t;
seg.touch = Helper.DistancePtSegSqr2D(pos[0], pos[1], pos[2], seg.p[0], seg.p[1], seg.p[2], seg.q[0], seg.q[1], seg.q[2], out t) < r * r;
}
}
private float ProcessSample(float[] vcand, float cs, float[] pos, float rad, float[] vel, float[] dvel, ObstacleAvoidanceDebugData debug = null)
{
float tmin = _params.horizTime;
float side = 0;
int nside = 0;
for (int i = 0; i < _nCircles; i++)
{
ObstacleCircle cir = _circles[i];
float[] vab = Helper.VScale(vcand[0], vcand[1], vcand[2], 2);
vab = Helper.VSub(vab[0], vab[1], vab[2], vel[0], vel[1], vel[2]);
side += (float)Math.Max(0.0, Math.Min(1.0, Math.Min(Helper.VDot2D(cir.dp, vab) * 0.5f + 0.5f, Helper.VDot2D(cir.np, vab) * 2f)));
nside++;
float htmin = 0, htmax = 0;
if (SweetCircleCircle(pos, rad, vab, cir.p, cir.rad, ref htmin, ref htmax) == 0)
{
continue;
}
if (htmin < 0.0f && htmax > 0.0f)
{
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
if (htmin < tmin)
{
tmin = htmin;
}
}
}
for (int i = 0; i < _nSegments; i++)
{
ObstacleSegment seg = _segments[i];
float htmin = 0;
if (seg.touch)
{
float[] sdir = Helper.VSub(seg.q[0], seg.q[1], seg.q[2], seg.p[0], seg.p[1], seg.p[2]);
float[] snorm = new float[3];
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
if (Helper.VDot2D(snorm, vcand) < 0.0f)
{
continue;
}
htmin = 0.0f;
}
else
{
if (isectRaySeg(pos, vcand, seg.p, seg.q, ref htmin) == 0)
{
continue;
}
}
htmin *= 2.0f;
if (htmin < tmin)
{
tmin = htmin;
}
}
if (nside > 0)
{
side /= nside;
}
float vpen = _params.weightDesVel * (Helper.VDist2D(vcand, dvel) * _invVmax);
float vcpen = _params.weightCurVel * (Helper.VDist2D(vcand, vel) * _invVmax);
float spen = _params.weightSide * side;
float tpen = _params.weightToi * (1.0f / (0.1f + tmin * _invHorizTime));
float penalty = vpen + vcpen + spen + tpen;
if (debug != null)
{
debug.AddSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
}
return(penalty);
}