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::types::*;
6
7/// Floating-point sine calculation.
8pub fn sin_f32(x: f32) -> f32 {
9    x.sin()
10}
11
12/// Floating-point cosine calculation.
13pub fn cos_f32(x: f32) -> f32 {
14    x.cos()
15}
16
17/// Floating-point sine and cosine calculation.
18pub fn sin_cos_f32(theta: f32, sin_val: &mut f32, cos_val: &mut f32) {
19    let rad = theta * (core::f32::consts::PI / 180.0);
20    *sin_val = rad.sin();
21    *cos_val = rad.cos();
22}
23
24/// Q31 sine and cosine calculation.
25pub fn sin_cos_q31(theta: q31, sin_val: &mut q31, cos_val: &mut q31) {
26    let theta_f = (theta as f64) / 2147483648.0 * core::f64::consts::PI;
27    let s = theta_f.sin();
28    let c = theta_f.cos();
29
30    *sin_val = (s * 2147483647.0).clamp(-2147483648.0, 2147483647.0) as q31;
31    *cos_val = (c * 2147483647.0).clamp(-2147483648.0, 2147483647.0) as q31;
32}
33
34/// Q31 sine function.
35pub fn sin_q31(x: q31) -> q31 {
36    let mut s = 0;
37    let mut c = 0;
38    sin_cos_q31(x, &mut s, &mut c);
39    s
40}
41
42/// Q31 cosine function.
43pub fn cos_q31(x: q31) -> q31 {
44    let mut s = 0;
45    let mut c = 0;
46    sin_cos_q31(x, &mut s, &mut c);
47    c
48}
49
50/// Floating-point square root function.
51pub fn sqrt_f32(in_val: f32, out_val: &mut f32) -> Status {
52    if in_val < 0.0 {
53        *out_val = 0.0;
54        Status::ArgumentError
55    } else {
56        *out_val = in_val.sqrt();
57        Status::Success
58    }
59}
60
61/// Q31 square root function.
62pub fn sqrt_q31(in_val: q31, out_val: &mut q31) -> Status {
63    if in_val < 0 {
64        *out_val = 0;
65        Status::ArgumentError
66    } else {
67        let f = in_val as f64 / 2147483648.0;
68        let res = f.sqrt();
69        *out_val = (res * 2147483647.0).clamp(0.0, 2147483647.0) as q31;
70        Status::Success
71    }
72}
73
74/// Q15 square root function.
75pub fn sqrt_q15(in_val: q15, out_val: &mut q15) -> Status {
76    if in_val < 0 {
77        *out_val = 0;
78        Status::ArgumentError
79    } else {
80        let f = in_val as f32 / 32768.0;
81        let res = f.sqrt();
82        *out_val = (res * 32767.0).clamp(0.0, 32767.0) as q15;
83        Status::Success
84    }
85}
86
87/// Vector square root function.
88pub fn vsqrt_f32(src: &[f32], dst: &mut [f32]) {
89    let len = src.len().min(dst.len());
90    for i in 0..len {
91        if src[i] < 0.0 {
92            dst[i] = 0.0;
93        } else {
94            dst[i] = src[i].sqrt();
95        }
96    }
97}
98
99/// Fixed-point division for Q31 types (numerator / denominator).
100pub fn divide_q31(numerator: q31, denominator: q31, quotient: &mut q31, shift: &mut i16) -> Status {
101    if denominator == 0 {
102        return Status::ArgumentError;
103    }
104    let n = numerator as i64;
105    let d = denominator as i64;
106
107    let res = (n << 31) / d;
108    if res > i32::MAX as i64 || res < i32::MIN as i64 {
109        *shift = 0;
110        *quotient = res.clamp(i32::MIN as i64, i32::MAX as i64) as q31;
111    } else {
112        *shift = 0;
113        *quotient = res as q31;
114    }
115    Status::Success
116}
117
118/// Fixed-point division for Q15 types (numerator / denominator).
119pub fn divide_q15(numerator: q15, denominator: q15, quotient: &mut q15, shift: &mut i16) -> Status {
120    if denominator == 0 {
121        return Status::ArgumentError;
122    }
123    let n = numerator as i32;
124    let d = denominator as i32;
125
126    let res = (n << 15) / d;
127    *shift = 0;
128    *quotient = res.clamp(i16::MIN as i32, i16::MAX as i32) as q15;
129    Status::Success
130}
131
132/// Floating-point natural logarithm.
133pub fn log_f32(x: f32) -> f32 {
134    x.ln()
135}
136
137/// Floating-point exponential.
138pub fn exp_f32(x: f32) -> f32 {
139    x.exp()
140}
141
142/// Floating-point arc-tangent 2.
143pub fn atan2_f32(y: f32, x: f32, res: &mut f32) -> Status {
144    *res = y.atan2(x);
145    Status::Success
146}
147
148/// Q31 arc-tangent 2.
149pub fn atan2_q31(y: q31, x: q31, res: &mut q31) -> Status {
150    let y_f = y as f64 / 2147483648.0;
151    let x_f = x as f64 / 2147483648.0;
152    let ang = y_f.atan2(x_f) / core::f64::consts::PI;
153
154    *res = (ang * 2147483647.0).clamp(-2147483648.0, 2147483647.0) as q31;
155    Status::Success
156}
157
158/// Q15 arc-tangent 2.
159pub fn atan2_q15(y: q15, x: q15, res: &mut q15) -> Status {
160    let y_f = y as f32 / 32768.0;
161    let x_f = x as f32 / 32768.0;
162    let ang = y_f.atan2(x_f) / core::f32::consts::PI;
163    *res = (ang * 32767.0).clamp(-32768.0, 32767.0) as q15;
164    Status::Success
165}
embedded_dsp/fast_math.rs

embedded_dsp/
fast_math.rs
