embedded_dsp/
svf.rs

1//! State Variable Filter with simultaneous lowpass, highpass, bandpass, notch, and peak outputs.
2//!
3//! Unlike a biquad, the cutoff and resonance can be swept every sample without recomputing a
4//! coefficient set, and all five responses are available from a single [`process`](StateVariableFilter::process)
5//! call. Ported from Andrew Simper's "Double Sampled, Stable State Variable Filter"
6//! (musicdsp.org), which internally runs two half-rate passes per sample for stability at high
7//! cutoff/resonance settings.
8
9#[allow(unused_imports)]
10use crate::math::FloatMath;
11
12/// Simultaneous low/high/band/notch/peak state-variable filter.
13#[derive(Debug, Clone, Copy)]
14pub struct StateVariableFilter {
15    sample_rate_hz: f32,
16    cutoff_max_hz: f32,
17    resonance: f32,
18    pre_drive: f32,
19    drive: f32,
20    freq: f32,
21    damp: f32,
22    low: f32,
23    high: f32,
24    band: f32,
25    notch: f32,
26    out_low: f32,
27    out_high: f32,
28    out_band: f32,
29    out_notch: f32,
30    out_peak: f32,
31}
32
33impl StateVariableFilter {
34    /// Creates a filter for `sample_rate_hz`, defaulting to a 200 Hz cutoff and 0.5 resonance.
35    pub fn new(sample_rate_hz: f32) -> Self {
36        let mut svf = Self {
37            sample_rate_hz,
38            cutoff_max_hz: sample_rate_hz / 3.0,
39            resonance: 0.5,
40            pre_drive: 0.5,
41            drive: 0.0,
42            freq: 0.25,
43            damp: 0.0,
44            low: 0.0,
45            high: 0.0,
46            band: 0.0,
47            notch: 0.0,
48            out_low: 0.0,
49            out_high: 0.0,
50            out_band: 0.0,
51            out_notch: 0.0,
52            out_peak: 0.0,
53        };
54        svf.set_cutoff(200.0);
55        svf.set_resonance(0.5);
56        svf
57    }
58
59    /// Sets the cutoff frequency in Hz. Clamped to `(0, sample_rate_hz / 3]` to keep the
60    /// double-sampled topology stable.
61    pub fn set_cutoff(&mut self, cutoff_hz: f32) {
62        let cutoff_hz = cutoff_hz.clamp(1.0e-6, self.cutoff_max_hz);
63        // *2.0 because the filter runs two half-rate passes per input sample.
64        self.freq = 2.0
65            * (core::f32::consts::PI * (cutoff_hz / (self.sample_rate_hz * 2.0)).min(0.25)).sin();
66        self.recompute_damp();
67    }
68
69    /// Sets the resonance, clamped to `[0.0, 1.0]` to guarantee stability.
70    pub fn set_resonance(&mut self, resonance: f32) {
71        self.resonance = resonance.clamp(0.0, 1.0);
72        self.recompute_damp();
73        self.drive = self.pre_drive * self.resonance;
74    }
75
76    /// Sets the drive, which shapes how hard the resonant peak saturates. Typical range `0.0..=1.0`.
77    pub fn set_drive(&mut self, drive: f32) {
78        self.pre_drive = (drive * 0.1).clamp(0.0, 1.0);
79        self.drive = self.pre_drive * self.resonance;
80    }
81
82    fn recompute_damp(&mut self) {
83        let res_damp = 2.0 * (1.0 - self.resonance.powf(0.25));
84        let freq_damp = (2.0 / self.freq - self.freq * 0.5).min(2.0);
85        self.damp = res_damp.min(freq_damp);
86    }
87
88    /// Processes one input sample, updating all five simultaneous outputs.
89    pub fn process(&mut self, input: f32) {
90        self.pass(input);
91        self.out_low = 0.5 * self.low;
92        self.out_high = 0.5 * self.high;
93        self.out_band = 0.5 * self.band;
94        self.out_peak = 0.5 * (self.low - self.high);
95        self.out_notch = 0.5 * self.notch;
96
97        self.pass(input);
98        self.out_low += 0.5 * self.low;
99        self.out_high += 0.5 * self.high;
100        self.out_band += 0.5 * self.band;
101        self.out_peak += 0.5 * (self.low - self.high);
102        self.out_notch += 0.5 * self.notch;
103    }
104
105    #[inline]
106    fn pass(&mut self, input: f32) {
107        self.notch = input - self.damp * self.band;
108        self.low += self.freq * self.band;
109        self.high = self.notch - self.low;
110        self.band += self.freq * self.high - self.drive * self.band * self.band * self.band;
111
112        // At a lightly-damped resonance near the cutoff ceiling (`sample_rate_hz / 3`), the
113        // cubic drive term isn't always enough to keep this loop from numerically diverging,
114        // particularly at low sample rates where realistic cutoffs sit closer to that ceiling.
115        // Clamp generously — well outside any level this filter produces in normal operation —
116        // so a runaway degrades to a bounded, loud output instead of NaN/Inf.
117        const STATE_LIMIT: f32 = 1.0e3;
118        self.low = self.low.clamp(-STATE_LIMIT, STATE_LIMIT);
119        self.high = self.high.clamp(-STATE_LIMIT, STATE_LIMIT);
120        self.band = self.band.clamp(-STATE_LIMIT, STATE_LIMIT);
121        self.notch = self.notch.clamp(-STATE_LIMIT, STATE_LIMIT);
122    }
123
124    /// Lowpass output from the most recent [`process`](Self::process) call.
125    pub fn low(&self) -> f32 {
126        self.out_low
127    }
128
129    /// Highpass output from the most recent [`process`](Self::process) call.
130    pub fn high(&self) -> f32 {
131        self.out_high
132    }
133
134    /// Bandpass output from the most recent [`process`](Self::process) call.
135    pub fn band(&self) -> f32 {
136        self.out_band
137    }
138
139    /// Notch (band-stop) output from the most recent [`process`](Self::process) call.
140    pub fn notch(&self) -> f32 {
141        self.out_notch
142    }
143
144    /// Peak output from the most recent [`process`](Self::process) call.
145    pub fn peak(&self) -> f32 {
146        self.out_peak
147    }
148
149    /// Resets the filter's internal state. Cutoff, resonance, and drive are left unchanged.
150    pub fn reset(&mut self) {
151        self.low = 0.0;
152        self.high = 0.0;
153        self.band = 0.0;
154        self.notch = 0.0;
155        self.out_low = 0.0;
156        self.out_high = 0.0;
157        self.out_band = 0.0;
158        self.out_notch = 0.0;
159        self.out_peak = 0.0;
160    }
161}
embedded_dsp/svf.rs

embedded_dsp/
svf.rs
