use crate::consts::{MAX_ABSOLUTE_DIFFERENCE, MIN_SEPARATION_VALUE, STRICT_MAX_ABSOLUTE_DIFFERENCE};
use crate::ManipulatorGroup;
use glam::{BVec2, DMat2, DVec2};
use std::f64::consts::PI;
#[derive(Copy, Clone, PartialEq)]
pub enum TValue {
Parametric(f64),
Euclidean(f64),
EuclideanWithinError { t: f64, error: f64 },
}
#[derive(Copy, Clone, PartialEq)]
pub enum TValueType {
Parametric,
Euclidean,
}
#[derive(Copy, Clone, PartialEq)]
pub enum SubpathTValue {
Parametric { segment_index: usize, t: f64 },
GlobalParametric(f64),
Euclidean { segment_index: usize, t: f64 },
GlobalEuclidean(f64),
EuclideanWithinError { segment_index: usize, t: f64, error: f64 },
GlobalEuclideanWithinError { t: f64, error: f64 },
}
#[derive(Copy, Clone)]
pub enum Join {
Bevel,
Miter(Option<f64>),
Round,
}
#[derive(Copy, Clone)]
pub enum Cap {
Butt,
Round,
Square,
}
fn compute_abc_through_points(start_point: DVec2, point_on_curve: DVec2, end_point: DVec2, t_to_nth_power: f64, nth_power_of_one_minus_t: f64) -> [DVec2; 3] {
let point_c_ratio = nth_power_of_one_minus_t / (t_to_nth_power + nth_power_of_one_minus_t);
let c = point_c_ratio * start_point + (1. - point_c_ratio) * end_point;
let ab_bc_ratio = (t_to_nth_power + nth_power_of_one_minus_t - 1.).abs() / (t_to_nth_power + nth_power_of_one_minus_t);
let a = point_on_curve + (point_on_curve - c) / ab_bc_ratio;
[a, point_on_curve, c]
}
pub fn compute_abc_for_quadratic_through_points(start_point: DVec2, point_on_curve: DVec2, end_point: DVec2, t: f64) -> [DVec2; 3] {
let t_squared = t * t;
let one_minus_t = 1. - t;
let squared_one_minus_t = one_minus_t * one_minus_t;
compute_abc_through_points(start_point, point_on_curve, end_point, t_squared, squared_one_minus_t)
}
pub fn compute_abc_for_cubic_through_points(start_point: DVec2, point_on_curve: DVec2, end_point: DVec2, t: f64) -> [DVec2; 3] {
let t_cubed = t * t * t;
let one_minus_t = 1. - t;
let cubed_one_minus_t = one_minus_t * one_minus_t * one_minus_t;
compute_abc_through_points(start_point, point_on_curve, end_point, t_cubed, cubed_one_minus_t)
}
pub fn get_closest_point_in_lut(lut: &[DVec2], point: DVec2) -> (usize, f64) {
lut.iter().enumerate().map(|(i, p)| (i, point.distance_squared(*p))).min_by(|x, y| (x.1).total_cmp(&(y.1))).unwrap()
}
pub fn solve_linear(a: f64, b: f64) -> [Option<f64>; 3] {
if a.abs() > MAX_ABSOLUTE_DIFFERENCE {
[Some(-b / a), None, None]
} else {
[None; 3]
}
}
pub fn solve_quadratic(discriminant: f64, two_times_a: f64, b: f64, c: f64) -> [Option<f64>; 3] {
let mut roots = [None; 3];
if two_times_a.abs() <= STRICT_MAX_ABSOLUTE_DIFFERENCE {
roots = solve_linear(b, c);
} else if discriminant.abs() <= STRICT_MAX_ABSOLUTE_DIFFERENCE {
roots[0] = Some(-b / (two_times_a));
} else if discriminant > 0. {
let root_discriminant = discriminant.sqrt();
roots[0] = Some((-b + root_discriminant) / (two_times_a));
roots[1] = Some((-b - root_discriminant) / (two_times_a));
}
roots
}
fn cube_root(f: f64) -> f64 {
if f < 0. {
-(-f).cbrt()
} else {
f.cbrt()
}
}
pub fn solve_reformatted_cubic(discriminant: f64, a: f64, p: f64, q: f64) -> [Option<f64>; 3] {
let mut roots = [None; 3];
if discriminant.abs() <= STRICT_MAX_ABSOLUTE_DIFFERENCE {
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. {
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 {
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
}
pub fn solve_cubic(a: f64, b: f64, c: f64, d: f64) -> [Option<f64>; 3] {
if a.abs() <= STRICT_MAX_ABSOLUTE_DIFFERENCE {
if b.abs() <= STRICT_MAX_ABSOLUTE_DIFFERENCE {
solve_linear(c, d)
} else {
let discriminant = c * c - 4. * b * d;
solve_quadratic(discriminant, 2. * b, c, d)
}
} else {
let new_a = b / a;
let new_b = c / a;
let new_c = d / a;
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)
}
}
pub fn do_rectangles_overlap(rectangle1: [DVec2; 2], rectangle2: [DVec2; 2]) -> bool {
let [bottom_left1, top_right1] = rectangle1;
let [bottom_left2, top_right2] = rectangle2;
top_right1.x >= bottom_left2.x && top_right2.x >= bottom_left1.x && top_right2.y >= bottom_left1.y && top_right1.y >= bottom_left2.y
}
pub fn line_intersection(point1: DVec2, point1_slope_vector: DVec2, point2: DVec2, point2_slope_vector: DVec2) -> DVec2 {
assert!(point1_slope_vector.normalize() != point2_slope_vector.normalize());
if f64_compare(point1_slope_vector.x, 0., MAX_ABSOLUTE_DIFFERENCE) {
let m2 = point2_slope_vector.y / point2_slope_vector.x;
let b2 = point2.y - m2 * point2.x;
DVec2::new(point1.x, point1.x * m2 + b2)
}
else if f64_compare(point2_slope_vector.x, 0., MAX_ABSOLUTE_DIFFERENCE) {
let m1 = point1_slope_vector.y / point1_slope_vector.x;
let b1 = point1.y - m1 * point1.x;
DVec2::new(point2.x, point2.x * m1 + b1)
}
else {
let m1 = point1_slope_vector.y / point1_slope_vector.x;
let b1 = point1.y - m1 * point1.x;
let m2 = point2_slope_vector.y / point2_slope_vector.x;
let b2 = point2.y - m2 * point2.x;
let intersection_x = (b2 - b1) / (m1 - m2);
DVec2::new(intersection_x, intersection_x * m1 + b1)
}
}
pub fn are_points_collinear(p1: DVec2, p2: DVec2, p3: DVec2) -> bool {
let matrix = DMat2::from_cols(p1 - p2, p2 - p3);
f64_compare(matrix.determinant() / 2., 0., MAX_ABSOLUTE_DIFFERENCE)
}
pub fn compute_circle_center_from_points(p1: DVec2, p2: DVec2, p3: DVec2) -> Option<DVec2> {
if are_points_collinear(p1, p2, p3) {
return None;
}
let midpoint_a = p1.lerp(p2, 0.5);
let midpoint_b = p2.lerp(p3, 0.5);
let midpoint_c = p3.lerp(p1, 0.5);
let tangent_a = (p1 - p2).perp();
let tangent_b = (p2 - p3).perp();
let tangent_c = (p3 - p1).perp();
let intersect_a_b = line_intersection(midpoint_a, tangent_a, midpoint_b, tangent_b);
let intersect_b_c = line_intersection(midpoint_b, tangent_b, midpoint_c, tangent_c);
let intersect_c_a = line_intersection(midpoint_c, tangent_c, midpoint_a, tangent_a);
Some((intersect_a_b + intersect_b_c + intersect_c_a) / 3.)
}
pub fn f64_compare(a: f64, b: f64, max_abs_diff: f64) -> bool {
(a - b).abs() < max_abs_diff
}
pub fn f64_approximately_in_range(value: f64, min: f64, max: f64, max_abs_diff: f64) -> bool {
(min..=max).contains(&value) || f64_compare(value, min, max_abs_diff) || f64_compare(value, max, max_abs_diff)
}
pub fn dvec2_compare(a: DVec2, b: DVec2, max_abs_diff: f64) -> BVec2 {
BVec2::new((a.x - b.x).abs() < max_abs_diff, (a.y - b.y).abs() < max_abs_diff)
}
pub fn dvec2_approximately_in_range(point: DVec2, min_corner: DVec2, max_corner: DVec2, max_abs_diff: f64) -> BVec2 {
(point.cmpge(min_corner) & point.cmple(max_corner)) | dvec2_compare(point, min_corner, max_abs_diff) | dvec2_compare(point, max_corner, max_abs_diff)
}
pub fn scale_point_from_direction_vector(point: DVec2, direction_unit_vector: DVec2, should_flip_direction: bool, distance: f64) -> DVec2 {
let should_reverse_factor = if should_flip_direction { -1. } else { 1. };
point + distance * direction_unit_vector * should_reverse_factor
}
pub fn scale_point_from_origin(point: DVec2, origin: DVec2, should_flip_direction: bool, distance: f64) -> DVec2 {
scale_point_from_direction_vector(point, (origin - point).normalize(), should_flip_direction, distance)
}
pub fn compute_circular_subpath_details<ManipulatorGroupId: crate::Identifier>(
left: DVec2,
arc_point: DVec2,
right: DVec2,
center: DVec2,
angle: Option<f64>,
) -> (DVec2, ManipulatorGroup<ManipulatorGroupId>, DVec2) {
let center_to_arc_point = arc_point - center;
let handle_offset_factor = if let Some(angle) = angle { 4. / 3. * (angle / 4.).tan() } else { 0.551784777779014 };
(
left - (left - center).perp() * handle_offset_factor,
ManipulatorGroup::new(
arc_point,
Some(arc_point + center_to_arc_point.perp() * handle_offset_factor),
Some(arc_point - center_to_arc_point.perp() * handle_offset_factor),
),
right + (right - center).perp() * handle_offset_factor,
)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::consts::MAX_ABSOLUTE_DIFFERENCE;
fn f64_compare_vector(a: Vec<f64>, b: Vec<f64>, max_abs_diff: f64) -> bool {
a.len() == b.len() && a.into_iter().zip(b).all(|(a, b)| f64_compare(a, b, max_abs_diff))
}
fn collect_roots(roots: [Option<f64>; 3]) -> Vec<f64> {
roots.into_iter().flatten().collect()
}
#[test]
fn test_solve_linear() {
assert!(collect_roots(solve_linear(0., 0.)).is_empty());
assert!(collect_roots(solve_linear(0., 1.)).is_empty());
assert!(collect_roots(solve_linear(2., -8.)) == vec![4.]);
}
#[test]
fn test_solve_cubic() {
let roots1 = collect_roots(solve_cubic(1., 0., 0., 0.));
assert!(roots1 == vec![0.]);
let roots2 = collect_roots(solve_cubic(1., 3., 0., -4.));
assert!(roots2 == vec![1., -2.]);
let roots3 = collect_roots(solve_cubic(1., 0., 0., -1.));
assert!(roots3 == vec![1.]);
let roots4 = collect_roots(solve_cubic(1., 3., 0., 2.));
assert!(f64_compare_vector(roots4, vec![-3.196], MAX_ABSOLUTE_DIFFERENCE));
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));
let roots6 = collect_roots(solve_cubic(0., 3., 0., -3.));
assert!(roots6 == vec![1., -1.]);
let roots7 = collect_roots(solve_cubic(0., 0., 1., -1.));
assert!(roots7 == vec![1.]);
}
#[test]
fn test_do_rectangles_overlap() {
assert!(do_rectangles_overlap([DVec2::new(0., 0.), DVec2::new(20., 20.)], [DVec2::new(10., 10.), DVec2::new(30., 20.)]));
assert!(do_rectangles_overlap([DVec2::new(0., 0.), DVec2::new(10., 10.)], [DVec2::new(10., 10.), DVec2::new(30., 30.)]));
assert!(do_rectangles_overlap([DVec2::new(0., 0.), DVec2::new(10., 10.)], [DVec2::new(2., 2.), DVec2::new(6., 4.)]));
assert!(!do_rectangles_overlap([DVec2::new(0., 0.), DVec2::new(10., 10.)], [DVec2::new(20., 0.), DVec2::new(30., 10.)]));
assert!(!do_rectangles_overlap([DVec2::new(0., 0.), DVec2::new(10., 10.)], [DVec2::new(0., 20.), DVec2::new(20., 30.)]));
}
#[test]
fn test_find_intersection() {
let start1 = DVec2::new(0., 10.);
let end1 = DVec2::new(0., 4.);
let start_direction1 = DVec2::new(1., 2.);
let end_direction1 = DVec2::new(1., 5.);
assert!(line_intersection(start1, start_direction1, end1, end_direction1) == DVec2::new(2., 14.));
let start2 = DVec2::new(0., 0.);
let end2 = DVec2::new(8., 0.);
let start_direction2 = DVec2::new(1., 1.);
let end_direction2 = DVec2::new(1., -1.);
assert!(line_intersection(start2, start_direction2, end2, end_direction2) == DVec2::new(4., 4.));
}
#[test]
fn test_are_points_collinear() {
assert!(are_points_collinear(DVec2::new(2., 4.), DVec2::new(6., 8.), DVec2::new(4., 6.)));
assert!(!are_points_collinear(DVec2::new(1., 4.), DVec2::new(6., 8.), DVec2::new(4., 6.)));
}
#[test]
fn test_compute_circle_center_from_points() {
let center1 = compute_circle_center_from_points(DVec2::new(0., 1.), DVec2::new(-1., 0.), DVec2::new(1., 0.));
assert_eq!(center1.unwrap(), DVec2::new(0., 0.));
let center2 = compute_circle_center_from_points(DVec2::new(-1., 0.), DVec2::new(0., 1.), DVec2::new(1., 0.));
assert_eq!(center2.unwrap(), DVec2::new(0., 0.));
}
}