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); } } }