embedded_dsp/
fast_math.rs

1//! Fast math functions (sin, cos, sin_cos, sqrt, vsqrt, divide, log, exp, atan2).
2
3#[allow(unused_imports)]
4use crate::math::FloatMath;
5use crate::math::{isqrt_u32, isqrt_u64};
6use crate::types::*;
7
8/// Floating-point sine calculation.
9pub fn sin_f32(x: f32) -> f32 {
10    x.sin()
11}
12
13/// Floating-point cosine calculation.
14pub fn cos_f32(x: f32) -> f32 {
15    x.cos()
16}
17
18/// Floating-point sine and cosine calculation.
19pub fn sin_cos_f32(theta: f32, sin_val: &mut f32, cos_val: &mut f32) {
20    let rad = theta * (core::f32::consts::PI / 180.0);
21    *sin_val = rad.sin();
22    *cos_val = rad.cos();
23}
24
25const fn atan_taylor(x: f32) -> f32 {
26    let x2 = x * x;
27    let x3 = x2 * x;
28    let x5 = x3 * x2;
29    let x7 = x5 * x2;
30    let x9 = x7 * x2;
31    x - x3 / 3.0 + x5 / 5.0 - x7 / 7.0 + x9 / 9.0
32}
33
34const fn gen_cordic_atan_q31() -> [i32; 32] {
35    let mut t = [0i32; 32];
36    let mut i = 0;
37    while i < 32 {
38        let x = 1.0f32 / ((1u32 << i) as f32);
39        let a = atan_taylor(x) / core::f32::consts::PI * 2147483648.0;
40        t[i] = a as i32;
41        i += 1;
42    }
43    t
44}
45
46/// `atan(2^-i) / π` in Q1.31 (CORDIC angle table).
47const CORDIC_ATAN_Q31: [i32; 32] = gen_cordic_atan_q31();
48
49/// CORDIC K ≈ 0.607252935 in Q1.31.
50const CORDIC_K_Q31: i32 = 1_304_063_564;
51
52fn cordic_rotate_q31(theta: i32) -> (i32, i32) {
53    let mut x = CORDIC_K_Q31;
54    let mut y = 0i32;
55    let mut z = theta;
56    let mut i = 0;
57    while i < 31 {
58        let x_sh = x >> i;
59        let y_sh = y >> i;
60        if z >= 0 {
61            x = x.saturating_sub(y_sh);
62            y = y.saturating_add(x_sh);
63            z = z.saturating_sub(CORDIC_ATAN_Q31[i]);
64        } else {
65            x = x.saturating_add(y_sh);
66            y = y.saturating_sub(x_sh);
67            z = z.saturating_add(CORDIC_ATAN_Q31[i]);
68        }
69        i += 1;
70    }
71    (x, y)
72}
73
74/// First-quadrant `atan(y/x) / π` in Q1.31. `x` and `y` must be `>= 0`.
75fn cordic_atan_first_q31(mut x: i32, mut y: i32) -> i32 {
76    if x == 0 {
77        return if y == 0 { 0 } else { 1 << 30 }; // 0.5 → π/2
78    }
79    if y == 0 {
80        return 0;
81    }
82    while x < (1 << 30) && y < (1 << 30) && (x > 0 || y > 0) {
83        let nx = x.saturating_mul(2);
84        let ny = y.saturating_mul(2);
85        if nx / 2 != x || ny / 2 != y {
86            break;
87        }
88        x = nx;
89        y = ny;
90    }
91    let mut z = 0i32;
92    let mut i = 0;
93    while i < 31 {
94        let x_sh = x >> i;
95        let y_sh = y >> i;
96        if y >= 0 {
97            x = x.saturating_add(y_sh);
98            y = y.saturating_sub(x_sh);
99            z = z.saturating_add(CORDIC_ATAN_Q31[i]);
100        } else {
101            x = x.saturating_sub(y_sh);
102            y = y.saturating_add(x_sh);
103            z = z.saturating_sub(CORDIC_ATAN_Q31[i]);
104        }
105        i += 1;
106    }
107    z.max(0)
108}
109
110fn atan2_from_xy_q31(y: i32, x: i32) -> i32 {
111    if x == 0 && y == 0 {
112        return 0;
113    }
114    let ax = if x == i32::MIN { i32::MAX } else { x.abs() };
115    let ay = if y == i32::MIN { i32::MAX } else { y.abs() };
116    let a = cordic_atan_first_q31(ax, ay);
117    match (x >= 0, y >= 0) {
118        (true, true) => a,
119        (true, false) => a.saturating_neg(),
120        (false, true) => i32::MAX.saturating_sub(a),
121        (false, false) => a.saturating_sub(i32::MAX),
122    }
123}
124
125/// Q31 sine and cosine. `theta` is in CMSIS units: `[-1, 1) → [-π, π)`.
126pub fn sin_cos_q31(theta: q31, sin_val: &mut q31, cos_val: &mut q31) {
127    let (c, s) = cordic_rotate_q31(theta);
128    *cos_val = c;
129    *sin_val = s;
130}
131
132/// Q31 sine function.
133pub fn sin_q31(x: q31) -> q31 {
134    let mut s = 0;
135    let mut c = 0;
136    sin_cos_q31(x, &mut s, &mut c);
137    s
138}
139
140/// Q31 cosine function.
141pub fn cos_q31(x: q31) -> q31 {
142    let mut s = 0;
143    let mut c = 0;
144    sin_cos_q31(x, &mut s, &mut c);
145    c
146}
147
148/// Floating-point square root function.
149pub fn sqrt_f32(in_val: f32, out_val: &mut f32) -> Status {
150    if in_val < 0.0 {
151        *out_val = 0.0;
152        Status::ArgumentError
153    } else {
154        *out_val = in_val.sqrt();
155        Status::Success
156    }
157}
158
159/// Q31 square root (`sqrt(x / 2^31) * 2^31`).
160pub fn sqrt_q31(in_val: q31, out_val: &mut q31) -> Status {
161    if in_val < 0 {
162        *out_val = 0;
163        Status::ArgumentError
164    } else {
165        let n = (in_val as u64) << 31;
166        *out_val = isqrt_u64(n).min(i32::MAX as u64) as q31;
167        Status::Success
168    }
169}
170
171/// Q15 square root (`sqrt(x / 2^15) * 2^15`).
172pub fn sqrt_q15(in_val: q15, out_val: &mut q15) -> Status {
173    if in_val < 0 {
174        *out_val = 0;
175        Status::ArgumentError
176    } else {
177        let n = (in_val as u32) << 15;
178        *out_val = isqrt_u32(n).min(i16::MAX as u32) as q15;
179        Status::Success
180    }
181}
182
183/// Vector square root function.
184pub fn vsqrt_f32(src: &[f32], dst: &mut [f32]) {
185    let len = src.len().min(dst.len());
186    for i in 0..len {
187        if src[i] < 0.0 {
188            dst[i] = 0.0;
189        } else {
190            dst[i] = src[i].sqrt();
191        }
192    }
193}
194
195/// Fixed-point division for Q31 types (numerator / denominator).
196pub fn divide_q31(numerator: q31, denominator: q31, quotient: &mut q31, shift: &mut i16) -> Status {
197    if denominator == 0 {
198        return Status::ArgumentError;
199    }
200    let n = numerator as i64;
201    let d = denominator as i64;
202
203    let res = (n << 31) / d;
204    if res > i32::MAX as i64 || res < i32::MIN as i64 {
205        *shift = 0;
206        *quotient = res.clamp(i32::MIN as i64, i32::MAX as i64) as q31;
207    } else {
208        *shift = 0;
209        *quotient = res as q31;
210    }
211    Status::Success
212}
213
214/// Fixed-point division for Q15 types (numerator / denominator).
215pub fn divide_q15(numerator: q15, denominator: q15, quotient: &mut q15, shift: &mut i16) -> Status {
216    if denominator == 0 {
217        return Status::ArgumentError;
218    }
219    let n = numerator as i32;
220    let d = denominator as i32;
221
222    let res = (n << 15) / d;
223    *shift = 0;
224    *quotient = res.clamp(i16::MIN as i32, i16::MAX as i32) as q15;
225    Status::Success
226}
227
228/// Floating-point natural logarithm.
229pub fn log_f32(x: f32) -> f32 {
230    x.ln()
231}
232
233/// Floating-point exponential.
234pub fn exp_f32(x: f32) -> f32 {
235    x.exp()
236}
237
238/// Floating-point arc-tangent 2.
239pub fn atan2_f32(y: f32, x: f32, res: &mut f32) -> Status {
240    *res = y.atan2(x);
241    Status::Success
242}
243
244/// Q31 arc-tangent 2. Result is `atan2(y, x) / π` in Q1.31 (`[-1, 1)`).
245pub fn atan2_q31(y: q31, x: q31, res: &mut q31) -> Status {
246    *res = atan2_from_xy_q31(y, x);
247    Status::Success
248}
249
250/// Q15 arc-tangent 2. Result is `atan2(y, x) / π` in Q1.15.
251pub fn atan2_q15(y: q15, x: q15, res: &mut q15) -> Status {
252    let z = atan2_from_xy_q31((y as i32) << 16, (x as i32) << 16);
253    *res = (z >> 16) as q15;
254    Status::Success
255}
embedded_dsp/fast_math.rs

embedded_dsp/
fast_math.rs
