postfish/bessel.c

   1 /*
   2  *
   3  *  postfish
   4  *
   5  *      Copyright (C) 2002-2005 Monty
   6  *
   7  *  Postfish is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License as published by
   9  *  the Free Software Foundation; either version 2, or (at your option)
  10  *  any later version.
  11  *
  12  *  Postfish is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU General Public License
  18  *  along with Postfish; see the file COPYING.  If not, write to the
  19  *  Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  20  *
  21  *
  22  */
  23
  24 /* code derived directly from mkfilter by the late A.J. Fisher,
  25    University of York <fisher@minster.york.ac.uk> September 1992; this
  26    is only the minimum code needed to build an arbitrary Bessel filter */
  27
  28 #include "postfish.h"
  29 #include "bessel.h"
  30
  31 typedef struct {
  32   double re, im;
  33 } complex;
  34
  35 static complex bessel_poles[] = {
  36   { -1.00000000000e+00, 0.00000000000e+00},
  37   { -1.10160133059e+00, 6.36009824757e-01},
  38   { -1.32267579991e+00, 0.00000000000e+00},
  39   { -1.04740916101e+00, 9.99264436281e-01},
  40   { -1.37006783055e+00, 4.10249717494e-01},
  41   { -9.95208764350e-01, 1.25710573945e+00},
  42   { -1.50231627145e+00, 0.00000000000e+00},
  43   { -1.38087732586e+00, 7.17909587627e-01},
  44   { -9.57676548563e-01, 1.47112432073e+00},
  45   { -1.57149040362e+00, 3.20896374221e-01},
  46   { -1.38185809760e+00, 9.71471890712e-01},
  47   { -9.30656522947e-01, 1.66186326894e+00},
  48   { -1.68436817927e+00, 0.00000000000e+00},
  49   { -1.61203876622e+00, 5.89244506931e-01},
  50   { -1.37890321680e+00, 1.19156677780e+00},
  51   { -9.09867780623e-01, 1.83645135304e+00},
  52   { -1.75740840040e+00, 2.72867575103e-01},
  53   { -1.63693941813e+00, 8.22795625139e-01},
  54   { -1.37384121764e+00, 1.38835657588e+00},
  55   { -8.92869718847e-01, 1.99832584364e+00},
  56   { -1.85660050123e+00, 0.00000000000e+00},
  57   { -1.80717053496e+00, 5.12383730575e-01},
  58   { -1.65239648458e+00, 1.03138956698e+00},
  59   { -1.36758830979e+00, 1.56773371224e+00},
  60   { -8.78399276161e-01, 2.14980052431e+00},
  61   { -1.92761969145e+00, 2.41623471082e-01},
  62   { -1.84219624443e+00, 7.27257597722e-01},
  63   { -1.66181024140e+00, 1.22110021857e+00},
  64   { -1.36069227838e+00, 1.73350574267e+00},
  65   { -8.65756901707e-01, 2.29260483098e+00},
  66 };
  67
  68 #define TWOPI       (2.0 * M_PIl)
  69 #define EPS         1e-10
  70 #define MAXPZ       8
  71
  72 typedef struct {
  73   complex poles[MAXPZ], zeros[MAXPZ];
  74   int numpoles, numzeros;
  75 } pzrep;
  76
  77 static complex cdiv(complex z1, complex z2){
  78   double mag = (z2.re * z2.re) + (z2.im * z2.im);
  79   complex ret={((z1.re * z2.re) + (z1.im * z2.im)) / mag,
  80                ((z1.im * z2.re) - (z1.re * z2.im)) / mag};
  81   return ret;
  82 }
  83
  84 static complex cmul(complex z1, complex z2){
  85   complex ret={z1.re*z2.re - z1.im*z2.im,
  86                z1.re*z2.im + z1.im*z2.re};
  87   return ret;
  88 }
  89 static complex cadd(complex z1, complex z2){
  90   z1.re+=z2.re;
  91   z1.im+=z2.im;
  92   return z1;
  93 }
  94
  95 static complex csub(complex z1, complex z2){
  96   z1.re-=z2.re;
  97   z1.im-=z2.im;
  98   return z1;
  99 }
 100
 101 static complex cconj(complex z){
 102   z.im = -z.im;
 103   return z;
 104 }
 105
 106 static complex eval(complex coeffs[], int npz, complex z){
 107   complex sum = (complex){0.0,0.0};
 108   int i;
 109   for (i = npz; i >= 0; i--) sum = cadd(cmul(sum, z), coeffs[i]);
 110   return sum;
 111 }
 112
 113 static complex evaluate(complex topco[], int nz, complex botco[], int np, complex z){
 114   return cdiv(eval(topco, nz, z),eval(botco, np, z));
 115 }
 116
 117 static complex blt(complex pz){
 118   complex two={2.,0.};
 119   return cdiv(cadd(two, pz), csub(two, pz));
 120 }
 121
 122 static void multin(complex w, int npz, complex coeffs[]){
 123   int i;
 124   complex nw = (complex){-w.re , -w.im};
 125   for (i = npz; i >= 1; i--)
 126     coeffs[i] = cadd(cmul(nw, coeffs[i]), coeffs[i-1]);
 127   coeffs[0] = cmul(nw, coeffs[0]);
 128 }
 129
 130 static void expand(complex pz[], int npz, complex coeffs[]){
 131   /* compute product of poles or zeros as a polynomial of z */
 132   int i;
 133   coeffs[0] = (complex){1.0,0.0};
 134   for (i=0; i < npz; i++) coeffs[i+1] = (complex){0.0,0.0};
 135   for (i=0; i < npz; i++) multin(pz[i], npz, coeffs);
 136   /* check computed coeffs of z^k are all real */
 137   for (i=0; i < npz+1; i++){
 138     if (fabs(coeffs[i].im) > EPS){
 139       fprintf(stderr, "mkfilter: coeff of z^%d is not real; poles/zeros are not complex conjugates\n", i);
 140       exit(1);
 141     }
 142   }
 143 }
 144
 145 double mkbessel(double raw_alpha,int order,double *ycoeff){
 146   int i,p= (order*order)/4;
 147   pzrep splane, zplane;
 148   complex topcoeffs[MAXPZ+1], botcoeffs[MAXPZ+1];
 149   double warped_alpha;
 150   complex dc_gain;
 151
 152   memset(&splane,0,sizeof(splane));
 153   memset(&zplane,0,sizeof(zplane));
 154
 155   if (order & 1) splane.poles[splane.numpoles++] = bessel_poles[p++];
 156   for (i = 0; i < order/2; i++){
 157     splane.poles[splane.numpoles++] = bessel_poles[p];
 158     splane.poles[splane.numpoles++] = cconj(bessel_poles[p++]);
 159   }
 160
 161   warped_alpha = tan(M_PIl * raw_alpha) / M_PIl;
 162   for (i = 0; i < splane.numpoles; i++){
 163     splane.poles[i].re *= TWOPI * warped_alpha;
 164     splane.poles[i].im *= TWOPI * warped_alpha;
 165   }
 166
 167   zplane.numpoles = splane.numpoles;
 168   zplane.numzeros = splane.numzeros;
 169   for (i=0; i < zplane.numpoles; i++)
 170     zplane.poles[i] = blt(splane.poles[i]);
 171   for (i=0; i < zplane.numzeros; i++)
 172     zplane.zeros[i] = blt(splane.zeros[i]);
 173   while (zplane.numzeros < zplane.numpoles)
 174     zplane.zeros[zplane.numzeros++] = (complex){-1.0,0.};
 175
 176   expand(zplane.zeros, zplane.numzeros, topcoeffs);
 177   expand(zplane.poles, zplane.numpoles, botcoeffs);
 178   dc_gain = evaluate(topcoeffs, zplane.numzeros, botcoeffs, zplane.numpoles, (complex){1.0,0.0});
 179
 180   for(i=0;i<order;i++)
 181     ycoeff[order-i-1] = -(botcoeffs[i].re / botcoeffs[zplane.numpoles].re);
 182
 183   return hypot(dc_gain.re,dc_gain.im);
 184 }
 185
 186 /* applies a 2nd order filter (attack) that is decay-limited by a
 187    first-order freefall filter (decay) */
 188 void compute_iir_symmetric_limited(float *x, int n, iir_state *is,
 189                                    iir_filter *attack, iir_filter *limit){
 190   double a_c0=attack->c[0],l_c0=limit->c[0];
 191   double a_c1=attack->c[1];
 192   double a_g=1./attack->g;
 193
 194   double x0=is->x[0],x1=is->x[1];
 195   double y0=is->y[0],y1=is->y[1];
 196
 197   int i=0;
 198
 199   if(zerome(y0) && zerome(y1)) y0=y1=0.;
 200
 201   while(i<n){
 202     double ya= (x[i]+x0*2.+x1)*a_g + y0*a_c0+y1*a_c1;
 203     double yl= y0*l_c0;
 204     if(ya<yl)ya=yl;
 205
 206     x1=x0;x0=x[i];
 207     y1=y0;x[i]=y0=ya;
 208     i++;
 209   }
 210
 211   is->x[0]=x0;is->x[1]=x1;
 212   is->y[0]=y0;is->y[1]=y1;
 213 }
 214
 215 /* applies a 2nd order filter (decay) to decay from peak value only,
 216    decay limited by a first-order freefall filter (limit) with the
 217    same alpha as decay */
 218 void compute_iir_decay_limited(float *x, int n, iir_state *is,
 219                                iir_filter *decay, iir_filter *limit){
 220   double d_c0=decay->c[0],l_c0=limit->c[0];
 221   double d_c1=decay->c[1];
 222   double d_g=1./decay->g;
 223
 224   double x0=is->x[0],x1=is->x[1];
 225   double y0=is->y[0],y1=is->y[1];
 226
 227   int i=0;
 228
 229   if(zerome(y0) && zerome(y1)) y0=y1=0.;
 230
 231   while(i<n){
 232     double yd= (x[i]+x0*2.+x1)*d_g + y0*d_c0+y1*d_c1;
 233     double yl= y0*l_c0;
 234     if(yd<yl)yd=yl;
 235     if(yd<x[i])y1=y0=yd=x[i];
 236
 237     x1=x0;x0=x[i];
 238     y1=y0;x[i]=y0=yd;
 239     i++;
 240   }
 241
 242   is->x[0]=x0;is->x[1]=x1;
 243   is->y[0]=y0;is->y[1]=y1;
 244 }
 245
 246 /* applies a 2nd order filter (attack) that is decay-limited by a
 247    first-order filter (decay) */
 248 void compute_iir_freefall_limited(float *x, int n, iir_state *is,
 249                                   iir_filter *attack, iir_filter *limit){
 250   double a_c0=attack->c[0],l_c0=limit->c[0];
 251   double a_c1=attack->c[1];
 252   double a_g=1./attack->g;
 253
 254   double x0=is->x[0],x1=is->x[1];
 255   double y0=is->y[0],y1=is->y[1];
 256
 257   int i=0;
 258
 259   if(zerome(y0) && zerome(y1)) y0=y1=0.;
 260
 261   while(i<n){
 262     double ya= (x0*2.+x1)*a_g + y0*a_c0+y1*a_c1;
 263     double yl= y0*l_c0;
 264
 265     if(x[i]<ya){
 266       x1=x0;x0=0;
 267     }else{
 268       ya+= x[i]*a_g;
 269       x1=x0;x0=x[i];
 270     }
 271
 272     if(ya<yl)ya=yl;
 273
 274     y1=y0;x[i]=y0=ya;
 275     i++;
 276   }
 277
 278   is->x[0]=x0;is->x[1]=x1;
 279   is->y[0]=y0;is->y[1]=y1;
 280 }
 281
 282 void compute_iir_freefallonly1(float *x, int n, iir_state *is,
 283                            iir_filter *decay){
 284   double d_c0=decay->c[0];
 285
 286   double x0=is->x[0];
 287   double y0=is->y[0];
 288   int i=0;
 289
 290   if(zerome(y0)){
 291     y0=0.;
 292   }
 293
 294   while(i<n){
 295     double yd;
 296
 297     yd = y0*d_c0;
 298     x0=x[i];
 299
 300     /* if we're not in freefall, be sure x[i]_out == x[i]_in  */
 301     if(x[i]>yd){
 302       yd=x[i];
 303     }else{
 304       x[i]=yd;
 305     }
 306
 307     y0=yd;
 308     i++;
 309   }
 310
 311   is->x[0]=x0;
 312   is->y[0]=y0;
 313
 314 }
 315
 316 /* a new experiment; bessel followers constructed specifically for
 317    compand functions.  Hard and soft knee are implemented as part of
 318    the follower. */
 319
 320 #define prologue   double a_c0=attack->c[0],l_c0=limit->c[0]; \
 321                    double a_c1=attack->c[1]; \
 322                    double a_g=1./(knee * attack->g);\
 323                    double x0=is->x[0],x1=is->x[1]; \
 324                    double y0=is->y[0],y1=is->y[1]; \
 325                    int i=0; \
 326                    if(zerome(y0) && zerome(y1)) y0=y1=0.; \
 327                    while(i<n)
 328
 329 /* Three delicious filters in one: The 'attack' filter is a fast
 330    second-order Bessel used two ways; as a direct RMS follower and as
 331    a filter pegged to free-fall to the knee value.  The 'limit' filter
 332    is a first-order free-fall (to 0) Bessel used to limit the 'attack'
 333    filter to a linear-dB decay. Output is normalized to place the knee
 334    at 1.0 (0dB) */
 335
 336 void compute_iir_over_soft(float *x, int n, iir_state *is,
 337                            iir_filter *attack, iir_filter *limit,
 338                            float knee, float mult, float *adj){
 339   double xag=4./attack->g;
 340   prologue {
 341
 342     double ya = (x[i]+x0*2.+x1)*a_g;
 343     if(ya<xag)ya=xag;
 344     ya += y0*a_c0;
 345     ya += y1*a_c1;
 346     double yl = y0*l_c0;
 347     if(ya<yl)ya=yl;
 348
 349     x1=x0;x0=x[i];
 350     y1=y0;y0=ya;
 351     adj[i++]-= todB_a((float)ya)*mult;
 352
 353   }
 354
 355   is->x[0]=x0;is->x[1]=x1;
 356   is->y[0]=y0;is->y[1]=y1;
 357 }
 358
 359 void compute_iir_under_soft(float *x, int n, iir_state *is,
 360                             iir_filter *attack, iir_filter *limit,
 361                             float knee, float mult, float *adj){
 362   double xag=4./attack->g;
 363   prologue {
 364
 365     double ya = (x[i]+x0*2.+x1)*a_g;
 366     if(ya>xag)ya=xag;
 367     ya += y0*a_c0;
 368     ya += y1*a_c1;
 369     double yl = y0*l_c0;
 370     if(ya<yl)ya=yl;
 371
 372     x1=x0;x0=x[i];
 373     y1=y0;y0=ya;
 374     adj[i++]-= todB_a((float)ya)*mult;
 375
 376   }
 377
 378   is->x[0]=x0;is->x[1]=x1;
 379   is->y[0]=y0;is->y[1]=y1;
 380 }
 381
 382 void compute_iir_over_hard(float *x, int n, iir_state *is,
 383                            iir_filter *attack, iir_filter *limit,
 384                            float knee, float mult, float *adj){
 385   prologue {
 386
 387     double ya = (x[i]+x0*2.+x1)*a_g;
 388     ya += y0*a_c0;
 389     ya += y1*a_c1;
 390     double yl = y0*l_c0;
 391     if(ya<yl)ya=yl;
 392
 393     x1=x0;x0=x[i];
 394     y1=y0;y0=ya;
 395     if(ya>1.)adj[i]-= todB_a((float)ya)*mult;
 396     i++;
 397   }
 398
 399   is->x[0]=x0;is->x[1]=x1;
 400   is->y[0]=y0;is->y[1]=y1;
 401 }
 402
 403 void compute_iir_under_hard(float *x, int n, iir_state *is,
 404                             iir_filter *attack, iir_filter *limit,
 405                             float knee, float mult, float *adj){
 406   prologue {
 407
 408     double ya = (x[i]+x0*2.+x1)*a_g;
 409     ya += y0*a_c0;
 410     ya += y1*a_c1;
 411     double yl = y0*l_c0;
 412     if(ya<yl)ya=yl;
 413
 414     x1=x0;x0=x[i];
 415     y1=y0;y0=ya;
 416     if(ya<1.)adj[i]-= todB_a((float)ya)*mult;
 417     i++;
 418   }
 419
 420   is->x[0]=x0;is->x[1]=x1;
 421   is->y[0]=y0;is->y[1]=y1;
 422 }
 423
 424 /* One more take on the above; we need to be able to gently slew
 425    multiplier changes from one block to the next.  Although it seems
 426    absurd to make eight total compander follower variants, doing this
 427    inline avoids a copy later on, and these are enough of a CPU sink
 428    that the silliness is actually worth it. */
 429
 430 void compute_iir_over_soft_del(float *x, int n, iir_state *is,
 431                                iir_filter *attack, iir_filter *limit,
 432                                float knee, float mult, float mult2,
 433                                float *adj){
 434   float multadd=(mult2-mult)/n;
 435   double xag=4./attack->g;
 436   prologue {
 437
 438     double ya = (x[i]+x0*2.+x1)*a_g;
 439     if(ya<xag)ya=xag;
 440     ya += y0*a_c0;
 441     ya += y1*a_c1;
 442     double yl = y0*l_c0;
 443     if(ya<yl)ya=yl;
 444
 445     x1=x0;x0=x[i];
 446     y1=y0;y0=ya;
 447     adj[i++]-= todB_a((float)ya)*mult;
 448     mult+=multadd;
 449
 450   }
 451
 452   is->x[0]=x0;is->x[1]=x1;
 453   is->y[0]=y0;is->y[1]=y1;
 454 }
 455
 456 void compute_iir_under_soft_del(float *x, int n, iir_state *is,
 457                                 iir_filter *attack, iir_filter *limit,
 458                                 float knee, float mult, float mult2,
 459                                 float *adj){
 460   float multadd=(mult2-mult)/n;
 461   double xag=4./attack->g;
 462   prologue {
 463
 464     double ya = (x[i]+x0*2.+x1)*a_g;
 465     if(ya>xag)ya=xag;
 466     ya += y0*a_c0;
 467     ya += y1*a_c1;
 468     double yl = y0*l_c0;
 469     if(ya<yl)ya=yl;
 470
 471     x1=x0;x0=x[i];
 472     y1=y0;y0=ya;
 473     adj[i++]-= todB_a((float)ya)*mult;
 474     mult+=multadd;
 475
 476   }
 477
 478   is->x[0]=x0;is->x[1]=x1;
 479   is->y[0]=y0;is->y[1]=y1;
 480 }
 481
 482 void compute_iir_over_hard_del(float *x, int n, iir_state *is,
 483                                iir_filter *attack, iir_filter *limit,
 484                                float knee, float mult, float mult2,
 485                                float *adj){
 486   float multadd=(mult2-mult)/n;
 487   prologue {
 488
 489     double ya = (x[i]+x0*2.+x1)*a_g;
 490     ya += y0*a_c0;
 491     ya += y1*a_c1;
 492     double yl = y0*l_c0;
 493     if(ya<yl)ya=yl;
 494
 495     x1=x0;x0=x[i];
 496     y1=y0;y0=ya;
 497     if(ya>1.)adj[i]-= todB_a((float)ya)*mult;
 498     mult+=multadd;
 499     i++;
 500   }
 501
 502   is->x[0]=x0;is->x[1]=x1;
 503   is->y[0]=y0;is->y[1]=y1;
 504 }
 505
 506 void compute_iir_under_hard_del(float *x, int n, iir_state *is,
 507                                 iir_filter *attack, iir_filter *limit,
 508                                 float knee, float mult, float mult2,
 509                                 float *adj){
 510   float multadd=(mult2-mult)/n;
 511   prologue {
 512
 513     double ya = (x[i]+x0*2.+x1)*a_g;
 514     ya += y0*a_c0;
 515     ya += y1*a_c1;
 516     double yl = y0*l_c0;
 517     if(ya<yl)ya=yl;
 518
 519     x1=x0;x0=x[i];
 520     y1=y0;y0=ya;
 521     if(ya<1.)adj[i]-= todB_a((float)ya)*mult;
 522     mult+=multadd;
 523     i++;
 524   }
 525
 526   is->x[0]=x0;is->x[1]=x1;
 527   is->y[0]=y0;is->y[1]=y1;
 528 }
 529
 530 void reset_iir(iir_state *is,float value){
 531   int i;
 532   for(i=0;i<MAXORDER;i++){
 533     is->x[i]=(double)value;
 534     is->y[i]=(double)value;
 535   }
 536 }
 537