1 files changed, 80 insertions, 0 deletions
diff --git a/src/biquad.cxx b/src/biquad.cxx
new file mode 100644
index 0000000..372b89e
--- /dev/null
+++ b/src/biquad.cxx
@@ -0,0 +1,80 @@
+#include <iostream>
+#include "biquad.h"
+#include "bad_thing.h"
+using namespace std;
+
+// gory-detail constructor
+biquad::biquad(vector<double> const _c, vector<double> const _d,
+                vector<C> _zeros, vector<C> _poles)
+: c(_c), d(_d), zeros(_zeros), poles(_poles),
+  ws1(0), ws2(0)
+{
+    if (d[0] != 1.) throw bad_thing("biquad: d0 must be 1.");
+}
+
+// constructor in terms of zero and pole position,
+// assuming conjugate pairs:
+biquad::biquad(C const zero, C const pole)
+///// nume = (z-zero)(z-zerobar)
+/////      = z^2 - 2 zero.real + |zero|^2
+: biquad({1., -2.*zero.real(), norm(zero)},
+         {1., -2.*pole.real(), norm(pole)},
+         {zero, conj(zero)},
+         {pole, conj(pole)})
+{}
+
+C constexpr biquad::C0;
+C constexpr biquad::C1;
+
+vector<C> solve(vector<double> const poly) {
+    double disc(1 - 4. * poly[0] * poly[2] / poly[1] / poly[1]);
+    C inner(biquad::C1 + sqrt(C(disc, 0)));
+    C root1 = C(-poly[1]/2/poly[0], 0) * inner;
+    return { root1,
+             C(poly[2]/poly[0], 0) / root1 };
+}
+
+// constructor in terms of coefficients:
+biquad::biquad(vector<double> const _c, vector<double> const _d)
+: biquad(_c, _d,
+       {C0, C0}, {C0, C0})
+{
+  zeros = solve(c);
+  poles = solve(d);
+}
+
+double biquad::step(double const Vin){
+    double q1(ws1);
+    double q2(ws2);
+    double Vout = c[0]*Vin + q1;
+    ws1 = c[1]*Vin - d[1]*Vout + q2;
+    ws2 = c[2]*Vin - d[2]*Vout;
+    return Vout;
+}
+
+// transfer function
+C biquad::xfunc(C const z) const {
+    C nume = (c[0]*z + c[1])*z + c[2];
+    C denom =     (z + d[1])*z + d[2];
+    return nume / denom;
+}
+
+void biquad::please_normalize(C const z) {
+    double denom(abs(xfunc(z)));
+//    cout << "denom: " << denom << endl;
+//    cout << "c2: " << c[2] << endl;
+//    cout << "z: " << z << endl;
+    for (auto &coeff : c) {
+      coeff /= denom;
+    }
+}
+
+// same as above, but with some error checking:
+
+void biquad::normalize(C const z) {
+  if (abs(abs(z) - 1.) > 1e-10) {
+//
+    throw bad_thing("normalizing to z not on unit circle");
+  }
+  please_normalize(z);
+}