Source/WebCore/platform/audio/Biquad.cpp

   1 /*
   2  * Copyright (C) 2010 Google Inc. All rights reserved.
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions
   6  * are met:
   7  *
   8  * 1.  Redistributions of source code must retain the above copyright
   9  *     notice, this list of conditions and the following disclaimer.
  10  * 2.  Redistributions in binary form must reproduce the above copyright
  11  *     notice, this list of conditions and the following disclaimer in the
  12  *     documentation and/or other materials provided with the distribution.
  13  * 3.  Neither the name of Apple Computer, Inc. ("Apple") nor the names of
  14  *     its contributors may be used to endorse or promote products derived
  15  *     from this software without specific prior written permission.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY APPLE AND ITS CONTRIBUTORS "AS IS" AND ANY
  18  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  19  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  20  * DISCLAIMED. IN NO EVENT SHALL APPLE OR ITS CONTRIBUTORS BE LIABLE FOR ANY
  21  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  22  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  23  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  24  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  25  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  26  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  27  */
  28
  29 #include "config.h"
  30
  31 #if ENABLE(WEB_AUDIO)
  32
  33 #include "Biquad.h"
  34
  35 #include "DenormalDisabler.h"
  36 #include <algorithm>
  37 #include <stdio.h>
  38 #include <wtf/MathExtras.h>
  39
  40 #if OS(DARWIN)
  41 #include <Accelerate/Accelerate.h>
  42 #endif
  43
  44 namespace WebCore {
  45
  46 const int kBufferSize = 1024;
  47
  48 Biquad::Biquad()
  49 {
  50 #if OS(DARWIN)
  51     // Allocate two samples more for filter history
  52     m_inputBuffer.allocate(kBufferSize + 2);
  53     m_outputBuffer.allocate(kBufferSize + 2);
  54 #endif
  55
  56     // Initialize as pass-thru (straight-wire, no filter effect)
  57     m_b0 = 1;
  58     m_b1 = 0;
  59     m_b2 = 0;
  60     m_a1 = 0;
  61     m_a2 = 0;
  62
  63     reset(); // clear filter memory
  64 }
  65
  66 void Biquad::process(const float* sourceP, float* destP, size_t framesToProcess)
  67 {
  68 #if OS(DARWIN)
  69     // Use vecLib if available
  70     processFast(sourceP, destP, framesToProcess);
  71 #else
  72     int n = framesToProcess;
  73
  74     // Create local copies of member variables
  75     double x1 = m_x1;
  76     double x2 = m_x2;
  77     double y1 = m_y1;
  78     double y2 = m_y2;
  79
  80     double b0 = m_b0;
  81     double b1 = m_b1;
  82     double b2 = m_b2;
  83     double a1 = m_a1;
  84     double a2 = m_a2;
  85
  86     while (n--) {
  87         // FIXME: this can be optimized by pipelining the multiply adds...
  88         float x = *sourceP++;
  89         float y = b0*x + b1*x1 + b2*x2 - a1*y1 - a2*y2;
  90
  91         *destP++ = y;
  92
  93         // Update state variables
  94         x2 = x1;
  95         x1 = x;
  96         y2 = y1;
  97         y1 = y;
  98     }
  99
 100     // Local variables back to member. Flush denormals here so we
 101     // don't slow down the inner loop above.
 102     m_x1 = DenormalDisabler::flushDenormalFloatToZero(x1);
 103     m_x2 = DenormalDisabler::flushDenormalFloatToZero(x2);
 104     m_y1 = DenormalDisabler::flushDenormalFloatToZero(y1);
 105     m_y2 = DenormalDisabler::flushDenormalFloatToZero(y2);
 106
 107     m_b0 = b0;
 108     m_b1 = b1;
 109     m_b2 = b2;
 110     m_a1 = a1;
 111     m_a2 = a2;
 112 #endif
 113 }
 114
 115 #if OS(DARWIN)
 116
 117 // Here we have optimized version using Accelerate.framework
 118
 119 void Biquad::processFast(const float* sourceP, float* destP, size_t framesToProcess)
 120 {
 121     double filterCoefficients[5];
 122     filterCoefficients[0] = m_b0;
 123     filterCoefficients[1] = m_b1;
 124     filterCoefficients[2] = m_b2;
 125     filterCoefficients[3] = m_a1;
 126     filterCoefficients[4] = m_a2;
 127
 128     double* inputP = m_inputBuffer.data();
 129     double* outputP = m_outputBuffer.data();
 130
 131     double* input2P = inputP + 2;
 132     double* output2P = outputP + 2;
 133
 134     // Break up processing into smaller slices (kBufferSize) if necessary.
 135
 136     int n = framesToProcess;
 137
 138     while (n > 0) {
 139         int framesThisTime = n < kBufferSize ? n : kBufferSize;
 140
 141         // Copy input to input buffer
 142         for (int i = 0; i < framesThisTime; ++i)
 143             input2P[i] = *sourceP++;
 144
 145         processSliceFast(inputP, outputP, filterCoefficients, framesThisTime);
 146
 147         // Copy output buffer to output (converts float -> double).
 148         for (int i = 0; i < framesThisTime; ++i)
 149             *destP++ = static_cast<float>(output2P[i]);
 150
 151         n -= framesThisTime;
 152     }
 153 }
 154
 155 void Biquad::processSliceFast(double* sourceP, double* destP, double* coefficientsP, size_t framesToProcess)
 156 {
 157     // Use double-precision for filter stability
 158     vDSP_deq22D(sourceP, 1, coefficientsP, destP, 1, framesToProcess);
 159
 160     // Save history.  Note that sourceP and destP reference m_inputBuffer and m_outputBuffer respectively.
 161     // These buffers are allocated (in the constructor) with space for two extra samples so it's OK to access
 162     // array values two beyond framesToProcess.
 163     sourceP[0] = sourceP[framesToProcess - 2 + 2];
 164     sourceP[1] = sourceP[framesToProcess - 1 + 2];
 165     destP[0] = destP[framesToProcess - 2 + 2];
 166     destP[1] = destP[framesToProcess - 1 + 2];
 167 }
 168
 169 #endif // OS(DARWIN)
 170
 171
 172 void Biquad::reset()
 173 {
 174     m_x1 = m_x2 = m_y1 = m_y2 = 0;
 175
 176 #if OS(DARWIN)
 177     // Two extra samples for filter history
 178     double* inputP = m_inputBuffer.data();
 179     inputP[0] = 0;
 180     inputP[1] = 0;
 181
 182     double* outputP = m_outputBuffer.data();
 183     outputP[0] = 0;
 184     outputP[1] = 0;
 185 #endif
 186 }
 187
 188 void Biquad::setLowpassParams(double cutoff, double resonance)
 189 {
 190     resonance = std::max(0.0, resonance); // can't go negative
 191     // Limit cutoff to 0 to 1.
 192     cutoff = std::max(0.0, std::min(cutoff, 1.0));
 193
 194     double g = pow(10.0, 0.05 * resonance);
 195     double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
 196
 197     if (cutoff == 1) {
 198         // When cutoff is 1, the z-transform is 1.
 199         m_b0 = 1;
 200         m_b1 = 0;
 201         m_b2 = 0;
 202         m_a1 = 0;
 203         m_a2 = 0;
 204     } else if (cutoff > 0) {
 205         // Compute biquad coefficients for lowpass filter
 206         double theta = piDouble * cutoff;
 207         double sn = 0.5 * d * sin(theta);
 208         double beta = 0.5 * (1 - sn) / (1 + sn);
 209         double gamma = (0.5 + beta) * cos(theta);
 210         double alpha = 0.25 * (0.5 + beta - gamma);
 211
 212         m_b0 = 2 * alpha;
 213         m_b1 = 2 * 2 * alpha;
 214         m_b2 = 2 * alpha;
 215         m_a1 = 2 * -gamma;
 216         m_a2 = 2 * beta;
 217     } else {
 218         // When cutoff is zero, nothing gets through the filter, so set
 219         // coefficients up correctly.
 220         m_b0 = 0;
 221         m_b1 = 0;
 222         m_b2 = 0;
 223         m_a1 = 0;
 224         m_a2 = 0;
 225     }
 226 }
 227
 228 void Biquad::setHighpassParams(double cutoff, double resonance)
 229 {
 230     resonance = std::max(0.0, resonance); // can't go negative
 231
 232     // Limit cutoff to 0 to 1.
 233     cutoff = std::max(0.0, std::min(cutoff, 1.0));
 234
 235     double g = pow(10.0, 0.05 * resonance);
 236     double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
 237
 238     if (cutoff == 1) {
 239         // The z-transform is 0.
 240         m_b0 = 0;
 241         m_b1 = 0;
 242         m_b2 = 0;
 243         m_a1 = 0;
 244         m_a2 = 0;
 245     } else if (cutoff > 0) {
 246         // Compute biquad coefficients for highpass filter
 247         double theta = piDouble * cutoff;
 248         double sn = 0.5 * d * sin(theta);
 249         double beta = 0.5 * (1 - sn) / (1 + sn);
 250         double gamma = (0.5 + beta) * cos(theta);
 251         double alpha = 0.25 * (0.5 + beta + gamma);
 252
 253         m_b0 = 2 * alpha;
 254         m_b1 = 2 * -2 * alpha;
 255         m_b2 = 2 * alpha;
 256         m_a1 = 2 * -gamma;
 257         m_a2 = 2 * beta;
 258     } else {
 259       // When cutoff is zero, we need to be careful because the above
 260       // gives a quadratic divided by the same quadratic, with poles
 261       // and zeros on the unit circle in the same place. When cutoff
 262       // is zero, the z-transform is 1.
 263       m_b0 = 1;
 264       m_b1 = 0;
 265       m_b2 = 0;
 266       m_a1 = 0;
 267       m_a2 = 0;
 268     }
 269 }
 270
 271 void Biquad::setNormalizedCoefficients(double b0, double b1, double b2, double a0, double a1, double a2)
 272 {
 273     double a0Inverse = 1 / a0;
 274
 275     m_b0 = b0 * a0Inverse;
 276     m_b1 = b1 * a0Inverse;
 277     m_b2 = b2 * a0Inverse;
 278     m_a1 = a1 * a0Inverse;
 279     m_a2 = a2 * a0Inverse;
 280 }
 281
 282 void Biquad::setLowShelfParams(double frequency, double dbGain)
 283 {
 284     // Clip frequencies to between 0 and 1, inclusive.
 285     frequency = std::max(0.0, std::min(frequency, 1.0));
 286
 287     double A = pow(10.0, dbGain / 40);
 288
 289     if (frequency == 1) {
 290         // The z-transform is a constant gain.
 291         setNormalizedCoefficients(A * A, 0, 0,
 292                                   1, 0, 0);
 293     } else if (frequency > 0) {
 294         double w0 = piDouble * frequency;
 295         double S = 1; // filter slope (1 is max value)
 296         double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
 297         double k = cos(w0);
 298         double k2 = 2 * sqrt(A) * alpha;
 299         double aPlusOne = A + 1;
 300         double aMinusOne = A - 1;
 301
 302         double b0 = A * (aPlusOne - aMinusOne * k + k2);
 303         double b1 = 2 * A * (aMinusOne - aPlusOne * k);
 304         double b2 = A * (aPlusOne - aMinusOne * k - k2);
 305         double a0 = aPlusOne + aMinusOne * k + k2;
 306         double a1 = -2 * (aMinusOne + aPlusOne * k);
 307         double a2 = aPlusOne + aMinusOne * k - k2;
 308
 309         setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 310     } else {
 311         // When frequency is 0, the z-transform is 1.
 312         setNormalizedCoefficients(1, 0, 0,
 313                                   1, 0, 0);
 314     }
 315 }
 316
 317 void Biquad::setHighShelfParams(double frequency, double dbGain)
 318 {
 319     // Clip frequencies to between 0 and 1, inclusive.
 320     frequency = std::max(0.0, std::min(frequency, 1.0));
 321
 322     double A = pow(10.0, dbGain / 40);
 323
 324     if (frequency == 1) {
 325         // The z-transform is 1.
 326         setNormalizedCoefficients(1, 0, 0,
 327                                   1, 0, 0);
 328     } else if (frequency > 0) {
 329         double w0 = piDouble * frequency;
 330         double S = 1; // filter slope (1 is max value)
 331         double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
 332         double k = cos(w0);
 333         double k2 = 2 * sqrt(A) * alpha;
 334         double aPlusOne = A + 1;
 335         double aMinusOne = A - 1;
 336
 337         double b0 = A * (aPlusOne + aMinusOne * k + k2);
 338         double b1 = -2 * A * (aMinusOne + aPlusOne * k);
 339         double b2 = A * (aPlusOne + aMinusOne * k - k2);
 340         double a0 = aPlusOne - aMinusOne * k + k2;
 341         double a1 = 2 * (aMinusOne - aPlusOne * k);
 342         double a2 = aPlusOne - aMinusOne * k - k2;
 343
 344         setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 345     } else {
 346         // When frequency = 0, the filter is just a gain, A^2.
 347         setNormalizedCoefficients(A * A, 0, 0,
 348                                   1, 0, 0);
 349     }
 350 }
 351
 352 void Biquad::setPeakingParams(double frequency, double Q, double dbGain)
 353 {
 354     // Clip frequencies to between 0 and 1, inclusive.
 355     frequency = std::max(0.0, std::min(frequency, 1.0));
 356
 357     // Don't let Q go negative, which causes an unstable filter.
 358     Q = std::max(0.0, Q);
 359
 360     double A = pow(10.0, dbGain / 40);
 361
 362     if (frequency > 0 && frequency < 1) {
 363         if (Q > 0) {
 364             double w0 = piDouble * frequency;
 365             double alpha = sin(w0) / (2 * Q);
 366             double k = cos(w0);
 367
 368             double b0 = 1 + alpha * A;
 369             double b1 = -2 * k;
 370             double b2 = 1 - alpha * A;
 371             double a0 = 1 + alpha / A;
 372             double a1 = -2 * k;
 373             double a2 = 1 - alpha / A;
 374
 375             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 376         } else {
 377             // When Q = 0, the above formulas have problems. If we look at
 378             // the z-transform, we can see that the limit as Q->0 is A^2, so
 379             // set the filter that way.
 380             setNormalizedCoefficients(A * A, 0, 0,
 381                                       1, 0, 0);
 382         }
 383     } else {
 384         // When frequency is 0 or 1, the z-transform is 1.
 385         setNormalizedCoefficients(1, 0, 0,
 386                                   1, 0, 0);
 387     }
 388 }
 389
 390 void Biquad::setAllpassParams(double frequency, double Q)
 391 {
 392     // Clip frequencies to between 0 and 1, inclusive.
 393     frequency = std::max(0.0, std::min(frequency, 1.0));
 394
 395     // Don't let Q go negative, which causes an unstable filter.
 396     Q = std::max(0.0, Q);
 397
 398     if (frequency > 0 && frequency < 1) {
 399         if (Q > 0) {
 400             double w0 = piDouble * frequency;
 401             double alpha = sin(w0) / (2 * Q);
 402             double k = cos(w0);
 403
 404             double b0 = 1 - alpha;
 405             double b1 = -2 * k;
 406             double b2 = 1 + alpha;
 407             double a0 = 1 + alpha;
 408             double a1 = -2 * k;
 409             double a2 = 1 - alpha;
 410
 411             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 412         } else {
 413             // When Q = 0, the above formulas have problems. If we look at
 414             // the z-transform, we can see that the limit as Q->0 is -1, so
 415             // set the filter that way.
 416             setNormalizedCoefficients(-1, 0, 0,
 417                                       1, 0, 0);
 418         }
 419     } else {
 420         // When frequency is 0 or 1, the z-transform is 1.
 421         setNormalizedCoefficients(1, 0, 0,
 422                                   1, 0, 0);
 423     }
 424 }
 425
 426 void Biquad::setNotchParams(double frequency, double Q)
 427 {
 428     // Clip frequencies to between 0 and 1, inclusive.
 429     frequency = std::max(0.0, std::min(frequency, 1.0));
 430
 431     // Don't let Q go negative, which causes an unstable filter.
 432     Q = std::max(0.0, Q);
 433
 434     if (frequency > 0 && frequency < 1) {
 435         if (Q > 0) {
 436             double w0 = piDouble * frequency;
 437             double alpha = sin(w0) / (2 * Q);
 438             double k = cos(w0);
 439
 440             double b0 = 1;
 441             double b1 = -2 * k;
 442             double b2 = 1;
 443             double a0 = 1 + alpha;
 444             double a1 = -2 * k;
 445             double a2 = 1 - alpha;
 446
 447             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 448         } else {
 449             // When Q = 0, the above formulas have problems. If we look at
 450             // the z-transform, we can see that the limit as Q->0 is 0, so
 451             // set the filter that way.
 452             setNormalizedCoefficients(0, 0, 0,
 453                                       1, 0, 0);
 454         }
 455     } else {
 456         // When frequency is 0 or 1, the z-transform is 1.
 457         setNormalizedCoefficients(1, 0, 0,
 458                                   1, 0, 0);
 459     }
 460 }
 461
 462 void Biquad::setBandpassParams(double frequency, double Q)
 463 {
 464     // No negative frequencies allowed.
 465     frequency = std::max(0.0, frequency);
 466
 467     // Don't let Q go negative, which causes an unstable filter.
 468     Q = std::max(0.0, Q);
 469
 470     if (frequency > 0 && frequency < 1) {
 471         double w0 = piDouble * frequency;
 472         if (Q > 0) {
 473             double alpha = sin(w0) / (2 * Q);
 474             double k = cos(w0);
 475
 476             double b0 = alpha;
 477             double b1 = 0;
 478             double b2 = -alpha;
 479             double a0 = 1 + alpha;
 480             double a1 = -2 * k;
 481             double a2 = 1 - alpha;
 482
 483             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 484         } else {
 485             // When Q = 0, the above formulas have problems. If we look at
 486             // the z-transform, we can see that the limit as Q->0 is 1, so
 487             // set the filter that way.
 488             setNormalizedCoefficients(1, 0, 0,
 489                                       1, 0, 0);
 490         }
 491     } else {
 492         // When the cutoff is zero, the z-transform approaches 0, if Q
 493         // > 0. When both Q and cutoff are zero, the z-transform is
 494         // pretty much undefined. What should we do in this case?
 495         // For now, just make the filter 0. When the cutoff is 1, the
 496         // z-transform also approaches 0.
 497         setNormalizedCoefficients(0, 0, 0,
 498                                   1, 0, 0);
 499     }
 500 }
 501
 502 void Biquad::setZeroPolePairs(const Complex &zero, const Complex &pole)
 503 {
 504     m_b0 = 1;
 505     m_b1 = -2 * zero.real();
 506
 507     double zeroMag = abs(zero);
 508     m_b2 = zeroMag * zeroMag;
 509
 510     m_a1 = -2 * pole.real();
 511
 512     double poleMag = abs(pole);
 513     m_a2 = poleMag * poleMag;
 514 }
 515
 516 void Biquad::setAllpassPole(const Complex &pole)
 517 {
 518     Complex zero = Complex(1, 0) / pole;
 519     setZeroPolePairs(zero, pole);
 520 }
 521
 522 void Biquad::getFrequencyResponse(int nFrequencies,
 523                                   const float* frequency,
 524                                   float* magResponse,
 525                                   float* phaseResponse)
 526 {
 527     // Evaluate the Z-transform of the filter at given normalized
 528     // frequency from 0 to 1.  (1 corresponds to the Nyquist
 529     // frequency.)
 530     //
 531     // The z-transform of the filter is
 532     //
 533     // H(z) = (b0 + b1*z^(-1) + b2*z^(-2))/(1 + a1*z^(-1) + a2*z^(-2))
 534     //
 535     // Evaluate as
 536     //
 537     // b0 + (b1 + b2*z1)*z1
 538     // --------------------
 539     // 1 + (a1 + a2*z1)*z1
 540     //
 541     // with z1 = 1/z and z = exp(j*pi*frequency). Hence z1 = exp(-j*pi*frequency)
 542
 543     // Make local copies of the coefficients as a micro-optimization.
 544     double b0 = m_b0;
 545     double b1 = m_b1;
 546     double b2 = m_b2;
 547     double a1 = m_a1;
 548     double a2 = m_a2;
 549
 550     for (int k = 0; k < nFrequencies; ++k) {
 551         double omega = -piDouble * frequency[k];
 552         Complex z = Complex(cos(omega), sin(omega));
 553         Complex numerator = b0 + (b1 + b2 * z) * z;
 554         Complex denominator = Complex(1, 0) + (a1 + a2 * z) * z;
 555         Complex response = numerator / denominator;
 556         magResponse[k] = static_cast<float>(abs(response));
 557         phaseResponse[k] = static_cast<float>(atan2(imag(response), real(response)));
 558     }
 559 }
 560
 561 } // namespace WebCore
 562
 563 #endif // ENABLE(WEB_AUDIO)