lyon_geom/
utils.rs

Help
1use crate::scalar::{Float, Scalar};
2use crate::{vector, Point, Vector};
3use arrayvec::ArrayVec;
4
5#[inline]
6pub fn min_max<S: Float>(a: S, b: S) -> (S, S) {
7    if a < b {
8        (a, b)
9    } else {
10        (b, a)
11    }
12}
13
14#[inline]
15pub fn tangent<S: Float>(v: Vector<S>) -> Vector<S> {
16    vector(-v.y, v.x)
17}
18
19#[inline]
20pub fn normalized_tangent<S: Scalar>(v: Vector<S>) -> Vector<S> {
21    tangent(v).normalize()
22}
23
24/// Angle between vectors v1 and v2 (oriented clockwise assuming y points downwards).
25/// The result is a number between `0` and `2 * PI`.
26///
27/// ex: `directed_angle([0,1], [1,0]) = 3/2 Pi rad`
28///
29/// ```text
30///     x       __
31///   0-->     /  \
32///  y|       |  x--> v2
33///   v        \ |v1
34///              v
35/// ```
36///
37/// Or, assuming y points upwards:
38/// `directed_angle([0,-1], [1,0]) = 1/2 Pi rad`
39///
40/// ```text
41///   ^           v2
42///  y|          x-->
43///   0-->    v1 | /
44///     x        v-
45/// ```
46///
47#[inline]
48pub fn directed_angle<S: Scalar>(v1: Vector<S>, v2: Vector<S>) -> S {
49    let angle = S::fast_atan2(v2.y, v2.x) - S::fast_atan2(v1.y, v1.x);
50
51    if angle < S::ZERO {
52        angle + S::TWO * S::PI()
53    } else {
54        angle
55    }
56}
57
58pub fn directed_angle2<S: Scalar>(center: Point<S>, a: Point<S>, b: Point<S>) -> S {
59    directed_angle(a - center, b - center)
60}
61
62pub fn cubic_polynomial_roots<S: Scalar>(a: S, b: S, c: S, d: S) -> ArrayVec<S, 3> {
63    let mut result = ArrayVec::new();
64
65    let m = a.abs().max(b.abs()).max(c.abs()).max(d.abs());
66    let epsilon = S::epsilon_for(m);
67
68    if S::abs(a) < epsilon {
69        if S::abs(b) < epsilon {
70            if S::abs(c) < epsilon {
71                return result;
72            }
73            // linear equation
74            result.push(-d / c);
75            return result;
76        }
77        // quadratic equation
78        let delta = c * c - S::FOUR * b * d;
79        if delta > S::ZERO {
80            let sqrt_delta = S::sqrt(delta);
81            result.push((-c - sqrt_delta) / (S::TWO * b));
82            result.push((-c + sqrt_delta) / (S::TWO * b));
83        } else if S::abs(delta) < epsilon {
84            result.push(-c / (S::TWO * b));
85        }
86        return result;
87    }
88
89    let frac_1_3 = S::ONE / S::THREE;
90
91    let bn = b / a;
92    let cn = c / a;
93    let dn = d / a;
94
95    let delta0 = (S::THREE * cn - bn * bn) / S::NINE;
96    let delta1 = (S::NINE * bn * cn - S::value(27.0) * dn - S::TWO * bn * bn * bn) / S::value(54.0);
97    let delta_01 = delta0 * delta0 * delta0 + delta1 * delta1;
98
99    if delta_01 >= S::ZERO {
100        let delta_p_sqrt = delta1 + S::sqrt(delta_01);
101        let delta_m_sqrt = delta1 - S::sqrt(delta_01);
102
103        let s = delta_p_sqrt.signum() * S::abs(delta_p_sqrt).powf(frac_1_3);
104        let t = delta_m_sqrt.signum() * S::abs(delta_m_sqrt).powf(frac_1_3);
105
106        result.push(-bn * frac_1_3 + (s + t));
107
108        // Don't add the repeated root when s + t == 0.
109        if S::abs(s - t) < epsilon && S::abs(s + t) >= epsilon {
110            result.push(-bn * frac_1_3 - (s + t) / S::TWO);
111        }
112    } else {
113        let theta = S::acos(delta1 / S::sqrt(-delta0 * delta0 * delta0));
114        let two_sqrt_delta0 = S::TWO * S::sqrt(-delta0);
115        result.push(two_sqrt_delta0 * Float::cos(theta * frac_1_3) - bn * frac_1_3);
116        result.push(
117            two_sqrt_delta0 * Float::cos((theta + S::TWO * S::PI()) * frac_1_3) - bn * frac_1_3,
118        );
119        result.push(
120            two_sqrt_delta0 * Float::cos((theta + S::FOUR * S::PI()) * frac_1_3) - bn * frac_1_3,
121        );
122    }
123
124    //result.sort();
125
126    result
127}
128
129#[test]
130fn cubic_polynomial() {
131    fn assert_approx_eq(a: ArrayVec<f32, 3>, b: &[f32], epsilon: f32) {
132        for i in 0..a.len() {
133            if f32::abs(a[i] - b[i]) > epsilon {
134                std::println!("{a:?} != {b:?}");
135            }
136            assert!((a[i] - b[i]).abs() <= epsilon);
137        }
138        assert_eq!(a.len(), b.len());
139    }
140
141    assert_approx_eq(
142        cubic_polynomial_roots(2.0, -4.0, 2.0, 0.0),
143        &[0.0, 1.0],
144        0.0000001,
145    );
146    assert_approx_eq(
147        cubic_polynomial_roots(-1.0, 1.0, -1.0, 1.0),
148        &[1.0],
149        0.000001,
150    );
151    assert_approx_eq(
152        cubic_polynomial_roots(-2.0, 2.0, -1.0, 10.0),
153        &[2.0],
154        0.00005,
155    );
156    // (x - 1)^3, with a triple root, should only return one root.
157    assert_approx_eq(
158        cubic_polynomial_roots(1.0, -3.0, 3.0, -1.0),
159        &[1.0],
160        0.00005,
161    );
162
163    // Quadratics.
164    assert_approx_eq(
165        cubic_polynomial_roots(0.0, 1.0, -5.0, -14.0),
166        &[-2.0, 7.0],
167        0.00005,
168    );
169    // (x - 3)^2, with a double root, should only return one root.
170    assert_approx_eq(cubic_polynomial_roots(0.0, 1.0, -6.0, 9.0), &[3.0], 0.00005);
171
172    // Linear.
173    assert_approx_eq(cubic_polynomial_roots(0.0, 0.0, 2.0, 1.0), &[-0.5], 0.00005);
174
175    // Constant.
176    assert_approx_eq(cubic_polynomial_roots(0.0, 0.0, 0.0, 0.0), &[], 0.00005);
177}
lyon_geom/utils.rs

lyon_geom/
utils.rs
