embedded_dsp/
support.rs

1//! Support functions (copy, fill, format conversions q7/q15/q31/f32, sort, barycenter, weighted sum).
2
3use crate::types::*;
4
5// --- Copy & Fill ---
6
7pub fn copy_f32(src: &[f32], dst: &mut [f32]) {
8    let len = src.len().min(dst.len());
9    dst[..len].copy_from_slice(&src[..len]);
10}
11
12pub fn copy_q31(src: &[q31], dst: &mut [q31]) {
13    let len = src.len().min(dst.len());
14    dst[..len].copy_from_slice(&src[..len]);
15}
16
17pub fn copy_q15(src: &[q15], dst: &mut [q15]) {
18    let len = src.len().min(dst.len());
19    dst[..len].copy_from_slice(&src[..len]);
20}
21
22pub fn copy_q7(src: &[q7], dst: &mut [q7]) {
23    let len = src.len().min(dst.len());
24    dst[..len].copy_from_slice(&src[..len]);
25}
26
27pub fn fill_f32(value: f32, dst: &mut [f32]) {
28    dst.fill(value);
29}
30
31pub fn fill_q31(value: q31, dst: &mut [q31]) {
32    dst.fill(value);
33}
34
35pub fn fill_q15(value: q15, dst: &mut [q15]) {
36    dst.fill(value);
37}
38
39pub fn fill_q7(value: q7, dst: &mut [q7]) {
40    dst.fill(value);
41}
42
43// --- Type Conversions ---
44
45pub fn q7_to_q15(src: &[q7], dst: &mut [q15]) {
46    let len = src.len().min(dst.len());
47    for i in 0..len {
48        dst[i] = (src[i] as i16) << 8;
49    }
50}
51
52pub fn q7_to_q31(src: &[q7], dst: &mut [q31]) {
53    let len = src.len().min(dst.len());
54    for i in 0..len {
55        dst[i] = (src[i] as i32) << 24;
56    }
57}
58
59pub fn q7_to_f32(src: &[q7], dst: &mut [f32]) {
60    let len = src.len().min(dst.len());
61    for i in 0..len {
62        dst[i] = (src[i] as f32) / 128.0;
63    }
64}
65
66pub fn q15_to_q7(src: &[q15], dst: &mut [q7]) {
67    let len = src.len().min(dst.len());
68    for i in 0..len {
69        dst[i] = (src[i] >> 8) as q7;
70    }
71}
72
73pub fn q15_to_q31(src: &[q15], dst: &mut [q31]) {
74    let len = src.len().min(dst.len());
75    for i in 0..len {
76        dst[i] = (src[i] as i32) << 16;
77    }
78}
79
80pub fn q15_to_f32(src: &[q15], dst: &mut [f32]) {
81    let len = src.len().min(dst.len());
82    for i in 0..len {
83        dst[i] = (src[i] as f32) / 32768.0;
84    }
85}
86
87pub fn q31_to_q7(src: &[q31], dst: &mut [q7]) {
88    let len = src.len().min(dst.len());
89    for i in 0..len {
90        dst[i] = (src[i] >> 24) as q7;
91    }
92}
93
94pub fn q31_to_q15(src: &[q31], dst: &mut [q15]) {
95    let len = src.len().min(dst.len());
96    for i in 0..len {
97        dst[i] = (src[i] >> 16) as q15;
98    }
99}
100
101pub fn q31_to_f32(src: &[q31], dst: &mut [f32]) {
102    let len = src.len().min(dst.len());
103    for i in 0..len {
104        dst[i] = (src[i] as f32) / 2147483648.0;
105    }
106}
107
108pub fn f32_to_q7(src: &[f32], dst: &mut [q7]) {
109    let len = src.len().min(dst.len());
110    for i in 0..len {
111        dst[i] = (src[i] * 128.0).clamp(-128.0, 127.0) as q7;
112    }
113}
114
115pub fn f32_to_q15(src: &[f32], dst: &mut [q15]) {
116    let len = src.len().min(dst.len());
117    for i in 0..len {
118        dst[i] = (src[i] * 32768.0).clamp(-32768.0, 32767.0) as q15;
119    }
120}
121
122pub fn f32_to_q31(src: &[f32], dst: &mut [q31]) {
123    let len = src.len().min(dst.len());
124    for i in 0..len {
125        dst[i] = (src[i] * 2147483648.0).clamp(-2147483648.0, 2147483647.0) as q31;
126    }
127}
128
129// --- Sorting, Barycenter, Weighted Sum ---
130
131/// Insertion sort for f32 (ascending or descending order).
132pub fn sort_f32(src: &[f32], dst: &mut [f32], dir_ascending: bool) {
133    let len = src.len().min(dst.len());
134    dst[..len].copy_from_slice(&src[..len]);
135    let slice = &mut dst[..len];
136    for i in 1..len {
137        let mut j = i;
138        while j > 0 {
139            let swap_needed = if dir_ascending {
140                slice[j - 1] > slice[j]
141            } else {
142                slice[j - 1] < slice[j]
143            };
144            if swap_needed {
145                slice.swap(j - 1, j);
146                j -= 1;
147            } else {
148                break;
149            }
150        }
151    }
152}
153
154/// Compute barycenter of points weighted by given weights.
155pub fn barycenter_f32(
156    in_pts: &[f32],
157    weights: &[f32],
158    out_center: &mut [f32],
159    num_vecs: usize,
160    vec_dim: usize,
161) -> Status {
162    if in_pts.len() < num_vecs * vec_dim || weights.len() < num_vecs || out_center.len() < vec_dim {
163        return Status::LengthError;
164    }
165    out_center[..vec_dim].fill(0.0);
166    let mut weight_sum = 0.0f32;
167    for i in 0..num_vecs {
168        let w = weights[i];
169        weight_sum += w;
170        for d in 0..vec_dim {
171            out_center[d] += in_pts[i * vec_dim + d] * w;
172        }
173    }
174    if weight_sum != 0.0 {
175        for d in 0..vec_dim {
176            out_center[d] /= weight_sum;
177        }
178    }
179    Status::Success
180}
181
182/// Compute weighted sum of values.
183pub fn weighted_sum_f32(in_vals: &[f32], weights: &[f32]) -> f32 {
184    let len = in_vals.len().min(weights.len());
185    let mut sum = 0.0f32;
186    let mut w_sum = 0.0f32;
187    for i in 0..len {
188        sum += in_vals[i] * weights[i];
189        w_sum += weights[i];
190    }
191    if w_sum != 0.0 {
192        sum / w_sum
193    } else {
194        0.0
195    }
196}
embedded_dsp/support.rs

embedded_dsp/
support.rs
