Add butterworth low & high pass filters

libs/audio/include/psemek/audio/effect/butterworth.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <psemek/audio/stream.hpp>
+
+namespace psemek::audio
+{
+
+	// Second-order
+	stream_ptr butterworth_low_pass(stream_ptr stream, float cutoff_frequency);
+	stream_ptr butterworth_high_pass(stream_ptr stream, float cutoff_frequency);
+
+}

libs/audio/include/psemek/audio/effect/filter.hpp

@@ -1,15 +1,14 @@
 #pragma once
 
 #include <psemek/audio/stream.hpp>
+#include <psemek/util/array.hpp>
 
 namespace psemek::audio
 {
 
-	// Turns x[n] into y[n] = a0*x[n] + a1*x[n-1]
-	stream_ptr feedforward_filter(stream_ptr stream, float a0, float a1);
-
-	// Turns x[n] into y[n] = a0*x[n] + b1*y[n-1]
-	stream_ptr feedback_filter(stream_ptr stream, float a0, float b1);
+	// Turns x[n] into y[n] = b0*x[n] + b1*x[n-1] + ... - a1*y[n-1] - a2*y[n-2] - ...
+	// NB: a[0] is implicitly 1 and is omitted
+	stream_ptr linear_filter(stream_ptr stream, util::array<float> a, util::array<float> b);
 
 	stream_ptr low_pass_filter(stream_ptr stream);
 	stream_ptr high_pass_filter(stream_ptr stream);

libs/audio/source/butterworth.cpp

@@ -0,0 +1,59 @@
+#include <psemek/audio/effect/butterworth.hpp>
+#include <psemek/audio/effect/filter.hpp>
+#include <psemek/audio/constants.hpp>
+#include <psemek/math/constants.hpp>
+
+namespace psemek::audio
+{
+
+	stream_ptr butterworth_low_pass(stream_ptr stream, float cutoff_frequency)
+	{
+		float omega = 2.f * static_cast<float>(math::pi) * cutoff_frequency * audio::inv_frequency;
+		omega = 2.f * audio::frequency * std::tan(omega / 2.f);
+
+		float gamma = 2.f * audio::frequency / omega;
+		float sqrt2 = std::sqrt(2.f);
+
+		float a0 = gamma * gamma + sqrt2 * gamma + 1.f;
+		float a1 = - 2.f * gamma * gamma + 2.f;
+		float a2 = gamma * gamma - sqrt2 * gamma + 1.f;
+
+		float b0 = 1.f;
+		float b1 = 2.f;
+		float b2 = 1.f;
+
+		a1 /= a0;
+		a2 /= a0;
+		b0 /= a0;
+		b1 /= a0;
+		b2 /= a0;
+
+		return linear_filter(std::move(stream), {a1, a2}, {b0, b1, b2});
+	}
+
+	stream_ptr butterworth_high_pass(stream_ptr stream, float cutoff_frequency)
+	{
+		float omega = 2.f * static_cast<float>(math::pi) * (audio::frequency / 2.f - cutoff_frequency) * audio::inv_frequency;
+		omega = 2.f * audio::frequency * std::tan(omega / 2.f);
+
+		float gamma = 2.f * audio::frequency / omega;
+		float sqrt2 = std::sqrt(2.f);
+
+		float a0 = gamma * gamma + sqrt2 * gamma + 1.f;
+		float a1 = 2.f * gamma * gamma - 2.f;
+		float a2 = gamma * gamma - sqrt2 * gamma + 1.f;
+
+		float b0 = 1.f;
+		float b1 = - 2.f;
+		float b2 = 1.f;
+
+		a1 /= a0;
+		a2 /= a0;
+		b0 /= a0;
+		b1 /= a0;
+		b2 /= a0;
+
+		return linear_filter(std::move(stream), {a1, a2}, {b0, b1, b2});
+	}
+
+}

libs/audio/source/effect/filter.cpp

@@ -6,14 +6,19 @@ namespace psemek::audio
 	namespace
 	{
 
-		struct feedforward_filter_impl
+		struct linear_filter_impl
 			: stream
 		{
-			feedforward_filter_impl(stream_ptr stream, float a0, float a1)
+			linear_filter_impl(stream_ptr stream, util::array<float> a, util::array<float> b)
 				: stream_(std::move(stream))
-				, a0_(a0)
-				, a1_(a1)
-			{}
+				, a_(std::move(a))
+				, b_(std::move(b))
+			{
+				prev_in_[0].resize(b_.size(), 0.f);
+				prev_in_[1].resize(b_.size(), 0.f);
+				prev_out_[0].resize(a_.size(), 0.f);
+				prev_out_[1].resize(a_.size(), 0.f);
+			}
 
 			std::optional<std::size_t> length() const override
 			{
@@ -24,13 +29,32 @@ namespace psemek::audio
 			{
 				std::size_t count = stream_->read(samples);
 
+				// TODO: optimize? Maybe split into 3 loops (first N, main part, last N) to
+				// get rid of shift_right on each iteration
 				for (std::size_t i = 0; i < count; i += 2)
 				{
-					std::swap(prev_[0], samples[i + 0]);
-					std::swap(prev_[1], samples[i + 1]);
+					for (std::size_t s : {0, 1})
+					{
+						if (!prev_in_[s].empty())
+						{
+							std::shift_right(prev_in_[s].begin(), prev_in_[s].end(), 1);
+							prev_in_[s][0] = samples[i + s];
+						}
 
-					samples[i + 0] = prev_[0] * a0_ + samples[i + 0] * a1_;
-					samples[i + 1] = prev_[1] * a0_ + samples[i + 1] * a1_;
+						float value = 0.f;
+						for (std::size_t j = 0; j < b_.size(); ++j)
+							value += prev_in_[s][j] * b_[j];
+						for (std::size_t j = 0; j < a_.size(); ++j)
+							value -= prev_out_[s][j] * a_[j];
+
+						samples[i + s] = value;
+
+						if (!prev_out_[s].empty())
+						{
+							std::shift_right(prev_out_[s].begin(), prev_out_[s].end(), 1);
+							prev_out_[s][0] = value;
+						}
+					}
 				}
 
 				return count;
@@ -43,72 +67,27 @@ namespace psemek::audio
 
 		private:
 			stream_ptr stream_;
-			float a0_;
-			float a1_;
-			float prev_[2]{0.f};
-		};
-
-		struct feedback_filter_impl
-			: stream
-		{
-			feedback_filter_impl(stream_ptr stream, float a0, float b1)
-				: stream_(std::move(stream))
-				, a0_(a0)
-				, b1_(b1)
-			{}
-
-			std::optional<std::size_t> length() const override
-			{
-				return stream_->length();
-			}
-
-			std::size_t read(util::span<float> samples) override
-			{
-				std::size_t count = stream_->read(samples);
-
-				for (std::size_t i = 0; i < count; i += 2)
-				{
-					samples[i + 0] = prev_[0] * b1_ + samples[i + 0] * a0_;
-					samples[i + 1] = prev_[1] * b1_ + samples[i + 1] * a0_;
-					prev_[0] = samples[i + 0];
-					prev_[1] = samples[i + 1];
-				}
-
-				return count;
-			}
-
-			std::size_t played() const override
-			{
-				return stream_->played();
-			}
-
-		private:
-			stream_ptr stream_;
-			float a0_;
-			float b1_;
-			float prev_[2]{0.f};
+			util::array<float> a_;
+			util::array<float> b_;
+			util::array<float> prev_in_[2];
+			util::array<float> prev_out_[2];
 		};
 
 	}
 
-	stream_ptr feedforward_filter(stream_ptr stream, float a0, float a1)
+	stream_ptr linear_filter(stream_ptr stream, util::array<float> a, util::array<float> b)
 	{
-		return std::make_shared<feedforward_filter_impl>(std::move(stream), a0, a1);
-	}
-
-	stream_ptr feedback_filter(stream_ptr stream, float a0, float b1)
-	{
-		return std::make_shared<feedback_filter_impl>(std::move(stream), a0, b1);
+		return std::make_shared<linear_filter_impl>(std::move(stream), std::move(a), std::move(b));
 	}
 
 	stream_ptr low_pass_filter(stream_ptr stream)
 	{
-		return feedforward_filter(std::move(stream), 1.f, 1.f);
+		return linear_filter(std::move(stream), {}, {1.f, 1.f});
 	}
 
 	stream_ptr high_pass_filter(stream_ptr stream)
 	{
-		return feedforward_filter(std::move(stream), 1.f, -1.f);
+		return linear_filter(std::move(stream), {}, {1.f, -1.f});
 	}
 
 }