qualia_audio/features/pitch/
yin_fft.rs1use crate::features::fft::radix2::fft_radix2;
25use crate::features::pitch::confidence::pitch_confidence;
26use crate::features::pitch::estimate::{
27 absolute_threshold, cmnd_in_place, parabolic_min, PitchEstimate,
28};
29use crate::types::AudioError;
30
31#[inline]
35pub fn yin_fft_scratch_len(n: usize, max_lag: usize) -> usize {
36 let l = n.max(1).next_power_of_two();
37 4 * l + n + max_lag + 2
38}
39
40pub fn yin_fft_pitch(
51 frame: &[f32],
52 sample_rate: f32,
53 min_hz: f32,
54 max_hz: f32,
55 threshold: f32,
56 scratch: &mut [f32],
57) -> Result<PitchEstimate, AudioError> {
58 let params_ok = sample_rate.is_finite()
59 && sample_rate > 0.0
60 && min_hz.is_finite()
61 && min_hz > 0.0
62 && max_hz.is_finite()
63 && max_hz > min_hz
64 && threshold.is_finite();
65 if !params_ok {
66 return Err(AudioError::InvalidParameter);
67 }
68 let n = frame.len();
69 if n < 8 {
70 return Err(AudioError::InvalidParameter);
71 }
72
73 let min_lag = ((sample_rate / max_hz).floor() as usize).max(2);
74 let max_lag = ((sample_rate / min_hz).floor() as usize).min(n / 2);
75 if max_lag < min_lag + 2 {
76 return Err(AudioError::InvalidParameter);
77 }
78
79 let l = n.next_power_of_two();
80 if scratch.len() < yin_fft_scratch_len(n, max_lag) {
81 return Err(AudioError::OutputBufferTooSmall);
82 }
83 let w = n - max_lag; let (comp_a, rest) = scratch.split_at_mut(2 * l);
87 let (comp_b, rest) = rest.split_at_mut(2 * l);
88 let (psum, rest) = rest.split_at_mut(n + 1);
89 let (diff, _) = rest.split_at_mut(max_lag + 1);
90
91 for i in 0..l {
93 let av = if i < w { frame[i] } else { 0.0 };
94 let bv = if i < n { frame[i] } else { 0.0 };
95 comp_a[2 * i] = av;
96 comp_a[2 * i + 1] = 0.0;
97 comp_b[2 * i] = bv;
98 comp_b[2 * i + 1] = 0.0;
99 }
100
101 fft_radix2(comp_a, false)?;
103 fft_radix2(comp_b, false)?;
104 for k in 0..l {
105 let ar = comp_a[2 * k];
106 let ai = comp_a[2 * k + 1];
107 let br = comp_b[2 * k];
108 let bi = comp_b[2 * k + 1];
109 comp_a[2 * k] = ar * br + ai * bi;
111 comp_a[2 * k + 1] = ar * bi - ai * br;
112 }
113 fft_radix2(comp_a, true)?; let mut run = 0.0f64;
117 psum[0] = 0.0;
118 for k in 0..n {
119 let x = frame[k] as f64;
120 run += x * x;
121 psum[k + 1] = run as f32;
122 }
123 let e0 = psum[w];
124
125 diff[0] = 0.0;
127 for tau in 1..=max_lag {
128 let sliding = psum[tau + w] - psum[tau];
129 let corr = comp_a[2 * tau];
130 let d = e0 + sliding - 2.0 * corr;
131 diff[tau] = if d > 0.0 { d } else { 0.0 };
132 }
133
134 cmnd_in_place(diff, max_lag);
136 let tau = absolute_threshold(diff, min_lag, max_lag, threshold);
137 let (better_tau, aperiodicity) = parabolic_min(diff, tau, min_lag, max_lag);
138
139 if !(better_tau.is_finite() && better_tau > 0.0) {
140 return Ok(PitchEstimate::unvoiced());
141 }
142 let confidence = pitch_confidence(aperiodicity);
143 if aperiodicity > 1.0 || (diff[tau] >= threshold && aperiodicity > 0.5) {
144 return Ok(PitchEstimate {
145 f0_hz: 0.0,
146 confidence,
147 });
148 }
149 Ok(PitchEstimate {
150 f0_hz: sample_rate / better_tau,
151 confidence,
152 })
153}
154
155#[cfg(test)]
156mod tests {
157 use super::*;
158 use core::f32::consts::TAU;
159
160 const SR: f32 = 44_100.0;
161
162 fn sine(freq: f32, n: usize, sr: f32) -> Vec<f32> {
163 (0..n).map(|i| (TAU * freq * i as f32 / sr).sin()).collect()
164 }
165
166 fn cents(a: f32, b: f32) -> f32 {
167 1200.0 * (a / b).log2().abs()
168 }
169
170 #[test]
171 fn recovers_440_within_one_percent() {
172 let frame = sine(440.0, 2048, SR);
173 let mut scratch = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
174 let est = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch).expect("yinfft");
175 let err = (est.f0_hz - 440.0).abs() / 440.0;
176 assert!(err < 0.01, "f0={} err={err}", est.f0_hz);
177 assert!(
178 cents(est.f0_hz, 440.0) < 10.0,
179 "cents={}",
180 cents(est.f0_hz, 440.0)
181 );
182 assert!(est.confidence > 0.9, "conf={}", est.confidence);
183 }
184
185 #[test]
186 fn recovers_220_within_one_percent() {
187 let frame = sine(220.0, 2048, SR);
188 let mut scratch = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
189 let est = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch).expect("yinfft");
190 let err = (est.f0_hz - 220.0).abs() / 220.0;
191 assert!(err < 0.01, "f0={} err={err}", est.f0_hz);
192 assert!(est.confidence > 0.9, "conf={}", est.confidence);
193 }
194
195 #[test]
198 fn parabolic_beats_integer_lag() {
199 let f0 = SR / 100.5;
200 let frame = sine(f0, 2048, SR);
201 let mut scratch = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
202 let est = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch).expect("yinfft");
203 let c = cents(est.f0_hz, f0);
204 assert!(c < 5.0, "recovered {} Hz vs {f0} Hz ({c} cents)", est.f0_hz);
205 assert!(cents(SR / 100.0, f0) > 8.0);
206 }
207
208 #[test]
210 fn matches_direct_yin() {
211 use crate::features::pitch::yin::yin_pitch;
212 let frame = sine(329.63, 2048, SR); let mut s1 = vec![0.0f32; 1200];
214 let mut s2 = vec![0.0f32; yin_fft_scratch_len(2048, 551)];
215 let a = yin_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut s1).expect("yin");
216 let b = yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut s2).expect("yinfft");
217 assert!(
218 cents(a.f0_hz, b.f0_hz) < 2.0,
219 "yin={} yinfft={}",
220 a.f0_hz,
221 b.f0_hz
222 );
223 }
224
225 #[test]
226 fn rejects_short_scratch() {
227 let frame = sine(440.0, 2048, SR);
228 let mut scratch = vec![0.0f32; 16];
229 assert_eq!(
230 yin_fft_pitch(&frame, SR, 80.0, 1000.0, 0.1, &mut scratch),
231 Err(AudioError::OutputBufferTooSmall)
232 );
233 }
234}