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

sim_lib_audio_dsp/
filter.rs
