140 lines
3.9 KiB
C++
140 lines
3.9 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;
|
|
|
|
TEMPLATE
|
|
struct svf_matrix {
|
|
T a, b, c, d, memory;
|
|
};
|
|
|
|
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;
|
|
}
|
|
|
|
TEMPLATE static svf_matrix<T>
|
|
svf_gen_matrix(svf s)
|
|
{
|
|
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.A1[0]*s.C[2];
|
|
CA[1] = s.A0[1]*s.C[1] + s.A1[1]*s.C[2];
|
|
|
|
cb = s.C[1]*s.B[0] + s.C[2]*s.B[1];
|
|
|
|
svf_matrix<T> mat;
|
|
mat.memory = T(0, 0, 0, 0);
|
|
mat.a = T(s.C[0], 0, s.C[1], s.C[2]);
|
|
mat.b = T( cb, s.C[0], CA[0], CA[1]);
|
|
mat.c = T( AB[0], s.B[0], AA0[0], AA0[1]);
|
|
mat.d = T( AB[1], s.B[1], AA1[0], AA1[1]);
|
|
return mat;
|
|
}
|
|
|
|
TEMPLATE INNER void
|
|
svf_run_block_mat(svf_matrix<T> *RESTRICT mat, T *RESTRICT buf, ulong count)
|
|
{
|
|
T t1, t2, t3, t4;
|
|
T memory = mat->memory;
|
|
for (ulong i = 0; i < count/2; i++) {
|
|
memory[0] = buf[i][0];
|
|
memory[1] = buf[i][1];
|
|
t1 = mat->a*memory;
|
|
t2 = mat->b*memory;
|
|
t3 = mat->c*memory;
|
|
t4 = mat->d*memory;
|
|
memory[0] = t1[0] + t1[1] + t1[2] + t1[3];
|
|
memory[1] = t2[0] + t2[1] + t2[2] + t2[3];
|
|
memory[2] = t3[0] + t3[1] + t3[2] + t3[3];
|
|
memory[3] = t4[0] + t4[1] + t4[2] + t4[3];
|
|
buf[i][0] = memory[0];
|
|
buf[i][1] = memory[1];
|
|
}
|
|
mat->memory = memory;
|
|
}
|