1pub(crate) const HALF: usize = 16;
2pub(crate) const MIN_RATIO: f32 = 1.0 / 16.0;
3pub(crate) const MAX_RATIO: f32 = 8.0;
4pub(crate) const MAX_TAPS: usize = 2 * HALF * (MAX_RATIO as usize) + 2;
5
6const STEPS: usize = 256;
7const CUTOFF: f64 = 0.9;
8const BETA: f64 = 7.0;
9
10#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
11pub enum Resample {
12 #[default]
13 Sinc,
14 Cubic,
15 Linear,
16}
17
18pub(crate) fn hermite(previous: f32, current: f32, next: f32, after: f32, t: f32) -> f32 {
19 let c0 = current;
20 let c1 = (next - previous) * 0.5;
21 let c2 = previous - current * 2.5 + next * 2.0 - after * 0.5;
22 let c3 = (after - previous) * 0.5 + (current - next) * 1.5;
23 ((c3 * t + c2) * t + c1) * t + c0
24}
25
26pub(crate) struct Kernel {
27 table: Vec<f32>,
28}
29
30pub(crate) struct Taps {
31 pub(crate) first: i64,
32 pub(crate) count: usize,
33 pub(crate) weights: [f32; MAX_TAPS],
34}
35
36impl Kernel {
37 pub(crate) fn new() -> Self {
38 let len = HALF * STEPS + 2;
39 let table = (0..len)
40 .map(|i| {
41 let x = i as f64 / STEPS as f64;
42 (windowed_sinc(x)) as f32
43 })
44 .collect();
45 Self { table }
46 }
47
48 pub(crate) fn reach(scale: f32) -> u64 {
49 (HALF as f32 * scale.max(1.0)).ceil() as u64 + 1
50 }
51
52 fn weight(&self, distance: f32) -> f32 {
53 let x = distance * STEPS as f32;
54 let index = x as usize;
55 if index + 1 >= self.table.len() {
56 return 0.0;
57 }
58 let frac = x - index as f32;
59 self.table[index] + (self.table[index + 1] - self.table[index]) * frac
60 }
61
62 pub(crate) fn taps(&self, position: f64, scale: f32) -> Taps {
63 let scale = scale.clamp(1.0, MAX_RATIO);
64 let reach = f64::from(HALF as f32 * scale);
65 let first = (position - reach).floor() as i64 + 1;
66 let last = (position + reach).floor() as i64;
67 let count = ((last - first + 1).max(0) as usize).min(MAX_TAPS);
68 let inverse = 1.0 / scale;
69 let mut weights = [0.0f32; MAX_TAPS];
70 let mut sum = 0.0f32;
71 for (j, slot) in weights.iter_mut().enumerate().take(count) {
72 let distance = ((first + j as i64) as f64 - position).abs() as f32 * inverse;
73 let weight = self.weight(distance);
74 *slot = weight;
75 sum += weight;
76 }
77 if sum != 0.0 {
78 for slot in weights.iter_mut().take(count) {
79 *slot /= sum;
80 }
81 }
82 Taps {
83 first,
84 count,
85 weights,
86 }
87 }
88}
89
90fn windowed_sinc(x: f64) -> f64 {
91 let half = HALF as f64;
92 if x >= half {
93 return 0.0;
94 }
95 let y = CUTOFF * x;
96 let sinc = if y == 0.0 {
97 1.0
98 } else {
99 pfx_core::sim::sin(std::f64::consts::PI * y) / (std::f64::consts::PI * y)
100 };
101 let t = x / half;
102 let window = bessel_i0(BETA * (1.0 - t * t).max(0.0).sqrt()) / bessel_i0(BETA);
103 CUTOFF * sinc * window
104}
105
106fn bessel_i0(x: f64) -> f64 {
107 let mut sum = 1.0;
108 let mut term = 1.0;
109 let quarter = x * x / 4.0;
110 for k in 1..64 {
111 term *= quarter / (k * k) as f64;
112 sum += term;
113 if term < sum * 1e-16 {
114 break;
115 }
116 }
117 sum
118}
119
120#[cfg(feature = "vorbis")]
121pub(crate) struct StreamResampler {
122 kernel: Kernel,
123 step: f64,
124 scale: f32,
125 channels: usize,
126 pending: Vec<f32>,
127 base: u64,
128 position: f64,
129 passthrough: bool,
130}
131
132#[cfg(feature = "vorbis")]
133impl StreamResampler {
134 pub(crate) fn new(from: u32, to: u32, channels: usize) -> Self {
135 let step = f64::from(from) / f64::from(to);
136 Self {
137 kernel: Kernel::new(),
138 step,
139 scale: (step as f32).clamp(1.0, MAX_RATIO),
140 channels: channels.max(1),
141 pending: Vec::new(),
142 base: 0,
143 position: 0.0,
144 passthrough: from == to,
145 }
146 }
147
148 pub(crate) fn push(&mut self, input: &[f32], out: &mut Vec<f32>) {
149 if self.passthrough {
150 out.extend_from_slice(input);
151 return;
152 }
153 self.pending.extend_from_slice(input);
154 self.emit(out, false);
155 self.trim();
156 }
157
158 pub(crate) fn finish(&mut self, out: &mut Vec<f32>) {
159 if self.passthrough {
160 return;
161 }
162 self.emit(out, true);
163 self.pending.clear();
164 }
165
166 fn emit(&mut self, out: &mut Vec<f32>, flush: bool) {
167 let frames = (self.pending.len() / self.channels) as u64;
168 let end = self.base + frames;
169 let reach = Kernel::reach(self.scale);
170 loop {
171 let floor = self.position.floor() as u64;
172 if flush {
173 if self.position >= end as f64 {
174 break;
175 }
176 } else if floor + reach + 1 > end {
177 break;
178 }
179 let taps = self.kernel.taps(self.position, self.scale);
180 for channel in 0..self.channels {
181 let mut sum = 0.0f32;
182 for (j, weight) in taps.weights[..taps.count].iter().enumerate() {
183 let k = taps.first + j as i64;
184 if k < self.base as i64 || k >= end as i64 {
185 continue;
186 }
187 let index = (k as u64 - self.base) as usize * self.channels + channel;
188 sum += self.pending[index] * weight;
189 }
190 out.push(sum);
191 }
192 self.position += self.step;
193 }
194 }
195
196 fn trim(&mut self) {
197 let reach = Kernel::reach(self.scale);
198 let keep = (self.position.floor() as u64).saturating_sub(reach + 1);
199 if keep > self.base {
200 let drop = ((keep - self.base) as usize).min(self.pending.len() / self.channels);
201 self.pending.drain(..drop * self.channels);
202 self.base += drop as u64;
203 }
204 }
205}
206
207#[cfg(test)]
208pub(crate) fn tone_level(samples: &[f32], rate: f32, frequency: f32) -> f32 {
209 let n = samples.len();
210 let mut re = 0.0f64;
211 let mut im = 0.0f64;
212 let mut window_sum = 0.0f64;
213 for (i, sample) in samples.iter().enumerate() {
214 let window = 0.5 - 0.5 * (std::f64::consts::TAU * i as f64 / n as f64).cos();
215 let phase = std::f64::consts::TAU * f64::from(frequency) * i as f64 / f64::from(rate);
216 re += f64::from(*sample) * window * phase.cos();
217 im += f64::from(*sample) * window * phase.sin();
218 window_sum += window;
219 }
220 (2.0 * (re * re + im * im).sqrt() / window_sum) as f32
221}
222
223#[cfg(test)]
224mod tests {
225 use super::*;
226
227 #[test]
228 fn the_weights_sum_to_one_at_every_phase() {
229 let kernel = Kernel::new();
230 for scale in [1.0, 1.5, 2.0, 4.0] {
231 for k in 0..20 {
232 let taps = kernel.taps(100.0 + f64::from(k) / 20.0, scale);
233 let sum: f32 = taps.weights[..taps.count].iter().sum();
234 assert!((sum - 1.0).abs() < 1e-5, "{sum}");
235 }
236 }
237 }
238
239 #[test]
240 fn cubic_hermite_passes_through_its_points_and_keeps_a_line() {
241 assert_eq!(hermite(1.0, 2.0, 3.0, 4.0, 0.0), 2.0);
242 assert_eq!(hermite(1.0, 2.0, 3.0, 4.0, 1.0), 3.0);
243 assert_eq!(hermite(1.0, 2.0, 3.0, 4.0, 0.25), 2.25);
244 }
245
246 #[test]
247 fn a_half_frame_position_reads_between_the_samples() {
248 let kernel = Kernel::new();
249 let tone = |t: f64| (std::f64::consts::TAU * 1000.0 * t / 48_000.0).sin();
250 let taps = kernel.taps(500.25, 1.0);
251 let value: f64 = (0..taps.count)
252 .map(|j| tone((taps.first + j as i64) as f64) * f64::from(taps.weights[j]))
253 .sum();
254 assert!((value - tone(500.25)).abs() < 1e-3, "{value}");
255 }
256
257 #[cfg(feature = "vorbis")]
258 #[test]
259 fn a_stream_resample_matches_the_tone_it_carries() {
260 let rate_in = 44_100u32;
261 let rate_out = 48_000u32;
262 let input: Vec<f32> = (0..20_000)
263 .map(|i| 0.5 * (std::f32::consts::TAU * 1000.0 * i as f32 / rate_in as f32).sin())
264 .collect();
265 let mut resampler = StreamResampler::new(rate_in, rate_out, 1);
266 let mut out = Vec::new();
267 for chunk in input.chunks(777) {
268 resampler.push(chunk, &mut out);
269 }
270 resampler.finish(&mut out);
271 let expected = 20_000.0 * 48_000.0 / 44_100.0;
272 assert!((out.len() as f64 - expected).abs() < 2.0, "{}", out.len());
273 let worst = out
274 .iter()
275 .enumerate()
276 .skip(64)
277 .take(out.len() - 128)
278 .map(|(i, s)| {
279 let want =
280 0.5 * (std::f32::consts::TAU * 1000.0 * i as f32 / rate_out as f32).sin();
281 (s - want).abs()
282 })
283 .fold(0.0, f32::max);
284 assert!(worst < 2e-3, "{worst}");
285 }
286}