1use super::real;
2use crate::geometry::Real;
3use core::f32::consts::PI;
4use core::ops::{Add, AddAssign, Neg, Sub, SubAssign};
5#[cfg(not(feature = "fixed_point"))]
6#[allow(unused_imports)]
7use micromath::F32Ext;
8
9pub(crate) mod angle_consts {
10 use super::{real, Angle};
11
12 pub(crate) const ANGLE_90DEG: Angle = Angle(real::FRAC_PI_2);
13 pub(crate) const ANGLE_180DEG: Angle = Angle(real::PI);
14 pub(crate) const ANGLE_360DEG: Angle = Angle(real::TAU);
15}
16
17/// Angle.
18///
19/// `Angle` is used to define the value of an angle.
20///
21/// # Examples
22///
23/// ## Create an `Angle` from a value
24///
25/// ```rust
26/// use embedded_graphics::geometry::{Angle, AngleUnit};
27/// use core::f32::consts::PI;
28///
29/// // Create an angle using the `from_degrees` constructor method
30/// let angle_a = Angle::from_degrees(10.0);
31/// let angle_b = Angle::from_radians(PI);
32///
33/// // Angles can also be created using the [AngleUnit] trait
34/// let angle_c = 30.0.deg();
35/// let angle_d = PI.rad();
36/// ```
37#[derive(Copy, Clone, PartialEq, PartialOrd, Debug)]
38#[cfg_attr(feature = "defmt", derive(::defmt::Format))]
39pub struct Angle(Real);
40
41impl Angle {
42 /// Creates an angle defined in degrees.
43 pub fn from_degrees(angle: f32) -> Self {
44 Angle((angle * PI / 180.0).into())
45 }
46
47 /// Creates an angle defined in radians.
48 pub fn from_radians(angle: f32) -> Self {
49 Angle(angle.into())
50 }
51
52 /// Creates a zero degree angle.
53 pub fn zero() -> Self {
54 Angle(0.into())
55 }
56
57 /// Compute the absolute value of the angle.
58 pub fn abs(self) -> Self {
59 Angle(self.0.abs())
60 }
61
62 /// Normalize the angle to less than one full rotation (ie. in the range 0..360).
63 pub fn normalize(self) -> Self {
64 Angle(self.0.rem_euclid((2.0 * PI).into()))
65 }
66
67 /// Return numerical value of the angle in degree
68 pub fn to_degrees(self) -> f32 {
69 let angle: f32 = self.0.into();
70 180.0 * angle / PI
71 }
72
73 /// Return numerical value of the angle in radian
74 pub fn to_radians(self) -> f32 {
75 self.0.into()
76 }
77}
78
79/// AngleUnit trait.
80///
81/// `AngleUnit` is a trait to convert numbers into angle by appending .deg() or .rad()
82/// to the number as if it was a unit.
83///
84/// # Examples
85///
86/// ## Create an `Angle` from a value using `AngleUnit`
87///
88/// ```rust
89/// use embedded_graphics::geometry::AngleUnit;
90/// use core::f32::consts::PI;
91///
92/// // Create an angle using the `AngleUnit` methods
93/// let angle_a = 30.0.deg();
94/// let angle_b = PI.rad();
95/// ```
96pub trait AngleUnit {
97 /// Convert a number (interpreted as degrees) to an `Angle`.
98 fn deg(self) -> Angle;
99
100 /// Convert a number (interpreted as radians) to an `Angle`.
101 fn rad(self) -> Angle;
102}
103
104impl AngleUnit for f32 {
105 fn deg(self) -> Angle {
106 Angle::from_degrees(self)
107 }
108
109 fn rad(self) -> Angle {
110 Angle::from_radians(self)
111 }
112}
113
114pub(crate) trait Trigonometry {
115 /// Get the sine of the angle.
116 fn sin(self) -> Real;
117
118 /// Get the cosine of the angle.
119 fn cos(self) -> Real;
120
121 /// Get the tangent of the angle.
122 fn tan(self) -> Option<Real>;
123}
124
125#[cfg(not(feature = "fixed_point"))]
126impl Trigonometry for Angle {
127 fn sin(self) -> Real {
128 let angle: f32 = self.0.into();
129 angle.sin().into()
130 }
131
132 fn cos(self) -> Real {
133 let angle: f32 = self.0.into();
134 angle.cos().into()
135 }
136
137 fn tan(self) -> Option<Real> {
138 let angle: f32 = self.0.into();
139 let tan: f32 = angle.tan();
140 // FRAC_PI_2.tan() has no value, but the approximate method used by micromath actually return a huge
141 // value which is > 20000000.0, so we check for this to decide that the angle was approximately
142 // FRAC_PI_2 and that tan() has actually no value.
143 if tan.is_nan() || tan.abs() > 20000000.0 {
144 None
145 } else {
146 Some(tan.into())
147 }
148 }
149}
150
151#[cfg(feature = "fixed_point")]
152impl Trigonometry for Angle {
153 fn sin(self) -> Real {
154 use fixed::types::I16F16;
155 const SIN: [I16F16; 91] = [
156 // Ideally we could make the compiler generate those values, but for now sin() is not a const fn,
157 // so it can't be used here. Here is how it would look like:
158 // I16F16::from_bits((0f64.sin() * (1 << 16) as f64).round()))),
159 I16F16::from_bits(0),
160 I16F16::from_bits(1144),
161 I16F16::from_bits(2287),
162 I16F16::from_bits(3430),
163 I16F16::from_bits(4572),
164 I16F16::from_bits(5712),
165 I16F16::from_bits(6850),
166 I16F16::from_bits(7987),
167 I16F16::from_bits(9121),
168 I16F16::from_bits(10252),
169 I16F16::from_bits(11380),
170 I16F16::from_bits(12505),
171 I16F16::from_bits(13626),
172 I16F16::from_bits(14742),
173 I16F16::from_bits(15855),
174 I16F16::from_bits(16962),
175 I16F16::from_bits(18064),
176 I16F16::from_bits(19161),
177 I16F16::from_bits(20252),
178 I16F16::from_bits(21336),
179 I16F16::from_bits(22415),
180 I16F16::from_bits(23486),
181 I16F16::from_bits(24550),
182 I16F16::from_bits(25607),
183 I16F16::from_bits(26656),
184 I16F16::from_bits(27697),
185 I16F16::from_bits(28729),
186 I16F16::from_bits(29753),
187 I16F16::from_bits(30767),
188 I16F16::from_bits(31772),
189 I16F16::from_bits(32768),
190 I16F16::from_bits(33754),
191 I16F16::from_bits(34729),
192 I16F16::from_bits(35693),
193 I16F16::from_bits(36647),
194 I16F16::from_bits(37590),
195 I16F16::from_bits(38521),
196 I16F16::from_bits(39441),
197 I16F16::from_bits(40348),
198 I16F16::from_bits(41243),
199 I16F16::from_bits(42126),
200 I16F16::from_bits(42995),
201 I16F16::from_bits(43852),
202 I16F16::from_bits(44695),
203 I16F16::from_bits(45525),
204 I16F16::from_bits(46341),
205 I16F16::from_bits(47143),
206 I16F16::from_bits(47930),
207 I16F16::from_bits(48703),
208 I16F16::from_bits(49461),
209 I16F16::from_bits(50203),
210 I16F16::from_bits(50931),
211 I16F16::from_bits(51643),
212 I16F16::from_bits(52339),
213 I16F16::from_bits(53020),
214 I16F16::from_bits(53684),
215 I16F16::from_bits(54332),
216 I16F16::from_bits(54963),
217 I16F16::from_bits(55578),
218 I16F16::from_bits(56175),
219 I16F16::from_bits(56756),
220 I16F16::from_bits(57319),
221 I16F16::from_bits(57865),
222 I16F16::from_bits(58393),
223 I16F16::from_bits(58903),
224 I16F16::from_bits(59396),
225 I16F16::from_bits(59870),
226 I16F16::from_bits(60326),
227 I16F16::from_bits(60764),
228 I16F16::from_bits(61183),
229 I16F16::from_bits(61584),
230 I16F16::from_bits(61966),
231 I16F16::from_bits(62328),
232 I16F16::from_bits(62672),
233 I16F16::from_bits(62997),
234 I16F16::from_bits(63303),
235 I16F16::from_bits(63589),
236 I16F16::from_bits(63856),
237 I16F16::from_bits(64104),
238 I16F16::from_bits(64332),
239 I16F16::from_bits(64540),
240 I16F16::from_bits(64729),
241 I16F16::from_bits(64898),
242 I16F16::from_bits(65048),
243 I16F16::from_bits(65177),
244 I16F16::from_bits(65287),
245 I16F16::from_bits(65376),
246 I16F16::from_bits(65446),
247 I16F16::from_bits(65496),
248 I16F16::from_bits(65526),
249 I16F16::from_bits(65536),
250 ];
251 let degree: i32 = (Real::from(180) * self.0 / real::PI).round().into();
252 let degree = degree.rem_euclid(360) as usize;
253 let sin = if degree <= 90 {
254 SIN[degree]
255 } else if degree <= 180 {
256 SIN[180 - degree]
257 } else if degree <= 270 {
258 -SIN[degree - 180]
259 } else {
260 -SIN[360 - degree]
261 };
262 sin.into()
263 }
264
265 fn cos(self) -> Real {
266 (self + angle_consts::ANGLE_90DEG).sin()
267 }
268
269 fn tan(self) -> Option<Real> {
270 let cos = self.cos();
271 if cos != Real::zero() {
272 Some(self.sin() / cos)
273 } else {
274 None
275 }
276 }
277}
278
279impl Add for Angle {
280 type Output = Angle;
281
282 fn add(self, other: Angle) -> Angle {
283 Angle(self.0 + other.0)
284 }
285}
286
287impl AddAssign for Angle {
288 fn add_assign(&mut self, other: Angle) {
289 self.0 += other.0;
290 }
291}
292
293impl Sub for Angle {
294 type Output = Angle;
295
296 fn sub(self, other: Angle) -> Angle {
297 Angle(self.0 - other.0)
298 }
299}
300
301impl SubAssign for Angle {
302 fn sub_assign(&mut self, other: Angle) {
303 self.0 -= other.0;
304 }
305}
306
307impl Neg for Angle {
308 type Output = Angle;
309
310 fn neg(self) -> Angle {
311 Angle(-self.0)
312 }
313}
314
315#[cfg(test)]
316mod tests {
317 use super::*;
318 use float_cmp::{approx_eq, ApproxEq, F32Margin};
319
320 impl ApproxEq for Angle {
321 type Margin = F32Margin;
322
323 fn approx_eq<M: Into<Self::Margin>>(self, other: Self, margin: M) -> bool {
324 self.0.approx_eq(other.0, margin.into())
325 }
326 }
327
328 #[test]
329 fn angles_can_be_added() {
330 let left = Angle::from_degrees(10.0);
331 let right = Angle::from_degrees(30.0);
332
333 assert!(approx_eq!(
334 Angle,
335 left + right,
336 Angle::from_degrees(40.0),
337 epsilon = 0.0001
338 ));
339 }
340
341 #[test]
342 fn angles_can_be_subtracted() {
343 let left = Angle::from_degrees(30.0);
344 let right = Angle::from_degrees(10.0);
345
346 assert!(approx_eq!(
347 Angle,
348 left - right,
349 Angle::from_degrees(20.0),
350 epsilon = 0.0001
351 ));
352 }
353
354 #[test]
355 fn angles_can_be_absoluted() {
356 let angle = Angle::from_degrees(30.0).abs();
357 assert_eq!(angle, Angle::from_degrees(30.0));
358
359 let angle = Angle::from_degrees(-30.0).abs();
360 assert_eq!(angle, Angle::from_degrees(30.0));
361 }
362
363 #[test]
364 fn angle_unit() {
365 assert_eq!(180.0.deg(), Angle::from_degrees(180.0));
366 assert_eq!(PI.rad(), Angle::from_radians(PI));
367 }
368
369 #[test]
370 fn from_radians() {
371 assert_eq!(Angle(PI.into()), Angle::from_radians(PI));
372 }
373
374 #[test]
375 fn to_radians() {
376 let angle = Angle(PI.into()).to_radians();
377 assert!(approx_eq!(f32, angle, PI, epsilon = 0.0001));
378 }
379
380 #[test]
381 fn from_degrees() {
382 let angle = Angle::from_degrees(180.0);
383 assert!(approx_eq!(f32, angle.0.into(), PI, epsilon = 0.0001));
384 }
385
386 #[test]
387 fn to_degrees() {
388 let angle = Angle(PI.into()).to_degrees();
389 assert!(approx_eq!(f32, angle, 180.0, epsilon = 0.001));
390 }
391
392 #[test]
393 fn sin_correct() {
394 let degree_sin_pairs = [
395 (-90.0, -1.0),
396 (-60.0, -0.86602540),
397 (-45.0, -0.70710678),
398 (-30.0, -0.5),
399 (0.0, 0.0),
400 (30.0, 0.5),
401 (45.0, 0.70710678),
402 (60.0, 0.86602540),
403 (90.0, 1.0),
404 (120.0, 0.86602540),
405 (135.0, 0.70710678),
406 (150.0, 0.5),
407 (180.0, 0.0),
408 (210.0, -0.5),
409 (225.0, -0.70710678),
410 (240.0, -0.86602540),
411 (270.0, -1.0),
412 ];
413
414 for (angle, sin) in &degree_sin_pairs {
415 assert!(approx_eq!(
416 Real,
417 angle.deg().sin(),
418 (*sin).into(),
419 epsilon = 0.0001
420 ));
421 }
422 }
423
424 #[test]
425 fn cos_correct() {
426 let degree_cos_pairs = [
427 (-90.0, 0.0),
428 (-60.0, 0.5),
429 (-45.0, 0.70710678),
430 (-30.0, 0.86602540),
431 (0.0, 1.0),
432 (30.0, 0.86602540),
433 (45.0, 0.70710678),
434 (60.0, 0.5),
435 (90.0, 0.0),
436 (120.0, -0.5),
437 (135.0, -0.70710678),
438 (150.0, -0.86602540),
439 (180.0, -1.0),
440 (210.0, -0.86602540),
441 (225.0, -0.70710678),
442 (240.0, -0.5),
443 (270.0, -0.0),
444 ];
445
446 for (angle, cos) in &degree_cos_pairs {
447 assert!(approx_eq!(
448 Real,
449 angle.deg().cos(),
450 (*cos).into(),
451 epsilon = 0.0001
452 ));
453 }
454 }
455
456 #[test]
457 fn tan_correct() {
458 let degree_tan_pairs = [
459 (-60.0, -1.73205080),
460 (-45.0, -1.0),
461 (-30.0, -0.57735026),
462 (0.0, 0.0),
463 (30.0, 0.57735026),
464 (45.0, 1.0),
465 (60.0, 1.73205080),
466 ];
467
468 for (angle, tan) in &degree_tan_pairs {
469 assert!(approx_eq!(
470 Real,
471 angle.deg().tan().unwrap(),
472 (*tan).into(),
473 epsilon = 0.0001
474 ));
475 }
476
477 assert_eq!((-90.0.deg()).tan(), None);
478 assert_eq!(90.0.deg().tan(), None);
479 }
480}
481