1use core::f64::consts::PI as PI_F64;
2
3use super::{CodecError, Sample};
4
5#[cfg(feature = "std")]
6use super::PcmBuf;
7
8pub const NUM_PHASES: usize = 256;
10pub const TAPS_PER_PHASE: usize = 24;
12pub const COEFFS_LEN: usize = NUM_PHASES * TAPS_PER_PHASE;
14
15const Q15_SHIFT: u32 = 15;
18
19#[inline]
21fn fsin(x: f64) -> f64 {
22 #[cfg(feature = "std")]
23 {
24 f64::sin(x)
25 }
26 #[cfg(not(feature = "std"))]
27 {
28 libm::sin(x)
29 }
30}
31
32#[inline]
34fn fsqrt(x: f64) -> f64 {
35 #[cfg(feature = "std")]
36 {
37 f64::sqrt(x)
38 }
39 #[cfg(not(feature = "std"))]
40 {
41 libm::sqrt(x)
42 }
43}
44
45#[inline]
47fn fround(x: f64) -> f64 {
48 #[cfg(feature = "std")]
49 {
50 x.round()
51 }
52 #[cfg(not(feature = "std"))]
53 {
54 libm::round(x)
55 }
56}
57
58pub struct Resampler<'a> {
69 input_rate: usize,
70 output_rate: usize,
71 coeffs: &'a mut [i16],
72 taps_per_phase: usize,
73 history: [i16; 2 * TAPS_PER_PHASE],
78 w: usize,
80 pos: usize,
87}
88
89fn bessel_i0(x: f64) -> f64 {
90 let mut sum = 1.0_f64;
91 let mut term = 1.0_f64;
92 let x_sq = x * x * 0.25;
93
94 for m in 1..=30 {
95 term *= x_sq / (m * m) as f64;
96 sum += term;
97 if term < 1e-15 * sum {
98 break;
99 }
100 }
101 sum
102}
103
104fn kaiser_window(n: usize, n_total: usize, beta: f64) -> f64 {
105 if n_total <= 1 {
106 return 1.0;
107 }
108 let alpha = (n_total - 1) as f64 / 2.0;
109 let x = (n as f64 - alpha) / alpha;
110 let arg = beta * fsqrt(1.0 - x * x);
111 bessel_i0(arg) / bessel_i0(beta)
112}
113
114impl<'a> Resampler<'a> {
115 pub fn new(
122 input_rate: usize,
123 output_rate: usize,
124 coeffs: &'a mut [i16],
125 ) -> Result<Self, CodecError> {
126 if coeffs.len() < COEFFS_LEN {
127 return Err(CodecError::BufferTooSmall);
128 }
129 if input_rate == 0 || output_rate == 0 {
130 return Err(CodecError::InvalidInput);
131 }
132
133 const KAISER_BETA: f64 = 7.0;
134
135 let ratio = output_rate as f64 / input_rate as f64;
136 let num_phases = NUM_PHASES;
137 let taps_per_phase = TAPS_PER_PHASE;
138 let filter_len = num_phases * taps_per_phase;
139
140 if input_rate.checked_add(output_rate).is_none()
145 || output_rate.checked_mul(NUM_PHASES).is_none()
146 {
147 return Err(CodecError::InvalidInput);
148 }
149
150 let coeffs = &mut coeffs[..filter_len];
151
152 let cutoff = if ratio < 1.0 {
153 ratio * 0.5 * 0.95
154 } else {
155 0.5 * 0.95
156 };
157
158 let center = (taps_per_phase as f64 - 1.0) / 2.0;
159
160 let mut phase_coeffs: [f64; TAPS_PER_PHASE] = [0.0; TAPS_PER_PHASE];
163
164 for p in 0..num_phases {
165 let mut sum = 0.0_f64;
166
167 for t in 0..taps_per_phase {
168 let x = t as f64 - center - (p as f64 / num_phases as f64);
169
170 let sinc_val = if x.abs() < 1e-10 {
171 2.0 * cutoff
172 } else {
173 let x_pi = x * PI_F64;
174 fsin(x_pi * 2.0 * cutoff) / x_pi
175 };
176
177 let full_filter_idx = t * num_phases + p;
178 let window = kaiser_window(full_filter_idx, filter_len, KAISER_BETA);
179
180 phase_coeffs[t] = sinc_val * window;
181 sum += phase_coeffs[t];
182 }
183
184 for t in 0..taps_per_phase {
185 let q = fround((phase_coeffs[t] / sum) * 32768.0);
187 coeffs[p * taps_per_phase + t] = q.clamp(-32768.0, 32767.0) as i16;
188 }
189 }
190
191 Ok(Self {
192 input_rate,
193 output_rate,
194 coeffs,
195 taps_per_phase,
196 history: [0; 2 * TAPS_PER_PHASE],
197 w: TAPS_PER_PHASE,
198 pos: 0,
199 })
200 }
201
202 pub fn input_rate(&self) -> usize {
203 self.input_rate
204 }
205
206 pub fn output_rate(&self) -> usize {
207 self.output_rate
208 }
209
210 #[inline(always)]
217 fn dot_q15(history: &[i16; TAPS_PER_PHASE], coeffs: &[i16; TAPS_PER_PHASE]) -> i32 {
218 let mut acc: i64 = 0;
219 for i in 0..TAPS_PER_PHASE {
220 acc += ((history[i] as i32) * (coeffs[i] as i32)) as i64;
221 }
222 (acc >> Q15_SHIFT) as i32
223 }
224
225 pub fn resample_into(
231 &mut self,
232 input: &[Sample],
233 out: &mut [Sample],
234 ) -> Result<usize, CodecError> {
235 if self.input_rate == self.output_rate {
236 if out.len() < input.len() {
237 return Err(CodecError::BufferTooSmall);
238 }
239 out[..input.len()].copy_from_slice(input);
240 return Ok(input.len());
241 }
242
243 let taps = self.taps_per_phase;
244 let two_taps = 2 * taps;
245 let mut written = 0usize;
246 let (step, interval) = (self.input_rate, self.output_rate);
250 let mut pos = self.pos;
251 let expected: u128 = {
257 let ticks = (input.len() as u128) * (interval as u128);
258 if ticks > pos as u128 {
259 (ticks - pos as u128).div_ceil(step as u128)
260 } else {
261 0
262 }
263 };
264
265 for &sample in input {
266 if self.w == two_taps {
268 self.history.copy_within(taps..two_taps, 0);
269 self.w = taps;
270 }
271 self.history[self.w] = sample;
272 self.w += 1;
273
274 if pos < interval {
276 let base = self.w - taps;
277 let win: &[i16; TAPS_PER_PHASE] =
278 self.history[base..base + taps].try_into().unwrap();
279
280 while pos < interval {
281 let phase_idx = pos * NUM_PHASES / interval;
284 let offset = phase_idx * taps;
285 let phase_coeffs: &[i16; TAPS_PER_PHASE] =
286 self.coeffs[offset..offset + taps].try_into().unwrap();
287 let out_sample = Self::dot_q15(win, phase_coeffs);
288
289 if written >= out.len() {
290 self.pos = pos;
291 return Err(CodecError::BufferTooSmall);
292 }
293 out[written] = out_sample.clamp(i16::MIN as i32, i16::MAX as i32) as i16;
294 written += 1;
295 pos += step;
296 }
297 }
298 pos -= interval;
299 }
300 self.pos = pos;
301
302 debug_assert_eq!(
306 written as u128,
307 expected,
308 "resampler produced {written} samples, exact timing requires {expected} \
309 for {} inputs ({input_rate} -> {output_rate} at pos {}): output \
310 sizes would drift across chunk boundaries",
311 input.len(),
312 self.pos,
313 input_rate = self.input_rate,
314 output_rate = self.output_rate,
315 );
316
317 Ok(written)
318 }
319
320 pub fn max_output_samples(&self, n_input: usize) -> usize {
323 if self.input_rate == self.output_rate {
324 return n_input;
325 }
326 let count = (n_input as u128 * self.output_rate as u128).div_ceil(self.input_rate as u128);
329 count.min(usize::MAX as u128) as usize
330 }
331
332 #[cfg(feature = "std")]
334 pub fn resample(&mut self, input: &[Sample]) -> PcmBuf {
335 let max = self.max_output_samples(input.len());
336 let mut out = vec![0i16; max];
337 match self.resample_into(input, &mut out) {
338 Ok(n) => {
339 debug_assert!(
340 n <= max,
341 "resampler emitted {n} samples but `max_output_samples` \
342 said {max}; buffer sizing invariant broken"
343 );
344 out.truncate(n);
345 out
346 }
347 Err(e) => {
348 if cfg!(debug_assertions) {
354 panic!("resample_into failed inside `resample`: {e:?}");
355 }
356 Vec::new()
357 }
358 }
359 }
360
361 pub fn reset(&mut self) {
362 self.history.fill(0);
363 self.w = self.taps_per_phase;
364 self.pos = 0;
365 }
366}
367
368#[cfg(feature = "std")]
372pub fn resample(input: &[Sample], input_sample_rate: u32, output_sample_rate: u32) -> PcmBuf {
373 if input_sample_rate == output_sample_rate {
374 return input.to_vec();
375 }
376 let mut coeffs = vec![0i16; COEFFS_LEN];
377 let mut r = match Resampler::new(
378 input_sample_rate as usize,
379 output_sample_rate as usize,
380 &mut coeffs,
381 ) {
382 Ok(r) => r,
383 Err(_) => return Vec::new(),
384 };
385 r.resample(input)
386}
387
388#[cfg(feature = "std")]
397pub struct BoxedResampler {
398 inner: Resampler<'static>,
403 #[allow(dead_code)]
405 coeffs: Box<[i16]>,
406}
407
408#[cfg(feature = "std")]
409impl BoxedResampler {
410 pub fn new(input_rate: usize, output_rate: usize) -> Result<Self, CodecError> {
415 let mut coeffs = vec![0i16; COEFFS_LEN].into_boxed_slice();
416 let inner = unsafe {
422 let ptr = coeffs.as_mut_ptr();
423 let loan: &'static mut [i16] = core::slice::from_raw_parts_mut(ptr, coeffs.len());
424 Resampler::new(input_rate, output_rate, loan)?
425 };
426 Ok(Self { inner, coeffs })
427 }
428
429 pub fn resample(&mut self, input: &[Sample]) -> PcmBuf {
431 self.inner.resample(input)
432 }
433
434 pub fn resample_into(
436 &mut self,
437 input: &[Sample],
438 out: &mut [Sample],
439 ) -> Result<usize, CodecError> {
440 self.inner.resample_into(input, out)
441 }
442
443 pub fn max_output_samples(&self, n_input: usize) -> usize {
445 self.inner.max_output_samples(n_input)
446 }
447
448 pub fn reset(&mut self) {
450 self.inner.reset();
451 }
452}
453
454#[cfg(test)]
455mod tests {
456 use super::*;
457 use std::f32::consts::PI as PI_F32;
458 use std::time::Instant;
459
460 fn new_resampler(input_rate: usize, output_rate: usize) -> Resampler<'static> {
461 let coeffs: &'static mut [i16] = Box::leak(vec![0i16; COEFFS_LEN].into_boxed_slice());
463 Resampler::new(input_rate, output_rate, coeffs).expect("resampler init")
464 }
465
466 #[test]
467 fn test_exact_sample_counts_for_repeated_20ms_blocks() {
468 for input_rate in [8000, 16000, 44100, 48000] {
469 for output_rate in [8000, 16000, 44100, 48000] {
470 let mut r = new_resampler(input_rate, output_rate);
471 let input = vec![1000; input_rate / 50];
472 let mut output = vec![0; output_rate / 50];
473 for block in 0..10 {
474 let n = r.resample_into(&input, &mut output).unwrap_or_else(|e| {
475 panic!("{input_rate} -> {output_rate}, block {block}: {e:?}")
476 });
477 assert_eq!(
478 n,
479 output.len(),
480 "{input_rate} -> {output_rate}, block {block}"
481 );
482 }
483 }
484 }
485 }
486
487 #[test]
488 fn test_exact_timing_survives_unaligned_chunks_and_reset() {
489 for (input_rate, output_rate) in [
490 (16000, 48000),
491 (48000, 16000),
492 (44100, 48000),
493 (48000, 44100),
494 ] {
495 let input: Vec<i16> = (0..2000).map(|i| ((i * 37 % 200) - 100) as i16).collect();
496 let mut r = new_resampler(input_rate, output_rate);
497 let expected = r.resample(&input);
498 assert_eq!(
499 expected.len(),
500 (input.len() * output_rate).div_ceil(input_rate)
501 );
502 for chunk_size in [1, 2, 3, 5, 7, 11, 23, 24, 25, 47, 88, 160, 319] {
506 r.reset();
507 let mut got = Vec::new();
508 let mut consumed = 0;
509 for chunk in input.chunks(chunk_size) {
510 got.extend(r.resample(chunk));
511 consumed += chunk.len();
512 assert_eq!(
513 got.len(),
514 (consumed * output_rate).div_ceil(input_rate),
515 "{input_rate} -> {output_rate}, chunk {chunk_size}, consumed {consumed}"
516 );
517 assert!(r.resample(&[]).is_empty());
518 }
519 assert_eq!(
520 got, expected,
521 "{input_rate} -> {output_rate}, chunk {chunk_size}"
522 );
523 }
524 r.reset();
525 assert_eq!(r.resample(&input), expected);
526 }
527 }
528
529 #[cfg(feature = "opus")]
530 #[test]
531 fn test_resampled_20ms_frames_encode_as_opus() {
532 use crate::{Encoder, opus::OpusEncoder};
533
534 for input_rate in [8000, 16000] {
535 let mut r = new_resampler(input_rate, 48000);
536 let mut encoder = OpusEncoder::new_default();
537 for _ in 0..3 {
538 let pcm = r.resample(&vec![0; input_rate / 50]);
539 assert_eq!(pcm.len(), 960);
540 assert!(!encoder.encode(&pcm).is_empty());
541 }
542 }
543 }
544
545 #[test]
546 fn test_resample_8k_to_16k() {
547 let mut resampler = new_resampler(8000, 16000);
548 let input = vec![1000i16; 80];
549 let output = resampler.resample(&input);
550 assert!(output.len() >= 150 && output.len() <= 170);
551 for &s in &output[48..output.len().saturating_sub(48)] {
552 assert!((s - 1000).abs() < 100, "Value {} is too far from 1000", s);
553 }
554 }
555
556 #[test]
557 fn test_resample_16k_to_8k() {
558 let mut resampler = new_resampler(16000, 8000);
559 let input = vec![1000i16; 160];
560 let output = resampler.resample(&input);
561 assert!(output.len() >= 75 && output.len() <= 85);
562 let skip = output.len() / 4;
563 for &s in &output[skip..output.len() - skip] {
564 assert!((s - 1000).abs() < 100, "Value {} is too far from 1000", s);
565 }
566 }
567
568 #[test]
569 fn test_frequency_response_downsample() {
570 let mut resampler = new_resampler(16000, 8000);
571 let freq = 2000.0_f32; let samples: Vec<i16> = (0..160)
573 .map(|i| ((i as f32 * freq * 2.0 * PI_F32 / 16000.0).sin() * 10000.0) as i16)
574 .collect();
575
576 let output = resampler.resample(&samples);
577
578 let input_rms: f32 = samples
580 .iter()
581 .map(|&s| (s as f32).powi(2))
582 .sum::<f32>()
583 .sqrt()
584 / samples.len() as f32;
585 let output_rms: f32 = output
586 .iter()
587 .map(|&s| (s as f32).powi(2))
588 .sum::<f32>()
589 .sqrt()
590 / output.len() as f32;
591
592 assert!(
593 output_rms > input_rms * 0.7,
594 "Too much attenuation: input_rms={}, output_rms={}",
595 input_rms,
596 output_rms
597 );
598 }
599
600 #[test]
601 fn test_aliasing_suppression() {
602 let mut resampler = new_resampler(16000, 8000);
603 let freq = 7000.0_f32; let samples: Vec<i16> = (0..1600)
605 .map(|i| ((i as f32 * freq * 2.0 * PI_F32 / 16000.0).sin() * 10000.0) as i16)
606 .collect();
607
608 let output = resampler.resample(&samples);
609
610 let output_rms: f32 =
611 (output.iter().map(|&s| (s as f32).powi(2)).sum::<f32>() / output.len() as f32).sqrt();
612 let input_rms: f32 = 10000.0 / 1.414; assert!(
615 output_rms < input_rms / 50.0,
616 "Aliasing not sufficiently suppressed: output_rms={}",
617 output_rms
618 );
619 }
620
621 #[test]
622 fn test_performance_48k_to_8k() {
623 let mut resampler = new_resampler(48000, 8000);
624 let input = vec![0i16; 48000];
625
626 let start = Instant::now();
627 let iterations = 100;
628 for _ in 0..iterations {
629 let _ = resampler.resample(&input);
630 resampler.reset();
631 }
632 let duration = start.elapsed();
633 let per_second = duration.as_secs_f64() / iterations as f64;
634 println!(
635 "Resampling 1s of 48kHz to 8kHz (24 taps) took: {:.4}ms",
636 per_second * 1000.0
637 );
638 assert!(
639 per_second < 0.1,
640 "Performance regression: {}ms",
641 per_second * 1000.0
642 );
643 }
644
645 #[test]
646 fn test_continuity_between_chunks() {
647 let input_rate = 16000;
648 let output_rate = 8000;
649
650 let freq = 1000.0_f32;
651 let total_samples = 3200;
652 let input: Vec<i16> = (0..total_samples)
653 .map(|i| ((i as f32 * freq * 2.0 * PI_F32 / input_rate as f32).sin() * 5000.0) as i16)
654 .collect();
655
656 let mut resampler1 = new_resampler(input_rate, output_rate);
657 let output1 = resampler1.resample(&input);
658
659 let mut resampler2 = new_resampler(input_rate, output_rate);
660 let mid = input.len() / 2;
661 let mut output2 = resampler2.resample(&input[..mid]);
662 output2.extend_from_slice(&resampler2.resample(&input[mid..]));
663
664 assert_eq!(output1.len(), output2.len(), "Output lengths differ");
665
666 let max_diff: i16 = output1
667 .iter()
668 .zip(output2.iter())
669 .map(|(a, b)| (a - b).abs())
670 .max()
671 .unwrap_or(0);
672
673 assert!(
674 max_diff < 100,
675 "Large discontinuity between chunks: max_diff={}",
676 max_diff
677 );
678 }
679
680 #[test]
685 fn test_q15_core_matches_float_reference() {
686 let fin = 16000usize;
687 let fout = 48000usize;
688 let mut coeffs = vec![0i16; COEFFS_LEN];
689 let mut r = Resampler::new(fin, fout, &mut coeffs).unwrap();
690
691 let input: Vec<i16> = (0..2000)
692 .map(|i| (((i * 37) % 200 - 100) * 60) as i16)
693 .collect();
694 let got = r.resample(&input);
695
696 let taps = TAPS_PER_PHASE;
699 let mut hist = [0.0f32; TAPS_PER_PHASE];
700 let mut want = Vec::new();
701 for (input_index, &s) in input.iter().enumerate() {
702 hist.copy_within(1..taps, 0);
703 hist[taps - 1] = s as f32;
704 while want.len() * fin < (input_index + 1) * fout {
705 let phase = (want.len() * fin % fout) * NUM_PHASES / fout;
706 let mut acc = 0.0f32;
707 for t in 0..taps {
708 acc += hist[t] * (coeffs[phase * taps + t] as f32 / 32768.0);
709 }
710 want.push(acc.round().clamp(-32768.0, 32767.0) as i16);
711 }
712 }
713
714 assert_eq!(got.len(), want.len());
715 let se: f64 = got
716 .iter()
717 .zip(&want)
718 .map(|(&a, &b)| {
719 let d = a as f64 - b as f64;
720 d * d
721 })
722 .sum();
723 let sig: f64 = want.iter().map(|&b| (b as f64) * (b as f64)).sum();
724 let snr = 10.0 * (sig / se.max(1.0)).log10();
725 println!("Q15 vs f32-accumulator SNR = {:.1} dB", snr);
726 assert!(
727 snr > 60.0,
728 "integer core diverges from float: {:.1} dB",
729 snr
730 );
731 }
732
733 #[cfg(feature = "std")]
737 #[test]
738 fn test_boxed_resampler_matches_borrowed() {
739 let input_rate = 48000usize;
740 let output_rate = 8000usize;
741 let input: Vec<i16> = (0..4800)
742 .map(|i| ((i as f32 * 0.05).sin() * 8000.0) as i16)
743 .collect();
744
745 let expected = {
746 let mut coeffs = vec![0i16; COEFFS_LEN];
747 let mut borrowed = Resampler::new(input_rate, output_rate, &mut coeffs).unwrap();
748 borrowed.resample(&input)
749 };
750
751 let mut boxed = BoxedResampler::new(input_rate, output_rate).unwrap();
752 let expected_max = (input.len() * output_rate).div_ceil(input_rate);
753 assert_eq!(boxed.max_output_samples(input.len()), expected_max);
754 let got = boxed.resample(&input);
755 assert_eq!(
756 expected, got,
757 "BoxedResampler output must match borrowed Resampler"
758 );
759
760 let mut moved = boxed;
765 let again = moved.resample(&input);
766 assert_eq!(expected.len(), again.len());
767
768 moved.reset();
771 let after_reset = moved.resample(&input);
772 assert_eq!(expected, after_reset);
773
774 assert!(
775 BoxedResampler::new(0, 8000).is_err(),
776 "zero input rate must error"
777 );
778 assert!(
779 BoxedResampler::new(48000, 0).is_err(),
780 "zero output rate must error"
781 );
782 }
783}