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

idsp/
atan2.rs
