apps/plugins/pdbox/PDa/src/d_filter.c

   1 /* Copyright (c) 1997-1999 Miller Puckette.
   2 * For information on usage and redistribution, and for a DISCLAIMER OF ALL
   3 * WARRANTIES, see the file, "LICENSE.txt," in this distribution.  */
   4
   5 /*  "filters", both linear and nonlinear.
   6 */
   7 #include "m_pd.h"
   8 #include <math.h>
   9
  10 /* ---------------- hip~ - 1-pole 1-zero hipass filter. ----------------- */
  11
  12 typedef struct hipctl
  13 {
  14     float c_x;
  15     float c_coef;
  16 } t_hipctl;
  17
  18 typedef struct sighip
  19 {
  20     t_object x_obj;
  21     float x_sr;
  22     float x_hz;
  23     t_hipctl x_cspace;
  24     t_hipctl *x_ctl;
  25     float x_f;
  26 } t_sighip;
  27
  28 t_class *sighip_class;
  29 static void sighip_ft1(t_sighip *x, t_floatarg f);
  30
  31 static void *sighip_new(t_floatarg f)
  32 {
  33     t_sighip *x = (t_sighip *)pd_new(sighip_class);
  34     inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"), gensym("ft1"));
  35     outlet_new(&x->x_obj, gensym("signal"));
  36     x->x_sr = 44100;
  37     x->x_ctl = &x->x_cspace;
  38     x->x_cspace.c_x = 0;
  39     sighip_ft1(x, f);
  40     x->x_f = 0;
  41     return (x);
  42 }
  43
  44 static void sighip_ft1(t_sighip *x, t_floatarg f)
  45 {
  46     if (f < 0) f = 0;
  47     x->x_hz = f;
  48     x->x_ctl->c_coef = 1 - f * (2 * 3.14159) / x->x_sr;
  49     if (x->x_ctl->c_coef < 0)
  50         x->x_ctl->c_coef = 0;
  51     else if (x->x_ctl->c_coef > 1)
  52         x->x_ctl->c_coef = 1;
  53 }
  54
  55 static t_int *sighip_perform(t_int *w)
  56 {
  57     float *in = (float *)(w[1]);
  58     float *out = (float *)(w[2]);
  59     t_hipctl *c = (t_hipctl *)(w[3]);
  60     int n = (t_int)(w[4]);
  61     int i;
  62     float last = c->c_x;
  63     float coef = c->c_coef;
  64     if (coef < 1)
  65     {
  66         for (i = 0; i < n; i++)
  67         {
  68             float new = *in++ + coef * last;
  69             *out++ = new - last;
  70             last = new;
  71         }
  72         if (PD_BIGORSMALL(last))
  73             last = 0;
  74         c->c_x = last;
  75     }
  76     else
  77     {
  78         for (i = 0; i < n; i++)
  79             *out++ = *in++;
  80         c->c_x = 0;
  81     }
  82     return (w+5);
  83 }
  84
  85 static void sighip_dsp(t_sighip *x, t_signal **sp)
  86 {
  87     x->x_sr = sp[0]->s_sr;
  88     sighip_ft1(x,  x->x_hz);
  89     dsp_add(sighip_perform, 4,
  90         sp[0]->s_vec, sp[1]->s_vec,
  91             x->x_ctl, sp[0]->s_n);
  92
  93 }
  94
  95 static void sighip_clear(t_sighip *x, t_floatarg q)
  96 {
  97     x->x_cspace.c_x = 0;
  98 }
  99
 100 void sighip_setup(void)
 101 {
 102     sighip_class = class_new(gensym("hip~"), (t_newmethod)sighip_new, 0,
 103         sizeof(t_sighip), 0, A_DEFFLOAT, 0);
 104     CLASS_MAINSIGNALIN(sighip_class, t_sighip, x_f);
 105     class_addmethod(sighip_class, (t_method)sighip_dsp, gensym("dsp"), 0);
 106     class_addmethod(sighip_class, (t_method)sighip_ft1,
 107         gensym("ft1"), A_FLOAT, 0);
 108     class_addmethod(sighip_class, (t_method)sighip_clear, gensym("clear"), 0);
 109 }
 110
 111 /* ---------------- lop~ - 1-pole lopass filter. ----------------- */
 112
 113 typedef struct lopctl
 114 {
 115     float c_x;
 116     float c_coef;
 117 } t_lopctl;
 118
 119 typedef struct siglop
 120 {
 121     t_object x_obj;
 122     float x_sr;
 123     float x_hz;
 124     t_lopctl x_cspace;
 125     t_lopctl *x_ctl;
 126     float x_f;
 127 } t_siglop;
 128
 129 t_class *siglop_class;
 130
 131 static void siglop_ft1(t_siglop *x, t_floatarg f);
 132
 133 static void *siglop_new(t_floatarg f)
 134 {
 135     t_siglop *x = (t_siglop *)pd_new(siglop_class);
 136     inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"), gensym("ft1"));
 137     outlet_new(&x->x_obj, gensym("signal"));
 138     x->x_sr = 44100;
 139     x->x_ctl = &x->x_cspace;
 140     x->x_cspace.c_x = 0;
 141     siglop_ft1(x, f);
 142     x->x_f = 0;
 143     return (x);
 144 }
 145
 146 static void siglop_ft1(t_siglop *x, t_floatarg f)
 147 {
 148     if (f < 0) f = 0;
 149     x->x_hz = f;
 150     x->x_ctl->c_coef = f * (2 * 3.14159) / x->x_sr;
 151     if (x->x_ctl->c_coef > 1)
 152         x->x_ctl->c_coef = 1;
 153     else if (x->x_ctl->c_coef < 0)
 154         x->x_ctl->c_coef = 0;
 155 }
 156
 157 static void siglop_clear(t_siglop *x, t_floatarg q)
 158 {
 159     x->x_cspace.c_x = 0;
 160 }
 161
 162 static t_int *siglop_perform(t_int *w)
 163 {
 164     float *in = (float *)(w[1]);
 165     float *out = (float *)(w[2]);
 166     t_lopctl *c = (t_lopctl *)(w[3]);
 167     int n = (t_int)(w[4]);
 168     int i;
 169     float last = c->c_x;
 170     float coef = c->c_coef;
 171     float feedback = 1 - coef;
 172     for (i = 0; i < n; i++)
 173         last = *out++ = coef * *in++ + feedback * last;
 174     if (PD_BIGORSMALL(last))
 175         last = 0;
 176     c->c_x = last;
 177     return (w+5);
 178 }
 179
 180 static void siglop_dsp(t_siglop *x, t_signal **sp)
 181 {
 182     x->x_sr = sp[0]->s_sr;
 183     siglop_ft1(x,  x->x_hz);
 184     dsp_add(siglop_perform, 4,
 185         sp[0]->s_vec, sp[1]->s_vec,
 186             x->x_ctl, sp[0]->s_n);
 187
 188 }
 189
 190 void siglop_setup(void)
 191 {
 192     siglop_class = class_new(gensym("lop~"), (t_newmethod)siglop_new, 0,
 193         sizeof(t_siglop), 0, A_DEFFLOAT, 0);
 194     CLASS_MAINSIGNALIN(siglop_class, t_siglop, x_f);
 195     class_addmethod(siglop_class, (t_method)siglop_dsp, gensym("dsp"), 0);
 196     class_addmethod(siglop_class, (t_method)siglop_ft1,
 197         gensym("ft1"), A_FLOAT, 0);
 198     class_addmethod(siglop_class, (t_method)siglop_clear, gensym("clear"), 0);
 199 }
 200
 201 /* ---------------- bp~ - 2-pole bandpass filter. ----------------- */
 202
 203 typedef struct bpctl
 204 {
 205     float c_x1;
 206     float c_x2;
 207     float c_coef1;
 208     float c_coef2;
 209     float c_gain;
 210 } t_bpctl;
 211
 212 typedef struct sigbp
 213 {
 214     t_object x_obj;
 215     float x_sr;
 216     float x_freq;
 217     float x_q;
 218     t_bpctl x_cspace;
 219     t_bpctl *x_ctl;
 220     float x_f;
 221 } t_sigbp;
 222
 223 t_class *sigbp_class;
 224
 225 static void sigbp_docoef(t_sigbp *x, t_floatarg f, t_floatarg q);
 226
 227 static void *sigbp_new(t_floatarg f, t_floatarg q)
 228 {
 229     t_sigbp *x = (t_sigbp *)pd_new(sigbp_class);
 230     inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"), gensym("ft1"));
 231     inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"), gensym("ft2"));
 232     outlet_new(&x->x_obj, gensym("signal"));
 233     x->x_sr = 44100;
 234     x->x_ctl = &x->x_cspace;
 235     x->x_cspace.c_x1 = 0;
 236     x->x_cspace.c_x2 = 0;
 237     sigbp_docoef(x, f, q);
 238     x->x_f = 0;
 239     return (x);
 240 }
 241
 242 static float sigbp_qcos(float f)
 243 {
 244     if (f >= -(0.5f*3.14159f) && f <= 0.5f*3.14159f)
 245     {
 246         float g = f*f;
 247         return (((g*g*g * (-1.0f/720.0f) + g*g*(1.0f/24.0f)) - g*0.5) + 1);
 248     }
 249     else return (0);
 250 }
 251
 252 static void sigbp_docoef(t_sigbp *x, t_floatarg f, t_floatarg q)
 253 {
 254     float r, oneminusr, omega;
 255     if (f < 0.001) f = 10;
 256     if (q < 0) q = 0;
 257     x->x_freq = f;
 258     x->x_q = q;
 259     omega = f * (2.0f * 3.14159f) / x->x_sr;
 260     if (q < 0.001) oneminusr = 1.0f;
 261     else oneminusr = omega/q;
 262     if (oneminusr > 1.0f) oneminusr = 1.0f;
 263     r = 1.0f - oneminusr;
 264     x->x_ctl->c_coef1 = 2.0f * sigbp_qcos(omega) * r;
 265     x->x_ctl->c_coef2 = - r * r;
 266     x->x_ctl->c_gain = 2 * oneminusr * (oneminusr + r * omega);
 267     /* post("r %f, omega %f, coef1 %f, coef2 %f",
 268         r, omega, x->x_ctl->c_coef1, x->x_ctl->c_coef2); */
 269 }
 270
 271 static void sigbp_ft1(t_sigbp *x, t_floatarg f)
 272 {
 273     sigbp_docoef(x, f, x->x_q);
 274 }
 275
 276 static void sigbp_ft2(t_sigbp *x, t_floatarg q)
 277 {
 278     sigbp_docoef(x, x->x_freq, q);
 279 }
 280
 281 static void sigbp_clear(t_sigbp *x, t_floatarg q)
 282 {
 283     x->x_ctl->c_x1 = x->x_ctl->c_x2 = 0;
 284 }
 285
 286 static t_int *sigbp_perform(t_int *w)
 287 {
 288     float *in = (float *)(w[1]);
 289     float *out = (float *)(w[2]);
 290     t_bpctl *c = (t_bpctl *)(w[3]);
 291     int n = (t_int)(w[4]);
 292     int i;
 293     float last = c->c_x1;
 294     float prev = c->c_x2;
 295     float coef1 = c->c_coef1;
 296     float coef2 = c->c_coef2;
 297     float gain = c->c_gain;
 298     for (i = 0; i < n; i++)
 299     {
 300         float output =  *in++ + coef1 * last + coef2 * prev;
 301         *out++ = gain * output;
 302         prev = last;
 303         last = output;
 304     }
 305     if (PD_BIGORSMALL(last))
 306         last = 0;
 307     if (PD_BIGORSMALL(prev))
 308         prev = 0;
 309     c->c_x1 = last;
 310     c->c_x2 = prev;
 311     return (w+5);
 312 }
 313
 314 static void sigbp_dsp(t_sigbp *x, t_signal **sp)
 315 {
 316     x->x_sr = sp[0]->s_sr;
 317     sigbp_docoef(x, x->x_freq, x->x_q);
 318     dsp_add(sigbp_perform, 4,
 319         sp[0]->s_vec, sp[1]->s_vec,
 320             x->x_ctl, sp[0]->s_n);
 321
 322 }
 323
 324 void sigbp_setup(void)
 325 {
 326     sigbp_class = class_new(gensym("bp~"), (t_newmethod)sigbp_new, 0,
 327         sizeof(t_sigbp), 0, A_DEFFLOAT, A_DEFFLOAT, 0);
 328     CLASS_MAINSIGNALIN(sigbp_class, t_sigbp, x_f);
 329     class_addmethod(sigbp_class, (t_method)sigbp_dsp, gensym("dsp"), 0);
 330     class_addmethod(sigbp_class, (t_method)sigbp_ft1,
 331         gensym("ft1"), A_FLOAT, 0);
 332     class_addmethod(sigbp_class, (t_method)sigbp_ft2,
 333         gensym("ft2"), A_FLOAT, 0);
 334     class_addmethod(sigbp_class, (t_method)sigbp_clear, gensym("clear"), 0);
 335 }
 336
 337 /* ---------------- biquad~ - raw biquad filter ----------------- */
 338
 339 typedef struct biquadctl
 340 {
 341     float c_x1;
 342     float c_x2;
 343     float c_fb1;
 344     float c_fb2;
 345     float c_ff1;
 346     float c_ff2;
 347     float c_ff3;
 348 } t_biquadctl;
 349
 350 typedef struct sigbiquad
 351 {
 352     t_object x_obj;
 353     float x_f;
 354     t_biquadctl x_cspace;
 355     t_biquadctl *x_ctl;
 356 } t_sigbiquad;
 357
 358 t_class *sigbiquad_class;
 359
 360 static void sigbiquad_list(t_sigbiquad *x, t_symbol *s, int argc, t_atom *argv);
 361
 362 static void *sigbiquad_new(t_symbol *s, int argc, t_atom *argv)
 363 {
 364     t_sigbiquad *x = (t_sigbiquad *)pd_new(sigbiquad_class);
 365     outlet_new(&x->x_obj, gensym("signal"));
 366     x->x_ctl = &x->x_cspace;
 367     x->x_cspace.c_x1 = x->x_cspace.c_x2 = 0;
 368     sigbiquad_list(x, s, argc, argv);
 369     x->x_f = 0;
 370     return (x);
 371 }
 372
 373 static t_int *sigbiquad_perform(t_int *w)
 374 {
 375     float *in = (float *)(w[1]);
 376     float *out = (float *)(w[2]);
 377     t_biquadctl *c = (t_biquadctl *)(w[3]);
 378     int n = (t_int)(w[4]);
 379     int i;
 380     float last = c->c_x1;
 381     float prev = c->c_x2;
 382     float fb1 = c->c_fb1;
 383     float fb2 = c->c_fb2;
 384     float ff1 = c->c_ff1;
 385     float ff2 = c->c_ff2;
 386     float ff3 = c->c_ff3;
 387     for (i = 0; i < n; i++)
 388     {
 389         float output =  *in++ + fb1 * last + fb2 * prev;
 390         if (PD_BIGORSMALL(output))
 391             output = 0;
 392         *out++ = ff1 * output + ff2 * last + ff3 * prev;
 393         prev = last;
 394         last = output;
 395     }
 396     c->c_x1 = last;
 397     c->c_x2 = prev;
 398     return (w+5);
 399 }
 400
 401 static void sigbiquad_list(t_sigbiquad *x, t_symbol *s, int argc, t_atom *argv)
 402 {
 403     float fb1 = atom_getfloatarg(0, argc, argv);
 404     float fb2 = atom_getfloatarg(1, argc, argv);
 405     float ff1 = atom_getfloatarg(2, argc, argv);
 406     float ff2 = atom_getfloatarg(3, argc, argv);
 407     float ff3 = atom_getfloatarg(4, argc, argv);
 408     float discriminant = fb1 * fb1 + 4 * fb2;
 409     t_biquadctl *c = x->x_ctl;
 410     if (discriminant < 0) /* imaginary roots -- resonant filter */
 411     {
 412             /* they're conjugates so we just check that the product
 413             is less than one */
 414         if (fb2 >= -1.0f) goto stable;
 415     }
 416     else    /* real roots */
 417     {
 418             /* check that the parabola 1 - fb1 x - fb2 x^2 has a
 419                 vertex between -1 and 1, and that it's nonnegative
 420                 at both ends, which implies both roots are in [1-,1]. */
 421         if (fb1 <= 2.0f && fb1 >= -2.0f &&
 422             1.0f - fb1 -fb2 >= 0 && 1.0f + fb1 - fb2 >= 0)
 423                 goto stable;
 424     }
 425         /* if unstable, just bash to zero */
 426     fb1 = fb2 = ff1 = ff2 = ff3 = 0;
 427 stable:
 428     c->c_fb1 = fb1;
 429     c->c_fb2 = fb2;
 430     c->c_ff1 = ff1;
 431     c->c_ff2 = ff2;
 432     c->c_ff3 = ff3;
 433 }
 434
 435 static void sigbiquad_set(t_sigbiquad *x, t_symbol *s, int argc, t_atom *argv)
 436 {
 437     t_biquadctl *c = x->x_ctl;
 438     c->c_x1 = atom_getfloatarg(0, argc, argv);
 439     c->c_x2 = atom_getfloatarg(1, argc, argv);
 440 }
 441
 442 static void sigbiquad_dsp(t_sigbiquad *x, t_signal **sp)
 443 {
 444     dsp_add(sigbiquad_perform, 4,
 445         sp[0]->s_vec, sp[1]->s_vec,
 446             x->x_ctl, sp[0]->s_n);
 447
 448 }
 449
 450 void sigbiquad_setup(void)
 451 {
 452     sigbiquad_class = class_new(gensym("biquad~"), (t_newmethod)sigbiquad_new,
 453         0, sizeof(t_sigbiquad), 0, A_GIMME, 0);
 454     CLASS_MAINSIGNALIN(sigbiquad_class, t_sigbiquad, x_f);
 455     class_addmethod(sigbiquad_class, (t_method)sigbiquad_dsp, gensym("dsp"), 0);
 456     class_addlist(sigbiquad_class, sigbiquad_list);
 457     class_addmethod(sigbiquad_class, (t_method)sigbiquad_set, gensym("set"),
 458         A_GIMME, 0);
 459     class_addmethod(sigbiquad_class, (t_method)sigbiquad_set, gensym("clear"),
 460         A_GIMME, 0);
 461 }
 462
 463 /* ---------------- samphold~ - sample and hold  ----------------- */
 464
 465 typedef struct sigsamphold
 466 {
 467     t_object x_obj;
 468     float x_f;
 469     float x_lastin;
 470     float x_lastout;
 471 } t_sigsamphold;
 472
 473 t_class *sigsamphold_class;
 474
 475 static void *sigsamphold_new(void)
 476 {
 477     t_sigsamphold *x = (t_sigsamphold *)pd_new(sigsamphold_class);
 478     inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
 479     outlet_new(&x->x_obj, gensym("signal"));
 480     x->x_lastin = 0;
 481     x->x_lastout = 0;
 482     x->x_f = 0;
 483     return (x);
 484 }
 485
 486 static t_int *sigsamphold_perform(t_int *w)
 487 {
 488     float *in1 = (float *)(w[1]);
 489     float *in2 = (float *)(w[2]);
 490     float *out = (float *)(w[3]);
 491     t_sigsamphold *x = (t_sigsamphold *)(w[4]);
 492     int n = (t_int)(w[5]);
 493     int i;
 494     float lastin = x->x_lastin;
 495     float lastout = x->x_lastout;
 496     for (i = 0; i < n; i++, *in1++)
 497     {
 498         float next = *in2++;
 499         if (next < lastin) lastout = *in1;
 500         *out++ = lastout;
 501         lastin = next;
 502     }
 503     x->x_lastin = lastin;
 504     x->x_lastout = lastout;
 505     return (w+6);
 506 }
 507
 508 static void sigsamphold_dsp(t_sigsamphold *x, t_signal **sp)
 509 {
 510     dsp_add(sigsamphold_perform, 5,
 511         sp[0]->s_vec, sp[1]->s_vec, sp[2]->s_vec,
 512             x, sp[0]->s_n);
 513 }
 514
 515 static void sigsamphold_reset(t_sigsamphold *x)
 516 {
 517     x->x_lastin = 1e20;
 518 }
 519
 520 static void sigsamphold_set(t_sigsamphold *x, t_float f)
 521 {
 522     x->x_lastout = f;
 523 }
 524
 525 void sigsamphold_setup(void)
 526 {
 527     sigsamphold_class = class_new(gensym("samphold~"),
 528         (t_newmethod)sigsamphold_new, 0, sizeof(t_sigsamphold), 0, 0);
 529     CLASS_MAINSIGNALIN(sigsamphold_class, t_sigsamphold, x_f);
 530     class_addmethod(sigsamphold_class, (t_method)sigsamphold_set,
 531         gensym("set"), A_FLOAT, 0);
 532     class_addmethod(sigsamphold_class, (t_method)sigsamphold_reset,
 533         gensym("reset"), 0);
 534     class_addmethod(sigsamphold_class, (t_method)sigsamphold_dsp,
 535         gensym("dsp"), 0);
 536 }
 537
 538 /* ------------------------ setup routine ------------------------- */
 539
 540 void d_filter_setup(void)
 541 {
 542     sighip_setup();
 543     siglop_setup();
 544     sigbp_setup();
 545     sigbiquad_setup();
 546     sigsamphold_setup();
 547 }
 548