diff --git a/libraries/bezier-rs/src/bezier/solvers.rs b/libraries/bezier-rs/src/bezier/solvers.rs index 9d5fb811..6330d935 100644 --- a/libraries/bezier-rs/src/bezier/solvers.rs +++ b/libraries/bezier-rs/src/bezier/solvers.rs @@ -134,15 +134,20 @@ impl Bezier { match self.handles { BezierHandles::Linear => [[None; 3]; 2], BezierHandles::Quadratic { handle } => { - let a = handle - self.start; - let b = self.end - handle; - let b_minus_a = b - a; - [utils::solve_linear(b_minus_a.x, a.x), utils::solve_linear(b_minus_a.y, a.y)] + let d0 = handle - self.start; + let d1 = self.end - handle; + let dd = d1 - d0; + let a = (dd.x != 0.).then(|| -d0.x / dd.x); + let b = (dd.y != 0.).then(|| -d0.y / dd.y); + [[a, None, None], [b, None, None]] } BezierHandles::Cubic { handle_start, handle_end } => { - let a = 3. * (-self.start + 3. * handle_start - 3. * handle_end + self.end); - let b = 6. * (self.start - 2. * handle_start + handle_end); - let c = 3. * (handle_start - self.start); + let d0 = handle_start - self.start; + let d1 = handle_end - handle_start; + let d2 = self.end - handle_end; + let a = d0 - 2. * d1 + d2; + let b = 2. * (d1 - d0); + let c = d0; let discriminant = b * b - 4. * a * c; let two_times_a = 2. * a; [ @@ -592,9 +597,27 @@ impl Bezier { /// /// Cast a ray to the left and count intersections. pub fn winding(&self, target_point: DVec2) -> i32 { - let extrema = self.get_extrema_t_list(); - extrema + let [x_extrema_t, y_extrema_t] = self.unrestricted_local_extrema(); + let mut x_extrema_t = x_extrema_t.map(|t| t.filter(|&t| t > 0. && t < 1.)); + let mut y_extrema_t = y_extrema_t.map(|t| t.filter(|&t| t > 0. && t < 1.)); + + let mut results = [None; 8]; + results[7] = Some(1.); + for i in (0..7).rev() { + let Some(min) = x_extrema_t.iter_mut().chain(y_extrema_t.iter_mut()).max_by(|a, b| a.partial_cmp(b).unwrap()) else { + results[i] = Some(0.); + break; + }; + if let Some(value) = min.take() { + results[i] = Some(value); + } else { + results[i] = Some(0.); + break; + } + } + results .windows(2) + .flat_map(|t| t[0].and_then(|first| t[1].map(|second| [first, second]))) .map(|t| self.trim(TValue::Parametric(t[0]), TValue::Parametric(t[1])).pre_split_winding_number(target_point)) .sum() } @@ -737,7 +760,7 @@ mod tests { let p = DVec2::new(127., 121.); validate(bz, p); - let bz = Bezier::from_cubic_coordinates(55.0, 30.0, 85.0, 140.0, 175.0, 30.0, 185.0, 160.0); + let bz = Bezier::from_cubic_coordinates(55., 30., 85., 140., 175., 30., 185., 160.); let p = DVec2::new(17., 172.); validate(bz, p); } diff --git a/libraries/bezier-rs/src/subpath/solvers.rs b/libraries/bezier-rs/src/subpath/solvers.rs index 810f6f81..7f2eb992 100644 --- a/libraries/bezier-rs/src/subpath/solvers.rs +++ b/libraries/bezier-rs/src/subpath/solvers.rs @@ -99,6 +99,11 @@ impl Subpath { test1 && test2 } + /// Computes the winding number contribution of the subpath. + pub fn winding_order(&self, point: DVec2) -> i32 { + self.iter().map(|segment| segment.winding(point)).sum() + } + /// Returns a list of `t` values that correspond to the self intersection points of the subpath. For each intersection point, the returned `t` value is the smaller of the two that correspond to the point. /// - `error` - For intersections with non-linear beziers, `error` defines the threshold for bounding boxes to be considered an intersection point. /// - `minimum_separation`: the minimum difference two adjacent `t`-values must have when comparing adjacent `t`-values in sorted order. @@ -287,21 +292,7 @@ impl Subpath { shape.set_closed(true); shape.apply_transform(DAffine2::from_translation((-offset_x, -offset_y).into())); - const SIN_13DEG: f64 = 0.22495105434; - const COS_13DEG: f64 = 0.97437006478; - let rotated_subpath = |ray_direction: DVec2| { - // Rotate the bezier and the line by the angle that the line makes with the x axis - let angle = ray_direction.angle_between(DVec2::new(0., 1.)); - let rotation_matrix = DMat2::from_angle(angle); - - let mut prerotated = shape.clone(); - prerotated.apply_transform(DAffine2::from_angle(angle)); - (rotation_matrix, prerotated) - }; - // The directions use prime numbers to reduce the likelihood of running across two anchor points simultaneously - let (matrix1, prerotated1) = rotated_subpath(DVec2::new(SIN_13DEG, COS_13DEG)); - let (matrix2, prerotated2) = rotated_subpath(DVec2::new(-COS_13DEG, -SIN_13DEG)); - let point_in_shape_checker = |point: DVec2| shape.point_inside_prerotated(point, matrix1, matrix2, &prerotated1, &prerotated2); + let point_in_shape_checker = |point: DVec2| shape.winding_order(point) != 0; let square_edges_intersect_shape_checker = |corner1: DVec2, size: f64| { let corner2 = corner1 + DVec2::splat(size); diff --git a/libraries/bezier-rs/src/utils.rs b/libraries/bezier-rs/src/utils.rs index 9e04c58e..898e9f5e 100644 --- a/libraries/bezier-rs/src/utils.rs +++ b/libraries/bezier-rs/src/utils.rs @@ -1,8 +1,7 @@ -use crate::consts::{MAX_ABSOLUTE_DIFFERENCE, MIN_SEPARATION_VALUE, STRICT_MAX_ABSOLUTE_DIFFERENCE}; +use crate::consts::{MAX_ABSOLUTE_DIFFERENCE, STRICT_MAX_ABSOLUTE_DIFFERENCE}; use crate::ManipulatorGroup; use glam::{BVec2, DMat2, DVec2}; -use std::f64::consts::PI; #[derive(Copy, Clone, PartialEq)] /// A structure which can be used to reference a particular point along a `Bezier`. @@ -122,51 +121,6 @@ pub fn solve_quadratic(discriminant: f64, two_times_a: f64, b: f64, c: f64) -> [ roots } -/// Compute the cube root of a number. -fn cube_root(f: f64) -> f64 { - if f < 0. { - -(-f).cbrt() - } else { - f.cbrt() - } -} - -// TODO: Use an `impl Iterator` return type instead of a `Vec` -/// Solve a cubic of the form `x^3 + px + q`, derivation from: . -pub fn solve_reformatted_cubic(discriminant: f64, a: f64, p: f64, q: f64) -> [Option; 3] { - let mut roots = [None; 3]; - if discriminant.abs() <= STRICT_MAX_ABSOLUTE_DIFFERENCE { - // When discriminant is 0 (check for approximation because of floating point errors), all roots are real, and 2 are repeated - // filter out repeated roots (ie. roots whose distance is less than some epsilon) - let q_divided_by_2 = q / 2.; - let a_divided_by_3 = a / 3.; - let root_1 = 2. * cube_root(-q_divided_by_2) - a_divided_by_3; - let root_2 = cube_root(q_divided_by_2) - a_divided_by_3; - if (root_1 - root_2).abs() > MIN_SEPARATION_VALUE { - roots[0] = Some(root_1); - } - roots[1] = Some(root_2); - } else if discriminant > 0. { - // When discriminant > 0, there is one real and two imaginary roots - let q_divided_by_2 = q / 2.; - let square_root_discriminant = discriminant.sqrt(); - - roots[0] = Some(cube_root(-q_divided_by_2 + square_root_discriminant) - cube_root(q_divided_by_2 + square_root_discriminant) - a / 3.); - } else { - // Otherwise, discriminant < 0 and there are three real roots - let p_divided_by_3 = p / 3.; - let a_divided_by_3 = a / 3.; - let cube_root_r = (-p_divided_by_3).sqrt(); - let phi = (-q / (2. * cube_root_r.powi(3))).acos(); - - let two_times_cube_root_r = 2. * cube_root_r; - roots[0] = Some(two_times_cube_root_r * (phi / 3.).cos() - a_divided_by_3); - roots[1] = Some(two_times_cube_root_r * ((phi + 2. * PI) / 3.).cos() - a_divided_by_3); - roots[2] = Some(two_times_cube_root_r * ((phi + 4. * PI) / 3.).cos() - a_divided_by_3); - } - roots -} - // TODO: Use an `impl Iterator` return type instead of a `Vec` /// Solve a cubic of the form `ax^3 + bx^2 + ct + d`. pub fn solve_cubic(a: f64, b: f64, c: f64, d: f64) -> [Option; 3] { @@ -180,16 +134,45 @@ pub fn solve_cubic(a: f64, b: f64, c: f64, d: f64) -> [Option; 3] { solve_quadratic(discriminant, 2. * b, c, d) } } else { - // convert at^3 + bt^2 + ct + d ==> t^3 + a't^2 + b't + c' - let new_a = b / a; - let new_b = c / a; - let new_c = d / a; - - // Refactor cubic to be of the form: a(t^3 + pt + q), derivation from: https://trans4mind.com/personal_development/mathematics/polynomials/cubicAlgebra.htm - let p = (3. * new_b - new_a * new_a) / 3.; - let q = (2. * new_a.powi(3) - 9. * new_a * new_b + 27. * new_c) / 27.; - let discriminant = (p / 3.).powi(3) + (q / 2.).powi(2); - solve_reformatted_cubic(discriminant, new_a, p, q) + // https://momentsingraphics.de/CubicRoots.html + let d_recip = a.recip(); + const ONETHIRD: f64 = 1. / 3.; + let scaled_c2 = b * (ONETHIRD * d_recip); + let scaled_c1 = c * (ONETHIRD * d_recip); + let scaled_c0 = d * d_recip; + if !(scaled_c0.is_finite() && scaled_c1.is_finite() && scaled_c2.is_finite()) { + // cubic coefficient is zero or nearly so. + return solve_quadratic(c * c - 4. * b * d, 2. * b, c, d); + } + 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. * d0 * d2 - d1 * d1; + // de is called "Depressed.x", Depressed.y = d0 + let de = (-2. * c2).mul_add(d0, d1); + if d < 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. { + let t1 = (-d0).sqrt().copysign(de); + [Some(t1 - c2), Some(-2. * t1 - c2).filter(|&a| a != 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_f64.sqrt(); + let r1 = 0.5 * (-th_cos + ss3); + let r2 = 0.5 * (-th_cos - ss3); + let t = 2. * (-d0).sqrt(); + [Some(t.mul_add(r0, -c2)), Some(t.mul_add(r1, -c2)), Some(t.mul_add(r2, -c2))] + } } } @@ -321,7 +304,8 @@ mod tests { a.len() == b.len() && a.into_iter().zip(b).all(|(a, b)| f64_compare(a, b, max_abs_diff)) } - fn collect_roots(roots: [Option; 3]) -> Vec { + fn collect_roots(mut roots: [Option; 3]) -> Vec { + roots.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap()); roots.into_iter().flatten().collect() } @@ -342,7 +326,7 @@ mod tests { assert!(roots1 == vec![0.]); let roots2 = collect_roots(solve_cubic(1., 3., 0., -4.)); - assert!(roots2 == vec![1., -2.]); + assert!(roots2 == vec![-2., 1.]); // p == 0 let roots3 = collect_roots(solve_cubic(1., 0., 0., -1.)); @@ -354,11 +338,11 @@ mod tests { // discriminant < 0 let roots5 = collect_roots(solve_cubic(1., 3., 0., -1.)); - assert!(f64_compare_vector(roots5, vec![0.532, -2.879, -0.653], MAX_ABSOLUTE_DIFFERENCE)); + assert!(f64_compare_vector(roots5, vec![-2.879, -0.653, 0.532], MAX_ABSOLUTE_DIFFERENCE)); // quadratic let roots6 = collect_roots(solve_cubic(0., 3., 0., -3.)); - assert!(roots6 == vec![1., -1.]); + assert!(roots6 == vec![-1., 1.]); // linear let roots7 = collect_roots(solve_cubic(0., 0., 1., -1.));