127 lines
3.7 KiB
C
127 lines
3.7 KiB
C
/* via http://nbviewer.ipython.org/urls/music-synthesizer-for-android.googlecode.com/git/lab/Second%20order%20sections%20in%20matrix%20form.ipynb */
|
|
|
|
typedef struct {
|
|
float A0[2], A1[2], B[2], C[3], memory[2];
|
|
} svf;
|
|
|
|
typedef struct {
|
|
double A0[2], A1[2], B[2], C[3];
|
|
} svf_interim;
|
|
|
|
typedef struct {
|
|
v4sf a, b, c, d, memory;
|
|
} svf_matrix;
|
|
|
|
static svf_interim
|
|
svf_design(double w0, double Q, double c0, double c1, double c2, double gc)
|
|
{
|
|
svf_interim svfi;
|
|
double g = tan(0.5*w0)*gc;
|
|
double a1 = 1/(1 + g*(g + 1/Q));
|
|
double a2 = g*a1;
|
|
double a3 = g*a2;
|
|
svfi.A0[0] = 2*a1 - 1;
|
|
svfi.A0[1] = -2*a2;
|
|
svfi.A1[0] = 2*a2;
|
|
svfi.A1[1] = 1 - 2*a3;
|
|
svfi.B[0] = 2*a2;
|
|
svfi.B[1] = 2*a3;
|
|
double v0[3] = {1, 0, 0};
|
|
double v1[3] = {a2, a1, -a2};
|
|
double v2[3] = {a3, a2, 1 - a3};
|
|
svfi.C[0] = v0[0]*c0 + v1[0]*c1 + v2[0]*c2;
|
|
svfi.C[1] = v0[1]*c0 + v1[1]*c1 + v2[1]*c2;
|
|
svfi.C[2] = v0[2]*c0 + v1[2]*c1 + v2[2]*c2;
|
|
return svfi;
|
|
}
|
|
|
|
static svf
|
|
svf_gen_raw(filter_t type, double w0, double A, double Q)
|
|
{
|
|
double As = sqrt(A);
|
|
|
|
svf_interim svfi;
|
|
|
|
#define d(Q,c0,c1,c2,gc) svfi = svf_design(w0,Q,c0,c1,c2,gc)
|
|
switch (type) {
|
|
case FILT_PEAKING: d(Q*A, 1, (A*A - 1)/A/Q, 0, 1); break;
|
|
case FILT_LOWSHELF: d(Q, 1, (A - 1)/Q, A*A - 1, 1/As); break;
|
|
case FILT_HIGHSHELF: d(Q, A*A, (1 - A)*A/Q, 1 - A*A, As); break;
|
|
case FILT_LOWPASS: d(Q, 0, 0, 1, 1); break;
|
|
case FILT_HIGHPASS: d(Q, 1, -1/Q, -1, 1); break;
|
|
case FILT_ALLPASS: d(Q, 0, 0, 0, 1); break; // TODO: implement
|
|
case FILT_BANDPASS: d(Q, 0, 1, 0, 1); break; // FIXME: no gain
|
|
case FILT_BANDPASS_2: d(Q, 0, 0, 0, 1); break; // TODO: implement
|
|
case FILT_NOTCH: d(Q, 1, -1/Q, 0, 1); break;
|
|
case FILT_GAIN: d(Q, 0, 0, 0, 1); break; // TODO: implement
|
|
}
|
|
#undef d
|
|
|
|
svf s;
|
|
s.A0[0] = svfi.A0[0];
|
|
s.A0[1] = svfi.A0[1];
|
|
s.A1[0] = svfi.A1[0];
|
|
s.A1[1] = svfi.A1[1];
|
|
s.B[0] = svfi.B[0];
|
|
s.B[1] = svfi.B[1];
|
|
s.C[0] = svfi.C[0];
|
|
s.C[1] = svfi.C[1];
|
|
s.C[2] = svfi.C[2];
|
|
s.memory[0] = 0;
|
|
s.memory[1] = 0;
|
|
return s;
|
|
}
|
|
|
|
static svf
|
|
svf_gen(filter_t type, double fc, double gain, double bw, double fs)
|
|
{
|
|
double w0 = ANGULAR_LIM(fc, fs);
|
|
double A = DB2LIN(gain/2);
|
|
//double Q = M_SQRT1_2/bw;
|
|
double Q = M_SQRT1_2*(1 - (w0/M_PI)*(w0/M_PI))/bw;
|
|
return svf_gen_raw(type, w0, A, Q);
|
|
}
|
|
|
|
INNER float
|
|
svf_run(svf *s, float x)
|
|
{
|
|
float y = s->C[0]*x + s->C[1]*s->memory[0] + s->C[2]*s->memory[1];
|
|
float temp = s->memory[0];
|
|
s->memory[0] = s->B[0]*x + s->A0[0]*temp + s->A0[1]*s->memory[1];
|
|
s->memory[1] = s->B[1]*x + s->A1[0]*temp + s->A1[1]*s->memory[1];
|
|
return y;
|
|
}
|
|
|
|
static svf_matrix
|
|
svf_gen_matrix(svf s)
|
|
{
|
|
// note: does not copy memory
|
|
/*
|
|
AA = dot(A, A);
|
|
AB = dot(A, B);
|
|
CA = dot(C[1:], A);
|
|
cb = dot(C[1:], B);
|
|
return [[ C[0], 0, C[1], C[2]],
|
|
[ cb, C[0], CA[0], CA[1]],
|
|
[AB[0], B[0], AA[0][0], AA[0][1]],
|
|
[AB[1], B[1], AA[1][0], AA[1][1]]];
|
|
*/
|
|
float AA0[2], AA1[2], AB[2], CA[2], cb;
|
|
AA0[0] = s.A0[0]*s.A0[0] + s.A0[1]*s.A1[0];
|
|
AA1[0] = s.A1[0]*s.A0[0] + s.A1[1]*s.A1[0];
|
|
AA0[1] = s.A0[0]*s.A0[1] + s.A0[1]*s.A1[1];
|
|
AA1[1] = s.A1[0]*s.A0[1] + s.A1[1]*s.A1[1];
|
|
AB[0] = s.A0[0]*s.B[0] + s.A0[1]*s.B[1];
|
|
AB[1] = s.A1[0]*s.B[0] + s.A1[1]*s.B[1];
|
|
CA[0] = s.A0[0]*s.C[1] + s.A0[1]*s.C[2];
|
|
CA[1] = s.A1[0]*s.C[1] + s.A1[1]*s.C[2];
|
|
cb = s.C[1]*s.B[0] + s.C[2]*s.B[1];
|
|
|
|
svf_matrix mat;
|
|
mat.memory = (v4sf){0, 0, 0, 0};
|
|
mat.a = (v4sf){s.C[0], 0, s.C[1], s.C[2]};
|
|
mat.b = (v4sf){ cb, s.C[0], CA[0], CA[1]};
|
|
mat.c = (v4sf){ AB[0], s.B[0], AA0[0], AA0[1]};
|
|
mat.d = (v4sf){ AB[1], s.B[1], AA1[0], AA1[1]};
|
|
return mat;
|
|
}
|