use super::{PcmBuf, Sample};
use std::f32::consts::PI;
pub struct Resampler {
input_rate: usize,
output_rate: usize,
ratio: f64,
coeffs: Vec<f32>,
num_phases: usize,
taps_per_phase: usize,
history: Vec<f32>,
current_pos: f64,
}
impl Resampler {
pub fn new(input_rate: usize, output_rate: usize) -> Self {
let ratio = output_rate as f64 / input_rate as f64;
let num_phases = 128;
let taps_per_phase = 16; let filter_len = num_phases * taps_per_phase;
let mut raw_coeffs = vec![0.0f32; filter_len];
let cutoff = if ratio < 1.0 {
ratio as f32 * 0.45
} else {
0.45f32
};
let center = (taps_per_phase as f32 - 1.0) / 2.0;
for p in 0..num_phases {
let mut sum = 0.0;
let mut phase_coeffs = vec![0.0f32; taps_per_phase];
for t in 0..taps_per_phase {
let x = t as f32 - center - (p as f32 / num_phases as f32);
let val = if x.abs() < 1e-6 {
2.0 * cutoff
} else {
let x_pi = x * PI;
(x_pi * 2.0 * cutoff).sin() / x_pi
};
let window = 0.54
- 0.46
* (2.0 * PI * (t as f32 * num_phases as f32 + p as f32)
/ (filter_len as f32 - 1.0))
.cos();
phase_coeffs[t] = val * window;
sum += phase_coeffs[t];
}
for t in 0..taps_per_phase {
raw_coeffs[p * taps_per_phase + t] = phase_coeffs[t] / sum;
}
}
Self {
input_rate,
output_rate,
ratio,
coeffs: raw_coeffs,
num_phases,
taps_per_phase,
history: vec![0.0; taps_per_phase],
current_pos: 0.0,
}
}
#[inline(always)]
fn dot_product(a: &[f32], b: &[f32]) -> f32 {
#[cfg(target_arch = "aarch64")]
{
unsafe {
use std::arch::aarch64::*;
let mut sumv = vdupq_n_f32(0.0);
for i in (0..16).step_by(4) {
let av = vld1q_f32(a.as_ptr().add(i));
let bv = vld1q_f32(b.as_ptr().add(i));
sumv = vfmaq_f32(sumv, av, bv);
}
vaddvq_f32(sumv)
}
}
#[cfg(all(target_arch = "x86_64", target_feature = "avx"))]
{
unsafe {
use std::arch::x86_64::*;
let av = _mm256_loadu_ps(a.as_ptr());
let bv = _mm256_loadu_ps(b.as_ptr());
let mut sumv = _mm256_mul_ps(av, bv);
let av2 = _mm256_loadu_ps(a.as_ptr().add(8));
let bv2 = _mm256_loadu_ps(b.as_ptr().add(8));
sumv = _mm256_add_ps(sumv, _mm256_mul_ps(av2, bv2));
let x128 = _mm_add_ps(_mm256_extractf128_ps(sumv, 1), _mm256_castps256_ps128(sumv));
let x64 = _mm_add_ps(x128, _mm_movehl_ps(x128, x128));
let x32 = _mm_add_ss(x64, _mm_shuffle_ps(x64, x64, 0x55));
_mm_cvtss_f32(x32)
}
}
#[cfg(not(any(
target_arch = "aarch64",
all(target_arch = "x86_64", target_feature = "avx")
)))]
{
a.iter().zip(b).map(|(x, y)| x * y).sum()
}
}
pub fn resample(&mut self, input: &[Sample]) -> PcmBuf {
if self.input_rate == self.output_rate {
return input.to_vec();
}
let mut output = Vec::with_capacity((input.len() as f64 * self.ratio) as usize + 1);
let inv_ratio = 1.0 / self.ratio;
let taps = self.taps_per_phase;
let num_phases_f = self.num_phases as f64;
for &sample in input {
self.history.copy_within(1..taps, 0);
self.history[taps - 1] = sample as f32;
while self.current_pos < 1.0 {
let phase_idx = (self.current_pos * num_phases_f) as usize;
let offset = phase_idx * taps;
let phase_coeffs = &self.coeffs[offset..offset + taps];
let out_sample = Self::dot_product(phase_coeffs, &self.history);
output.push(out_sample.clamp(i16::MIN as f32, i16::MAX as f32) as i16);
self.current_pos += inv_ratio;
}
self.current_pos -= 1.0;
}
output
}
}
pub fn resample(input: &[Sample], input_sample_rate: u32, output_sample_rate: u32) -> PcmBuf {
if input_sample_rate == output_sample_rate {
return input.to_vec();
}
let mut r = Resampler::new(input_sample_rate as usize, output_sample_rate as usize);
r.resample(input)
}
#[cfg(test)]
mod tests {
use super::*;
use std::time::Instant;
#[test]
fn test_resample_8k_to_16k() {
let mut resampler = Resampler::new(8000, 16000);
let input = vec![1000i16; 80];
let output = resampler.resample(&input);
assert!(output.len() >= 150 && output.len() <= 170);
for &s in &output[20..output.len() - 20] {
assert!((s - 1000).abs() < 100, "Value {} is too far from 1000", s);
}
}
#[test]
fn test_resample_16k_to_8k() {
let mut resampler = Resampler::new(16000, 8000);
let input = vec![1000i16; 160];
let output = resampler.resample(&input);
assert!(output.len() >= 75 && output.len() <= 85);
for &s in &output[20..output.len() - 20] {
assert!((s - 1000).abs() < 100, "Value {} is too far from 1000", s);
}
}
#[test]
fn test_performance() {
let mut resampler = Resampler::new(48000, 8000);
let input = vec![0i16; 48000];
let start = Instant::now();
let iterations = 100;
for _ in 0..iterations {
let _ = resampler.resample(&input);
}
let duration = start.elapsed();
let per_second = duration.as_secs_f64() / iterations as f64;
println!(
"Resampling 1s of 48kHz to 8kHz took: {:.4}ms",
per_second * 1000.0
);
assert!(per_second < 0.05);
}
}