From fc60ea5bab043fa5faeb1ab38e9ace18b237bde6 Mon Sep 17 00:00:00 2001 From: Keavon Chambers Date: Fri, 14 Feb 2025 18:20:38 -0800 Subject: [PATCH] WIP --- libraries/bezier-rs/src/bezier/solvers.rs | 92 ++- libraries/bezier-rs/src/lib.rs | 2 + libraries/bezier-rs/src/quartic_solver.rs | 666 +++++++++++++++++++++ libraries/bezier-rs/src/quartic_solver2.rs | 540 +++++++++++++++++ 4 files changed, 1292 insertions(+), 8 deletions(-) create mode 100644 libraries/bezier-rs/src/quartic_solver.rs create mode 100644 libraries/bezier-rs/src/quartic_solver2.rs diff --git a/libraries/bezier-rs/src/bezier/solvers.rs b/libraries/bezier-rs/src/bezier/solvers.rs index 62fc1b0027..01fae00c6b 100644 --- a/libraries/bezier-rs/src/bezier/solvers.rs +++ b/libraries/bezier-rs/src/bezier/solvers.rs @@ -110,10 +110,43 @@ impl Bezier { /// #[must_use] pub fn tangents_to_point(self, point: DVec2) -> Vec { - let sbasis: crate::SymmetricalBasisPair = to_symmetrical_basis_pair(self); - let derivative = sbasis.derivative(); - let cross = (sbasis - point).cross(&derivative); - SymmetricalBasis::roots(&cross) + match self.handles { + BezierHandles::Linear => Vec::new(), + BezierHandles::Quadratic { handle } => { + // Represent the quadratic in standard form: + // p(t) = p0 + 2(p1 - p0)t + (p0 - 2*p1 + p2)t² + let a = self.start - 2. * handle + self.end; + let b = 2. * (handle - self.start); + let c = self.start - point; + + // Our polynomial is: (a.cross(b)) t² - 2*(d.cross(a)) t - (d.cross(b)) = 0. + let c2 = a.perp_dot(b); + let c1 = -2. * c.perp_dot(a); + let c0 = b.perp_dot(c); + + crate::quartic_solver2::solve_quadratic(c0, c1, c2).iter().copied().flatten().filter(|t| *t >= 0. && *t <= 1.).collect() + } + BezierHandles::Cubic { handle_start, handle_end } => { + let d = self.start - point; + let c = (handle_start - self.start) * 3.; + let b = (handle_end - handle_start) * 3. - c; + let a = self.end - self.start - c - b; + + // coefficients of x(t) \cross x'(t) + let c0 = d.perp_dot(c); + let c1 = 2. * d.perp_dot(b); + let c2 = c.perp_dot(b) + 3. * d.perp_dot(a); + let c3 = 2. * c.perp_dot(a); + let c4 = b.perp_dot(a); + + crate::quartic_solver2::solve_quartic(c0, c1, c2, c3, c4) + .iter() + .copied() + .flatten() + .filter(|t| *t >= 0. && *t <= 1.) + .collect() + } + } } /// Returns a normalized unit vector representing the direction of the normal at the point `t` along the curve. @@ -126,10 +159,53 @@ impl Bezier { /// #[must_use] pub fn normals_to_point(self, point: DVec2) -> Vec { - let sbasis = to_symmetrical_basis_pair(self); - let derivative = sbasis.derivative(); - let cross = (sbasis - point).dot(&derivative); - SymmetricalBasis::roots(&cross) + match self.handles { + BezierHandles::Linear => { + let point_a = point - self.start; + let point_b = self.end - self.start; + if point_b.length_squared() < MAX_ABSOLUTE_DIFFERENCE * MAX_ABSOLUTE_DIFFERENCE { + return Vec::new(); + } + + let t = point_a.dot(point_b) / point_b.length_squared(); + if !(0.0..=1.).contains(&t) { + return Vec::new(); + } + + vec![t] + } + BezierHandles::Quadratic { handle } => { + let a = self.start - 2. * handle + self.end; + let b = 2. * (handle - self.start); + let c = self.start - point; + + let c2 = a.dot(b); + let c1 = -2. * c.dot(a); + let c0 = b.dot(c); + + crate::quartic_solver2::solve_quadratic(c0, c1, c2).iter().copied().flatten().filter(|t| *t >= 0. && *t <= 1.).collect() + } + BezierHandles::Cubic { handle_start, handle_end } => { + let d = self.start - point; + let c = (handle_start - self.start) * 3.; + let b = (handle_end - handle_start) * 3. - c; + let a = self.end - self.start - c - b; + + // coefficients of x(t) \cdot x'(t) + let c0 = d.dot(c); + let c1 = 2. * d.dot(b); + let c2 = c.dot(b) + 3. * d.dot(a); + let c3 = 2. * c.dot(a); + let c4 = b.dot(a); + + crate::quartic_solver2::solve_quartic(c0, c1, c2, c3, c4) + .iter() + .copied() + .flatten() + .filter(|t| *t >= 0. && *t <= 1.) + .collect() + } + } } /// Returns the curvature, a scalar value for the derivative at the point `t` along the curve. diff --git a/libraries/bezier-rs/src/lib.rs b/libraries/bezier-rs/src/lib.rs index 06faf15d84..00c261b8ee 100644 --- a/libraries/bezier-rs/src/lib.rs +++ b/libraries/bezier-rs/src/lib.rs @@ -7,6 +7,8 @@ mod bezier; mod consts; mod poisson_disk; mod polynomial; +mod quartic_solver; +mod quartic_solver2; mod subpath; mod symmetrical_basis; mod utils; diff --git a/libraries/bezier-rs/src/quartic_solver.rs b/libraries/bezier-rs/src/quartic_solver.rs new file mode 100644 index 0000000000..68c0d8eb41 --- /dev/null +++ b/libraries/bezier-rs/src/quartic_solver.rs @@ -0,0 +1,666 @@ +#![allow(non_snake_case)] +#![allow(clippy::too_many_arguments)] + +use core::f64; + +const CUBIC_RESCAL_FACT: f64 = 3.488062113727083E+102; //= pow(DBL_MAX,1.0/3.0)/1.618034; +const QUART_RESCAL_FACT: f64 = 7.156344627944542E+76; // = pow(DBL_MAX,1.0/4.0)/1.618034; +const MACHEPS: f64 = 2.2204460492503131E-16; // DBL_EPSILON + +const M_PI: f64 = std::f64::consts::PI; + +#[derive(Copy, Clone, Debug, Default)] +struct Complex(f64, f64); +impl std::ops::Add for Complex { + type Output = Self; + fn add(self, rhs: Self) -> Self { + Complex(self.0 + rhs.0, self.1 + rhs.1) + } +} +impl std::ops::Sub for Complex { + type Output = Self; + fn sub(self, rhs: Self) -> Self { + Complex(self.0 - rhs.0, self.1 - rhs.1) + } +} +impl std::ops::Mul for Complex { + type Output = Self; + fn mul(self, rhs: Self) -> Self { + Complex(self.0 * rhs.0 - self.1 * rhs.1, self.0 * rhs.1 + self.1 * rhs.0) + } +} +impl std::ops::Div for Complex { + type Output = Self; + fn div(self, rhs: Self) -> Self { + let d = rhs.0 * rhs.0 + rhs.1 * rhs.1; + Complex((self.0 * rhs.0 + self.1 * rhs.1) / d, (self.1 * rhs.0 - self.0 * rhs.1) / d) + } +} +impl std::ops::Neg for Complex { + type Output = Self; + fn neg(self) -> Self { + Complex(-self.0, -self.1) + } +} +impl Complex { + fn new(real: f64, imag: f64) -> Self { + Complex(real, imag) + } + + fn real(real: f64) -> Self { + Complex(real, 0.0) + } + + fn imag(imag: f64) -> Self { + Complex(0.0, imag) + } + + fn conj(self) -> Self { + Complex(self.0, -self.1) + } +} + +fn fabs(x: f64) -> f64 { + x.abs() +} +fn copysign(x: f64, y: f64) -> f64 { + x.copysign(y) +} +fn sqrt(x: f64) -> f64 { + x.sqrt() +} +fn oqs_max2(a: f64, b: f64) -> f64 { + if a >= b { + a + } else { + b + } +} +fn oqs_max3(a: f64, b: f64, c: f64) -> f64 { + let t = oqs_max2(a, b); + oqs_max2(t, c) +} +fn acos(x: f64) -> f64 { + x.acos() +} +fn cos(x: f64) -> f64 { + x.cos() +} +fn cbrt(x: f64) -> f64 { + x.cbrt() +} +fn pow(x: f64, y: f64) -> f64 { + x.powf(y) +} +fn cabs(x: Complex) -> f64 { + (x.0 * x.0 + x.1 * x.1).sqrt() +} +fn csqrt(x: Complex) -> Complex { + let r = (x.0 * x.0 + x.1 * x.1).sqrt(); + let t = 0.5 * (x.1 / x.0).atan(); + Complex(r.cos(), r.sin()) * Complex(t.cos(), t.sin()) +} + +fn oqs_solve_cubic_analytic_depressed_handle_inf(b: f64, c: f64) -> f64 { + /* find analytically the dominant root of a depressed cubic x^3+b*x+c + * where coefficients b and c are large (see sec. 2.2 in the manuscript) */ + + const PI2: f64 = M_PI / 2.0; + const TWOPI: f64 = 2.0 * M_PI; + + let Q = -b / 3.0; + let R = 0.5 * c; + + if R == 0. { + return if b <= 0. { sqrt(-b) } else { 0. }; + } + + let KK = if fabs(Q) < fabs(R) { + let QR = Q / R; + let QRSQ = QR * QR; + 1.0 - Q * QRSQ + } else { + let RQ = R / Q; + copysign(1.0, Q) * (RQ * RQ / Q - 1.0) + }; + + if KK < 0.0 { + let sqrtQ = sqrt(Q); + let theta = acos((R / fabs(Q)) / sqrtQ); + if theta < PI2 { + -2.0 * sqrtQ * cos(theta / 3.0) + } else { + -2.0 * sqrtQ * cos((theta + TWOPI) / 3.0) + } + } else { + let A = if fabs(Q) < fabs(R) { + -copysign(1.0, R) * cbrt(fabs(R) * (1.0 + sqrt(KK))) + } else { + -copysign(1.0, R) * cbrt(fabs(R) + sqrt(fabs(Q)) * fabs(Q) * sqrt(KK)) + }; + let B = if A == 0.0 { 0.0 } else { Q / A }; + A + B + } +} + +fn oqs_solve_cubic_analytic_depressed(b: f64, c: f64) -> f64 { + /* find analytically the dominant root of a depressed cubic x^3+b*x+c + * (see sec. 2.2 in the manuscript) */ + + let Q = -b / 3.0; + let R = 0.5 * c; + if fabs(Q) > 1E102 || fabs(R) > 1E154 { + return oqs_solve_cubic_analytic_depressed_handle_inf(b, c); + } + + let Q3 = Q * Q * Q; + let R2 = R * R; + if R2 < Q3 { + let theta = acos(R / sqrt(Q3)); + let sqrtQ = -2.0 * sqrt(Q); + if theta < M_PI / 2. { + sqrtQ * cos(theta / 3.0) + } else { + sqrtQ * cos((theta + 2.0 * M_PI) / 3.0) + } + } else { + let A = -copysign(1.0, R) * pow(fabs(R) + sqrt(R2 - Q3), 1.0 / 3.0); + let B = if A == 0.0 { 0.0 } else { Q / A }; + A + B /* this is always largest root even if A=B */ + } +} + +fn oqs_calc_phi0(a: f64, b: f64, c: f64, d: f64, scaled: bool) -> f64 { + /* find phi0 as the dominant root of the depressed and shifted cubic + * in eq. (79) (see also the discussion in sec. 2.2 of the manuscript) */ + let mut diskr = 9. * a * a - 24. * b; + /* eq. (87) */ + let s = if diskr > 0.0 { + diskr = sqrt(diskr); + if a > 0.0 { + -2. * b / (3. * a + diskr) + } else { + -2. * b / (3. * a - diskr) + } + } else { + -a / 4. + }; + /* eqs. (83) */ + let aq = a + 4. * s; + let bq = b + 3. * s * (a + 2. * s); + let cq = c + s * (2. * b + s * (3. * a + 4. * s)); + let dq = d + s * (c + s * (b + s * (a + s))); + let gg = bq * bq / 9.; + let hh = aq * cq; + + let mut g = hh - 4. * dq - 3. * gg; /* eq. (85) */ + let mut h = (8. * dq + hh - 2. * gg) * bq / 3. - cq * cq - dq * aq * aq; /* eq. (86) */ + + let mut rmax = oqs_solve_cubic_analytic_depressed(g, h); + if rmax.is_nan() || rmax.is_infinite() { + rmax = oqs_solve_cubic_analytic_depressed_handle_inf(g, h); + if (rmax.is_nan() || rmax.is_infinite()) && scaled { + // try harder: rescale also the depressed cubic if quartic has been already rescaled + let rfact = CUBIC_RESCAL_FACT; + let rfactsq = rfact * rfact; + // let ggss = gg / rfactsq; + // let hhss = hh / rfactsq; + let dqss = dq / rfactsq; + let aqs = aq / rfact; + let bqs = bq / rfact; + let cqs = cq / rfact; + let ggss = bqs * bqs / 9.0; + let hhss = aqs * cqs; + g = hhss - 4.0 * dqss - 3.0 * ggss; + h = (8.0 * dqss + hhss - 2.0 * ggss) * bqs / 3. - cqs * (cqs / rfact) - (dq / rfact) * aqs * aqs; + rmax = oqs_solve_cubic_analytic_depressed(g, h); + rmax = if rmax.is_nan() || rmax.is_infinite() { + oqs_solve_cubic_analytic_depressed_handle_inf(g, h) + } else { + rmax + }; + rmax *= rfact; + } + } + + /* Newton-Raphson used to refine phi0 (see end of sec. 2.2 in the manuscript) */ + let mut x = rmax; + let xsq = x * x; + let xxx = x * xsq; + let gx = g * x; + let f = x * (xsq + g) + h; + let maxtt = if fabs(xxx) > fabs(gx) { fabs(xxx) } else { fabs(gx) }; + let maxtt = if fabs(h) > maxtt { fabs(h) } else { maxtt }; + + if fabs(f) > MACHEPS * maxtt { + // for (iter=0; iter < 8; iter++) { + for _ in 0..8 { + let df = 3.0 * xsq + g; + if df == 0. { + break; + } + let xold = x; + x += -f / df; + let fold = f; + let xsq = x * x; + let f = x * (xsq + g) + h; + if f == 0. { + break; + } + + if fabs(f) >= fabs(fold) { + x = xold; + break; + } + } + } + x +} + +fn oqs_calc_err_ldlt(b: f64, c: f64, d: f64, d2: f64, l1: f64, l2: f64, l3: f64) -> f64 { + /* Eqs. (29) and (30) in the manuscript */ + let mut sum = if b == 0. { fabs(d2 + l1 * l1 + 2.0 * l3) } else { fabs(((d2 + l1 * l1 + 2.0 * l3) - b) / b) }; + sum += if c == 0. { + fabs(2.0 * d2 * l2 + 2.0 * l1 * l3) + } else { + fabs(((2.0 * d2 * l2 + 2.0 * l1 * l3) - c) / c) + }; + sum += if d == 0. { fabs(d2 * l2 * l2 + l3 * l3) } else { fabs(((d2 * l2 * l2 + l3 * l3) - d) / d) }; + sum +} + +fn oqs_calc_err_abcd_cmplx(a: f64, b: f64, c: f64, d: f64, aq: Complex, bq: Complex, cq: Complex, dq: Complex) -> f64 { + /* Eqs. (68) and (69) in the manuscript for complex alpha1 (aq), beta1 (bq), alpha2 (cq) and beta2 (dq) */ + let mut sum = if d == 0. { cabs(bq * dq) } else { cabs((bq * dq - Complex::real(d)) / Complex::real(d)) }; + sum += if c == 0. { + cabs(bq * cq + aq * dq) + } else { + cabs(((bq * cq + aq * dq) - Complex::real(c)) / Complex::real(c)) + }; + sum += if b == 0. { + cabs(bq + aq * cq + dq) + } else { + cabs(((bq + aq * cq + dq) - Complex::real(b)) / Complex::real(b)) + }; + sum += if a == 0. { cabs(aq + cq) } else { cabs(((aq + cq) - Complex::real(a)) / Complex::real(a)) }; + sum +} + +fn oqs_calc_err_abcd(a: f64, b: f64, c: f64, d: f64, aq: f64, bq: f64, cq: f64, dq: f64) -> f64 { + /* Eqs. (68) and (69) in the manuscript for real alpha1 (aq), beta1 (bq), alpha2 (cq) and beta2 (dq)*/ + let mut sum = if d == 0. { fabs(bq * dq) } else { fabs((bq * dq - d) / d) }; + sum += if c == 0. { fabs(bq * cq + aq * dq) } else { fabs(((bq * cq + aq * dq) - c) / c) }; + sum += if b == 0. { fabs(bq + aq * cq + dq) } else { fabs(((bq + aq * cq + dq) - b) / b) }; + sum += if a == 0. { fabs(aq + cq) } else { fabs(((aq + cq) - a) / a) }; + sum +} + +fn oqs_calc_err_abc(a: f64, b: f64, c: f64, aq: f64, bq: f64, cq: f64, dq: f64) -> f64 { + /* Eqs. (48)-(51) in the manuscript */ + let mut sum = if c == 0. { fabs(bq * cq + aq * dq) } else { fabs(((bq * cq + aq * dq) - c) / c) }; + sum += if b == 0. { fabs(bq + aq * cq + dq) } else { fabs(((bq + aq * cq + dq) - b) / b) }; + sum += if a == 0. { fabs(aq + cq) } else { fabs(((aq + cq) - a) / a) }; + sum +} + +fn oqs_NRabcd(a: f64, b: f64, c: f64, d: f64, AQ: &mut f64, BQ: &mut f64, CQ: &mut f64, DQ: &mut f64) { + /* Newton-Raphson described in sec. 2.3 of the manuscript for complex + * coefficients a,b,c,d */ + let mut xold = [0.; 4]; + let mut dx = [0.; 4]; + let mut Jinv = [[0.; 4]; 4]; + + let mut x = [*AQ, *BQ, *CQ, *DQ]; + let vr = [d, c, b, a]; + let mut fvec = [x[1] * x[3] - d, x[1] * x[2] + x[0] * x[3] - c, x[1] + x[0] * x[2] + x[3] - b, x[0] + x[2] - a]; + let mut errf = 0.; + for k1 in 0..4 { + errf += if vr[k1] == 0. { fabs(fvec[k1]) } else { fabs(fvec[k1] / vr[k1]) }; + } + for _ in 0..8 { + let x02 = x[0] - x[2]; + let det = x[1] * x[1] + x[1] * (-x[2] * x02 - 2.0 * x[3]) + x[3] * (x[0] * x02 + x[3]); + if det == 0.0 { + break; + } + Jinv[0][0] = x02; + Jinv[0][1] = x[3] - x[1]; + Jinv[0][2] = x[1] * x[2] - x[0] * x[3]; + Jinv[0][3] = -x[1] * Jinv[0][1] - x[0] * Jinv[0][2]; + Jinv[1][0] = x[0] * Jinv[0][0] + Jinv[0][1]; + Jinv[1][1] = -x[1] * Jinv[0][0]; + Jinv[1][2] = -x[1] * Jinv[0][1]; + Jinv[1][3] = -x[1] * Jinv[0][2]; + Jinv[2][0] = -Jinv[0][0]; + Jinv[2][1] = -Jinv[0][1]; + Jinv[2][2] = -Jinv[0][2]; + Jinv[2][3] = Jinv[0][2] * x[2] + Jinv[0][1] * x[3]; + Jinv[3][0] = -x[2] * Jinv[0][0] - Jinv[0][1]; + Jinv[3][1] = Jinv[0][0] * x[3]; + Jinv[3][2] = x[3] * Jinv[0][1]; + Jinv[3][3] = x[3] * Jinv[0][2]; + + for k1 in 0..4 { + dx[k1] = 0.; + for k2 in 0..4 { + dx[k1] += Jinv[k1][k2] * fvec[k2]; + } + } + for k1 in 0..4 { + xold[k1] = x[k1]; + } + + for k1 in 0..4 { + x[k1] += -dx[k1] / det; + } + fvec[0] = x[1] * x[3] - d; + fvec[1] = x[1] * x[2] + x[0] * x[3] - c; + fvec[2] = x[1] + x[0] * x[2] + x[3] - b; + fvec[3] = x[0] + x[2] - a; + let errfold = errf; + errf = 0.; + for k1 in 0..4 { + errf += if vr[k1] == 0. { fabs(fvec[k1]) } else { fabs(fvec[k1] / vr[k1]) }; + } + if errf == 0. { + break; + } + if errf >= errfold { + for k1 in 0..4 { + x[k1] = xold[k1]; + } + break; + } + } + *AQ = x[0]; + *BQ = x[1]; + *CQ = x[2]; + *DQ = x[3]; +} + +fn oqs_solve_quadratic(a: f64, b: f64) -> [Complex; 2] { + let diskr = a * a - 4. * b; + if diskr >= 0.0 { + let div = if a >= 0.0 { -a - sqrt(diskr) } else { -a + sqrt(diskr) }; + + let zmax = div / 2.; + + let zmin = if zmax == 0.0 { 0.0 } else { b / zmax }; + + [Complex::real(zmax), Complex::real(zmin)] + } else { + let sqrtd = sqrt(-diskr); + [Complex::new(-a / 2., sqrtd / 2.), Complex::new(-a / 2., -sqrtd / 2.)] + } +} + +pub fn oqs_quartic_solver(coeff: [f64; 5]) -> [f64; 4] { + /* USAGE: + * + * This routine calculates the roots of the quartic equation + * + * coeff[4]*x^4 + coeff[3]*x^3 + coeff[2]*x^2 + coeff[1]*x + coeff[0] = 0 + * + * if coeff[4] != 0 + * + * the four roots will be stored in the complex array roots[] + * + * */ + // f64 resmin, bl311, dml3l3, aq1, bq1, cq1, dq1,aq,bq,cq,dq,d2,d3,l1,l3, errmin, gamma, del2; + let mut acx1 = Complex::default(); + let mut bcx1 = Complex::default(); + let mut ccx1 = Complex::default(); + let mut dcx1 = Complex::default(); + let mut acx = Complex::default(); + let mut bcx = Complex::default(); + let mut ccx = Complex::default(); + let mut dcx = Complex::default(); + let mut realcase = [0; 2]; + let mut d2m = [0.; 12]; + let mut l2m = [0.; 12]; + let mut res = [0.; 12]; + let mut errv = [0.; 3]; + let mut aqv = [0.; 3]; + let mut cqv = [0.; 3]; + let mut resmin = 0.0; + let mut err0 = 0.; + let mut rfact = 1.0; + + if coeff[4] == 0.0 { + println!("That's not a quartic!\n"); + return [0.; 4]; + } + let mut a = coeff[3] / coeff[4]; + let mut b = coeff[2] / coeff[4]; + let mut c = coeff[1] / coeff[4]; + let mut d = coeff[0] / coeff[4]; + let mut phi0 = oqs_calc_phi0(a, b, c, d, false); + + // simple polynomial rescaling + if phi0.is_nan() || phi0.is_infinite() { + rfact = QUART_RESCAL_FACT; + a /= rfact; + let rfactsq = rfact * rfact; + b /= rfactsq; + c /= rfactsq * rfact; + d /= rfactsq * rfactsq; + phi0 = oqs_calc_phi0(a, b, c, d, true); + } + let l1 = a / 2.; /* eq. (16) */ + let l3 = b / 6. + phi0 / 2.; /* eq. (18) */ + let del2 = c - a * l3; /* defined just after eq. (27) */ + let mut nsol = 0; + let bl311 = 2. * b / 3. - phi0 - l1 * l1; /* This is d2 as defined in eq. (20)*/ + let dml3l3 = d - l3 * l3; /* dml3l3 is d3 as defined in eq. (15) with d2=0 */ + let mut aq = 0.; + let mut bq = 0.; + let mut cq = 0.; + let mut dq = 0.; + let mut aq1 = 0.; + let mut bq1 = 0.; + let mut cq1 = 0.; + let mut dq1 = 0.; + + /* Three possible solutions for d2 and l2 (see eqs. (28) and discussion which follows) */ + if bl311 != 0.0 { + d2m[nsol] = bl311; + l2m[nsol] = del2 / (2.0 * d2m[nsol]); + res[nsol] = oqs_calc_err_ldlt(b, c, d, d2m[nsol], l1, l2m[nsol], l3); + nsol += 1; + } + if del2 != 0. { + l2m[nsol] = 2. * dml3l3 / del2; + if l2m[nsol] != 0. { + d2m[nsol] = del2 / (2. * l2m[nsol]); + res[nsol] = oqs_calc_err_ldlt(b, c, d, d2m[nsol], l1, l2m[nsol], l3); + nsol += 1; + } + + d2m[nsol] = bl311; + l2m[nsol] = 2.0 * dml3l3 / del2; + res[nsol] = oqs_calc_err_ldlt(b, c, d, d2m[nsol], l1, l2m[nsol], l3); + nsol += 1; + } + + let (d2, l2) = if nsol == 0 { + (0., 0.) + } else { + /* we select the (d2,l2) pair which minimizes errors */ + let mut kmin = 0; + for k1 in 0..nsol { + if k1 == 0 || res[k1] < resmin { + resmin = res[k1]; + kmin = k1; + } + } + (d2m[kmin], l2m[kmin]) + }; + + let mut whichcase = 0; + if d2 < 0.0 { + /* Case I eqs. (37)-(40) */ + let gamma = (-d2).sqrt(); + aq = l1 + gamma; + bq = l3 + gamma * l2; + + cq = l1 - gamma; + dq = l3 - gamma * l2; + if fabs(dq) < fabs(bq) { + dq = d / bq; + } else if fabs(dq) > fabs(bq) { + bq = d / dq + } + if fabs(aq) < fabs(cq) { + nsol = 0; + if dq != 0. { + aqv[nsol] = (c - bq * cq) / dq; /* see eqs. (47) */ + errv[nsol] = oqs_calc_err_abc(a, b, c, aqv[nsol], bq, cq, dq); + nsol += 1; + } + if cq != 0. { + aqv[nsol] = (b - dq - bq) / cq; /* see eqs. (47) */ + errv[nsol] = oqs_calc_err_abc(a, b, c, aqv[nsol], bq, cq, dq); + nsol += 1; + } + aqv[nsol] = a - cq; /* see eqs. (47) */ + errv[nsol] = oqs_calc_err_abc(a, b, c, aqv[nsol], bq, cq, dq); + nsol += 1; + /* we select the value of aq (i.e. alpha1 in the manuscript) which minimizes errors */ + let mut kmin = 0; + let mut errmin = 0.; + for k in 0..nsol { + if k == 0 || errv[k] < errmin { + kmin = k; + errmin = errv[k]; + } + } + aq = aqv[kmin]; + } else { + nsol = 0; + if bq != 0. { + cqv[nsol] = (c - aq * dq) / bq; /* see eqs. (53) */ + errv[nsol] = oqs_calc_err_abc(a, b, c, aq, bq, cqv[nsol], dq); + nsol += 1; + } + if aq != 0. { + cqv[nsol] = (b - bq - dq) / aq; /* see eqs. (53) */ + errv[nsol] = oqs_calc_err_abc(a, b, c, aq, bq, cqv[nsol], dq); + nsol += 1; + } + cqv[nsol] = a - aq; /* see eqs. (53) */ + errv[nsol] = oqs_calc_err_abc(a, b, c, aq, bq, cqv[nsol], dq); + nsol += 1; + /* we select the value of cq (i.e. alpha2 in the manuscript) which minimizes errors */ + let mut kmin = 0; + let mut errmin = 0.; + for k in 0..nsol { + if k == 0 || errv[k] < errmin { + kmin = k; + errmin = errv[k]; + } + } + cq = cqv[kmin]; + } + + realcase[0] = 1; + } else if d2 > 0. { + /* Case II eqs. (53)-(56) */ + let gamma = sqrt(d2); + acx = Complex::new(l1, gamma); + bcx = Complex::new(l3, gamma * l2); + ccx = acx.conj(); + dcx = bcx.conj(); + + realcase[0] = 0; + } else { + realcase[0] = -1; // d2=0 + } + + /* Case III: d2 is 0 or approximately 0 (in this case check which solution is better) */ + if realcase[0] == -1 || (fabs(d2) <= MACHEPS * oqs_max3(fabs(2. * b / 3.), fabs(phi0), l1 * l1)) { + let d3 = d - l3 * l3; + if realcase[0] == 1 { + err0 = oqs_calc_err_abcd(a, b, c, d, aq, bq, cq, dq); + } else if realcase[0] == 0 { + err0 = oqs_calc_err_abcd_cmplx(a, b, c, d, acx, bcx, ccx, dcx); + } + let err1 = if d3 <= 0. { + realcase[1] = 1; + aq1 = l1; + bq1 = l3 + sqrt(-d3); + cq1 = l1; + dq1 = l3 - sqrt(-d3); + if fabs(dq1) < fabs(bq1) { + dq1 = d / bq1 + } else if fabs(dq1) > fabs(bq1) { + bq1 = d / dq1 + }; + oqs_calc_err_abcd(a, b, c, d, aq1, bq1, cq1, dq1) /* eq. (68) */ + } + // complex + else { + realcase[1] = 0; + acx1 = Complex::real(l1); + bcx1 = Complex::new(l3, sqrt(d3)); + ccx1 = Complex::real(l1); + dcx1 = bcx1.conj(); + oqs_calc_err_abcd_cmplx(a, b, c, d, acx1, bcx1, ccx1, dcx1) + }; + if realcase[0] == -1 || err1 < err0 { + whichcase = 1; // d2 = 0 + if realcase[1] == 1 { + aq = aq1; + bq = bq1; + cq = cq1; + dq = dq1; + } else { + acx = acx1; + bcx = bcx1; + ccx = ccx1; + dcx = dcx1; + } + } + } + let mut roots = if realcase[whichcase] == 1 { + /* if alpha1, beta1, alpha2 and beta2 are real first refine + * the coefficient through a Newton-Raphson */ + oqs_NRabcd(a, b, c, d, &mut aq, &mut bq, &mut cq, &mut dq); + /* finally calculate the roots as roots of p1(x) and p2(x) (see end of sec. 2.1) */ + let qroots1 = oqs_solve_quadratic(aq, bq); + let qroots2 = oqs_solve_quadratic(cq, d); + [qroots1[0].0, qroots1[1].0, qroots2[0].0, qroots2[1].0] + } else { + /* complex coefficients of p1 and p2 */ + // d2!=0 + if whichcase == 0 { + let cdiskr = acx * acx / Complex::real(4.) - bcx; + /* calculate the roots as roots of p1(x) and p2(x) (see end of sec. 2.1) */ + let zx1 = -acx / Complex::real(2.) + csqrt(cdiskr); + let zx2 = -acx / Complex::real(2.) - csqrt(cdiskr); + let zxmax = if cabs(zx1) > cabs(zx2) { zx1 } else { zx2 }; + let zxmin = bcx / zxmax; + [zxmin.0, zxmin.conj().0, zxmax.0, zxmax.conj().0] + } + // d2 ~ 0 + else { + /* never gets here! */ + let cdiskr = csqrt(acx * acx - Complex::real(4.0) * bcx); + let zx1 = Complex::real(-0.5) * (acx + cdiskr); + let zx2 = Complex::real(-0.5) * (acx - cdiskr); + let zxmax1 = if cabs(zx1) > cabs(zx2) { zx1 } else { zx2 }; + let zxmin1 = bcx / zxmax1; + let cdiskr = csqrt(ccx * ccx - Complex::real(4.0) * dcx); + let zx1 = Complex::real(-0.5) * (ccx + cdiskr); + let zx2 = Complex::real(-0.5) * (ccx - cdiskr); + let zxmax2 = if cabs(zx1) > cabs(zx2) { zx1 } else { zx2 }; + let zxmin2 = dcx / zxmax2; + [zxmax1.0, zxmin1.0, zxmax2.0, zxmin2.0] + } + }; + if rfact != 1.0 { + for k in 0..4 { + roots[k] *= rfact; + } + } + roots +} diff --git a/libraries/bezier-rs/src/quartic_solver2.rs b/libraries/bezier-rs/src/quartic_solver2.rs new file mode 100644 index 0000000000..a0c9bcd5ac --- /dev/null +++ b/libraries/bezier-rs/src/quartic_solver2.rs @@ -0,0 +1,540 @@ +/// Find real roots of cubic equation. +/// +/// The implementation is not (yet) fully robust, but it does handle the case +/// where `c3` is zero (in that case, solving the quadratic equation). +/// +/// See: +/// +/// That implementation is in turn based on Jim Blinn's "How to Solve a Cubic +/// Equation", which is masterful. +/// +/// Return values of x for which c0 + c1 x + c2 x² + c3 x³ = 0. +pub fn solve_cubic(c0: f64, c1: f64, c2: f64, c3: f64) -> [Option; 3] { + let c3_recip = c3.recip(); + const ONETHIRD: f64 = 1. / 3.; + let scaled_c2 = c2 * (ONETHIRD * c3_recip); + let scaled_c1 = c1 * (ONETHIRD * c3_recip); + let scaled_c0 = c0 * c3_recip; + if !(scaled_c0.is_finite() && scaled_c1.is_finite() && scaled_c2.is_finite()) { + // cubic coefficient is zero or nearly so. + let [a, b] = solve_quadratic(c0, c1, c2); + return [a, b, None]; + } + let (c0, c1, c2) = (scaled_c0, scaled_c1, scaled_c2); + // (d0, d1, d2) is called "Delta" in article + let d0 = (-c2).mul_add(c2, c1); + let d1 = (-c1).mul_add(c2, c0); + let d2 = c2 * c0 - c1 * c1; + // d is called "Discriminant" + let d = 4.0 * d0 * d2 - d1 * d1; + // de is called "Depressed.x", Depressed.y = d0 + let de = (-2.0 * c2).mul_add(d0, d1); + // TODO: handle the cases where these intermediate results overflow. + if d < 0.0 { + let sq = (-0.25 * d).sqrt(); + let r = -0.5 * de; + let t1 = (r + sq).cbrt() + (r - sq).cbrt(); + [Some(t1 - c2), None, None] + } else if d == 0.0 { + let t1 = (-d0).sqrt().copysign(de); + [Some(t1 - c2), Some(-2.0 * t1 - c2), None] + } else { + let th = d.sqrt().atan2(-de) * ONETHIRD; + // (th_cos, th_sin) is called "CubicRoot" + let (th_sin, th_cos) = th.sin_cos(); + // (r0, r1, r2) is called "Root" + let r0 = th_cos; + let ss3 = th_sin * 3.0f64.sqrt(); + let r1 = 0.5 * (-th_cos + ss3); + let r2 = 0.5 * (-th_cos - ss3); + let t = 2.0 * (-d0).sqrt(); + [Some(t.mul_add(r0, -c2)), Some(t.mul_add(r1, -c2)), Some(t.mul_add(r2, -c2))] + } +} + +/// Find real roots of quadratic equation. +/// +/// Return values of x for which c0 + c1 x + c2 x² = 0. +/// +/// This function tries to be quite numerically robust. If the equation +/// is nearly linear, it will return the root ignoring the quadratic term; +/// the other root might be out of representable range. In the degenerate +/// case where all coefficients are zero, so that all values of x satisfy +/// the equation, a single `0.0` is returned. +pub fn solve_quadratic(c0: f64, c1: f64, c2: f64) -> [Option; 2] { + let sc0 = c0 * c2.recip(); + let sc1 = c1 * c2.recip(); + if !sc0.is_finite() || !sc1.is_finite() { + // c2 is zero or very small, treat as linear eqn + let root = -c0 / c1; + if root.is_finite() { + return [Some(root), None]; + } else if c0 == 0.0 && c1 == 0.0 { + // Degenerate case + return [Some(0.0), None]; + } + } + let arg = sc1 * sc1 - 4. * sc0; + let root1 = if !arg.is_finite() { + // Likely, calculation of sc1 * sc1 overflowed. Find one root + // using sc1 x + x² = 0, other root as sc0 / root1. + -sc1 + } else { + if arg < 0.0 { + return [None, None]; + } else if arg == 0.0 { + return [Some(-0.5 * sc1), None]; + } + // See https://math.stackexchange.com/questions/866331 + -0.5 * (sc1 + arg.sqrt().copysign(sc1)) + }; + let root2 = sc0 / root1; + if root2.is_finite() { + // Sort just to be friendly and make results deterministic. + if root2 > root1 { + [Some(root1), Some(root2)] + } else { + [Some(root2), Some(root1)] + } + } else { + [Some(root1), None] + } +} + +/// Compute epsilon relative to coefficient. +/// +/// A helper function from the Orellana and De Michele paper. +fn eps_rel(raw: f64, a: f64) -> f64 { + if a == 0.0 { + raw.abs() + } else { + ((raw - a) / a).abs() + } +} + +/// Find real roots of a quartic equation. +/// +/// This is a fairly literal implementation of the method described in: +/// Algorithm 1010: Boosting Efficiency in Solving Quartic Equations with +/// No Compromise in Accuracy, Orellana and De Michele, ACM +/// Transactions on Mathematical Software, Vol. 46, No. 2, May 2020. +pub fn solve_quartic(c0: f64, c1: f64, c2: f64, c3: f64, c4: f64) -> [Option; 4] { + if c4 == 0.0 { + let [a, b, c] = solve_cubic(c0, c1, c2, c3); + return [a, b, c, None]; + } + if c0 == 0.0 { + // Note: appends 0 root at end, doesn't sort. We might want to do that. + let [a, b, c] = solve_cubic(c1, c2, c3, c4); + return [a, b, c, Some(0.0)]; + } + let a = c3 / c4; + let b = c2 / c4; + let c = c1 / c4; + let d = c0 / c4; + if let Some(result) = solve_quartic_inner(a, b, c, d, false) { + return result; + } + // Do polynomial rescaling + const K_Q: f64 = 7.16e76; + for rescale in [false, true] { + if let Some(result) = solve_quartic_inner(a / K_Q, b / K_Q.powi(2), c / K_Q.powi(3), d / K_Q.powi(4), rescale) { + let [a, b, c, d] = result; + return [a.map(|x| x * K_Q), b.map(|x| x * K_Q), c.map(|x| x * K_Q), d.map(|x| x * K_Q)]; + } + } + // Overflow happened, just return no roots. + Default::default() +} + +fn solve_quartic_inner(a: f64, b: f64, c: f64, d: f64, rescale: bool) -> Option<[Option; 4]> { + factor_quartic_inner(a, b, c, d, rescale).map(|quadratics| { + let mut quartics = quadratics.into_iter().flatten().flat_map(|(a, b)| solve_quadratic(b, a, 1.0)); + [quartics.next().flatten(), quartics.next().flatten(), quartics.next().flatten(), quartics.next().flatten()] + }) +} + +/// Factor a quartic into two quadratics. +/// +/// Attempt to factor a quartic equation into two quadratic equations. Returns `None` either if there +/// is overflow (in which case rescaling might succeed) or the factorization would result in +/// complex coefficients. +/// +/// Discussion question: distinguish the two cases in return value? +pub fn factor_quartic_inner(a: f64, b: f64, c: f64, d: f64, rescale: bool) -> Option<[Option<(f64, f64)>; 2]> { + let calc_eps_q = |a1, b1, a2, b2| { + let eps_a = eps_rel(a1 + a2, a); + let eps_b = eps_rel(b1 + a1 * a2 + b2, b); + let eps_c = eps_rel(b1 * a2 + a1 * b2, c); + eps_a + eps_b + eps_c + }; + let calc_eps_t = |a1, b1, a2, b2| calc_eps_q(a1, b1, a2, b2) + eps_rel(b1 * b2, d); + let disc = 9. * a * a - 24. * b; + let s = if disc >= 0.0 { -2. * b / (3. * a + disc.sqrt().copysign(a)) } else { -0.25 * a }; + let a_prime = a + 4. * s; + let b_prime = b + 3. * s * (a + 2. * s); + let c_prime = c + s * (2. * b + s * (3. * a + 4. * s)); + let d_prime = d + s * (c + s * (b + s * (a + s))); + let g_prime; + let h_prime; + const K_C: f64 = 3.49e102; + if rescale { + let a_prime_s = a_prime / K_C; + let b_prime_s = b_prime / K_C; + let c_prime_s = c_prime / K_C; + let d_prime_s = d_prime / K_C; + g_prime = a_prime_s * c_prime_s - (4. / K_C) * d_prime_s - (1. / 3.) * b_prime_s.powi(2); + h_prime = (a_prime_s * c_prime_s + (8. / K_C) * d_prime_s - (2. / 9.) * b_prime_s.powi(2)) * (1. / 3.) * b_prime_s - c_prime_s * (c_prime_s / K_C) - a_prime_s.powi(2) * d_prime_s; + } else { + g_prime = a_prime * c_prime - 4. * d_prime - (1. / 3.) * b_prime.powi(2); + h_prime = (a_prime * c_prime + 8. * d_prime - (2. / 9.) * b_prime.powi(2)) * (1. / 3.) * b_prime - c_prime.powi(2) - a_prime.powi(2) * d_prime; + } + if !(g_prime.is_finite() && h_prime.is_finite()) { + return None; + } + let phi = depressed_cubic_dominant(g_prime, h_prime); + let phi = if rescale { phi * K_C } else { phi }; + let l_1 = a * 0.5; + let l_3 = (1. / 6.) * b + 0.5 * phi; + let delt_2 = c - a * l_3; + let d_2_cand_1 = (2. / 3.) * b - phi - l_1 * l_1; + let l_2_cand_1 = 0.5 * delt_2 / d_2_cand_1; + let l_2_cand_2 = 2. * (d - l_3 * l_3) / delt_2; + let d_2_cand_2 = 0.5 * delt_2 / l_2_cand_2; + let d_2_cand_3 = d_2_cand_1; + let l_2_cand_3 = l_2_cand_2; + let mut d_2_best = 0.0; + let mut l_2_best = 0.0; + let mut eps_l_best = 0.0; + for (i, (d_2, l_2)) in [(d_2_cand_1, l_2_cand_1), (d_2_cand_2, l_2_cand_2), (d_2_cand_3, l_2_cand_3)].iter().enumerate() { + let eps_0 = eps_rel(d_2 + l_1 * l_1 + 2. * l_3, b); + let eps_1 = eps_rel(2. * (d_2 * l_2 + l_1 * l_3), c); + let eps_2 = eps_rel(d_2 * l_2 * l_2 + l_3 * l_3, d); + let eps_l = eps_0 + eps_1 + eps_2; + if i == 0 || eps_l < eps_l_best { + d_2_best = *d_2; + l_2_best = *l_2; + eps_l_best = eps_l; + } + } + let d_2 = d_2_best; + let l_2 = l_2_best; + let mut alpha_1; + let mut beta_1; + let mut alpha_2; + let mut beta_2; + + if d_2 < 0.0 { + let sq = (-d_2).sqrt(); + alpha_1 = l_1 + sq; + beta_1 = l_3 + sq * l_2; + alpha_2 = l_1 - sq; + beta_2 = l_3 - sq * l_2; + if beta_2.abs() < beta_1.abs() { + beta_2 = d / beta_1; + } else if beta_2.abs() > beta_1.abs() { + beta_1 = d / beta_2; + } + let cands; + if alpha_1.abs() != alpha_2.abs() { + if alpha_1.abs() < alpha_2.abs() { + let a1_cand_1 = (c - beta_1 * alpha_2) / beta_2; + let a1_cand_2 = (b - beta_2 - beta_1) / alpha_2; + let a1_cand_3 = a - alpha_2; + // Note: cand 3 is first because it is infallible, simplifying logic + cands = [(a1_cand_3, alpha_2), (a1_cand_1, alpha_2), (a1_cand_2, alpha_2)]; + } else { + let a2_cand_1 = (c - alpha_1 * beta_2) / beta_1; + let a2_cand_2 = (b - beta_2 - beta_1) / alpha_1; + let a2_cand_3 = a - alpha_1; + cands = [(alpha_1, a2_cand_3), (alpha_1, a2_cand_1), (alpha_1, a2_cand_2)]; + } + let mut eps_q_best = 0.0; + for (i, (a1, a2)) in cands.iter().enumerate() { + if a1.is_finite() && a2.is_finite() { + let eps_q = calc_eps_q(*a1, beta_1, *a2, beta_2); + if i == 0 || eps_q < eps_q_best { + alpha_1 = *a1; + alpha_2 = *a2; + eps_q_best = eps_q; + } + } + } + } + } else if d_2 == 0.0 { + let d_3 = d - l_3 * l_3; + alpha_1 = l_1; + beta_1 = l_3 + (-d_3).sqrt(); + alpha_2 = l_1; + beta_2 = l_3 - (-d_3).sqrt(); + if beta_1.abs() > beta_2.abs() { + beta_2 = d / beta_1; + } else if beta_2.abs() > beta_1.abs() { + beta_1 = d / beta_2; + } + // TODO: handle case d_2 is very small? + } else { + // This case means no real roots; in the most general case we might want + // to factor into quadratic equations with complex coefficients. + return None; + } + // Newton-Raphson iteration on alpha/beta coeff's. + let mut eps_t = calc_eps_t(alpha_1, beta_1, alpha_2, beta_2); + for _ in 0..8 { + if eps_t == 0.0 { + break; + } + let f_0 = beta_1 * beta_2 - d; + let f_1 = beta_1 * alpha_2 + alpha_1 * beta_2 - c; + let f_2 = beta_1 + alpha_1 * alpha_2 + beta_2 - b; + let f_3 = alpha_1 + alpha_2 - a; + let c_1 = alpha_1 - alpha_2; + let det_j = beta_1 * beta_1 - beta_1 * (alpha_2 * c_1 + 2. * beta_2) + beta_2 * (alpha_1 * c_1 + beta_2); + if det_j == 0.0 { + break; + } + let inv = det_j.recip(); + let c_2 = beta_2 - beta_1; + let c_3 = beta_1 * alpha_2 - alpha_1 * beta_2; + let dz_0 = c_1 * f_0 + c_2 * f_1 + c_3 * f_2 - (beta_1 * c_2 + alpha_1 * c_3) * f_3; + let dz_1 = (alpha_1 * c_1 + c_2) * f_0 - beta_1 * c_1 * f_1 - beta_1 * c_2 * f_2 - beta_1 * c_3 * f_3; + let dz_2 = -c_1 * f_0 - c_2 * f_1 - c_3 * f_2 + (alpha_2 * c_3 + beta_2 * c_2) * f_3; + let dz_3 = -(alpha_2 * c_1 + c_2) * f_0 + beta_2 * c_1 * f_1 + beta_2 * c_2 * f_2 + beta_2 * c_3 * f_3; + let a1 = alpha_1 - inv * dz_0; + let b1 = beta_1 - inv * dz_1; + let a2 = alpha_2 - inv * dz_2; + let b2 = beta_2 - inv * dz_3; + let new_eps_t = calc_eps_t(a1, b1, a2, b2); + // We break if the new eps is equal, paper keeps going + if new_eps_t < eps_t { + alpha_1 = a1; + beta_1 = b1; + alpha_2 = a2; + beta_2 = b2; + eps_t = new_eps_t; + } else { + break; + } + } + Some([Some((alpha_1, beta_1)), Some((alpha_2, beta_2))]) +} + +/// Dominant root of depressed cubic x^3 + gx + h = 0. +/// +/// Section 2.2 of Orellana and De Michele. +// Note: some of the techniques in here might be useful to improve the +// cubic solver, and vice versa. +fn depressed_cubic_dominant(g: f64, h: f64) -> f64 { + let q = (-1. / 3.) * g; + let r = 0.5 * h; + let phi_0; + let k = if q.abs() < 1e102 && r.abs() < 1e154 { + None + } else if q.abs() < r.abs() { + Some(1. - q * (q / r).powi(2)) + } else { + Some(q.signum() * ((r / q).powi(2) / q - 1.0)) + }; + if k.is_some() && r == 0.0 { + if g > 0.0 { + phi_0 = 0.0; + } else { + phi_0 = (-g).sqrt(); + } + } else if k.map(|k| k < 0.0).unwrap_or_else(|| r * r < q.powi(3)) { + let t = if k.is_some() { r / q / q.sqrt() } else { r / q.powi(3).sqrt() }; + phi_0 = -2. * q.sqrt() * (t.abs().acos() * (1. / 3.)).cos().copysign(t); + } else { + let a = if let Some(k) = k { + if q.abs() < r.abs() { + -r * (1. + k.sqrt()) + } else { + -r - (q.abs().sqrt() * q * k.sqrt()).copysign(r) + } + } else { + -r - (r * r - q.powi(3)).sqrt().copysign(r) + } + .cbrt(); + let b = if a == 0.0 { 0.0 } else { q / a }; + phi_0 = a + b; + } + // Refine with Newton-Raphson iteration + let mut x = phi_0; + let mut f = (x * x + g) * x + h; + const EPS_M: f64 = 2.22045e-16; + if f.abs() < EPS_M * x.powi(3).max(g * x).max(h) { + return x; + } + for _ in 0..8 { + let delt_f = 3. * x * x + g; + if delt_f == 0.0 { + break; + } + let new_x = x - f / delt_f; + let new_f = (new_x * new_x + g) * new_x + h; + if new_f == 0.0 { + return new_x; + } + if new_f.abs() >= f.abs() { + break; + } + x = new_x; + f = new_f; + } + x +} + +/// Find real roots of a quintic equation. +/// +/// Return values of x for which c0 + c1 x + c2 x^2 + c3 x^3 + c4 x^4 + c5 x^5 = 0. +pub fn solve_quintic(c0: f64, c1: f64, c2: f64, c3: f64, c4: f64, c5: f64) -> [Option; 5] { + if c5 == 0.0 { + let [a, b, c, d] = solve_quartic(c0, c1, c2, c3, c4); + return [a, b, c, d, None]; + } + + // Normalize coefficients + let c0 = c0 / c5; + let c1 = c1 / c5; + let c2 = c2 / c5; + let c3 = c3 / c5; + let c4 = c4 / c5; + + // Define the quintic function + let quintic_fn = |x: f64| c0 + x * (c1 + x * (c2 + x * (c3 + x * c4))); + + // Find potential root intervals + let mut roots = Vec::new(); + let mut a = -10.0; // Initial lower bound + let mut ya = quintic_fn(a); + + for _ in 0..20 { + let b = a + 1.0; // Increment upper bound + let yb = quintic_fn(b); + + if ya.signum() != yb.signum() { + // Root is likely in the interval [a, b] + const EPSILON: f64 = 1e-7; + const N0: usize = 1; + const K1: f64 = 0.2; + + let root = solve_itp(quintic_fn, a, b, EPSILON, N0, K1, ya, yb); + roots.push(Some(root)); + } + + a = b; + ya = yb; + } + + // Convert the Vec> to a fixed-size array [Option; 5] + let mut result: [Option; 5] = [None; 5]; + for (i, root) in roots.into_iter().enumerate().take(5) { + result[i] = root; + } + + result +} + +/// Solve an arbitrary function for a zero-crossing. +/// +/// This uses the [ITP method], as described in the paper +/// [An Enhancement of the Bisection Method Average Performance Preserving Minmax Optimality]. +/// +/// The values of `ya` and `yb` are given as arguments rather than +/// computed from `f`, as the values may already be known, or they may +/// be less expensive to compute as special cases. +/// +/// It is assumed that `ya < 0.0` and `yb > 0.0`, otherwise unexpected +/// results may occur. +/// +/// The value of `epsilon` must be larger than 2^-63 times `b - a`, +/// otherwise integer overflow may occur. The `a` and `b` parameters +/// represent the lower and upper bounds of the bracket searched for a +/// solution. +/// +/// The ITP method has tuning parameters. This implementation hardwires +/// k2 to 2, both because it avoids an expensive floating point +/// exponentiation, and because this value has been tested to work well +/// with curve fitting problems. +/// +/// The `n0` parameter controls the relative impact of the bisection and +/// secant components. When it is 0, the number of iterations is +/// guaranteed to be no more than the number required by bisection (thus, +/// this method is strictly superior to bisection). However, when the +/// function is smooth, a value of 1 gives the secant method more of a +/// chance to engage, so the average number of iterations is likely +/// lower, though there can be one more iteration than bisection in the +/// worst case. +/// +/// The `k1` parameter is harder to characterize, and interested users +/// are referred to the paper, as well as encouraged to do empirical +/// testing. To match the paper, a value of `0.2 / (b - a)` is +/// suggested, and this is confirmed to give good results. +/// +/// When the function is monotonic, the returned result is guaranteed to +/// be within `epsilon` of the zero crossing. For more detailed analysis, +/// again see the paper. +/// +/// [ITP method]: https://en.wikipedia.org/wiki/ITP_Method +/// [An Enhancement of the Bisection Method Average Performance Preserving Minmax Optimality]: https://dl.acm.org/doi/10.1145/3423597 +#[allow(clippy::too_many_arguments)] +pub fn solve_itp(mut f: impl FnMut(f64) -> f64, mut a: f64, mut b: f64, epsilon: f64, n0: usize, k1: f64, mut ya: f64, mut yb: f64) -> f64 { + let n1_2 = (((b - a) / epsilon).log2().ceil() - 1.0).max(0.0) as usize; + let nmax = n0 + n1_2; + let mut scaled_epsilon = epsilon * (1u64 << nmax) as f64; + while b - a > 2.0 * epsilon { + let x1_2 = 0.5 * (a + b); + let r = scaled_epsilon - 0.5 * (b - a); + let xf = (yb * a - ya * b) / (yb - ya); + let sigma = x1_2 - xf; + // This has k2 = 2 hardwired for efficiency. + let delta = k1 * (b - a).powi(2); + let xt = if delta <= (x1_2 - xf).abs() { xf + delta.copysign(sigma) } else { x1_2 }; + let xitp = if (xt - x1_2).abs() <= r { xt } else { x1_2 - r.copysign(sigma) }; + let yitp = f(xitp); + if yitp > 0.0 { + b = xitp; + yb = yitp; + } else if yitp < 0.0 { + a = xitp; + ya = yitp; + } else { + return xitp; + } + scaled_epsilon *= 0.5; + } + 0.5 * (a + b) +} + +/// A variant ITP solver that allows fallible functions. +/// +/// Another difference: it returns the bracket that contains the root, +/// which may be important if the function has a discontinuity. +#[allow(clippy::too_many_arguments)] +pub(crate) fn solve_itp_fallible(mut f: impl FnMut(f64) -> Result, mut a: f64, mut b: f64, epsilon: f64, n0: usize, k1: f64, mut ya: f64, mut yb: f64) -> Result<(f64, f64), E> { + let n1_2 = (((b - a) / epsilon).log2().ceil() - 1.0).max(0.0) as usize; + let nmax = n0 + n1_2; + let mut scaled_epsilon = epsilon * (1u64 << nmax) as f64; + while b - a > 2.0 * epsilon { + let x1_2 = 0.5 * (a + b); + let r = scaled_epsilon - 0.5 * (b - a); + let xf = (yb * a - ya * b) / (yb - ya); + let sigma = x1_2 - xf; + // This has k2 = 2 hardwired for efficiency. + let delta = k1 * (b - a).powi(2); + let xt = if delta <= (x1_2 - xf).abs() { xf + delta.copysign(sigma) } else { x1_2 }; + let xitp = if (xt - x1_2).abs() <= r { xt } else { x1_2 - r.copysign(sigma) }; + let yitp = f(xitp)?; + if yitp > 0.0 { + b = xitp; + yb = yitp; + } else if yitp < 0.0 { + a = xitp; + ya = yitp; + } else { + return Ok((xitp, xitp)); + } + scaled_epsilon *= 0.5; + } + Ok((a, b)) +}