0d4560cb03
git-svn-id: svn://svn.rockbox.org/rockbox/trunk@21626 a1c6a512-1295-4272-9138-f99709370657
135 lines
3.6 KiB
C
135 lines
3.6 KiB
C
#include <m_pd.h>
|
|
#include <m_fixed.h>
|
|
|
|
typedef struct biquadctl
|
|
{
|
|
t_sample c_x1;
|
|
t_sample c_x2;
|
|
t_sample c_fb1;
|
|
t_sample c_fb2;
|
|
t_sample c_ff1;
|
|
t_sample c_ff2;
|
|
t_sample c_ff3;
|
|
} t_biquadctl;
|
|
|
|
typedef struct sigbiquad
|
|
{
|
|
t_object x_obj;
|
|
float x_f;
|
|
t_biquadctl x_cspace;
|
|
t_biquadctl *x_ctl;
|
|
} t_sigbiquad;
|
|
|
|
t_class *sigbiquad_class;
|
|
|
|
static void sigbiquad_list(t_sigbiquad *x, t_symbol *s, int argc, t_atom *argv);
|
|
|
|
static void *sigbiquad_new(t_symbol *s, int argc, t_atom *argv)
|
|
{
|
|
t_sigbiquad *x = (t_sigbiquad *)pd_new(sigbiquad_class);
|
|
outlet_new(&x->x_obj, gensym("signal"));
|
|
x->x_ctl = &x->x_cspace;
|
|
x->x_cspace.c_x1 = x->x_cspace.c_x2 = 0;
|
|
sigbiquad_list(x, s, argc, argv);
|
|
x->x_f = 0;
|
|
return (x);
|
|
}
|
|
|
|
static t_int *sigbiquad_perform(t_int *w)
|
|
{
|
|
t_sample *in = (t_sample *)(w[1]);
|
|
t_sample *out = (t_sample *)(w[2]);
|
|
t_biquadctl *c = (t_biquadctl *)(w[3]);
|
|
int n = (t_int)(w[4]);
|
|
int i;
|
|
t_sample last = c->c_x1;
|
|
t_sample prev = c->c_x2;
|
|
t_sample fb1 = c->c_fb1;
|
|
t_sample fb2 = c->c_fb2;
|
|
t_sample ff1 = c->c_ff1;
|
|
t_sample ff2 = c->c_ff2;
|
|
t_sample ff3 = c->c_ff3;
|
|
for (i = 0; i < n; i++)
|
|
{
|
|
t_sample output = *in++ + mult(fb1,last) + mult(fb2,prev);
|
|
if (PD_BADFLOAT(output))
|
|
output = 0;
|
|
*out++ = mult(ff1,output) + mult(ff2,last) + mult(ff3,prev);
|
|
prev = last;
|
|
last = output;
|
|
}
|
|
c->c_x1 = last;
|
|
c->c_x2 = prev;
|
|
return (w+5);
|
|
}
|
|
|
|
static void sigbiquad_list(t_sigbiquad *x, t_symbol *s, int argc, t_atom *argv)
|
|
{
|
|
#ifdef ROCKBOX
|
|
(void) s;
|
|
#endif
|
|
|
|
float fb1 = atom_getfloatarg(0, argc, argv);
|
|
float fb2 = atom_getfloatarg(1, argc, argv);
|
|
float ff1 = atom_getfloatarg(2, argc, argv);
|
|
float ff2 = atom_getfloatarg(3, argc, argv);
|
|
float ff3 = atom_getfloatarg(4, argc, argv);
|
|
float discriminant = fb1 * fb1 + 4 * fb2;
|
|
|
|
t_biquadctl *c = x->x_ctl;
|
|
if (discriminant < 0) /* imaginary roots -- resonant filter */
|
|
{
|
|
/* they're conjugates so we just check that the product
|
|
is less than one */
|
|
if (fb2 >= -1.0f) goto stable;
|
|
}
|
|
else /* real roots */
|
|
{
|
|
/* check that the parabola 1 - fb1 x - fb2 x^2 has a
|
|
vertex between -1 and 1, and that it's nonnegative
|
|
at both ends, which implies both roots are in [1-,1]. */
|
|
if (fb1 <= 2.0f && fb1 >= -2.0f &&
|
|
1.0f - fb1 -fb2 >= 0 && 1.0f + fb1 - fb2 >= 0)
|
|
goto stable;
|
|
}
|
|
/* if unstable, just bash to zero */
|
|
fb1 = fb2 = ff1 = ff2 = ff3 = 0;
|
|
stable:
|
|
c->c_fb1 = ftofix(fb1);
|
|
c->c_fb2 = ftofix(fb2);
|
|
c->c_ff1 = ftofix(ff1);
|
|
c->c_ff2 = ftofix(ff2);
|
|
c->c_ff3 = ftofix(ff3);
|
|
}
|
|
|
|
static void sigbiquad_set(t_sigbiquad *x, t_symbol *s, int argc, t_atom *argv)
|
|
{
|
|
#ifdef ROCKBOX
|
|
(void) s;
|
|
#endif
|
|
t_biquadctl *c = x->x_ctl;
|
|
c->c_x1 = atom_getfloatarg(0, argc, argv);
|
|
c->c_x2 = atom_getfloatarg(1, argc, argv);
|
|
}
|
|
|
|
static void sigbiquad_dsp(t_sigbiquad *x, t_signal **sp)
|
|
{
|
|
dsp_add(sigbiquad_perform, 4,
|
|
sp[0]->s_vec, sp[1]->s_vec,
|
|
x->x_ctl, sp[0]->s_n);
|
|
|
|
}
|
|
|
|
void biquad_tilde_setup(void)
|
|
{
|
|
sigbiquad_class = class_new(gensym("biquad~"), (t_newmethod)sigbiquad_new,
|
|
0, sizeof(t_sigbiquad), 0, A_GIMME, 0);
|
|
CLASS_MAINSIGNALIN(sigbiquad_class, t_sigbiquad, x_f);
|
|
class_addmethod(sigbiquad_class, (t_method)sigbiquad_dsp, gensym("dsp"), 0);
|
|
class_addlist(sigbiquad_class, sigbiquad_list);
|
|
class_addmethod(sigbiquad_class, (t_method)sigbiquad_set, gensym("set"),
|
|
A_GIMME, 0);
|
|
class_addmethod(sigbiquad_class, (t_method)sigbiquad_set, gensym("clear"),
|
|
A_GIMME, 0);
|
|
}
|
|
|