embedded_dsp/
complex_math.rs

1#[allow(unused_imports)]
2use crate::math::FloatMath;
3use crate::types::*;
4
5// --- Complex Addition ---
6
7pub fn cmplx_add_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
8    let len = src_a.len().min(src_b.len()).min(dst.len());
9    for i in 0..len {
10        dst[i] = src_a[i] + src_b[i];
11    }
12}
13
14pub fn cmplx_add_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
15    let len = src_a.len().min(src_b.len()).min(dst.len());
16    for i in 0..len {
17        dst[i] = src_a[i].saturating_add(src_b[i]);
18    }
19}
20
21pub fn cmplx_add_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
22    let len = src_a.len().min(src_b.len()).min(dst.len());
23    for i in 0..len {
24        dst[i] = src_a[i].saturating_add(src_b[i]);
25    }
26}
27
28// --- Complex Subtraction ---
29
30pub fn cmplx_sub_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
31    let len = src_a.len().min(src_b.len()).min(dst.len());
32    for i in 0..len {
33        dst[i] = src_a[i] - src_b[i];
34    }
35}
36
37pub fn cmplx_sub_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
38    let len = src_a.len().min(src_b.len()).min(dst.len());
39    for i in 0..len {
40        dst[i] = src_a[i].saturating_sub(src_b[i]);
41    }
42}
43
44pub fn cmplx_sub_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
45    let len = src_a.len().min(src_b.len()).min(dst.len());
46    for i in 0..len {
47        dst[i] = src_a[i].saturating_sub(src_b[i]);
48    }
49}
50
51// --- Complex Multiplication (Complex * Complex) ---
52
53pub fn cmplx_mult_cmplx_f32(src_a: &[f32], src_b: &[f32], dst: &mut [f32]) {
54    let num_samples = src_a.len() / 2;
55    let len = num_samples.min(src_b.len() / 2).min(dst.len() / 2);
56    for i in 0..len {
57        let ar = src_a[2 * i];
58        let ai = src_a[2 * i + 1];
59        let br = src_b[2 * i];
60        let bi = src_b[2 * i + 1];
61
62        dst[2 * i] = ar * br - ai * bi;
63        dst[2 * i + 1] = ar * bi + ai * br;
64    }
65}
66
67pub fn cmplx_mult_cmplx_q31(src_a: &[q31], src_b: &[q31], dst: &mut [q31]) {
68    let num_samples = src_a.len() / 2;
69    let len = num_samples.min(src_b.len() / 2).min(dst.len() / 2);
70    for i in 0..len {
71        let ar = src_a[2 * i];
72        let ai = src_a[2 * i + 1];
73        let br = src_b[2 * i];
74        let bi = src_b[2 * i + 1];
75
76        let real = ((ar as i64 * br as i64) >> 31) - ((ai as i64 * bi as i64) >> 31);
77        let imag = ((ar as i64 * bi as i64) >> 31) + ((ai as i64 * br as i64) >> 31);
78
79        dst[2 * i] = real.clamp(i32::MIN as i64, i32::MAX as i64) as i32;
80        dst[2 * i + 1] = imag.clamp(i32::MIN as i64, i32::MAX as i64) as i32;
81    }
82}
83
84pub fn cmplx_mult_cmplx_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
85    let num_samples = src_a.len() / 2;
86    let len = num_samples.min(src_b.len() / 2).min(dst.len() / 2);
87    for i in 0..len {
88        let ar = src_a[2 * i];
89        let ai = src_a[2 * i + 1];
90        let br = src_b[2 * i];
91        let bi = src_b[2 * i + 1];
92
93        let real = ((ar as i32 * br as i32) >> 15) - ((ai as i32 * bi as i32) >> 15);
94        let imag = ((ar as i32 * bi as i32) >> 15) + ((ai as i32 * br as i32) >> 15);
95
96        dst[2 * i] = real.clamp(i16::MIN as i32, i16::MAX as i32) as i16;
97        dst[2 * i + 1] = imag.clamp(i16::MIN as i32, i16::MAX as i32) as i16;
98    }
99}
100
101// --- Complex Multiplication (Complex * Real) ---
102
103pub fn cmplx_mult_real_f32(src_cmplx: &[f32], src_real: &[f32], dst: &mut [f32]) {
104    let num_samples = (src_cmplx.len() / 2).min(src_real.len()).min(dst.len() / 2);
105    for i in 0..num_samples {
106        let r = src_real[i];
107        dst[2 * i] = src_cmplx[2 * i] * r;
108        dst[2 * i + 1] = src_cmplx[2 * i + 1] * r;
109    }
110}
111
112pub fn cmplx_mult_real_q31(src_cmplx: &[q31], src_real: &[q31], dst: &mut [q31]) {
113    let num_samples = (src_cmplx.len() / 2).min(src_real.len()).min(dst.len() / 2);
114    for i in 0..num_samples {
115        let r = src_real[i];
116        dst[2 * i] = q31_mult(src_cmplx[2 * i], r);
117        dst[2 * i + 1] = q31_mult(src_cmplx[2 * i + 1], r);
118    }
119}
120
121pub fn cmplx_mult_real_q15(src_cmplx: &[q15], src_real: &[q15], dst: &mut [q15]) {
122    let num_samples = (src_cmplx.len() / 2).min(src_real.len()).min(dst.len() / 2);
123    for i in 0..num_samples {
124        let r = src_real[i];
125        dst[2 * i] = q15_mult(src_cmplx[2 * i], r);
126        dst[2 * i + 1] = q15_mult(src_cmplx[2 * i + 1], r);
127    }
128}
129
130// --- Complex Magnitude ---
131
132pub fn cmplx_mag_f32(src: &[f32], dst: &mut [f32]) {
133    let num_samples = (src.len() / 2).min(dst.len());
134    for i in 0..num_samples {
135        let r = src[2 * i];
136        let im = src[2 * i + 1];
137        dst[i] = (r * r + im * im).sqrt();
138    }
139}
140
141pub fn cmplx_mag_q31(src: &[q31], dst: &mut [q31]) {
142    let num_samples = (src.len() / 2).min(dst.len());
143    for i in 0..num_samples {
144        let r = src[2 * i] as i64;
145        let im = src[2 * i + 1] as i64;
146        let mag_sq = ((r * r + im * im) >> 31).clamp(0, i32::MAX as i64) as q31;
147        let mut mag = 0;
148        let _ = crate::fast_math::sqrt_q31(mag_sq, &mut mag);
149        dst[i] = mag;
150    }
151}
152
153pub fn cmplx_mag_q15(src: &[q15], dst: &mut [q15]) {
154    let num_samples = (src.len() / 2).min(dst.len());
155    for i in 0..num_samples {
156        let r = src[2 * i] as i32;
157        let im = src[2 * i + 1] as i32;
158        let mag_sq = ((r * r + im * im) >> 15).clamp(0, i16::MAX as i32) as q15;
159        let mut mag = 0;
160        let _ = crate::fast_math::sqrt_q15(mag_sq, &mut mag);
161        dst[i] = mag;
162    }
163}
164
165// --- Complex Magnitude Squared ---
166
167pub fn cmplx_mag_squared_f32(src: &[f32], dst: &mut [f32]) {
168    let num_samples = (src.len() / 2).min(dst.len());
169    for i in 0..num_samples {
170        let r = src[2 * i];
171        let im = src[2 * i + 1];
172        dst[i] = r * r + im * im;
173    }
174}
175
176pub fn cmplx_mag_squared_q31(src: &[q31], dst: &mut [q31]) {
177    let num_samples = (src.len() / 2).min(dst.len());
178    for i in 0..num_samples {
179        let r = src[2 * i] as i64;
180        let im = src[2 * i + 1] as i64;
181        let acc = ((r * r) >> 33) + ((im * im) >> 33);
182        dst[i] = acc.clamp(0, i32::MAX as i64) as q31;
183    }
184}
185
186pub fn cmplx_mag_squared_q15(src: &[q15], dst: &mut [q15]) {
187    let num_samples = (src.len() / 2).min(dst.len());
188    for i in 0..num_samples {
189        let r = src[2 * i] as i32;
190        let im = src[2 * i + 1] as i32;
191        let acc = ((r * r) >> 17) + ((im * im) >> 17);
192        dst[i] = acc.clamp(0, i16::MAX as i32) as q15;
193    }
194}
195
196// --- Complex Conjugate ---
197
198pub fn cmplx_conj_f32(src: &[f32], dst: &mut [f32]) {
199    let num_samples = (src.len() / 2).min(dst.len() / 2);
200    for i in 0..num_samples {
201        dst[2 * i] = src[2 * i];
202        dst[2 * i + 1] = -src[2 * i + 1];
203    }
204}
205
206pub fn cmplx_conj_q31(src: &[q31], dst: &mut [q31]) {
207    let num_samples = (src.len() / 2).min(dst.len() / 2);
208    for i in 0..num_samples {
209        dst[2 * i] = src[2 * i];
210        dst[2 * i + 1] = src[2 * i + 1].saturating_neg();
211    }
212}
213
214pub fn cmplx_conj_q15(src: &[q15], dst: &mut [q15]) {
215    let num_samples = (src.len() / 2).min(dst.len() / 2);
216    for i in 0..num_samples {
217        dst[2 * i] = src[2 * i];
218        dst[2 * i + 1] = src[2 * i + 1].saturating_neg();
219    }
220}
221
222// --- Complex Dot Product ---
223
224pub fn cmplx_dot_prod_f32(src_a: &[f32], src_b: &[f32]) -> Complex<f32> {
225    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
226    let mut real_sum = 0.0f32;
227    let mut imag_sum = 0.0f32;
228    for i in 0..num_samples {
229        let ar = src_a[2 * i];
230        let ai = src_a[2 * i + 1];
231        let br = src_b[2 * i];
232        let bi = src_b[2 * i + 1];
233
234        real_sum += ar * br - ai * bi;
235        imag_sum += ar * bi + ai * br;
236    }
237    Complex::new(real_sum, imag_sum)
238}
239
240pub fn cmplx_dot_prod_q31(src_a: &[q31], src_b: &[q31]) -> Complex<q63> {
241    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
242    let mut real_sum: q63 = 0;
243    let mut imag_sum: q63 = 0;
244    for i in 0..num_samples {
245        let ar = src_a[2 * i] as i64;
246        let ai = src_a[2 * i + 1] as i64;
247        let br = src_b[2 * i] as i64;
248        let bi = src_b[2 * i + 1] as i64;
249
250        real_sum += (ar * br - ai * bi) >> 14;
251        imag_sum += (ar * bi + ai * br) >> 14;
252    }
253    Complex::new(real_sum, imag_sum)
254}
255
256pub fn cmplx_dot_prod_q15(src_a: &[q15], src_b: &[q15]) -> Complex<q63> {
257    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
258    let mut real_sum: q63 = 0;
259    let mut imag_sum: q63 = 0;
260    for i in 0..num_samples {
261        let ar = src_a[2 * i] as i32;
262        let ai = src_a[2 * i + 1] as i32;
263        let br = src_b[2 * i] as i32;
264        let bi = src_b[2 * i + 1] as i32;
265
266        real_sum += (ar * br - ai * bi) as q63;
267        imag_sum += (ar * bi + ai * br) as q63;
268    }
269    Complex::new(real_sum, imag_sum)
270}
embedded_dsp/complex_math.rs

embedded_dsp/
complex_math.rs
