qualia_audio/features/peaks/
spectral_peaks.rs1use crate::types::AudioError;
4
5#[inline]
9fn parabolic(ym1: f32, y0: f32, yp1: f32) -> (f32, f32) {
10 let denom = ym1 - 2.0 * y0 + yp1;
11 if denom == 0.0 || !denom.is_finite() {
12 return (0.0, y0);
13 }
14 let off = (0.5 * (ym1 - yp1) / denom).clamp(-0.5, 0.5);
15 let peak = y0 - 0.25 * (ym1 - yp1) * off;
17 (off, peak)
18}
19
20pub fn spectral_peaks(
37 mag: &[f32],
38 sample_rate: f32,
39 max_peaks: usize,
40 out_freq: &mut [f32],
41 out_mag: &mut [f32],
42) -> Result<usize, AudioError> {
43 if sample_rate <= 0.0 || !sample_rate.is_finite() {
44 return Err(AudioError::InvalidParameter);
45 }
46 let cap = out_freq.len().min(out_mag.len()).min(max_peaks);
47 if mag.len() < 3 || cap == 0 {
48 return Ok(0);
49 }
50 let bin_hz = sample_rate / (2.0 * (mag.len() - 1) as f32);
51
52 let mut count: usize = 0;
53 for k in 1..mag.len() - 1 {
54 let y0 = mag[k];
55 if !(y0 > mag[k - 1] && y0 >= mag[k + 1] && y0 > 0.0) {
56 continue;
57 }
58 let (off, peak) = parabolic(mag[k - 1], y0, mag[k + 1]);
59 let freq = (k as f32 + off) * bin_hz;
60
61 if count < cap {
63 let mut j = count;
65 while j > 0 && out_mag[j - 1] < peak {
66 out_mag[j] = out_mag[j - 1];
67 out_freq[j] = out_freq[j - 1];
68 j -= 1;
69 }
70 out_mag[j] = peak;
71 out_freq[j] = freq;
72 count += 1;
73 } else if peak > out_mag[cap - 1] {
74 let mut j = cap - 1;
76 while j > 0 && out_mag[j - 1] < peak {
77 out_mag[j] = out_mag[j - 1];
78 out_freq[j] = out_freq[j - 1];
79 j -= 1;
80 }
81 out_mag[j] = peak;
82 out_freq[j] = freq;
83 }
84 }
85 Ok(count)
86}
87
88#[cfg(test)]
89mod tests {
90 use super::*;
91
92 #[test]
96 fn golden_peak_frequency() {
97 let sr = 44100.0f32;
98 let n = 513usize; let bin_hz = sr / (2.0 * (n - 1) as f32); let center = 100.3f32;
101 let mut mag = vec![0.0f32; n];
102 for (k, m) in mag.iter_mut().enumerate() {
103 let v = 1.0 - 0.01 * (k as f32 - center) * (k as f32 - center);
104 *m = v.max(0.0);
105 }
106 let mut fr = [0.0f32; 4];
107 let mut mg = [0.0f32; 4];
108 let np = spectral_peaks(&mag, sr, 4, &mut fr, &mut mg).expect("peaks");
109 assert_eq!(np, 1);
110 let true_hz = center * bin_hz; assert!(
112 (fr[0] - true_hz).abs() < 2.0,
113 "freq={} want={}",
114 fr[0],
115 true_hz
116 );
117 assert!(mg[0] > 0.99, "peak mag={}", mg[0]);
118 }
119
120 #[test]
122 fn strongest_first() {
123 let sr = 8000.0f32;
124 let n = 129usize; let mut mag = vec![0.0f32; n];
126 mag[19] = 0.5;
128 mag[20] = 1.0;
129 mag[21] = 0.5;
130 mag[59] = 1.5;
131 mag[60] = 3.0;
132 mag[61] = 1.5;
133 let mut fr = [0.0f32; 4];
134 let mut mg = [0.0f32; 4];
135 let np = spectral_peaks(&mag, sr, 4, &mut fr, &mut mg).expect("peaks");
136 assert_eq!(np, 2);
137 assert!(mg[0] > mg[1]);
138 assert!((fr[0] - 1875.0).abs() < 1.0, "freq0={}", fr[0]);
140 assert!((fr[1] - 625.0).abs() < 1.0, "freq1={}", fr[1]);
141 }
142
143 #[test]
145 fn max_peaks_limits() {
146 let sr = 8000.0f32;
147 let n = 129usize;
148 let mut mag = vec![0.0f32; n];
149 for c in [10usize, 30, 50, 70] {
150 mag[c - 1] = 0.5;
151 mag[c] = c as f32; mag[c + 1] = 0.5;
153 }
154 let mut fr = [0.0f32; 2];
155 let mut mg = [0.0f32; 2];
156 let np = spectral_peaks(&mag, sr, 2, &mut fr, &mut mg).expect("peaks");
157 assert_eq!(np, 2);
158 assert!((fr[0] - 70.0 * (sr / 256.0)).abs() < 1.0, "f0={}", fr[0]);
160 assert!((fr[1] - 50.0 * (sr / 256.0)).abs() < 1.0, "f1={}", fr[1]);
161 }
162
163 #[test]
164 fn rejects_bad_rate() {
165 let mag = [0.0f32, 1.0, 0.0];
166 let mut fr = [0.0f32; 2];
167 let mut mg = [0.0f32; 2];
168 assert_eq!(
169 spectral_peaks(&mag, 0.0, 2, &mut fr, &mut mg),
170 Err(AudioError::InvalidParameter)
171 );
172 }
173}