1//! Four quadrant arctangent approximation for a single-precision float.
2//!
3//! Method described at: <https://ieeexplore.ieee.org/document/6375931>
4
5use super::F32;
6use core::f32::consts::PI;
7
8impl F32 {
9 /// Approximates the four quadrant arctangent of `self` (`y`) and
10 /// `rhs` (`x`) in radians with a maximum error of `0.002`.
11 ///
12 /// - `x = 0`, `y = 0`: `0`
13 /// - `x >= 0`: `arctan(y/x)` -> `[-pi/2, pi/2]`
14 /// - `y >= 0`: `arctan(y/x) + pi` -> `(pi/2, pi]`
15 /// - `y < 0`: `arctan(y/x) - pi` -> `(-pi, -pi/2)`
16 pub fn atan2(self, rhs: Self) -> Self {
17 let n = self.atan2_norm(rhs);
18 PI / 2.0 * if n > 2.0 { n - 4.0 } else { n }
19 }
20
21 /// Approximates `atan2(y,x)` normalized to the `[0, 4)` range with a maximum
22 /// error of `0.1620` degrees.
23 pub(crate) fn atan2_norm(self, rhs: Self) -> Self {
24 const SIGN_MASK: u32 = 0x8000_0000;
25 const B: f32 = 0.596_227;
26
27 let y = self;
28 let x = rhs;
29
30 // Extract sign bits from floating point values
31 let ux_s = SIGN_MASK & x.to_bits();
32 let uy_s = SIGN_MASK & y.to_bits();
33
34 // Determine quadrant offset
35 let q = ((!ux_s & uy_s) >> 29 | ux_s >> 30) as f32;
36
37 // Calculate arctangent in the first quadrant
38 let bxy_a = (B * x * y).abs();
39 let n = bxy_a + y * y;
40 let atan_1q = n / (x * x + bxy_a + n);
41
42 // Translate it to the proper quadrant
43 let uatan_2q = (ux_s ^ uy_s) | atan_1q.to_bits();
44 Self(q) + Self::from_bits(uatan_2q)
45 }
46}
47
48#[cfg(test)]
49mod tests {
50 use super::F32;
51 use core::f32::consts::PI;
52
53 /// 0.1620 degrees in radians
54 const MAX_ERROR: f32 = 0.003;
55
56 #[test]
57 fn sanity_check() {
58 let test_vectors: &[(f32, f32, f32)] = &[
59 (0.0, 1.0, 0.0),
60 (0.0, -1.0, PI),
61 (3.0, 2.0, (3.0f32 / 2.0).atan()),
62 (2.0, -1.0, (2.0f32 / -1.0).atan() + PI),
63 (-2.0, -1.0, (-2.0f32 / -1.0).atan() - PI),
64 ];
65
66 for &(y, x, expected) in test_vectors {
67 let actual = F32(y).atan2(F32(x)).0;
68 let delta = actual - expected;
69
70 assert!(
71 delta <= MAX_ERROR,
72 "delta {} too large: {} vs {}",
73 delta,
74 actual,
75 expected
76 );
77 }
78 }
79}
80