sim_lib_audio_dsp/
resample.rs

1use std::{error::Error, f64::consts::PI, fmt};
2
3/// Fixed construction policy for [`PolyphaseResampler`].
4#[derive(Clone, Copy, Debug, PartialEq)]
5pub struct ResamplerPolicy {
6    /// Number of fractional-delay phases in the coefficient bank.
7    pub phases: usize,
8    /// Even number of windowed-sinc taps per phase.
9    pub taps: usize,
10    /// Fraction of the alias-free cutoff retained, in `(0, 1]`.
11    pub cutoff_ratio: f64,
12    /// Maximum input frames accepted by one callback call.
13    pub max_input_frames: usize,
14}
15
16impl Default for ResamplerPolicy {
17    fn default() -> Self {
18        Self {
19            phases: 1_024,
20            taps: 32,
21            cutoff_ratio: 0.94,
22            max_input_frames: 4_096,
23        }
24    }
25}
26
27/// Invalid construction or bounded callback request from a resampler.
28#[derive(Clone, Debug, PartialEq, Eq)]
29pub enum ResampleError {
30    /// A sample rate or channel count was zero.
31    InvalidFormat,
32    /// The coefficient-bank policy was outside its finite supported range.
33    InvalidPolicy,
34    /// Input or output samples did not form whole interleaved frames.
35    MisalignedBuffer,
36    /// One callback input exceeded the predeclared frame bound.
37    InputLimit {
38        /// Frames supplied by the caller.
39        supplied: usize,
40        /// Maximum frames admitted by the policy.
41        maximum: usize,
42    },
43    /// Caller output storage cannot hold every output implied by this input.
44    OutputTooSmall {
45        /// Output frames required before consuming input.
46        required: usize,
47        /// Output frames available in the caller buffer.
48        available: usize,
49    },
50    /// Long-running rational time arithmetic exceeded its representable range.
51    TimeOverflow,
52}
53
54impl fmt::Display for ResampleError {
55    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
56        match self {
57            Self::InvalidFormat => write!(f, "resampler rates and channels must be positive"),
58            Self::InvalidPolicy => write!(f, "resampler policy is outside supported bounds"),
59            Self::MisalignedBuffer => write!(f, "resampler buffer ends mid-frame"),
60            Self::InputLimit { supplied, maximum } => {
61                write!(
62                    f,
63                    "resampler input has {supplied} frames, exceeding {maximum}"
64                )
65            }
66            Self::OutputTooSmall {
67                required,
68                available,
69            } => write!(
70                f,
71                "resampler needs {required} output frames, but only {available} are available"
72            ),
73            Self::TimeOverflow => write!(f, "resampler rational time counter overflowed"),
74        }
75    }
76}
77
78impl Error for ResampleError {}
79
80/// Per-call accounting from [`PolyphaseResampler::process_interleaved`].
81#[derive(Clone, Copy, Debug, PartialEq, Eq)]
82pub struct ResampleReport {
83    /// Whole interleaved input frames consumed.
84    pub input_frames: usize,
85    /// Whole interleaved output frames written.
86    pub output_frames: usize,
87    /// Fixed causal filter latency measured in source-rate frames.
88    pub latency_input_frames: usize,
89}
90
91/// Streaming windowed-sinc polyphase resampler with fixed callback state.
92///
93/// Construction allocates the coefficient bank and the interleaved history
94/// ring. [`process_interleaved`](Self::process_interleaved) writes into caller
95/// storage and performs no allocation, locking, or I/O. The filter is causal:
96/// the first output is delayed until the right half of its impulse response is
97/// available, and callers may append zero input to drain a finite stream.
98#[derive(Clone, Debug, PartialEq)]
99pub struct PolyphaseResampler {
100    input_rate_hz: u32,
101    output_rate_hz: u32,
102    channels: usize,
103    policy: ResamplerPolicy,
104    coefficients: Vec<f32>,
105    history: Vec<f32>,
106    input_frames_seen: u128,
107    next_output_time: u128,
108}
109
110impl PolyphaseResampler {
111    /// Builds a fixed-state resampler and precomputes its Blackman-windowed
112    /// low-pass coefficient bank.
113    pub fn new(
114        input_rate_hz: u32,
115        output_rate_hz: u32,
116        channels: usize,
117        policy: ResamplerPolicy,
118    ) -> Result<Self, ResampleError> {
119        if input_rate_hz == 0 || output_rate_hz == 0 || channels == 0 {
120            return Err(ResampleError::InvalidFormat);
121        }
122        if policy.phases < 2
123            || policy.phases > 65_536
124            || policy.taps < 8
125            || policy.taps > 256
126            || !policy.taps.is_multiple_of(2)
127            || !policy.cutoff_ratio.is_finite()
128            || !(0.0..=1.0).contains(&policy.cutoff_ratio)
129            || policy.cutoff_ratio == 0.0
130            || policy.max_input_frames == 0
131        {
132            return Err(ResampleError::InvalidPolicy);
133        }
134        let coefficients = coefficient_bank(input_rate_hz, output_rate_hz, policy);
135        let history_len = policy
136            .taps
137            .checked_mul(channels)
138            .ok_or(ResampleError::InvalidPolicy)?;
139        Ok(Self {
140            input_rate_hz,
141            output_rate_hz,
142            channels,
143            policy,
144            coefficients,
145            history: vec![0.0; history_len],
146            input_frames_seen: 0,
147            next_output_time: 0,
148        })
149    }
150
151    /// Returns the configured input rate in hertz.
152    pub fn input_rate_hz(&self) -> u32 {
153        self.input_rate_hz
154    }
155
156    /// Returns the configured output rate in hertz.
157    pub fn output_rate_hz(&self) -> u32 {
158        self.output_rate_hz
159    }
160
161    /// Returns the interleaved channel count.
162    pub fn channels(&self) -> usize {
163        self.channels
164    }
165
166    /// Returns the fixed source-rate latency of the causal filter.
167    pub fn latency_input_frames(&self) -> usize {
168        self.policy.taps / 2
169    }
170
171    /// Returns the output frames that the next input block will produce.
172    pub fn required_output_frames(&self, input_frames: usize) -> Result<usize, ResampleError> {
173        if input_frames > self.policy.max_input_frames {
174            return Err(ResampleError::InputLimit {
175                supplied: input_frames,
176                maximum: self.policy.max_input_frames,
177            });
178        }
179        if input_frames == 0 {
180            return Ok(0);
181        }
182        let last_seen = self
183            .input_frames_seen
184            .checked_add(input_frames as u128)
185            .and_then(|value| value.checked_sub(1))
186            .ok_or(ResampleError::TimeOverflow)?;
187        let right = self.latency_input_frames() as u128;
188        let denominator = u128::from(self.output_rate_hz);
189        let step = u128::from(self.input_rate_hz);
190        let mut time = self.next_output_time;
191        let mut count = 0usize;
192        while time / denominator + right <= last_seen {
193            count = count.checked_add(1).ok_or(ResampleError::TimeOverflow)?;
194            time = time.checked_add(step).ok_or(ResampleError::TimeOverflow)?;
195        }
196        Ok(count)
197    }
198
199    /// Consumes interleaved source frames and writes every now-available
200    /// resampled frame into `output`.
201    ///
202    /// The call fails before consuming input when the buffers are misaligned,
203    /// the input bound is exceeded, or output storage is too small.
204    pub fn process_interleaved(
205        &mut self,
206        input: &[f32],
207        output: &mut [f32],
208    ) -> Result<ResampleReport, ResampleError> {
209        if !input.len().is_multiple_of(self.channels) || !output.len().is_multiple_of(self.channels)
210        {
211            return Err(ResampleError::MisalignedBuffer);
212        }
213        let input_frames = input.len() / self.channels;
214        let required = self.required_output_frames(input_frames)?;
215        let available = output.len() / self.channels;
216        if available < required {
217            return Err(ResampleError::OutputTooSmall {
218                required,
219                available,
220            });
221        }
222
223        let mut produced = 0usize;
224        for frame in 0..input_frames {
225            let absolute = self.input_frames_seen;
226            let ring_frame = (absolute % self.policy.taps as u128) as usize;
227            let ring_base = ring_frame * self.channels;
228            let input_base = frame * self.channels;
229            self.history[ring_base..ring_base + self.channels]
230                .copy_from_slice(&input[input_base..input_base + self.channels]);
231            self.input_frames_seen = self
232                .input_frames_seen
233                .checked_add(1)
234                .ok_or(ResampleError::TimeOverflow)?;
235
236            while self.output_ready(absolute) {
237                self.render_output_frame(produced, output)?;
238                produced += 1;
239                self.next_output_time = self
240                    .next_output_time
241                    .checked_add(u128::from(self.input_rate_hz))
242                    .ok_or(ResampleError::TimeOverflow)?;
243            }
244        }
245        debug_assert_eq!(produced, required);
246        Ok(ResampleReport {
247            input_frames,
248            output_frames: produced,
249            latency_input_frames: self.latency_input_frames(),
250        })
251    }
252
253    /// Clears time and sample history while retaining every allocation.
254    pub fn reset(&mut self) {
255        self.history.fill(0.0);
256        self.input_frames_seen = 0;
257        self.next_output_time = 0;
258    }
259
260    fn output_ready(&self, current_input: u128) -> bool {
261        self.next_output_time / u128::from(self.output_rate_hz)
262            + self.latency_input_frames() as u128
263            <= current_input
264    }
265
266    fn render_output_frame(
267        &self,
268        output_frame: usize,
269        output: &mut [f32],
270    ) -> Result<(), ResampleError> {
271        let denominator = u128::from(self.output_rate_hz);
272        let center = self.next_output_time / denominator;
273        let remainder = self.next_output_time % denominator;
274        let phase = ((remainder * self.policy.phases as u128) / denominator) as usize;
275        let coefficient_base = phase.min(self.policy.phases - 1) * self.policy.taps;
276        let left = self.policy.taps / 2 - 1;
277        for channel in 0..self.channels {
278            let mut sample = 0.0f64;
279            for tap in 0..self.policy.taps {
280                let source = signed_source(center, tap, left);
281                let value = source
282                    .and_then(|absolute| self.history_sample(absolute, channel))
283                    .unwrap_or(0.0);
284                sample += f64::from(value) * f64::from(self.coefficients[coefficient_base + tap]);
285            }
286            let at = output_frame
287                .checked_mul(self.channels)
288                .and_then(|base| base.checked_add(channel))
289                .ok_or(ResampleError::TimeOverflow)?;
290            output[at] = sample as f32;
291        }
292        Ok(())
293    }
294
295    fn history_sample(&self, absolute: u128, channel: usize) -> Option<f32> {
296        if absolute >= self.input_frames_seen {
297            return None;
298        }
299        let age = self.input_frames_seen - 1 - absolute;
300        if age >= self.policy.taps as u128 {
301            return None;
302        }
303        let frame = (absolute % self.policy.taps as u128) as usize;
304        Some(self.history[frame * self.channels + channel])
305    }
306
307    #[cfg(test)]
308    pub(crate) fn realtime_state_snapshot(&self) -> [usize; 2] {
309        [self.coefficients.capacity(), self.history.capacity()]
310    }
311}
312
313fn signed_source(center: u128, tap: usize, left: usize) -> Option<u128> {
314    if tap >= left {
315        center.checked_add((tap - left) as u128)
316    } else {
317        center.checked_sub((left - tap) as u128)
318    }
319}
320
321fn coefficient_bank(input_rate_hz: u32, output_rate_hz: u32, policy: ResamplerPolicy) -> Vec<f32> {
322    let rate_ratio = f64::from(output_rate_hz) / f64::from(input_rate_hz);
323    let cutoff = 0.5 * rate_ratio.min(1.0) * policy.cutoff_ratio;
324    let left = policy.taps / 2 - 1;
325    let mut coefficients = Vec::with_capacity(policy.phases * policy.taps);
326    for phase in 0..policy.phases {
327        let fraction = phase as f64 / policy.phases as f64;
328        let start = coefficients.len();
329        for tap in 0..policy.taps {
330            let distance = tap as f64 - left as f64 - fraction;
331            let ideal = 2.0 * cutoff * sinc(2.0 * cutoff * distance);
332            let window_position = tap as f64 / (policy.taps - 1) as f64;
333            let blackman = 0.42 - 0.5 * (2.0 * PI * window_position).cos()
334                + 0.08 * (4.0 * PI * window_position).cos();
335            coefficients.push((ideal * blackman) as f32);
336        }
337        let sum = coefficients[start..]
338            .iter()
339            .map(|value| f64::from(*value))
340            .sum::<f64>();
341        for value in &mut coefficients[start..] {
342            *value = (f64::from(*value) / sum) as f32;
343        }
344    }
345    coefficients
346}
347
348fn sinc(value: f64) -> f64 {
349    if value.abs() <= f64::EPSILON {
350        1.0
351    } else {
352        (PI * value).sin() / (PI * value)
353    }
354}
sim_lib_audio_dsp/resample.rs

sim_lib_audio_dsp/
resample.rs
