analog_filter.cpp

   1 /*
   2   ZynAddSubFX - a software synthesizer
   3
   4   AnalogFilter.C - Several analog filters (lowpass, highpass...)
   5   Copyright (C) 2002-2005 Nasca Octavian Paul
   6   Author: Nasca Octavian Paul
   7
   8   This program is free software; you can redistribute it and/or modify
   9   it under the terms of version 2 of the GNU General Public License
  10   as published by the Free Software Foundation.
  11
  12   This program 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 (version 2) for more details.
  16
  17   You should have received a copy of the GNU General Public License (version 2)
  18   along with this program; if not, write to the Free Software Foundation,
  19   Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  20
  21 */
  22
  23 #include <math.h>
  24 #include <stdio.h>
  25 #include <assert.h>
  26
  27 #include "globals.h"
  28 #include "filter_base.h"
  29 #include "analog_filter.h"
  30
  31 void
  32 AnalogFilter::init(
  33   float sample_rate,
  34   unsigned char Ftype,
  35   float Ffreq,
  36   float Fq,
  37   unsigned char Fstages,
  38   float gain)
  39 {
  40   int i;
  41
  42   m_sample_rate = sample_rate;
  43
  44   m_additional_stages = Fstages;
  45
  46   for (i = 0 ; i < 3 ; i++)
  47   {
  48     m_c_old[i] = 0.0;
  49     m_d_old[i] = 0.0;
  50     m_c[i]  =0.0;
  51     m_d[i] = 0.0;
  52   }
  53
  54   m_type = Ftype;
  55
  56   m_frequency = Ffreq;
  57
  58   m_q_factor = Fq;
  59
  60   m_gain = 1.0;
  61
  62   if (m_additional_stages >= MAX_FILTER_STAGES)
  63   {
  64     m_additional_stages = MAX_FILTER_STAGES;
  65   }
  66
  67   cleanup();
  68
  69   m_first_time = false;
  70
  71   m_above_nq = false;
  72   m_old_above_nq = false;
  73
  74   setfreq_and_q(Ffreq, Fq);
  75
  76   m_first_time = true;
  77
  78   m_d[0] = 0;                   // this is not used
  79   m_outgain = 1.0;
  80
  81   if (Ftype >= ZYN_FILTER_ANALOG_TYPE_PKF2 &&
  82       Ftype <= ZYN_FILTER_ANALOG_TYPE_HSH2)
  83   {
  84     setgain(gain);
  85   }
  86   else
  87   {
  88     m_outgain = dB2rap(gain);
  89   }
  90 }
  91
  92 void
  93 AnalogFilter::cleanup()
  94 {
  95   int i;
  96
  97   for (i=0 ; i < MAX_FILTER_STAGES + 1 ; i++)
  98   {
  99     m_x[i].c1 = 0.0;
 100     m_x[i].c2 = 0.0;
 101
 102     m_y[i].c1 = 0.0;
 103     m_y[i].c2 = 0.0;
 104
 105     m_x_old[i] = m_x[i];
 106     m_y_old[i] = m_y[i];
 107   };
 108
 109   m_needs_interpolation = false;
 110 }
 111
 112 void
 113 AnalogFilter::computefiltercoefs()
 114 {
 115   float tmp;
 116   float omega, sn, cs, alpha, beta;
 117   int zerocoefs = 0;            // this is used if the freq is too high
 118   float freq;
 119   float tmpq, tmpgain;
 120
 121   // do not allow frequencies bigger than samplerate/2
 122   freq = m_frequency;
 123
 124   if (freq > m_sample_rate / 2 - 500.0)
 125   {
 126     freq = m_sample_rate / 2 - 500.0;
 127     zerocoefs = 1;
 128   }
 129
 130   if (freq < 0.1)
 131   {
 132     freq = 0.1;
 133   }
 134
 135   // do not allow bogus Q
 136   if (m_q_factor < 0.0)
 137   {
 138     m_q_factor = 0.0;
 139   }
 140
 141   if (m_additional_stages == 0)
 142   {
 143     tmpq = m_q_factor;
 144     tmpgain = m_gain;
 145   }
 146   else
 147   {
 148     tmpq = (m_q_factor > 1.0 ? pow(m_q_factor, 1.0 / (m_additional_stages + 1)) : m_q_factor);
 149     tmpgain = pow(m_gain, 1.0 / (m_additional_stages + 1));
 150   }
 151
 152   // most of theese are implementations of
 153   // the "Cookbook formulae for audio EQ" by Robert Bristow-Johnson
 154   // The original location of the Cookbook is:
 155   // http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt
 156   switch(m_type)
 157   {
 158   case ZYN_FILTER_ANALOG_TYPE_LPF1:
 159     if (zerocoefs == 0)
 160     {
 161       tmp = exp(-2.0 * PI * freq / m_sample_rate);
 162     }
 163     else
 164     {
 165       tmp = 0.0;
 166     }
 167
 168     m_c[0] = 1.0-tmp;
 169     m_c[1] = 0.0;
 170     m_c[2] = 0.0;
 171
 172     m_d[1] = tmp;
 173     m_d[2] = 0.0;
 174
 175     m_order = 1;
 176
 177     break;
 178
 179   case ZYN_FILTER_ANALOG_TYPE_HPF1:
 180     if (zerocoefs == 0)
 181     {
 182       tmp = exp(-2.0 * PI * freq / m_sample_rate);
 183     }
 184     else
 185     {
 186       tmp = 0.0;
 187     }
 188
 189     m_c[0] = (1.0 + tmp) / 2.0;
 190     m_c[1] = -(1.0 + tmp) / 2.0;
 191     m_c[2] = 0.0;
 192
 193     m_d[1] = tmp;
 194     m_d[2] = 0.0;
 195
 196     m_order = 1;
 197
 198     break;
 199
 200   case ZYN_FILTER_ANALOG_TYPE_LPF2:
 201     if (zerocoefs == 0)
 202     {
 203       omega = 2 * PI * freq / m_sample_rate;
 204       sn = sin(omega);
 205       cs = cos(omega);
 206       alpha = sn / (2 * tmpq);
 207       tmp = 1 + alpha;
 208       m_c[0] = (1.0 - cs) / 2.0 / tmp;
 209       m_c[1] = (1.0 - cs) / tmp;
 210       m_c[2] = (1.0 - cs) / 2.0 / tmp;
 211       m_d[1] = -2 * cs / tmp * (-1);
 212       m_d[2] = (1 - alpha) / tmp * (-1);
 213     }
 214     else
 215     {
 216       m_c[0] = 1.0;
 217       m_c[1] = 0.0;
 218       m_c[2] = 0.0;
 219
 220       m_d[1] = 0.0;
 221       m_d[2] = 0.0;
 222     }
 223
 224     m_order = 2;
 225
 226     break;
 227
 228   case ZYN_FILTER_ANALOG_TYPE_HPF2:
 229     if (zerocoefs == 0)
 230     {
 231       omega = 2 * PI * freq / m_sample_rate;
 232       sn = sin(omega);
 233       cs = cos(omega);
 234       alpha = sn / (2 * tmpq);
 235       tmp = 1 + alpha;
 236       m_c[0] = (1.0 + cs) / 2.0 / tmp;
 237       m_c[1] = -(1.0 + cs) / tmp;
 238       m_c[2] = (1.0 + cs) / 2.0 / tmp;
 239       m_d[1] = -2 * cs / tmp * (-1);
 240       m_d[2] = (1 - alpha) / tmp * (-1);
 241     }
 242     else
 243     {
 244       m_c[0] = 0.0;
 245       m_c[1] = 0.0;
 246       m_c[2] = 0.0;
 247
 248       m_d[1] = 0.0;
 249       m_d[2] = 0.0;
 250     }
 251
 252     m_order = 2;
 253
 254     break;
 255
 256   case ZYN_FILTER_ANALOG_TYPE_BPF2:
 257     if (zerocoefs == 0)
 258     {
 259       omega = 2 * PI * freq / m_sample_rate;
 260       sn = sin(omega);
 261       cs = cos(omega);
 262       alpha = sn / (2 * tmpq);
 263       tmp = 1 + alpha;
 264       m_c[0] = alpha / tmp * sqrt(tmpq + 1);
 265       m_c[1] = 0;
 266       m_c[2] = -alpha / tmp * sqrt(tmpq + 1);
 267       m_d[1] = -2 * cs / tmp * (-1);
 268       m_d[2] = (1 - alpha) / tmp * (-1);
 269     }
 270     else
 271     {
 272       m_c[0] = 0.0;
 273       m_c[1] = 0.0;
 274       m_c[2] = 0.0;
 275
 276       m_d[1] = 0.0;
 277       m_d[2] = 0.0;
 278     }
 279
 280     m_order = 2;
 281
 282     break;
 283
 284   case ZYN_FILTER_ANALOG_TYPE_NF2:
 285     if (zerocoefs == 0)
 286     {
 287       omega = 2 * PI * freq / m_sample_rate;
 288       sn = sin(omega);
 289       cs = cos(omega);
 290       alpha = sn / (2 * sqrt(tmpq));
 291       tmp = 1 + alpha;
 292       m_c[0] = 1 / tmp;
 293       m_c[1] = -2 * cs / tmp;
 294       m_c[2] = 1 / tmp;
 295       m_d[1] = -2 * cs / tmp * (-1);
 296       m_d[2] = (1 - alpha) / tmp * (-1);
 297     }
 298     else
 299     {
 300       m_c[0] = 1.0;
 301       m_c[1] = 0.0;
 302       m_c[2] = 0.0;
 303
 304       m_d[1] = 0.0;
 305       m_d[2] = 0.0;
 306     }
 307
 308     m_order = 2;
 309
 310     break;
 311   case ZYN_FILTER_ANALOG_TYPE_PKF2:
 312     if (zerocoefs == 0)
 313     {
 314       omega = 2 * PI * freq / m_sample_rate;
 315       sn = sin(omega);
 316       cs = cos(omega);
 317       tmpq *= 3.0;
 318       alpha = sn / (2 * tmpq);
 319       tmp = 1 + alpha / tmpgain;
 320       m_c[0] = (1.0 + alpha * tmpgain) / tmp;
 321       m_c[1] = (-2.0 * cs) / tmp;
 322       m_c[2] = (1.0 - alpha * tmpgain) / tmp;
 323       m_d[1] = -2 * cs / tmp * (-1);
 324       m_d[2] = (1 - alpha / tmpgain) / tmp * (-1);
 325     }
 326     else
 327     {
 328       m_c[0] = 1.0;
 329       m_c[1] = 0.0;
 330       m_c[2] = 0.0;
 331
 332       m_d[1] = 0.0;
 333       m_d[2] = 0.0;
 334     }
 335
 336     m_order = 2;
 337
 338     break;
 339
 340   case ZYN_FILTER_ANALOG_TYPE_LSH2:
 341     if (zerocoefs == 0)
 342     {
 343       omega = 2 * PI * freq / m_sample_rate;
 344       sn = sin(omega);
 345       cs = cos(omega);
 346       tmpq = sqrt(tmpq);
 347       alpha = sn / (2 * tmpq);
 348       beta = sqrt(tmpgain) / tmpq;
 349       tmp = (tmpgain + 1.0) + (tmpgain - 1.0) * cs + beta * sn;
 350
 351       m_c[0] = tmpgain * ((tmpgain + 1.0) - (tmpgain - 1.0) * cs + beta * sn) / tmp;
 352       m_c[1] = 2.0 * tmpgain * ((tmpgain - 1.0) - (tmpgain + 1.0) * cs) / tmp;
 353       m_c[2] = tmpgain * ((tmpgain + 1.0) - (tmpgain - 1.0) * cs - beta * sn) / tmp;
 354       m_d[1] = -2.0 * ((tmpgain - 1.0) + (tmpgain + 1.0) * cs) / tmp * (-1);
 355       m_d[2] = ((tmpgain + 1.0) + (tmpgain - 1.0) * cs - beta * sn) / tmp * (-1);
 356     }
 357     else
 358     {
 359       m_c[0] = tmpgain;
 360       m_c[1] = 0.0;
 361       m_c[2] = 0.0;
 362
 363       m_d[1] = 0.0;
 364       m_d[2] = 0.0;
 365     }
 366
 367     m_order = 2;
 368
 369     break;
 370
 371   case ZYN_FILTER_ANALOG_TYPE_HSH2:
 372     if (zerocoefs == 0)
 373     {
 374       omega = 2 * PI * freq / m_sample_rate;
 375       sn = sin(omega);
 376       cs = cos(omega);
 377       tmpq = sqrt(tmpq);
 378       alpha = sn / (2 * tmpq);
 379       beta = sqrt(tmpgain) / tmpq;
 380       tmp = (tmpgain + 1.0) - (tmpgain - 1.0) * cs + beta * sn;
 381
 382       m_c[0] = tmpgain * ((tmpgain + 1.0) + (tmpgain - 1.0) * cs + beta * sn) / tmp;
 383       m_c[1] = -2.0 * tmpgain * ((tmpgain - 1.0) + (tmpgain + 1.0) * cs) / tmp;
 384       m_c[2] = tmpgain * ((tmpgain + 1.0) + (tmpgain - 1.0) * cs - beta * sn) / tmp;
 385       m_d[1] = 2.0 * ((tmpgain - 1.0) - (tmpgain + 1.0) * cs) / tmp * (-1);
 386       m_d[2] = ((tmpgain + 1.0) - (tmpgain - 1.0) * cs - beta * sn) / tmp * (-1);
 387     }
 388     else
 389     {
 390       m_c[0] = 1.0;
 391       m_c[1] = 0.0;
 392       m_c[2] = 0.0;
 393
 394       m_d[1] = 0.0;
 395       m_d[2] = 0.0;
 396     }
 397
 398     m_order = 2;
 399
 400     break;
 401
 402   default:
 403     assert(0);                  // wrong type
 404   }
 405 }
 406
 407
 408 void
 409 AnalogFilter::setfreq(float frequency)
 410 {
 411   float rap;
 412   bool nyquist_thresh;
 413   int i;
 414
 415   if (frequency < 0.1)
 416   {
 417     frequency = 0.1;
 418   }
 419
 420   rap = m_frequency / frequency;
 421   if (rap < 1.0)
 422   {
 423     rap = 1.0 / rap;
 424   }
 425
 426   m_old_above_nq = m_above_nq;
 427   m_above_nq = frequency > (m_sample_rate / 2 - 500.0);
 428
 429   nyquist_thresh = ZYN_BOOL_XOR(m_above_nq, m_old_above_nq);
 430
 431   // if the frequency is changed fast, it needs interpolation (now, filter and coeficients backup)
 432   if (rap > 3.0 || nyquist_thresh)
 433   {
 434     for (i = 0 ; i < 3 ; i++)
 435     {
 436       m_c_old[i] = m_c[i];
 437       m_d_old[i] = m_d[i];
 438     }
 439
 440     for (i = 0 ; i < MAX_FILTER_STAGES + 1 ; i++)
 441     {
 442       m_x_old[i] = m_x[i];
 443       m_y_old[i] = m_y[i];
 444     }
 445
 446     if (!m_first_time)
 447     {
 448       m_needs_interpolation = true;
 449     }
 450   }
 451
 452   m_frequency = frequency;
 453
 454   computefiltercoefs();
 455
 456   m_first_time = false;
 457 }
 458
 459 void AnalogFilter::setfreq_and_q(float frequency, float q_factor)
 460 {
 461   m_q_factor = q_factor;
 462
 463   setfreq(frequency);
 464 }
 465
 466 void AnalogFilter::setq(float q_factor)
 467 {
 468   m_q_factor = q_factor;
 469
 470   computefiltercoefs();
 471 }
 472
 473 void AnalogFilter::settype(int type)
 474 {
 475   m_type = type;
 476
 477   computefiltercoefs();
 478 }
 479
 480 void AnalogFilter::setgain(float dBgain)
 481 {
 482   m_gain = dB2rap(dBgain);
 483
 484   computefiltercoefs();
 485 }
 486
 487 void AnalogFilter::setstages(int additional_stages)
 488 {
 489   if (additional_stages >= MAX_FILTER_STAGES)
 490   {
 491     m_additional_stages = MAX_FILTER_STAGES - 1;
 492   }
 493   else
 494   {
 495     m_additional_stages = additional_stages;
 496   }
 497
 498   cleanup();
 499
 500   computefiltercoefs();
 501 }
 502
 503 void
 504 AnalogFilter::singlefilterout(
 505   float * smp,
 506   struct analog_filter_stage * x,
 507   struct analog_filter_stage * y,
 508   float * c,
 509   float * d)
 510 {
 511   int i;
 512   float y0;
 513
 514   if (m_order == 1)             // First order filter
 515   {
 516     for (i=0 ; i < SOUND_BUFFER_SIZE ; i++)
 517     {
 518       y0 = smp[i] * c[0] + x->c1 * c[1] + y->c1 * d[1];
 519       y->c1 = y0;
 520       x->c1 = smp[i];
 521       // output
 522       smp[i] = y0;
 523     }
 524   }
 525
 526   if (m_order == 2)             // Second order filter
 527   {
 528     for (i = 0 ; i < SOUND_BUFFER_SIZE ; i++)
 529     {
 530       y0 = smp[i] * c[0] + x->c1 * c[1] + x->c2 * c[2] + y->c1 * d[1] + y->c2 * d[2];
 531       y->c2 = y->c1;
 532       y->c1 = y0;
 533       x->c2 = x->c1;
 534       x->c1 = smp[i];
 535       //output
 536       smp[i] = y0;
 537     }
 538   }
 539 }
 540
 541 void AnalogFilter::filterout(float *smp)
 542 {
 543   int i;
 544   float x;
 545
 546   if (m_needs_interpolation)
 547   {
 548     for (i = 0 ; i < SOUND_BUFFER_SIZE ; i++)
 549     {
 550       m_interpolation_buffer[i] = smp[i];
 551     }
 552
 553     for (i = 0 ; i < 1 + m_additional_stages ; i++)
 554     {
 555       singlefilterout(m_interpolation_buffer, m_x_old + i , m_y_old + i , m_c_old , m_d_old);
 556     }
 557   }
 558
 559   for (i = 0 ; i < 1 + m_additional_stages ; i++)
 560   {
 561     singlefilterout(smp, m_x + i, m_y + i, m_c, m_d);
 562   }
 563
 564   if (m_needs_interpolation)
 565   {
 566     for (i = 0 ; i < SOUND_BUFFER_SIZE ; i++)
 567     {
 568       x = i / (float)SOUND_BUFFER_SIZE;
 569       smp[i] = m_interpolation_buffer[i] * (1.0 - x) + smp[i] * x;
 570     }
 571
 572     m_needs_interpolation = false;
 573   }
 574
 575   for (i = 0 ; i < SOUND_BUFFER_SIZE ; i++)
 576   {
 577     smp[i] *= m_outgain;
 578   }
 579 }
 580
 581 float
 582 AnalogFilter::H(float freq)
 583 {
 584   float fr;
 585   float x;
 586   float y;
 587   float h;
 588   int n;
 589
 590   fr = freq / m_sample_rate * PI * 2.0;
 591   x = m_c[0];
 592   y = 0.0;
 593
 594   for (n = 1 ; n < 3 ; n++)
 595   {
 596     x += cos(n * fr) * m_c[n];
 597     y -= sin(n * fr) * m_c[n];
 598   }
 599
 600   h = x * x + y * y;
 601
 602   x = 1.0;
 603   y = 0.0;
 604
 605   for (n = 1 ; n < 3 ; n++)
 606   {
 607     x -= cos(n * fr) * m_d[n];
 608     y += sin(n * fr) * m_d[n];
 609   }
 610
 611   h = h / (x * x + y * y);
 612
 613   return pow(h, (m_additional_stages + 1.0) / 2.0);
 614 }