#define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 31))
/* everything is s3.28 fixed point unless otherwise noted */
struct svf {
long f, q;
long lp, hp, bp;
};
/* cutoff is cutoff frequency in hz (integer).
res is s3.28 fixed point number from 0 to 1, where 0 means no resonence
(Q = 0.5), and 1 means self oscillation (Q = inf). formula for converting
Q to res parameter is 1/Q = 2*(1 - res) -> res = 1 - 1/(2*Q).
beware that filter is unstable and inaccurate at high (above and around 6khz,
at 44.1khz sample rate) frequencies. */
void setcoefs(long cutoff, long res, long sfreq, struct svf *filter)
{
const long one = 1 << 28;
const long twopi = 1686629713;
long w = 0x7fffffff/sfreq*cutoff; /* s0.31 */
filter->f = FRACMUL(twopi, w);
filter->q = 2*(one - res);
}
void dofilter(long *in, int num, struct svf *f)
{
int i;
for (i = 0; i < num; ++i) {
f->lp += FRACMUL(f->f, f->bp) << 3;
f->hp = in[i] - f->lp - (FRACMUL(f->q, f->bp) << 3);
f->bp += FRACMUL(f->f, f->hp) << 3;
in[i] = f->lp; /* or whatever filter you want */
}
}