1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
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);
}
|
