From b9ae12167da33dd7779f8fabe5d212cbb9962531 Mon Sep 17 00:00:00 2001
From: yaw-man <ywmn@proton.me>
Date: Thu, 22 Sep 2022 16:54:51 -0500
Subject: [PATCH] Changed filter design to specify bandwidths instead. Using
 partial fractions to obtain better gain control / less quantization noise
 (hopefully!)

---
 src/yaw-vowel/filter.cpp | 82 ++++++++++++++++++++++++++++------------
 src/yaw-vowel/filter.h   | 36 +++++++++++++-----
 2 files changed, 84 insertions(+), 34 deletions(-)

diff --git a/src/yaw-vowel/filter.cpp b/src/yaw-vowel/filter.cpp
index 32d8a98..30b276e 100644
--- a/src/yaw-vowel/filter.cpp
+++ b/src/yaw-vowel/filter.cpp
@@ -7,9 +7,7 @@ void Resonator::clear()
 
 inline float Resonator::process(float x)
 {
-  // Apply biquad filter.
-  float y = scale * x - xpp - app * ypp + ap * yp;
-  // Update biquad memory.
+  float y = cx * x + cxp * xp - cypp * ypp - cyp * yp;
   xpp = xp;
   xp = x;
   ypp = yp;
@@ -17,16 +15,18 @@ inline float Resonator::process(float x)
   return y;
 }
 
-void Resonator::set(double _app, double _ap, double _scale)
+void Resonator::set(double _cx, double _cxp, double _cyp, double _cypp)
 {
-  app = _app;
-  ap = _ap;
-  scale = _scale;
+  cx = _cx;
+  cxp = _cxp;
+  cyp = _cyp;
+  cypp = _cypp;
 }
 
-Filter::Filter(){};
+static constexpr double sampleRate = 48000.0;
 
-void Filter::process(float **outputs, const float **inputs, uint32_t frames)
+template <uint N>
+void ParallelResonators<N>::process(float **outputs, const float **inputs, uint32_t frames)
 {
   for (uint chn = 0; chn < 2; ++chn)
   {
@@ -38,22 +38,56 @@ void Filter::process(float **outputs, const float **inputs, uint32_t frames)
       }
     }
   }
-}
+};
 
-// Compute filter coefficients from
-// frequency as a proportion of the sample rate
-// REMEMBER: THAT'S DOUBLE THE Nyquist RATE, dummy
-// and resonance in [0, 1]
-void Filter::set(double x, double y, double p)
+template <uint N>
+void set(std::array<double, N> &angle, std::array<double, N> &radius)
 {
-  double r = p * 0.049 + 0.95; // Filter unstable at r = 1.
-  double app = r * r;
-  double xap = (1.0 + r * r) * cos(M_PI * x);
-  double yap = (1.0 + r * r) * cos(M_PI * y);
-  double scale = sqrt(0.5 - 0.5 * r * r);
-  for (auto &channelFilter : res)
+
+  //Get poles.
+  for ( uint i = 0; i < N; ++i )
   {
-    channelFilter[0].set(app, xap, scale);
-    channelFilter[1].set(app, yap, scale);
+    radius[i] = exp(-M_PI * radius[i] / sampleRate);
+    angle[i] = M_PI * 2.0 * angle[i] / sampleRate ;
   }
-}
\ No newline at end of file
+
+  //Partial fraction expansion.
+  //Compute residue of each pole, then combine with complex conjugate
+  //to form a real second-order section.
+  //Pass this to the associated filter.
+  for ( uint i = 0; i < N; ++i )
+  {
+
+    //Residue
+    double x = 0.0;
+    double y = 0.0;
+    for (uint j = 0; j < N; ++j)
+    {
+      if ( i == j ) continue;
+
+      // 1 - p_j / p_i
+      double re = 1.0 - radius[j] * cos(angle[j] - angle[i]) / radius[i];
+      double im = 0.0 - radius[j] * sin(angle[j] - angle[i]) / radius[i];
+
+      // (x + iy) * (re + i im) = re * x - y * im
+      x = re * x - y * im;
+      y = im * x + y * re;
+
+    }
+
+    //Pole
+    double u = radius[i] * cos(angle[i]);
+    double v = radius[i] * sin(angle[i]);
+
+    //Multiply complex conjugate pairs to get real biquad coefficients.
+    double cxp = -2.0 * (u * x + v * y);
+    double cx = 2.0 * x;
+    double cypp = radius[i] * radius[i];
+    double cyp = -2.0 * u;
+
+    //Save biquad coefs.
+    res[0][i].set(cx, cxp, cyp, cypp);
+    res[1][i].set(cx, cxp, cyp, cypp);
+  }
+
+};
\ No newline at end of file
diff --git a/src/yaw-vowel/filter.h b/src/yaw-vowel/filter.h
index 23395cb..3ec39cc 100644
--- a/src/yaw-vowel/filter.h
+++ b/src/yaw-vowel/filter.h
@@ -5,21 +5,21 @@
 class Resonator
 {
 public:
-    Resonator():
-    xp(0), xpp(0), yp(0), ypp(0), app(0), ap(0), scale(1){};
 
     inline float process(float x);
     void clear();
-    void set(double _app, double _ap, double _scale);
+    void set(double, double, double, double);
 
 private:
-    float xp;
-    float xpp;
-    float yp;
-    float ypp;
-    double app;
-    double ap;
-    double scale;
+    float xp = 0.0;
+    float xpp = 0.0;
+    float yp = 0.0;
+    float ypp = 0.0;
+
+    double cypp = 0.0;
+    double cyp = 0.0;
+    double cxp = 0.0;
+    double cx = 1.0;
 };
 
 class Filter
@@ -31,4 +31,20 @@ public:
 
 private:
     std::array<std::array<Resonator, 2>, 2> res;
+};
+
+
+//Uncapped resonators
+//Implemented in parallel
+//Use partial fraction expansion to ensure constant numerator.
+template< uint N >
+class ParallelResonators
+{
+public:
+    void process(float **outputs, const float **inputs, uint32_t frames);
+    void set(std::array<double, N> &hz, std::array<double, N> &bw);
+
+    private:
+    double scale;
+    std::array<std::array<Resonator, N>, 2> res{};
 };
\ No newline at end of file