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