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, 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 = 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(v1:a - center, v2: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}
178