1#[cfg(all(not(feature = "std"), feature = "libm"))]
2#[allow(unused_imports)]
3use crate::utils::no_std::FloatExt;
4
5pub fn signum(value: f64) -> f64 {
6 if value < 0.0 {
7 -1.0
8 } else if value == 0.0 {
9 0.0
10 } else {
11 1.0
12 }
13}
14
15pub fn lerp(start: f64, stop: f64, amount: f64) -> f64 {
16 (1.0 - amount).mul_add(start, amount * stop)
17}
18
19pub const fn sanitize_degrees_int(degrees: i32) -> u32 {
20 match degrees {
21 value if value < 0 => (value + 360) as u32,
22 value => value as u32 % 360,
23 }
24}
25
26pub fn sanitize_degrees_double(degrees: f64) -> f64 {
27 match degrees {
28 value if value < 0.0 => value + 360.0,
29 value => value % 360.0,
30 }
31}
32
33pub fn rotate_direction(from: f64, to: f64) -> f64 {
34 let increasing_difference = sanitize_degrees_double(to - from);
35
36 if increasing_difference <= 180.0 {
37 1.0
38 } else {
39 -1.0
40 }
41}
42
43pub fn difference_degrees(a: f64, b: f64) -> f64 {
44 180.0 - ((a - b).abs() - 180.0).abs()
45}
46
47pub fn matrix_multiply(row: [f64; 3], matrix: [[f64; 3]; 3]) -> [f64; 3] {
48 [
49 row[2].mul_add(
50 matrix[0][2],
51 row[0].mul_add(matrix[0][0], row[1] * matrix[0][1]),
52 ),
53 row[2].mul_add(
54 matrix[1][2],
55 row[0].mul_add(matrix[1][0], row[1] * matrix[1][1]),
56 ),
57 row[2].mul_add(
58 matrix[2][2],
59 row[0].mul_add(matrix[2][0], row[1] * matrix[2][1]),
60 ),
61 ]
62}
63
64#[cfg(test)]
65mod tests {
66 use float_cmp::assert_approx_eq;
67
68 use crate::utils::math::{lerp, matrix_multiply, signum};
69
70 use super::{
71 difference_degrees, rotate_direction, sanitize_degrees_double, sanitize_degrees_int,
72 };
73
74 #[test]
75 fn test_signum() {
76 let result1 = signum(-2.0);
77 let result2 = signum(0.0);
78 let result3 = signum(2.0);
79
80 assert_approx_eq!(f64, result1, -1.0);
81 assert_approx_eq!(f64, result2, 0.0);
82 assert_approx_eq!(f64, result3, 1.0);
83 }
84
85 #[test]
86 fn test_lerp() {
87 let result1 = lerp(0.0, 1.0, 0.5);
88 let result2 = lerp(0.0, 1.0, 0.2);
89 let result3 = lerp(0.0, 1.0, 0.8);
90 let result4 = lerp(0.0, 100.0, 0.5);
91
92 assert_approx_eq!(f64, result1, 0.5);
93 assert_approx_eq!(f64, result2, 0.2);
94 assert_approx_eq!(f64, result3, 0.8);
95 assert_approx_eq!(f64, result4, 50.0);
96 }
97
98 #[test]
99 fn test_sanitize_degrees_int() {
100 let result1 = sanitize_degrees_int(20);
101 let result2 = sanitize_degrees_int(360);
102 let result3 = sanitize_degrees_int(420);
103
104 assert_eq!(result1, 20);
105 assert_eq!(result2, 0);
106 assert_eq!(result3, 60);
107 }
108
109 #[test]
110 fn test_sanitize_degrees_double() {
111 let result1 = sanitize_degrees_double(20.0);
112 let result2 = sanitize_degrees_double(360.0);
113 let result3 = sanitize_degrees_double(420.0);
114
115 assert_approx_eq!(f64, result1, 20.0);
116 assert_approx_eq!(f64, result2, 0.0);
117 assert_approx_eq!(f64, result3, 60.0);
118 }
119
120 #[test]
121 fn test_rotation_direction() {
122 let mut from = 0.0;
123
124 while from < 360.0 {
125 let mut to = 7.5;
126
127 while to < 360.0 {
128 let expected_answer = _rotate_direction(from, to);
129 let actual_answer = rotate_direction(from, to);
130
131 assert_approx_eq!(f64, actual_answer, expected_answer);
132 assert_approx_eq!(f64, actual_answer.abs(), 1.0);
133
134 to += 15.0;
135 }
136
137 from += 15.0;
138 }
139 }
140
141 #[test]
142 fn test_difference_degrees() {
143 let result0 = difference_degrees(274.6219605304299, 77.72547409861208);
144 let result1 = difference_degrees(34.300891775453685, 345.71661212068005);
145 let result2 = difference_degrees(133.40137028102632, 242.91571888782715);
146 let result3 = difference_degrees(190.0917641906071, 212.50303144714914);
147 let result4 = difference_degrees(74.32650812140363, 8.216795031646305);
148 let result5 = difference_degrees(297.86357401167163, 40.68646508824719);
149 let result6 = difference_degrees(267.81615364209966, 278.2059089562928);
150 let result7 = difference_degrees(174.5904980548994, 23.563620558167703);
151 let result8 = difference_degrees(157.88312470562747, 136.9620280065816);
152 let result9 = difference_degrees(125.57678704737181, 232.55663167025088);
153 let result10 = difference_degrees(29.516420072682976, 282.618422595264);
154
155 assert_approx_eq!(f64, result0, 163.10351356818222);
156 assert_approx_eq!(f64, result1, 48.58427965477364);
157 assert_approx_eq!(f64, result2, 109.51434860680084);
158 assert_approx_eq!(f64, result3, 22.411267256542033);
159 assert_approx_eq!(f64, result4, 66.10971308975734);
160 assert_approx_eq!(f64, result5, 102.82289107657556);
161 assert_approx_eq!(f64, result6, 10.38975531419311);
162 assert_approx_eq!(f64, result7, 151.0268774967317);
163 assert_approx_eq!(f64, result8, 20.921096699045876);
164 assert_approx_eq!(f64, result9, 106.97984462287907);
165 assert_approx_eq!(f64, result10, 106.89799747741898);
166 }
167
168 #[test]
169 fn test_matrix_multiply() {
170 let result1 = matrix_multiply(
171 [0.0, 1.0, 2.0],
172 [[1.0, 2.0, 3.0], [2.0, 3.0, 4.0], [4.0, 5.0, 6.0]],
173 );
174 let result2 = matrix_multiply(
175 [3.0, 4.0, 5.0],
176 [[3.0, 7.0, 1.0], [5.0, 8.0, 2.0], [6.0, 9.0, 3.0]],
177 );
178
179 assert_approx_eq!(&[f64], &result1, &[8.0, 11.0, 17.0]);
180 assert_approx_eq!(&[f64], &result2, &[42.0, 57.0, 69.0]);
181 }
182
183 fn _rotate_direction(from: f64, to: f64) -> f64 {
186 let a = to - from;
187 let b = to - from + 360.0;
188 let c = to - from - 360.0;
189
190 let (a_abs, b_abs, c_abs) = (a.abs(), b.abs(), c.abs());
191
192 if a_abs <= b_abs && a_abs <= c_abs {
193 if a >= 0.0 {
194 1.0
195 } else {
196 -1.0
197 }
198 } else if b_abs <= a_abs && b_abs <= c_abs {
199 if b >= 0.0 {
200 1.0
201 } else {
202 -1.0
203 }
204 } else if c >= 0.0 {
205 1.0
206 } else {
207 -1.0
208 }
209 }
210}