public override void findBounds(ref double left, ref double bottom, ref double right, ref double top)
{
Vector2.pointBounds(_p0, ref left, ref bottom, ref right, ref top);
Vector2.pointBounds(_p3, ref left, ref bottom, ref right, ref top);
//
Vector2 a0 = _p1 - _p0;
Vector2 a1 = 2 * (_p2 - _p1 - a0);
Vector2 a2 = _p3 - 3 * _p2 + 3 * _p1 - _p0;
//
EqResult pars = new EqResult();
int solutions = EquationSolver.SolveQuadratic(ref pars, a2.x, a1.x, a0.x);
for (int i = 0; i < solutions; ++i)
{
double par = pars[i];
if (par > 0 && par < 1)
{
Vector2.pointBounds(point(par), ref left, ref bottom, ref right, ref top);
}
}
pars = new EqResult();
solutions = EquationSolver.SolveQuadratic(ref pars, a2.y, a1.y, a0.y);
for (int i = 0; i < solutions; ++i)
{
double par = pars[i];
if (par > 0 && par < 1)
{
Vector2.pointBounds(point(par), ref left, ref bottom, ref right, ref top);
}
}
}
public override SignedDistance signedDistance(Vector2 origin, out double param)
{
Vector2 qa = _p0 - origin;
Vector2 ab = _p1 - _p0;
Vector2 br = _p0 + _p2 - _p1 - _p1;//
double a = Vector2.dotProduct(br, br);
double b = 3 * Vector2.dotProduct(ab, br);
double c = 2 * Vector2.dotProduct(ab, ab) + Vector2.dotProduct(qa, br);
double d = Vector2.dotProduct(qa, ab);
EqResult t = new EqResult();
int solutions = EquationSolver.SolveCubic(ref t, a, b, c, d);
double minDistance = nonZeroSign(Vector2.crossProduct(ab, qa)) * qa.Length(); // distance from A
param = -Vector2.dotProduct(qa, ab) / Vector2.dotProduct(ab, ab);
{
double distance = nonZeroSign(
Vector2.crossProduct(_p2 - _p1, _p2 - origin)) * (_p2 - origin).Length(); // distance from B
if (Math.Abs(distance) < Math.Abs(minDistance))
{
minDistance = distance;
param = Vector2.dotProduct(origin - _p1, _p2 - _p1) / Vector2.dotProduct(_p2 - _p1, _p2 - _p1);
}
}
//possible solution -1,0,1,2
for (int i = 0; i < solutions; ++i)
{
double ti = t[i];
if (ti > 0 && ti < 1)
{
Vector2 endpoint = _p0 + 2 * ti * ab + ti * ti * br;
double distance = nonZeroSign(
Vector2.crossProduct(_p2 - _p0, endpoint - origin)) * (endpoint - origin).Length();
if (Math.Abs(distance) <= Math.Abs(minDistance))
{
minDistance = distance;
param = ti;
}
}
}
if (param >= 0 && param <= 1)
{
return(new SignedDistance(minDistance, 0));
}
if (param < .5)
{
return(new SignedDistance(minDistance, Math.Abs(Vector2.dotProduct(ab.normalize(), qa.normalize()))));
}
else
{
return(new SignedDistance(minDistance, Math.Abs(Vector2.dotProduct((_p2 - _p1).normalize(), (_p2 - origin).normalize()))));
}
}
public static int SolveCubic(ref EqResult x /*3*/, double a, double b, double c, double d)
{
// ax^3 + bx^2 + cx + d = 0
if (fabs(a) < EPSILON)
{
return(SolveQuadratic(ref x, b, c, d));
}
return(SolveCubicNormed(ref x, b / a, c / a, d / a));
}
static int SolveCubicNormed(ref EqResult x, double a, double b, double c)
{
double a2 = a * a;
double q = (a2 - 3 * b) / 9;
double r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
double r2 = r * r;
double q3 = q * q * q;
double A, B;
if (r2 < q3)
{
double t = r / Math.Sqrt(q3);
if (t < -1)
{
t = -1;
}
if (t > 1)
{
t = 1;
}
t = Math.Acos(t);
a /= 3; q = -2 * Math.Sqrt(q);
x.x0 = q * Math.Cos(t / 3) - a;
x.x1 = q * Math.Cos((t + 2 * Math.PI) / 3) - a;
x.x2 = q * Math.Cos((t - 2 * Math.PI) / 3) - a;
return(3);
}
else
{
A = -Math.Pow(fabs(r) + Math.Sqrt(r2 - q3), 1 / 3.0);
if (r < 0)
{
A = -A;
}
B = A == 0 ? 0 : q / A;
a /= 3;
x.x0 = (A + B) - a;
x.x1 = -0.5 * (A + B) - a;
x.x2 = 0.5 * Math.Sqrt(3.0) * (A - B);
if (fabs(x.x2) < EPSILON)
{
return(2);
}
return(1);
}
}
public static int SolveQuadratic(ref EqResult x, double a, double b, double c)
{
// ax^2 + bx + c = 0
if (fabs(a) < EPSILON)
{
if (fabs(b) < EPSILON)
{
if (c == 0)
{
return(-1);
}
return(0);
}
x.x0 = -c / b;
return(1);
}
double dscr = b * b - 4 * a * c;
if (dscr > 0)
{
dscr = Math.Sqrt(dscr);
x.x0 = (-b + dscr) / (2 * a);
x.x1 = (-b - dscr) / (2 * a);
return(2);
}
else if (dscr == 0)
{
x.x0 = -b / (2 * a);
return(1);
}
else
{
return(0);
}
}
