Add StateVariableFilter

CMakeLists.txt

@@ -65,6 +65,7 @@ if(NOT CMAKE_CROSSCOMPILING)
             "lib/grit/audio/envelope/envelope_follower_test.cpp"
 
             "lib/grit/audio/filter/biquad_test.cpp"
+            "lib/grit/audio/filter/state_variable_filter_test.cpp"
 
             "lib/grit/audio/music/note_test.cpp"
 

lib/CMakeLists.txt

@@ -41,6 +41,7 @@ target_sources(gritwave-grit INTERFACE
         "grit/audio/filter.hpp"
         "grit/audio/filter/biquad.hpp"
         "grit/audio/filter/dynamic_smoothing.hpp"
+        "grit/audio/filter/state_variable_filter.hpp"
 
         "grit/audio/mix.hpp"
         "grit/audio/mix/cross_fade.hpp"

lib/grit/audio/filter.hpp

@@ -5,3 +5,4 @@
 
 #include <grit/audio/filter/biquad.hpp>
 #include <grit/audio/filter/dynamic_smoothing.hpp>
+#include <grit/audio/filter/state_variable_filter.hpp>

lib/grit/audio/filter/state_variable_filter.hpp

@@ -0,0 +1,145 @@
+#pragma once
+
+#include <etl/cmath.hpp>
+#include <etl/concepts.hpp>
+#include <etl/numbers.hpp>
+#include <etl/type_traits.hpp>
+
+namespace grit {
+
+/// \ingroup grit-audio-filter
+enum struct StateVariableFilterType
+{
+    Highpass,
+    Bandpass,
+    Lowpass,
+    Notch,
+    Peak,
+    Allpass,
+};
+
+/// \brief State variable filter
+/// \details https://cytomic.com/files/dsp/SvfLinearTrapAllOutputs.pdf
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float, StateVariableFilterType Type>
+struct StateVariableFilter
+{
+    using SampleType = Float;
+
+    struct Parameter
+    {
+        Float cutoff    = Float(440);
+        Float resonance = Float(1) / etl::sqrt(Float(2));
+    };
+
+    StateVariableFilter() = default;
+
+    auto setParameter(Parameter const& parameter) -> void;
+    auto setSampleRate(Float sampleRate) -> void;
+    auto operator()(Float input) -> Float;
+    auto reset() -> void;
+
+private:
+    auto update() -> void;
+
+    Parameter _parameter{};
+    Float _sampleRate{0};
+
+    Float _g{0};
+    Float _k{0};
+    Float _gt0{0};
+    Float _gk0{0};
+
+    Float _ic1eq{0};
+    Float _ic2eq{0};
+};
+
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float>
+using StateVariableHighpass = StateVariableFilter<Float, StateVariableFilterType::Highpass>;
+
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float>
+using StateVariableBandpass = StateVariableFilter<Float, StateVariableFilterType::Bandpass>;
+
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float>
+using StateVariableLowpass = StateVariableFilter<Float, StateVariableFilterType::Lowpass>;
+
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float>
+using StateVariableNotch = StateVariableFilter<Float, StateVariableFilterType::Notch>;
+
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float>
+using StateVariablePeak = StateVariableFilter<Float, StateVariableFilterType::Peak>;
+
+/// \ingroup grit-audio-filter
+template<etl::floating_point Float>
+using StateVariableAllpass = StateVariableFilter<Float, StateVariableFilterType::Allpass>;
+
+template<etl::floating_point Float, StateVariableFilterType Type>
+auto StateVariableFilter<Float, Type>::setParameter(Parameter const& parameter) -> void
+{
+    _parameter = parameter;
+    update();
+}
+
+template<etl::floating_point Float, StateVariableFilterType Type>
+auto StateVariableFilter<Float, Type>::setSampleRate(Float sampleRate) -> void
+{
+    _sampleRate = sampleRate;
+    update();
+    reset();
+}
+
+template<etl::floating_point Float, StateVariableFilterType Type>
+auto StateVariableFilter<Float, Type>::operator()(Float x) -> Float
+{
+    auto const t0 = x - _ic2eq;
+    auto const v0 = _gt0 * t0 - _gk0 * _ic1eq;
+    auto const t1 = _g * v0;
+    auto const v1 = _ic1eq + t1;
+    auto const t2 = _g * v1;
+    auto const v2 = _ic2eq + t2;
+
+    _ic1eq = v1 + t1;
+    _ic2eq = v2 + t2;
+
+    if constexpr (Type == StateVariableFilterType::Highpass) {
+        return v0;
+    } else if constexpr (Type == StateVariableFilterType::Bandpass) {
+        return v1;
+    } else if constexpr (Type == StateVariableFilterType::Lowpass) {
+        return v2;
+    } else if constexpr (Type == StateVariableFilterType::Notch) {
+        return v0 + v2;
+    } else if constexpr (Type == StateVariableFilterType::Peak) {
+        return v0 - v2;
+    } else if constexpr (Type == StateVariableFilterType::Allpass) {
+        return v0 - _k * v1 + v2;
+    } else {
+        static_assert(etl::always_false<decltype(Type)>);
+    }
+}
+
+template<etl::floating_point Float, StateVariableFilterType Type>
+auto StateVariableFilter<Float, Type>::reset() -> void
+{
+    _ic1eq = Float(0);
+    _ic2eq = Float(0);
+}
+
+template<etl::floating_point Float, StateVariableFilterType Type>
+auto StateVariableFilter<Float, Type>::update() -> void
+{
+    auto w = static_cast<Float>(etl::numbers::pi) * _parameter.cutoff / _sampleRate;
+    _g     = etl::tan(w);
+    _k     = 1 / _parameter.resonance;
+
+    auto gk = _g + _k;
+    _gt0    = 1 / (1 + _g * gk);
+    _gk0    = gk * _gt0;
+}
+
+}  // namespace grit

lib/grit/audio/filter/state_variable_filter_test.cpp

@@ -0,0 +1,45 @@
+#include "state_variable_filter.hpp"
+
+#include <etl/random.hpp>
+
+#include <catch2/catch_get_random_seed.hpp>
+#include <catch2/catch_template_test_macros.hpp>
+#include <catch2/generators/catch_generators.hpp>
+
+TEMPLATE_PRODUCT_TEST_CASE(
+    "audio/filter: StateVariableFilter",
+    "",
+    (grit::StateVariableHighpass,
+     grit::StateVariableBandpass,
+     grit::StateVariableLowpass,
+     grit::StateVariableNotch,
+     grit::StateVariablePeak,
+     grit::StateVariableAllpass),
+    (float, double)
+)
+{
+    using Filter = TestType;
+    using Float  = typename Filter::SampleType;
+    STATIC_REQUIRE(etl::same_as<Float, float> or etl::same_as<Float, double>);
+
+    auto rng  = etl::xoshiro128plusplus{Catch::getSeed()};
+    auto dist = etl::uniform_real_distribution<Float>{Float(-1), Float(1)};
+
+    auto const fs = GENERATE(Float(1), Float(24000), Float(48000), Float(96000));
+    auto filter   = Filter{};
+    filter.setSampleRate(fs);
+    filter.setParameter({
+        .cutoff    = Float(fs * 0.1),
+        .resonance = Float(1) / etl::sqrt(Float(2)),
+    });
+
+    for (auto i{0}; i < 10'000; ++i) {
+        auto const x = dist(rng);
+        auto const y = filter(x);
+
+        CAPTURE(i);
+        CAPTURE(x);
+        CAPTURE(y);
+        REQUIRE(etl::isfinite(y));
+    }
+}