Add BiquadCoefficients::makeLowPass

gritwave · Feb 24, 2024 · e40d9bd · e40d9bd
1 parent 1360490
commit e40d9bd
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 0 deletions.
diff --git a/lib/grit/audio/filter/biquad.hpp b/lib/grit/audio/filter/biquad.hpp
@@ -2,7 +2,9 @@
 
 #include <etl/algorithm.hpp>
 #include <etl/array.hpp>
+#include <etl/cmath.hpp>
 #include <etl/concepts.hpp>
+#include <etl/numbers.hpp>
 #include <etl/span.hpp>
 
 namespace grit {
@@ -20,6 +22,26 @@ struct BiquadCoefficients
     {
         return {Float(1), Float(0), Float(0), Float(1), Float(0), Float(0)};
     }
+
+    [[nodiscard]] static constexpr auto makeLowPass(Float f0, Float q, Float fs) -> etl::array<Float, 6>
+    {
+        auto const omega0 = Float(2) * static_cast<Float>(etl::numbers::pi) * f0 / fs;
+        auto const d      = Float(1) / q;
+        auto const cos0   = etl::cos(omega0);
+        auto const sin0   = etl::sin(omega0);
+        auto const beta   = Float(0.5) * ((Float(1) - (d * Float(0.5)) * sin0) / (Float(1) + (d * Float(0.5)) * sin0));
+        auto const gamma  = (Float(0.5) + beta) * cos0;
+
+        auto const b0 = (Float(0.5) + beta - gamma) * Float(0.5);
+        auto const b1 = Float(0.5) + beta - gamma;
+        auto const b2 = b0;
+
+        auto const a0 = Float(1);
+        auto const a1 = Float(-2) * gamma;
+        auto const a2 = Float(2) * beta;
+
+        return {b0, b1, b2, a0, a1, a2};
+    }
 };
 
 /// \brief 2nd order IIR filter using the transpose direct form 2 structure.

diff --git a/lib/grit/audio/filter/biquad_test.cpp b/lib/grit/audio/filter/biquad_test.cpp
@@ -4,6 +4,7 @@
 
 #include <catch2/catch_get_random_seed.hpp>
 #include <catch2/catch_template_test_macros.hpp>
+#include <catch2/generators/catch_generators.hpp>
 #include <catch2/matchers/catch_matchers_floating_point.hpp>
 
 TEMPLATE_TEST_CASE("audio/filter: BiquadCoefficients::makeBypass", "", float, double)
@@ -22,6 +23,23 @@ TEMPLATE_TEST_CASE("audio/filter: BiquadCoefficients::makeBypass", "", float, do
     REQUIRE_THAT(bypass[5], Catch::Matchers::WithinAbs(0.0, 1e-6));
 }
 
+TEMPLATE_TEST_CASE("audio/filter: BiquadCoefficients::makeLowPass", "", float, double)
+{
+    using Float        = TestType;
+    using Coefficients = grit::BiquadCoefficients<Float>;
+
+    auto fs = GENERATE(Float(1.0), Float(22050), Float(44100), Float(192000));
+
+    auto const q  = Float(1) / etl::sqrt(Float(2));
+    auto const lp = Coefficients::makeLowPass(Float(0.01 * fs), q, Float(1 * fs));
+    REQUIRE_THAT(lp[0], Catch::Matchers::WithinAbs(0.0009446918438401619, 1e-6));
+    REQUIRE_THAT(lp[1], Catch::Matchers::WithinAbs(0.0018893836876803238, 1e-6));
+    REQUIRE_THAT(lp[2], Catch::Matchers::WithinAbs(0.0009446918438401619, 1e-6));
+    REQUIRE_THAT(lp[3], Catch::Matchers::WithinAbs(1.0, 1e-6));
+    REQUIRE_THAT(lp[4], Catch::Matchers::WithinAbs(-1.9111970674260732, 1e-6));
+    REQUIRE_THAT(lp[5], Catch::Matchers::WithinAbs(0.9149758348014339, 1e-6));
+}
+
 TEMPLATE_TEST_CASE("audio/filter: BiquadTDF2", "", float, double)
 {
     using Float  = TestType;