public override unsafe SignedDistance SignedDistance(Vector2 origin, ref double param)
{
var qa = _p[0] - origin;
var ab = _p[1] - _p[0];
var br = _p[0] + _p[2] - _p[1] - _p[1];
var coefficient = stackalloc[]
{
Vector2.Dot(br, br),
3 * Vector2.Dot(ab, br),
2 * Vector2.Dot(ab, ab) + Vector2.Dot(qa, br),
Vector2.Dot(qa, ab)
};
var t = stackalloc double[3];
var solutions = Equations.SolveCubic(t, coefficient);
var minDistance = Arithmetic.NonZeroSign(Vector2.Cross(ab, qa)) * qa.Length(); // distance from A
param = -Vector2.Dot(qa, ab) / Vector2.Dot(ab, ab);
{
var distance = Arithmetic.NonZeroSign(Vector2.Cross(_p[2] - _p[1], _p[2] - origin)) *
(_p[2] - origin).Length(); // distance from B
if (Math.Abs(distance) < Math.Abs(minDistance))
{
minDistance = distance;
param = Vector2.Dot(origin - _p[1], _p[2] - _p[1]) /
Vector2.Dot(_p[2] - _p[1], _p[2] - _p[1]);
}
}
for (var i = 0; i < solutions; ++i)
{
if (t[i] > 0 && t[i] < 1)
{
var endpoint = _p[0] + 2 * t[i] * ab + t[i] * t[i] * br;
var distance = Arithmetic.NonZeroSign(Vector2.Cross(_p[2] - _p[0], endpoint - origin)) *
(endpoint - origin).Length();
if (Math.Abs(distance) <= Math.Abs(minDistance))
{
minDistance = distance;
param = t[i];
}
}
}
if (param >= 0 && param <= 1)
{
return(new SignedDistance(minDistance, 0));
}
if (param < .5)
{
return(new SignedDistance(minDistance, Math.Abs(Vector2.Dot(ab.Normalize(), qa.Normalize()))));
}
return(new SignedDistance(minDistance,
Math.Abs(Vector2.Dot((_p[2] - _p[1]).Normalize(), (_p[2] - origin).Normalize()))));
}
private unsafe void BoundComputeAxis(double[] box, double *co)
{
var param = stackalloc double[2];
var solutions = Equations.SolveQuadratic(param, co);
for (var i = 0; i < solutions; ++i)
{
if (param[i] > 0 && param[i] < 1)
{
PointBounds(Point(param[i]), box);
}
}
}
