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 f64 / 2147483648.0;
145        let im = src[2 * i + 1] as f64 / 2147483648.0;
146        let mag = (r * r + im * im).sqrt();
147        dst[i] = (mag * 2147483647.0).clamp(0.0, 2147483647.0) as q31;
148    }
149}
150
151pub fn cmplx_mag_q15(src: &[q15], dst: &mut [q15]) {
152    let num_samples = (src.len() / 2).min(dst.len());
153    for i in 0..num_samples {
154        let r = src[2 * i] as f32 / 32768.0;
155        let im = src[2 * i + 1] as f32 / 32768.0;
156        let mag = (r * r + im * im).sqrt();
157        dst[i] = (mag * 32767.0).clamp(0.0, 32767.0) as q15;
158    }
159}
160
161// --- Complex Magnitude Squared ---
162
163pub fn cmplx_mag_squared_f32(src: &[f32], dst: &mut [f32]) {
164    let num_samples = (src.len() / 2).min(dst.len());
165    for i in 0..num_samples {
166        let r = src[2 * i];
167        let im = src[2 * i + 1];
168        dst[i] = r * r + im * im;
169    }
170}
171
172pub fn cmplx_mag_squared_q31(src: &[q31], dst: &mut [q31]) {
173    let num_samples = (src.len() / 2).min(dst.len());
174    for i in 0..num_samples {
175        let r = src[2 * i] as i64;
176        let im = src[2 * i + 1] as i64;
177        let acc = ((r * r) >> 33) + ((im * im) >> 33);
178        dst[i] = acc.clamp(0, i32::MAX as i64) as q31;
179    }
180}
181
182pub fn cmplx_mag_squared_q15(src: &[q15], dst: &mut [q15]) {
183    let num_samples = (src.len() / 2).min(dst.len());
184    for i in 0..num_samples {
185        let r = src[2 * i] as i32;
186        let im = src[2 * i + 1] as i32;
187        let acc = ((r * r) >> 17) + ((im * im) >> 17);
188        dst[i] = acc.clamp(0, i16::MAX as i32) as q15;
189    }
190}
191
192// --- Complex Conjugate ---
193
194pub fn cmplx_conj_f32(src: &[f32], dst: &mut [f32]) {
195    let num_samples = (src.len() / 2).min(dst.len() / 2);
196    for i in 0..num_samples {
197        dst[2 * i] = src[2 * i];
198        dst[2 * i + 1] = -src[2 * i + 1];
199    }
200}
201
202pub fn cmplx_conj_q31(src: &[q31], dst: &mut [q31]) {
203    let num_samples = (src.len() / 2).min(dst.len() / 2);
204    for i in 0..num_samples {
205        dst[2 * i] = src[2 * i];
206        dst[2 * i + 1] = src[2 * i + 1].saturating_neg();
207    }
208}
209
210pub fn cmplx_conj_q15(src: &[q15], dst: &mut [q15]) {
211    let num_samples = (src.len() / 2).min(dst.len() / 2);
212    for i in 0..num_samples {
213        dst[2 * i] = src[2 * i];
214        dst[2 * i + 1] = src[2 * i + 1].saturating_neg();
215    }
216}
217
218// --- Complex Dot Product ---
219
220pub fn cmplx_dot_prod_f32(src_a: &[f32], src_b: &[f32]) -> Complex<f32> {
221    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
222    let mut real_sum = 0.0f32;
223    let mut imag_sum = 0.0f32;
224    for i in 0..num_samples {
225        let ar = src_a[2 * i];
226        let ai = src_a[2 * i + 1];
227        let br = src_b[2 * i];
228        let bi = src_b[2 * i + 1];
229
230        real_sum += ar * br - ai * bi;
231        imag_sum += ar * bi + ai * br;
232    }
233    Complex::new(real_sum, imag_sum)
234}
235
236pub fn cmplx_dot_prod_q31(src_a: &[q31], src_b: &[q31]) -> Complex<q63> {
237    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
238    let mut real_sum: q63 = 0;
239    let mut imag_sum: q63 = 0;
240    for i in 0..num_samples {
241        let ar = src_a[2 * i] as i64;
242        let ai = src_a[2 * i + 1] as i64;
243        let br = src_b[2 * i] as i64;
244        let bi = src_b[2 * i + 1] as i64;
245
246        real_sum += (ar * br - ai * bi) >> 14;
247        imag_sum += (ar * bi + ai * br) >> 14;
248    }
249    Complex::new(real_sum, imag_sum)
250}
251
252pub fn cmplx_dot_prod_q15(src_a: &[q15], src_b: &[q15]) -> Complex<q63> {
253    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
254    let mut real_sum: q63 = 0;
255    let mut imag_sum: q63 = 0;
256    for i in 0..num_samples {
257        let ar = src_a[2 * i] as i32;
258        let ai = src_a[2 * i + 1] as i32;
259        let br = src_b[2 * i] as i32;
260        let bi = src_b[2 * i + 1] as i32;
261
262        real_sum += (ar * br - ai * bi) as q63;
263        imag_sum += (ar * bi + ai * br) as q63;
264    }
265    Complex::new(real_sum, imag_sum)
266}
embedded_dsp/complex_math.rs

embedded_dsp/
complex_math.rs
