idsp/
cordic.rs

1// https://www.st.com/resource/en/design_tip/dt0085-coordinate-rotation-digital-computer-algorithm-cordic-to-compute-trigonometric-and-hyperbolic-functions-stmicroelectronics.pdf
2
3include!(concat!(env!("OUT_DIR"), "/cordic_tables.rs"));
4
5const ROTATE: bool = false;
6const DEROTATE: bool = true;
7const CIRCULAR: u8 = 0;
8const HYPERBOLIC: u8 = 1;
9const LINEAR: u8 = 2;
10
11/// Generic CORDIC
12#[inline]
13fn cordic<const VECTORING: bool, const COORD: u8>(
14    mut x: i32,
15    mut y: i32,
16    mut z: i32,
17    iter: Option<usize>,
18) -> (i32, i32) {
19    // Microrotation table
20    let a = match COORD {
21        CIRCULAR => &CORDIC_CIRCULAR,
22        _ => &CORDIC_HYPERBOLIC,
23    };
24    // MSB
25    let left = if VECTORING {
26        x < 0
27    } else {
28        z.wrapping_sub(i32::MIN >> 1) < 0
29    };
30    if left {
31        x = -x;
32        y = -y;
33        z = z.wrapping_sub(i32::MIN);
34    }
35    // Hyperbolic repetition marker
36    let mut k = 4;
37    for (mut i, &(mut a)) in a[..iter.unwrap_or(a.len())].iter().enumerate() {
38        // For linear mode the microrotations are computed, not looked up
39        if COORD == LINEAR {
40            a = (i32::MIN as u32 >> i) as _;
41        }
42        // Hyperbolic starts at i = 1
43        if COORD == HYPERBOLIC {
44            i += 1;
45        }
46        // Hyperbolic repeats some rotations for convergence
47        let repeat = if COORD == HYPERBOLIC && i == k {
48            k = 3 * i + 1;
49            2
50        } else {
51            1
52        };
53        for _ in 0..repeat {
54            // "sigma"
55            let lower = if VECTORING { y <= 0 } else { z >= 0 };
56            let (dx, dy) = (y >> i, x >> i);
57            if lower {
58                if COORD == CIRCULAR {
59                    x -= dx;
60                } else if COORD == HYPERBOLIC {
61                    x += dx;
62                }
63                y += dy;
64                z = z.wrapping_sub(a);
65            } else {
66                if COORD == CIRCULAR {
67                    x += dx;
68                } else if COORD == HYPERBOLIC {
69                    x -= dx;
70                }
71                y -= dy;
72                z = z.wrapping_add(a);
73            }
74        }
75    }
76    (x, if VECTORING { z } else { y })
77}
78
79/// Returns `F*(x*cos(z*PI) - y*sin(z*PI)), F*(x*sin(z*PI) + y*cos(z*PI))`
80pub fn cos_sin(x: i32, y: i32, z: i32) -> (i32, i32) {
81    cordic::<ROTATE, CIRCULAR>(x, y, z, None)
82}
83
84/// Returns `F*sqrt(x**2 + y**2), z + atan2(y, x)/PI`
85pub fn sqrt_atan2(x: i32, y: i32, z: i32) -> (i32, i32) {
86    cordic::<DEROTATE, CIRCULAR>(x, y, z, None)
87}
88
89/// Returns `x, y + x*z`
90pub fn mul(x: i32, y: i32, z: i32) -> i32 {
91    cordic::<ROTATE, LINEAR>(x, y, z, None).1
92}
93
94/// Returns `x, z + y/x`
95pub fn div(x: i32, y: i32, z: i32) -> i32 {
96    cordic::<DEROTATE, LINEAR>(x, y, z, None).1
97}
98
99/// Returns `G*(x*cosh(z) + y*sinh(z)), G*(x*sinh(z) + y*cosh(z))`
100pub fn cosh_sinh(x: i32, y: i32, z: i32) -> (i32, i32) {
101    cordic::<ROTATE, HYPERBOLIC>(x, y, z, None)
102}
103
104/// Returns `G*sqrt(x**2 - y**2), z + atanh2(y, x)`
105pub fn sqrt_atanh2(x: i32, y: i32, z: i32) -> (i32, i32) {
106    cordic::<DEROTATE, HYPERBOLIC>(x, y, z, None)
107}
108
109#[cfg(test)]
110mod test {
111    use core::f64::consts::PI;
112    use log;
113    use quickcheck_macros::quickcheck;
114    use rand::{prelude::*, rngs::StdRng};
115
116    use super::*;
117
118    const Q31: f64 = (1i64 << 31) as _;
119
120    fn f2i(x: f64) -> i32 {
121        (x * Q31).round() as i64 as _
122    }
123    fn i2f(x: i32) -> f64 {
124        (x as f64 / Q31) as _
125    }
126
127    const F: f64 = CORDIC_CIRCULAR_GAIN.recip();
128    const G: f64 = CORDIC_HYPERBOLIC_GAIN.recip();
129
130    fn cos_sin_err(x: f64, y: f64, z: f64) -> f64 {
131        let xy = cos_sin(f2i(x * F), f2i(y * F), f2i(z));
132        let xy = (i2f(xy.0), i2f(xy.1));
133        let (s, c) = (z * PI).sin_cos();
134        let xy0 = (c * x - s * y, s * x + c * y);
135        let (dx, dy) = (xy.0 - xy0.0, xy.1 - xy0.1);
136        let dr = (dx.powi(2) + dy.powi(2)).sqrt();
137        let da = i2f(f2i(xy.1.atan2(xy.0) / PI - y.atan2(x) / PI - z));
138        log::debug!("{dx:.10},{dy:.10} ~ {dr:.10}@{da:.10}");
139        dr * Q31
140    }
141
142    fn sqrt_atan2_err(x: f64, y: f64) -> f64 {
143        let (r, z) = sqrt_atan2(f2i(x * F), f2i(y * F), 0);
144        let (r, z) = (i2f(r), i2f(z));
145        let r0 = (x.powi(2) + y.powi(2)).sqrt();
146        let z0 = y.atan2(x) / PI;
147        let da = i2f(f2i(z - z0));
148        let dr = ((r - r0).powi(2) + ((da * PI).sin() * r0).powi(2)).sqrt();
149        log::debug!("{dr:.10}@{da:.10}");
150        dr * Q31
151    }
152
153    fn sqrt_atanh2_err(x: f64, y: f64) -> f64 {
154        let (r, z) = sqrt_atanh2(f2i(x * G), f2i(y * G), 0);
155        let (r, z) = (i2f(r), i2f(z));
156        let r0 = (x.powi(2) - y.powi(2)).sqrt();
157        let z0 = (y / x).atanh();
158        let da = i2f(f2i(z - z0));
159        let dr = (r - r0).abs();
160        log::debug!("{dr:.10}@{da:.10}");
161        dr * Q31
162    }
163
164    #[test]
165    fn basic_rot() {
166        assert!((CORDIC_CIRCULAR_GAIN - 1.64676025812107).abs() < 1e-14,);
167
168        cos_sin_err(0.50, 0.2, 0.123);
169        cos_sin_err(0.01, 0.0, -0.35);
170        cos_sin_err(0.605, 0.0, 0.35);
171        cos_sin_err(-0.3, 0.4, 0.55);
172        cos_sin_err(-0.3, -0.4, -0.55);
173        cos_sin_err(-0.3, -0.4, 0.8);
174        cos_sin_err(-0.3, -0.4, -0.8);
175    }
176
177    fn test_values(n: usize) -> Vec<i32> {
178        let mut rng = StdRng::seed_from_u64(42);
179        let mut n: Vec<_> = core::iter::from_fn(|| Some(rng.random())).take(n).collect();
180        n.extend([
181            0,
182            1,
183            -1,
184            0xf,
185            -0xf,
186            0x55555555,
187            -0x55555555,
188            0x5aaaaaaa,
189            -0x5aaaaaaa,
190            0x7fffffff,
191            -0x7fffffff,
192            1 << 29,
193            -1 << 29,
194            1 << 30,
195            -1 << 30,
196            i32::MIN,
197            i32::MAX,
198        ]);
199        n
200    }
201
202    #[test]
203    fn meanmax_rot() {
204        let n = test_values(50);
205        let mut mean = 0.0;
206        let mut max: f64 = 0.0;
207        for x in n.iter() {
208            for y in n.iter() {
209                for z in n.iter() {
210                    let (x, y) = (i2f(*x), i2f(*y));
211                    if 1.0 - x.powi(2) - y.powi(2) <= 1e-9 {
212                        continue;
213                    }
214                    let dr = cos_sin_err(x, y, i2f(*z));
215                    mean += dr;
216                    max = max.max(dr);
217                }
218            }
219        }
220        mean /= n.len().pow(3) as f64;
221        log::info!("{mean} {max}");
222        assert!(mean < 5.0);
223        assert!(max < 24.0);
224    }
225
226    #[test]
227    fn meanmax_vect() {
228        let n = test_values(300);
229        let mut mean = 0.0;
230        let mut max: f64 = 0.0;
231        for x in n.iter() {
232            for y in n.iter() {
233                let (x, y) = (i2f(*x), i2f(*y));
234                if 1.0 - x.powi(2) - y.powi(2) <= 1e-9 {
235                    continue;
236                }
237                let dr = sqrt_atan2_err(x, y);
238                mean += dr;
239                max = max.max(dr);
240            }
241        }
242        mean /= n.len().pow(2) as f64;
243        log::info!("{mean} {max}");
244        assert!(mean < 8.0);
245        assert!(max < 30.0);
246    }
247
248    #[quickcheck]
249    fn check_rot(x: i32, y: i32, z: i32) -> bool {
250        let (x, y) = (i2f(x), i2f(y));
251        if CORDIC_CIRCULAR_GAIN.powi(2) * (x.powi(2) + y.powi(2)) >= 1.0 {
252            return true;
253        }
254        cos_sin_err(x, y, i2f(z)) <= 22.0
255    }
256
257    #[quickcheck]
258    fn check_vect(x: i32, y: i32) -> bool {
259        let (x, y) = (i2f(x), i2f(y));
260        if CORDIC_CIRCULAR_GAIN.powi(2) * (x.powi(2) + y.powi(2)) >= 1.0 {
261            return true;
262        }
263        sqrt_atan2_err(x, y) <= 29.0
264    }
265
266    #[quickcheck]
267    fn check_hyp_vect(x: i32, y: i32) -> bool {
268        let (x, y) = (i2f(x), i2f(y));
269        if CORDIC_HYPERBOLIC_GAIN.powi(2) * (x.powi(2) - y.powi(2)) >= 1.0 {
270            return true;
271        }
272        if y.abs() > x.abs() || (y / x).atanh() >= 1.1 {
273            return true;
274        }
275
276        sqrt_atanh2_err(x, y);
277        true
278    }
279
280    #[test]
281    fn test_atanh() {
282        sqrt_atanh2_err(0.8, 0.1);
283        sqrt_atanh2_err(0.8, 0.1);
284        sqrt_atanh2_err(0.8, 0.1);
285        sqrt_atanh2_err(0.99, 0.0);
286    }
287}
idsp/cordic.rs

idsp/
cordic.rs
