idsp/
atan2.rs

1fn divi(mut y: u32, mut x: u32) -> u32 {
2    debug_assert!(y <= x);
3    let z = y.leading_zeros().min(15);
4    y <<= z;
5    x += (1 << (15 - z)) - 1;
6    x >>= 16 - z;
7    if x == 0 {
8        0 // x == y == 0
9    } else {
10        ((y / x) << 15) + (1 << 14)
11    }
12}
13
14fn atani(x: u32) -> u32 {
15    const A: [i32; 6] = [
16        0x0517c2cd,
17        -0x06c6496b,
18        0x0fbdb021,
19        -0x25b32e0a,
20        0x43b34c81,
21        -0x3bc823dd,
22    ];
23    let x = x as i64;
24    let x2 = ((x * x) >> 32) as i32 as i64;
25    let r = A
26        .iter()
27        .rev()
28        .fold(0, |r, a| ((r as i64 * x2) >> 32) as i32 + a);
29    ((r as i64 * x) >> 28) as _
30}
31
32/// 2-argument arctangent function.
33///
34/// This implementation uses all integer arithmetic for fast
35/// computation.
36///
37/// # Arguments
38///
39/// * `y` - Y-axis component.
40/// * `x` - X-axis component.
41///
42/// # Returns
43///
44/// The angle between the x-axis and the ray to the point (x,y). The
45/// result range is from i32::MIN to i32::MAX, where i32::MIN
46/// represents -pi and, equivalently, +pi. i32::MAX represents one
47/// count less than +pi.
48pub fn atan2(mut y: i32, mut x: i32) -> i32 {
49    let mut k = 0u32;
50    if y < 0 {
51        y = y.saturating_neg();
52        k ^= u32::MAX;
53    }
54    if x < 0 {
55        x = x.saturating_neg();
56        k ^= u32::MAX >> 1;
57    }
58    if y > x {
59        (y, x) = (x, y);
60        k ^= u32::MAX >> 2;
61    }
62    let r = atani(divi(y as _, x as _));
63    (r ^ k) as _
64}
65
66#[cfg(test)]
67mod tests {
68    use super::*;
69    use core::f64::consts::PI;
70
71    #[test]
72    fn atan2_error() {
73        const N: isize = 201;
74        for i in 0..N {
75            let p = ((1. - 2. * i as f64 / N as f64) * i32::MIN as f64) as i32;
76            let pf = p as f64 / i32::MIN as f64 * -PI;
77            let y = (pf.sin() * i32::MAX as f64) as i32;
78            let x = (pf.cos() * i32::MAX as f64) as i32;
79            let _p0 = (y as f64).atan2(x as f64);
80            let pp = atan2(y, x);
81            let pe = -(pp as f64 / i32::MIN as f64);
82            println!(
83                "y:{:.5e}, x:{:.5e}, p/PI:{:.5e}: pe:{:.5e}, pe*PI-p0:{:.5e}",
84                y as f64 / i32::MAX as f64,
85                x as f64 / i32::MAX as f64,
86                pf / PI,
87                pe,
88                pe * PI - pf
89            );
90        }
91    }
92
93    fn angle_to_axis(angle: f64) -> f64 {
94        let angle = angle % (PI / 2.);
95        (PI / 2. - angle).min(angle)
96    }
97
98    #[test]
99    fn atan2_absolute_error() {
100        const N: usize = 321;
101        let mut test_vals = [0i32; N + 2];
102        let scale = (1i64 << 31) as f64;
103        for i in 0..N {
104            test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
105        }
106
107        assert!(test_vals.contains(&i32::MIN));
108        test_vals[N] = i32::MAX;
109        test_vals[N + 1] = 0;
110
111        let mut rms_err = 0f64;
112        let mut abs_err = 0f64;
113        let mut rel_err = 0f64;
114
115        for &x in test_vals.iter() {
116            for &y in test_vals.iter() {
117                let want = (y as f64).atan2(x as f64);
118                let have = atan2(y, x) as f64 * (PI / scale);
119                let err = (have - want).abs();
120                abs_err = abs_err.max(err);
121                rms_err += err * err;
122                if err > 3e-5 {
123                    println!("{:.5e}/{:.5e}: {:.5e} vs {:.5e}", y, x, have, want);
124                    println!("y/x {} {}", y, x);
125                    rel_err = rel_err.max(err / angle_to_axis(want));
126                }
127            }
128        }
129        rms_err = rms_err.sqrt() / test_vals.len() as f64;
130        println!("max abs err: {:.2e}", abs_err);
131        println!("rms abs err: {:.2e}", rms_err);
132        println!("max rel err: {:.2e}", rel_err);
133        assert!(abs_err < 1.2e-5);
134        assert!(rms_err < 4.2e-6);
135        assert!(rel_err < 1e-12);
136    }
137}
idsp/atan2.rs

idsp/
atan2.rs
