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 (ar, ai, br, bi) = (ar.to_bits(), ai.to_bits(), br.to_bits(), bi.to_bits());
77        let real = ((ar as i64 * br as i64) >> 31) - ((ai as i64 * bi as i64) >> 31);
78        let imag = ((ar as i64 * bi as i64) >> 31) + ((ai as i64 * br as i64) >> 31);
79
80        dst[2 * i] = q31::from_bits(real.clamp(i32::MIN as i64, i32::MAX as i64) as i32);
81        dst[2 * i + 1] = q31::from_bits(imag.clamp(i32::MIN as i64, i32::MAX as i64) as i32);
82    }
83}
84
85pub fn cmplx_mult_cmplx_q15(src_a: &[q15], src_b: &[q15], dst: &mut [q15]) {
86    let num_samples = src_a.len() / 2;
87    let len = num_samples.min(src_b.len() / 2).min(dst.len() / 2);
88    for i in 0..len {
89        let ar = src_a[2 * i];
90        let ai = src_a[2 * i + 1];
91        let br = src_b[2 * i];
92        let bi = src_b[2 * i + 1];
93
94        let (ar, ai, br, bi) = (ar.to_bits(), ai.to_bits(), br.to_bits(), bi.to_bits());
95        let real = ((ar as i32 * br as i32) >> 15) - ((ai as i32 * bi as i32) >> 15);
96        let imag = ((ar as i32 * bi as i32) >> 15) + ((ai as i32 * br as i32) >> 15);
97
98        dst[2 * i] = q15::from_bits(real.clamp(i16::MIN as i32, i16::MAX as i32) as i16);
99        dst[2 * i + 1] = q15::from_bits(imag.clamp(i16::MIN as i32, i16::MAX as i32) as i16);
100    }
101}
102
103// --- Complex Multiplication (Complex * Real) ---
104
105pub fn cmplx_mult_real_f32(src_cmplx: &[f32], src_real: &[f32], dst: &mut [f32]) {
106    let num_samples = (src_cmplx.len() / 2).min(src_real.len()).min(dst.len() / 2);
107    for i in 0..num_samples {
108        let r = src_real[i];
109        dst[2 * i] = src_cmplx[2 * i] * r;
110        dst[2 * i + 1] = src_cmplx[2 * i + 1] * r;
111    }
112}
113
114pub fn cmplx_mult_real_q31(src_cmplx: &[q31], src_real: &[q31], dst: &mut [q31]) {
115    let num_samples = (src_cmplx.len() / 2).min(src_real.len()).min(dst.len() / 2);
116    for i in 0..num_samples {
117        let r = src_real[i];
118        dst[2 * i] = q31_mult(src_cmplx[2 * i], r);
119        dst[2 * i + 1] = q31_mult(src_cmplx[2 * i + 1], r);
120    }
121}
122
123pub fn cmplx_mult_real_q15(src_cmplx: &[q15], src_real: &[q15], dst: &mut [q15]) {
124    let num_samples = (src_cmplx.len() / 2).min(src_real.len()).min(dst.len() / 2);
125    for i in 0..num_samples {
126        let r = src_real[i];
127        dst[2 * i] = q15_mult(src_cmplx[2 * i], r);
128        dst[2 * i + 1] = q15_mult(src_cmplx[2 * i + 1], r);
129    }
130}
131
132// --- Complex Magnitude ---
133
134pub fn cmplx_mag_f32(src: &[f32], dst: &mut [f32]) {
135    let num_samples = (src.len() / 2).min(dst.len());
136    for i in 0..num_samples {
137        let r = src[2 * i];
138        let im = src[2 * i + 1];
139        dst[i] = (r * r + im * im).sqrt();
140    }
141}
142
143pub fn cmplx_mag_q31(src: &[q31], dst: &mut [q31]) {
144    let num_samples = (src.len() / 2).min(dst.len());
145    for i in 0..num_samples {
146        let r = src[2 * i].to_bits() as i64;
147        let im = src[2 * i + 1].to_bits() as i64;
148        let mag_sq = q31::from_bits(((r * r + im * im) >> 31).clamp(0, i32::MAX as i64) as i32);
149        let mut mag = q31::ZERO;
150        let _ = crate::fast_math::sqrt_q31(mag_sq, &mut mag);
151        dst[i] = mag;
152    }
153}
154
155pub fn cmplx_mag_q15(src: &[q15], dst: &mut [q15]) {
156    let num_samples = (src.len() / 2).min(dst.len());
157    for i in 0..num_samples {
158        let r = src[2 * i].to_bits() as i32;
159        let im = src[2 * i + 1].to_bits() as i32;
160        let mag_sq = q15::from_bits(((r * r + im * im) >> 15).clamp(0, i16::MAX as i32) as i16);
161        let mut mag = q15::ZERO;
162        let _ = crate::fast_math::sqrt_q15(mag_sq, &mut mag);
163        dst[i] = mag;
164    }
165}
166
167// --- Complex Magnitude Squared ---
168
169pub fn cmplx_mag_squared_f32(src: &[f32], dst: &mut [f32]) {
170    let num_samples = (src.len() / 2).min(dst.len());
171    for i in 0..num_samples {
172        let r = src[2 * i];
173        let im = src[2 * i + 1];
174        dst[i] = r * r + im * im;
175    }
176}
177
178pub fn cmplx_mag_squared_q31(src: &[q31], dst: &mut [q31]) {
179    let num_samples = (src.len() / 2).min(dst.len());
180    for i in 0..num_samples {
181        let r = src[2 * i].to_bits() as i64;
182        let im = src[2 * i + 1].to_bits() as i64;
183        let acc = ((r * r) >> 33) + ((im * im) >> 33);
184        dst[i] = q31::from_bits(acc.clamp(0, i32::MAX as i64) as i32);
185    }
186}
187
188pub fn cmplx_mag_squared_q15(src: &[q15], dst: &mut [q15]) {
189    let num_samples = (src.len() / 2).min(dst.len());
190    for i in 0..num_samples {
191        let r = src[2 * i].to_bits() as i32;
192        let im = src[2 * i + 1].to_bits() as i32;
193        let acc = ((r * r) >> 17) + ((im * im) >> 17);
194        dst[i] = q15::from_bits(acc.clamp(0, i16::MAX as i32) as i16);
195    }
196}
197
198// --- Complex Conjugate ---
199
200pub fn cmplx_conj_f32(src: &[f32], dst: &mut [f32]) {
201    let num_samples = (src.len() / 2).min(dst.len() / 2);
202    for i in 0..num_samples {
203        dst[2 * i] = src[2 * i];
204        dst[2 * i + 1] = -src[2 * i + 1];
205    }
206}
207
208pub fn cmplx_conj_q31(src: &[q31], dst: &mut [q31]) {
209    let num_samples = (src.len() / 2).min(dst.len() / 2);
210    for i in 0..num_samples {
211        dst[2 * i] = src[2 * i];
212        dst[2 * i + 1] = src[2 * i + 1].saturating_neg();
213    }
214}
215
216pub fn cmplx_conj_q15(src: &[q15], dst: &mut [q15]) {
217    let num_samples = (src.len() / 2).min(dst.len() / 2);
218    for i in 0..num_samples {
219        dst[2 * i] = src[2 * i];
220        dst[2 * i + 1] = src[2 * i + 1].saturating_neg();
221    }
222}
223
224// --- Complex Dot Product ---
225
226pub fn cmplx_dot_prod_f32(src_a: &[f32], src_b: &[f32]) -> Complex<f32> {
227    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
228    let mut real_sum = 0.0f32;
229    let mut imag_sum = 0.0f32;
230    for i in 0..num_samples {
231        let ar = src_a[2 * i];
232        let ai = src_a[2 * i + 1];
233        let br = src_b[2 * i];
234        let bi = src_b[2 * i + 1];
235
236        real_sum += ar * br - ai * bi;
237        imag_sum += ar * bi + ai * br;
238    }
239    Complex::new(real_sum, imag_sum)
240}
241
242pub fn cmplx_dot_prod_q31(src_a: &[q31], src_b: &[q31]) -> Complex<q63> {
243    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
244    let mut real_sum: q63 = 0;
245    let mut imag_sum: q63 = 0;
246    for i in 0..num_samples {
247        let ar = src_a[2 * i].to_bits() as i64;
248        let ai = src_a[2 * i + 1].to_bits() as i64;
249        let br = src_b[2 * i].to_bits() as i64;
250        let bi = src_b[2 * i + 1].to_bits() as i64;
251
252        real_sum += (ar * br - ai * bi) >> 14;
253        imag_sum += (ar * bi + ai * br) >> 14;
254    }
255    Complex::new(real_sum, imag_sum)
256}
257
258pub fn cmplx_dot_prod_q15(src_a: &[q15], src_b: &[q15]) -> Complex<q63> {
259    let num_samples = (src_a.len() / 2).min(src_b.len() / 2);
260    let mut real_sum: q63 = 0;
261    let mut imag_sum: q63 = 0;
262    for i in 0..num_samples {
263        let ar = src_a[2 * i].to_bits() as i32;
264        let ai = src_a[2 * i + 1].to_bits() as i32;
265        let br = src_b[2 * i].to_bits() as i32;
266        let bi = src_b[2 * i + 1].to_bits() as i32;
267
268        real_sum += (ar * br - ai * bi) as i64;
269        imag_sum += (ar * bi + ai * br) as i64;
270    }
271    Complex::new(real_sum, imag_sum)
272}
embedded_dsp/complex_math.rs

embedded_dsp/
complex_math.rs
