simper_filter/
lib.rs

1use num_complex::Complex64;
2use num_traits::Float;
3use std::f64::consts::{PI, TAU};
4use thiserror::Error;
5
6#[derive(Error, Debug)]
7pub enum SvfError {
8    #[error("the sample rate must be set first")]
9    NoSampleRate,
10    #[error("the frequency is higher than nyqist")]
11    FrequencyOverNyqist,
12    #[error("the frequency is 0 or lower than 0")]
13    FrequencyTooLow,
14    #[error("q is lower than zero")]
15    NegativeQ,
16    #[error("fatal number conversion error")]
17    Fatal,
18}
19
20/// All available filter types for the State Variable Filter
21#[derive(Default, Clone, PartialEq, Eq)]
22pub enum FilterType {
23    #[default]
24    Lowpass,
25    Highpass,
26    Bandpass,
27    Notch,
28    Peak,
29    Allpass,
30    Bell,
31    Lowshelf,
32    Highshelf,
33}
34
35/// A State Variable Filter that is copied from Andrew Simper (Cytomic)
36/// https://cytomic.com/files/dsp/SvfLinearTrapOptimised2.pdf
37#[derive(Default)]
38pub struct Svf<F: Float> {
39    coefficients: SvfCoefficients<F>,
40    ic1eq: F,
41    ic2eq: F,
42}
43
44/// The filter coefficients that hold old necessary data to reproduce the filter
45#[derive(Default, Clone, PartialEq, Eq)]
46pub struct SvfCoefficients<F: Float> {
47    filter_type: FilterType,
48    sample_rate: F,
49    cutoff: F,
50    gain: F,
51    q: F,
52    a1: F,
53    a2: F,
54    a3: F,
55    m0: F,
56    m1: F,
57    m2: F,
58}
59
60impl<F: Float + Default> Svf<F> {
61    /// sets the filter to an inital response
62    /// use `Svf::default` otherwise
63    /// Parameters:
64    /// - filter_type: choose one of the filter types, like peak, lowpass or highpass
65    /// - sample_rate: the sample_rate of the audio buffer that the filter should be applied on
66    /// - frequency: the frequency in Hz where the cutoff of the filter should be
67    /// - q: the steepness of the filter
68    /// - gain: the gain boost or decrease of the filter
69    pub fn new(
70        filter_type: FilterType,
71        sample_rate: F,
72        cutoff: F,
73        q: F,
74        gain: F,
75    ) -> Result<Self, SvfError> {
76        let mut svf = Self::default();
77        svf.set(filter_type, sample_rate, cutoff, q, gain)?;
78        Ok(svf)
79    }
80
81    /// process helper function that calls `Svf::tick` on each sample
82    #[inline]
83    pub fn process(&mut self, input: &[F], output: &mut [F]) {
84        output
85            .iter_mut()
86            .zip(input)
87            .for_each(|(out_sample, in_sample)| {
88                *out_sample = self.tick(*in_sample);
89            });
90    }
91
92    /// Reset state of filter.
93    /// Can be used when the audio callback is restarted.
94    #[inline]
95    pub fn reset(&mut self) {
96        self.ic1eq = F::zero();
97        self.ic2eq = F::zero();
98    }
99
100    /// The set function is setting the current parameters of the filter.
101    /// Don't call from a different thread than tick.
102    /// Parameters:
103    /// - filter_type: choose one of the filter types, like peak, lowpass or highpass
104    /// - sample_rate: the sample_rate of the audio buffer that the filter should be applied on
105    /// - frequency: the frequency in Hz where the cutoff of the filter should be
106    /// - q: the steepness of the filter
107    /// - gain: the gain boost or decrease of the filter
108    #[inline]
109    pub fn set(
110        &mut self,
111        filter_type: FilterType,
112        sample_rate: F,
113        cutoff: F,
114        q: F,
115        gain: F,
116    ) -> Result<(), SvfError> {
117        self.coefficients
118            .set(filter_type, sample_rate, cutoff, q, gain)
119    }
120
121    /// Set new filter parameters from coefficients struct
122    pub fn set_coeffs(&mut self, coefficients: SvfCoefficients<F>) {
123        self.coefficients = coefficients;
124    }
125
126    /// The process that is applying the filter on a sample by sample basis
127    /// `process` is a utility function to call tick on a full frame
128    #[inline]
129    pub fn tick(&mut self, input: F) -> F {
130        let v0 = input;
131        let v3 = v0 - self.ic2eq;
132        let v1 = self.coefficients.a1 * self.ic1eq + self.coefficients.a2 * v3;
133        let v2 = self.ic2eq + self.coefficients.a2 * self.ic1eq + self.coefficients.a3 * v3;
134        let two = F::one() + F::one();
135        self.ic1eq = two * v1 - self.ic1eq;
136        self.ic2eq = two * v2 - self.ic2eq;
137
138        self.coefficients.m0 * v0 + self.coefficients.m1 * v1 + self.coefficients.m2 * v2
139    }
140
141    /// get a reference to the coefficients
142    /// can be used to clone it to the UI thread, to plot the response
143    pub fn coefficients(&self) -> &SvfCoefficients<F> {
144        &self.coefficients
145    }
146
147    /// utility method to get the response
148    /// should not be called from another thread than the set methods
149    pub fn get_response(&self, frequency: f64) -> Result<Complex64, SvfError> {
150        SvfResponse::response(&self.coefficients, frequency)
151    }
152}
153
154impl<F: Float> SvfCoefficients<F> {
155    /// The set function is setting the current parameters of the filter.
156    /// Don't call from a different thread than tick.
157    /// Parameters:
158    /// - filter_type: choose one of the filter types, like peak, lowpass or highpass
159    /// - sample_rate: the sample_rate of the audio buffer that the filter should be applied on
160    /// - frequency: the frequency in Hz where the cutoff of the filter should be
161    /// - q: the steepness of the filter
162    /// - gain: the gain boost or decrease of the filter
163    pub fn set(
164        &mut self,
165        filter_type: FilterType,
166        sample_rate: F,
167        cutoff_frequency: F,
168        q: F,
169        gain: F,
170    ) -> Result<(), SvfError> {
171        if q < F::zero() {
172            return Err(SvfError::NegativeQ);
173        }
174        if cutoff_frequency > sample_rate / F::from(2.0).unwrap() {
175            return Err(SvfError::FrequencyOverNyqist);
176        }
177        self.filter_type = filter_type;
178        self.sample_rate = sample_rate;
179        self.cutoff = cutoff_frequency;
180        self.q = q;
181        self.gain = gain;
182
183        match self.filter_type {
184            FilterType::Lowpass => {
185                let g =
186                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
187                let k = F::one() / self.q;
188                self.a1 = F::one() / (F::one() + g * (g + k));
189                self.a2 = g * self.a1;
190                self.a3 = g * self.a2;
191                self.m0 = F::zero();
192                self.m1 = F::zero();
193                self.m2 = F::one();
194            }
195            FilterType::Highpass => {
196                let g =
197                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
198                let k = F::one() / self.q;
199                self.a1 = F::one() / (F::one() + g * (g + k));
200                self.a2 = g * self.a1;
201                self.a3 = g * self.a2;
202                self.m0 = F::one();
203                self.m1 = -k;
204                self.m2 = -F::one();
205            }
206            FilterType::Bandpass => {
207                let g =
208                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
209                let k = F::one() / self.q;
210                self.a1 = F::one() / (F::one() + g * (g + k));
211                self.a2 = g * self.a1;
212                self.a3 = g * self.a2;
213                self.m0 = F::zero();
214                self.m1 = F::one();
215                self.m2 = F::zero();
216            }
217            FilterType::Notch => {
218                let g =
219                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
220                let k = F::one() / self.q;
221                self.a1 = F::one() / (F::one() + g * (g + k));
222                self.a2 = g * self.a1;
223                self.a3 = g * self.a2;
224                self.m0 = F::one();
225                self.m0 = F::one();
226                self.m1 = -k;
227                self.m2 = F::zero();
228            }
229            FilterType::Peak => {
230                let g =
231                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
232                let k = F::one() / self.q;
233                self.a1 = F::one() / (F::one() + g * (g + k));
234                self.a2 = g * self.a1;
235                self.a3 = g * self.a2;
236                self.m0 = F::one();
237                self.m1 = -k;
238                self.m2 = F::from(-2.).ok_or(SvfError::Fatal)?;
239            }
240            FilterType::Allpass => {
241                let g =
242                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
243                let k = F::one() / self.q;
244                self.a1 = F::one() / (F::one() + g * (g + k));
245                self.a2 = g * self.a1;
246                self.a3 = g * self.a2;
247                self.m0 = F::one();
248                self.m1 = F::from(-2.).ok_or(SvfError::Fatal)? * k;
249                self.m2 = F::zero();
250            }
251            FilterType::Bell => {
252                let a = F::sqrt(self.gain);
253                let g =
254                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate);
255                let k = F::one() / (self.q * a);
256                self.a1 = F::one() / (F::one() + g * (g + k));
257                self.a2 = g * self.a1;
258                self.a3 = g * self.a2;
259                self.m0 = F::one();
260                self.m1 = k * (a * a - F::one());
261                self.m2 = F::zero();
262            }
263            FilterType::Lowshelf => {
264                let a = F::sqrt(self.gain);
265                let g =
266                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate)
267                        / F::sqrt(a);
268                let k = F::one() / self.q;
269                self.a1 = F::one() / (F::one() + g * (g + k));
270                self.a2 = g * self.a1;
271                self.a3 = g * self.a2;
272                self.m0 = F::one();
273                self.m1 = k * (a - F::one());
274                self.m2 = a * a - F::one();
275            }
276            FilterType::Highshelf => {
277                let a = F::sqrt(self.gain);
278                let g =
279                    F::tan(F::from(PI).ok_or(SvfError::Fatal)? * self.cutoff / self.sample_rate)
280                        * F::sqrt(a);
281                let k = F::one() / self.q;
282                self.a1 = F::one() / (F::one() + g * (g + k));
283                self.a2 = g * self.a1;
284                self.a3 = g * self.a2;
285                self.m0 = a * a;
286                self.m1 = k * (F::one() - a) * a;
287                self.m2 = F::one() - a * a;
288            }
289        }
290        Ok(())
291    }
292}
293
294/// Coefficients can be copied in the UI Thread, so that the response can be shown
295pub struct SvfResponse;
296impl SvfResponse {
297    /// Retrieves the filter response of the current settings for a specific frequency
298    /// Can be used to plot the filter magnitue and phase
299    pub fn response<F: Float>(
300        coeffs: &SvfCoefficients<F>,
301        frequency: f64,
302    ) -> Result<Complex64, SvfError> {
303        if frequency <= 0.0 {
304            return Err(SvfError::FrequencyTooLow);
305        }
306        let nyquist = coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)? / 2.0;
307        if frequency > nyquist {
308            return Err(SvfError::FrequencyOverNyqist);
309        }
310
311        match coeffs.filter_type {
312            FilterType::Lowpass => SvfResponse::lowpass_response(coeffs, frequency),
313            FilterType::Highpass => SvfResponse::highpass_response(coeffs, frequency),
314            FilterType::Bandpass => SvfResponse::bandpass_response(coeffs, frequency),
315            FilterType::Notch => SvfResponse::notch_response(coeffs, frequency),
316            FilterType::Peak => SvfResponse::peak_response(coeffs, frequency),
317            FilterType::Allpass => SvfResponse::allpass_response(coeffs, frequency),
318            FilterType::Bell => SvfResponse::bell_response(coeffs, frequency),
319            FilterType::Lowshelf => SvfResponse::lowshelf_response(coeffs, frequency),
320            FilterType::Highshelf => SvfResponse::highshelf_response(coeffs, frequency),
321        }
322    }
323
324    fn lowpass_response<F: Float>(
325        coeffs: &SvfCoefficients<F>,
326        frequency: f64,
327    ) -> Result<Complex64, SvfError> {
328        let g = f64::tan(
329            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
330                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
331        );
332        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
333        let f = frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?;
334        let z = Complex64::from_polar(1.0, f);
335        let response = (g * g * (1.0 + z) * (1.0 + z))
336            / ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * k * (z * z - 1.0));
337        Ok(response)
338    }
339
340    fn highpass_response<F: Float>(
341        coeffs: &SvfCoefficients<F>,
342        frequency: f64,
343    ) -> Result<Complex64, SvfError> {
344        let g = f64::tan(
345            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
346                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
347        );
348        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
349        let f = frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?;
350        let z = Complex64::from_polar(1.0, f);
351        let response = ((z - 1.0) * (z - 1.0))
352            / ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * k * (z * z - 1.0));
353        Ok(response)
354    }
355
356    fn bandpass_response<F: Float>(
357        coeffs: &SvfCoefficients<F>,
358        frequency: f64,
359    ) -> Result<Complex64, SvfError> {
360        let g = f64::tan(
361            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
362                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
363        );
364        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
365        let f = frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?;
366        let z = Complex64::from_polar(1.0, f);
367        let response = (g * (z * z - 1.0))
368            / ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * k * (z * z - 1.0));
369        Ok(response)
370    }
371
372    fn notch_response<F: Float>(
373        coeffs: &SvfCoefficients<F>,
374        frequency: f64,
375    ) -> Result<Complex64, SvfError> {
376        let g = f64::tan(
377            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
378                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
379        );
380        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
381        let f = frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?;
382        let z = Complex64::from_polar(1.0, f);
383        let response = ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z))
384            / ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * k * (z * z - 1.0));
385        Ok(response)
386    }
387
388    fn peak_response<F: Float>(
389        coeffs: &SvfCoefficients<F>,
390        frequency: f64,
391    ) -> Result<Complex64, SvfError> {
392        let g = f64::tan(
393            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
394                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
395        );
396        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
397        let f = frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?;
398        let z = Complex64::from_polar(1.0, f);
399        // Note: this is the negation of the transfer function reported in the derivation.
400        let response = -((1.0 + g + (g - 1.0) * z) * (-1.0 + g + z + g * z))
401            / ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * k * (z * z - 1.0));
402        Ok(response)
403    }
404
405    fn allpass_response<F: Float>(
406        coeffs: &SvfCoefficients<F>,
407        frequency: f64,
408    ) -> Result<Complex64, SvfError> {
409        let g = f64::tan(
410            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
411                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
412        );
413        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
414        let f = frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?;
415        let z = Complex64::from_polar(1.0, f);
416        let response =
417            ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * (k - k * z * z))
418                / ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z) + g * k * (z * z - 1.0));
419        Ok(response)
420    }
421
422    fn bell_response<F: Float>(
423        coeffs: &SvfCoefficients<F>,
424        frequency: f64,
425    ) -> Result<Complex64, SvfError> {
426        let a = f64::sqrt(coeffs.gain.to_f64().ok_or(SvfError::Fatal)?);
427        let g = f64::tan(
428            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
429                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
430        );
431        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
432        let z = Complex64::from_polar(
433            1.0,
434            frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
435        );
436        let response = (g * k * (z * z - 1.0)
437            + a * (g * (1.0 + z) * ((a * a - 1.0) * k / a * (z - 1.0))
438                + ((z - 1.0) * (z - 1.0) + g * g * (1.0 + z) * (1.0 + z))))
439            / (g * k * (z * z - 1.0) + a * ((z - 1.0) * (z - 1.0) + g * g * (z + 1.0) * (z + 1.0)));
440        Ok(response)
441    }
442
443    fn lowshelf_response<F: Float>(
444        coeffs: &SvfCoefficients<F>,
445        frequency: f64,
446    ) -> Result<Complex64, SvfError> {
447        let a = f64::sqrt(coeffs.gain.to_f64().ok_or(SvfError::Fatal)?);
448        let g = f64::tan(
449            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
450                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
451        );
452        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
453        let z = Complex64::from_polar(
454            1.0,
455            frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
456        );
457        let sqrt_a = f64::sqrt(a);
458        let response = (a * (z - 1.0) * (z - 1.0)
459            + g * g * a * a * (z + 1.0) * (z + 1.0)
460            + sqrt_a * g * a * k * (z * z - 1.0))
461            / (a * (z - 1.0) * (z - 1.0)
462                + g * g * (1.0 + z) * (1.0 + z)
463                + sqrt_a * g * k * (z * z - 1.0));
464        Ok(response)
465    }
466
467    fn highshelf_response<F: Float>(
468        coeffs: &SvfCoefficients<F>,
469        frequency: f64,
470    ) -> Result<Complex64, SvfError> {
471        let a = f64::sqrt(coeffs.gain.to_f64().ok_or(SvfError::Fatal)?);
472        let g = f64::tan(
473            PI * coeffs.cutoff.to_f64().ok_or(SvfError::Fatal)?
474                / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
475        );
476        let k = 1.0 / coeffs.q.to_f64().ok_or(SvfError::Fatal)?;
477        let z = Complex64::from_polar(
478            1.0,
479            frequency * TAU / coeffs.sample_rate.to_f64().ok_or(SvfError::Fatal)?,
480        );
481        let sqrt_a = f64::sqrt(a);
482        let response = (sqrt_a
483            * g
484            * (1.0 + z)
485            * (-(a - 1.0) * a * k * (z - 1.0) + sqrt_a * g * (1.0 - a * a) * (1.0 + z))
486            + a * a
487                * ((z - 1.0) * (z - 1.0)
488                    + a * g * g * (1.0 + z) * (1.0 + z)
489                    + sqrt_a * g * k * (z * z - 1.0)))
490            / ((z - 1.0) * (z - 1.0)
491                + a * g * g * (1.0 + z) * (1.0 + z)
492                + sqrt_a * g * k * (z * z - 1.0));
493        Ok(response)
494    }
495}
496
497#[cfg(test)]
498mod tests {
499    use super::*;
500
501    #[test]
502    fn it_works() {
503        let mut filter = Svf::<f32>::default();
504        filter
505            .set(FilterType::Lowpass, 44100.0, 400.0, f32::sqrt(2.0), 0.0)
506            .unwrap();
507        let _response = filter.get_response(20.0).unwrap();
508        let input = vec![0.0; 100];
509        let mut output = vec![0.0; 100];
510        filter.process(&input, &mut output);
511    }
512}
simper_filter/lib.rs

simper_filter/
lib.rs
