sim_lib_audio_dsp/
filter.rs

1use std::f32::consts::PI;
2
3use sim_lib_audio_graph_core::{PrepareConfig, ProcessBlock, Processor};
4
5use crate::common::{
6    clamp_cutoff, db_to_gain, input_sample, prepare_channels, prepared_output_channels,
7};
8
9/// Mode of a [`OnePoleFilter`].
10#[derive(Clone, Copy, Debug, PartialEq, Eq)]
11pub enum OnePoleMode {
12    /// One-pole low-pass response.
13    LowPass,
14    /// One-pole high-pass response.
15    HighPass,
16}
17
18#[derive(Clone, Copy, Debug, Default, PartialEq)]
19struct OnePoleState {
20    z1: f32,
21}
22
23/// A per-channel one-pole low-/high-pass filter [`Processor`].
24#[derive(Clone, Debug, PartialEq)]
25pub struct OnePoleFilter {
26    mode: OnePoleMode,
27    cutoff_hz: f32,
28    sample_rate_hz: f32,
29    states: Vec<OnePoleState>,
30}
31
32impl OnePoleFilter {
33    /// Creates a low-pass one-pole filter at the given cutoff.
34    pub fn low_pass(cutoff_hz: f32) -> Self {
35        Self::new(OnePoleMode::LowPass, cutoff_hz)
36    }
37
38    /// Creates a high-pass one-pole filter at the given cutoff.
39    pub fn high_pass(cutoff_hz: f32) -> Self {
40        Self::new(OnePoleMode::HighPass, cutoff_hz)
41    }
42
43    /// Creates a one-pole filter with the given mode and cutoff in hertz.
44    pub fn new(mode: OnePoleMode, cutoff_hz: f32) -> Self {
45        Self {
46            mode,
47            cutoff_hz,
48            sample_rate_hz: 48_000.0,
49            states: Vec::new(),
50        }
51    }
52
53    fn alpha(&self) -> f32 {
54        let cutoff = clamp_cutoff(self.cutoff_hz, self.sample_rate_hz);
55        1.0 - (-2.0 * PI * cutoff / self.sample_rate_hz).exp()
56    }
57
58    #[cfg(all(test, not(debug_assertions)))]
59    pub(crate) fn realtime_state_snapshot(&self) -> Vec<usize> {
60        vec![self.states.capacity()]
61    }
62}
63
64impl Processor for OnePoleFilter {
65    fn prepare(&mut self, cfg: PrepareConfig) {
66        self.sample_rate_hz = cfg.sample_rate_hz as f32;
67        prepare_channels(
68            &mut self.states,
69            cfg.out_channels as usize,
70            OnePoleState::default(),
71        );
72    }
73
74    fn reset(&mut self) {
75        self.states.fill(OnePoleState::default());
76    }
77
78    fn process(&mut self, block: &mut ProcessBlock<'_>) {
79        let channels = prepared_output_channels(block, self.states.len(), "OnePoleFilter");
80        let alpha = self.alpha();
81        let frames = block.frames as usize;
82        for channel in 0..channels {
83            let state = &mut self.states[channel];
84            for frame in 0..frames {
85                let input = input_sample(block, channel, frame);
86                state.z1 += alpha * (input - state.z1);
87                block.out_audio[channel][frame] = match self.mode {
88                    OnePoleMode::LowPass => state.z1,
89                    OnePoleMode::HighPass => input - state.z1,
90                };
91            }
92        }
93    }
94}
95
96/// Response type of a [`BiquadFilter`].
97#[derive(Clone, Copy, Debug, PartialEq)]
98pub enum BiquadKind {
99    /// Low-pass response.
100    LowPass,
101    /// High-pass response.
102    HighPass,
103    /// Band-pass response.
104    BandPass,
105    /// Band-reject (notch) response.
106    Notch,
107    /// Peaking EQ with the given gain.
108    Peaking {
109        /// Peak gain in decibels.
110        gain_db: f32,
111    },
112}
113
114#[derive(Clone, Copy, Debug, PartialEq)]
115struct Coefficients {
116    b0: f32,
117    b1: f32,
118    b2: f32,
119    a1: f32,
120    a2: f32,
121}
122
123impl Default for Coefficients {
124    fn default() -> Self {
125        Self {
126            b0: 1.0,
127            b1: 0.0,
128            b2: 0.0,
129            a1: 0.0,
130            a2: 0.0,
131        }
132    }
133}
134
135#[derive(Clone, Copy, Debug, Default, PartialEq)]
136struct BiquadState {
137    z1: f32,
138    z2: f32,
139}
140
141/// A per-channel RBJ biquad filter [`Processor`].
142#[derive(Clone, Debug, PartialEq)]
143pub struct BiquadFilter {
144    kind: BiquadKind,
145    frequency_hz: f32,
146    q: f32,
147    sample_rate_hz: f32,
148    coefficients: Coefficients,
149    states: Vec<BiquadState>,
150}
151
152impl BiquadFilter {
153    /// Creates a biquad filter of the given kind, frequency, and Q (clamped to
154    /// `>= 0.05`).
155    pub fn new(kind: BiquadKind, frequency_hz: f32, q: f32) -> Self {
156        let mut filter = Self {
157            kind,
158            frequency_hz,
159            q: q.max(0.05),
160            sample_rate_hz: 48_000.0,
161            coefficients: Coefficients::default(),
162            states: Vec::new(),
163        };
164        filter.update_coefficients();
165        filter
166    }
167
168    /// Creates a low-pass biquad at the given frequency and Q.
169    pub fn low_pass(frequency_hz: f32, q: f32) -> Self {
170        Self::new(BiquadKind::LowPass, frequency_hz, q)
171    }
172
173    /// Creates a high-pass biquad at the given frequency and Q.
174    pub fn high_pass(frequency_hz: f32, q: f32) -> Self {
175        Self::new(BiquadKind::HighPass, frequency_hz, q)
176    }
177
178    /// Creates a band-pass biquad at the given frequency and Q.
179    pub fn band_pass(frequency_hz: f32, q: f32) -> Self {
180        Self::new(BiquadKind::BandPass, frequency_hz, q)
181    }
182
183    /// Creates a notch biquad at the given frequency and Q.
184    pub fn notch(frequency_hz: f32, q: f32) -> Self {
185        Self::new(BiquadKind::Notch, frequency_hz, q)
186    }
187
188    fn update_coefficients(&mut self) {
189        let frequency = clamp_cutoff(self.frequency_hz, self.sample_rate_hz);
190        let omega = 2.0 * PI * frequency / self.sample_rate_hz;
191        let sin = omega.sin();
192        let cos = omega.cos();
193        let alpha = sin / (2.0 * self.q.max(0.05));
194        let (b0, b1, b2, a0, a1, a2) = match self.kind {
195            BiquadKind::LowPass => (
196                (1.0 - cos) * 0.5,
197                1.0 - cos,
198                (1.0 - cos) * 0.5,
199                1.0 + alpha,
200                -2.0 * cos,
201                1.0 - alpha,
202            ),
203            BiquadKind::HighPass => (
204                (1.0 + cos) * 0.5,
205                -(1.0 + cos),
206                (1.0 + cos) * 0.5,
207                1.0 + alpha,
208                -2.0 * cos,
209                1.0 - alpha,
210            ),
211            BiquadKind::BandPass => (alpha, 0.0, -alpha, 1.0 + alpha, -2.0 * cos, 1.0 - alpha),
212            BiquadKind::Notch => (1.0, -2.0 * cos, 1.0, 1.0 + alpha, -2.0 * cos, 1.0 - alpha),
213            BiquadKind::Peaking { gain_db } => {
214                let amp = db_to_gain(gain_db).sqrt();
215                (
216                    1.0 + alpha * amp,
217                    -2.0 * cos,
218                    1.0 - alpha * amp,
219                    1.0 + alpha / amp,
220                    -2.0 * cos,
221                    1.0 - alpha / amp,
222                )
223            }
224        };
225        self.coefficients = Coefficients {
226            b0: b0 / a0,
227            b1: b1 / a0,
228            b2: b2 / a0,
229            a1: a1 / a0,
230            a2: a2 / a0,
231        };
232    }
233
234    #[cfg(all(test, not(debug_assertions)))]
235    pub(crate) fn realtime_state_snapshot(&self) -> Vec<usize> {
236        vec![self.states.capacity()]
237    }
238}
239
240impl Processor for BiquadFilter {
241    fn prepare(&mut self, cfg: PrepareConfig) {
242        self.sample_rate_hz = cfg.sample_rate_hz as f32;
243        self.update_coefficients();
244        prepare_channels(
245            &mut self.states,
246            cfg.out_channels as usize,
247            BiquadState::default(),
248        );
249    }
250
251    fn reset(&mut self) {
252        self.states.fill(BiquadState::default());
253    }
254
255    fn process(&mut self, block: &mut ProcessBlock<'_>) {
256        let channels = prepared_output_channels(block, self.states.len(), "BiquadFilter");
257        let c = self.coefficients;
258        let frames = block.frames as usize;
259        for channel in 0..channels {
260            let state = &mut self.states[channel];
261            for frame in 0..frames {
262                let input = input_sample(block, channel, frame);
263                let output = c.b0 * input + state.z1;
264                state.z1 = c.b1 * input - c.a1 * output + state.z2;
265                state.z2 = c.b2 * input - c.a2 * output;
266                block.out_audio[channel][frame] = output;
267            }
268        }
269    }
270}
271
272/// Output tap selected from a [`StateVariableFilter`].
273#[derive(Clone, Copy, Debug, PartialEq, Eq)]
274pub enum StateVariableMode {
275    /// Low-pass output.
276    LowPass,
277    /// High-pass output.
278    HighPass,
279    /// Band-pass output.
280    BandPass,
281    /// Notch (low + high) output.
282    Notch,
283}
284
285#[derive(Clone, Copy, Debug, Default, PartialEq)]
286struct SvfState {
287    ic1eq: f32,
288    ic2eq: f32,
289}
290
291/// A per-channel zero-delay-feedback state-variable filter [`Processor`].
292#[derive(Clone, Debug, PartialEq)]
293pub struct StateVariableFilter {
294    mode: StateVariableMode,
295    frequency_hz: f32,
296    q: f32,
297    sample_rate_hz: f32,
298    states: Vec<SvfState>,
299}
300
301impl StateVariableFilter {
302    /// Creates a state-variable filter with the given output mode, frequency,
303    /// and Q (clamped to `>= 0.05`).
304    pub fn new(mode: StateVariableMode, frequency_hz: f32, q: f32) -> Self {
305        Self {
306            mode,
307            frequency_hz,
308            q: q.max(0.05),
309            sample_rate_hz: 48_000.0,
310            states: Vec::new(),
311        }
312    }
313
314    fn process_sample(&self, state: &mut SvfState, input: f32) -> f32 {
315        let frequency = clamp_cutoff(self.frequency_hz, self.sample_rate_hz);
316        let g = (PI * frequency / self.sample_rate_hz).tan();
317        let k = 1.0 / self.q.max(0.05);
318        let a1 = 1.0 / (1.0 + g * (g + k));
319        let a2 = g * a1;
320        let a3 = g * a2;
321        let v3 = input - state.ic2eq;
322        let v1 = a1 * state.ic1eq + a2 * v3;
323        let v2 = state.ic2eq + a2 * state.ic1eq + a3 * v3;
324        state.ic1eq = 2.0 * v1 - state.ic1eq;
325        state.ic2eq = 2.0 * v2 - state.ic2eq;
326        let low = v2;
327        let high = input - k * v1 - v2;
328        match self.mode {
329            StateVariableMode::LowPass => low,
330            StateVariableMode::HighPass => high,
331            StateVariableMode::BandPass => v1,
332            StateVariableMode::Notch => low + high,
333        }
334    }
335
336    #[cfg(all(test, not(debug_assertions)))]
337    pub(crate) fn realtime_state_snapshot(&self) -> Vec<usize> {
338        vec![self.states.capacity()]
339    }
340}
341
342impl Processor for StateVariableFilter {
343    fn prepare(&mut self, cfg: PrepareConfig) {
344        self.sample_rate_hz = cfg.sample_rate_hz as f32;
345        prepare_channels(
346            &mut self.states,
347            cfg.out_channels as usize,
348            SvfState::default(),
349        );
350    }
351
352    fn reset(&mut self) {
353        self.states.fill(SvfState::default());
354    }
355
356    fn process(&mut self, block: &mut ProcessBlock<'_>) {
357        let channels = prepared_output_channels(block, self.states.len(), "StateVariableFilter");
358        let frames = block.frames as usize;
359        for channel in 0..channels {
360            for frame in 0..frames {
361                let input = input_sample(block, channel, frame);
362                let mut state = self.states[channel];
363                let output = self.process_sample(&mut state, input);
364                self.states[channel] = state;
365                block.out_audio[channel][frame] = output;
366            }
367        }
368    }
369}
sim_lib_audio_dsp/filter.rs

sim_lib_audio_dsp/
filter.rs
