tor-browser

Biquad.cpp (14289B)

---

/*
* Copyright (C) 2010 Google Inc. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1.  Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
* 2.  Redistributions in binary form must reproduce the above copyright
*     notice, this list of conditions and the following disclaimer in the
*     documentation and/or other materials provided with the distribution.
* 3.  Neither the name of Apple Computer, Inc. ("Apple") nor the names of
*     its contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY APPLE AND ITS CONTRIBUTORS "AS IS" AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL APPLE OR ITS CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "Biquad.h"

#include <float.h>
#include <math.h>

#include <algorithm>

#include "DenormalDisabler.h"
#include "fdlibm.h"

namespace WebCore {

Biquad::Biquad() {
 // Initialize as pass-thru (straight-wire, no filter effect)
 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);

 reset();  // clear filter memory
}

Biquad::~Biquad() = default;

void Biquad::process(const float* sourceP, float* destP,
                    size_t framesToProcess) {
 // Create local copies of member variables
 double x1 = m_x1;
 double x2 = m_x2;
 double y1 = m_y1;
 double y2 = m_y2;

 double b0 = m_b0;
 double b1 = m_b1;
 double b2 = m_b2;
 double a1 = m_a1;
 double a2 = m_a2;

 for (size_t i = 0; i < framesToProcess; ++i) {
   // FIXME: this can be optimized by pipelining the multiply adds...
   double x = sourceP[i];
   double y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;

   destP[i] = y;

   // Update state variables
   x2 = x1;
   x1 = x;
   y2 = y1;
   y1 = y;
 }

 // Avoid introducing a stream of subnormals when input is silent and the
 // tail approaches zero.
 if (x1 == 0.0 && x2 == 0.0 && (y1 != 0.0 || y2 != 0.0) &&
     fabs(y1) < FLT_MIN && fabs(y2) < FLT_MIN) {
   // Flush future values to zero (until there is new input).
   y1 = y2 = 0.0;
// Flush calculated values.
#ifndef HAVE_DENORMAL
   for (int i = framesToProcess; i-- && fabsf(destP[i]) < FLT_MIN;) {
     destP[i] = 0.0f;
   }
#endif
 }
 // Local variables back to member.
 m_x1 = x1;
 m_x2 = x2;
 m_y1 = y1;
 m_y2 = y2;
}

void Biquad::reset() { m_x1 = m_x2 = m_y1 = m_y2 = 0; }

void Biquad::setLowpassParams(double cutoff, double resonance) {
 // Limit cutoff to 0 to 1.
 cutoff = std::max(0.0, std::min(cutoff, 1.0));

 if (cutoff == 1) {
   // When cutoff is 1, the z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 } else if (cutoff > 0) {
   // Compute biquad coefficients for lowpass filter
   double g = fdlibm_pow(10.0, -0.05 * resonance);
   double w0 = M_PI * cutoff;
   double cos_w0 = fdlibm_cos(w0);
   double alpha = 0.5 * fdlibm_sin(w0) * g;

   double b1 = 1.0 - cos_w0;
   double b0 = 0.5 * b1;
   double b2 = b0;
   double a0 = 1.0 + alpha;
   double a1 = -2.0 * cos_w0;
   double a2 = 1.0 - alpha;

   setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 } else {
   // When cutoff is zero, nothing gets through the filter, so set
   // coefficients up correctly.
   setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
 }
}

void Biquad::setHighpassParams(double cutoff, double resonance) {
 // Limit cutoff to 0 to 1.
 cutoff = std::max(0.0, std::min(cutoff, 1.0));

 if (cutoff == 1) {
   // The z-transform is 0.
   setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
 } else if (cutoff > 0) {
   // Compute biquad coefficients for highpass filter
   double g = fdlibm_pow(10.0, -0.05 * resonance);
   double w0 = M_PI * cutoff;
   double cos_w0 = fdlibm_cos(w0);
   double alpha = 0.5 * fdlibm_sin(w0) * g;

   double b1 = -1.0 - cos_w0;
   double b0 = -0.5 * b1;
   double b2 = b0;
   double a0 = 1.0 + alpha;
   double a1 = -2.0 * cos_w0;
   double a2 = 1.0 - alpha;

   setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 } else {
   // When cutoff is zero, we need to be careful because the above
   // gives a quadratic divided by the same quadratic, with poles
   // and zeros on the unit circle in the same place. When cutoff
   // is zero, the z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 }
}

void Biquad::setNormalizedCoefficients(double b0, double b1, double b2,
                                      double a0, double a1, double a2) {
 double a0Inverse = 1 / a0;

 m_b0 = b0 * a0Inverse;
 m_b1 = b1 * a0Inverse;
 m_b2 = b2 * a0Inverse;
 m_a1 = a1 * a0Inverse;
 m_a2 = a2 * a0Inverse;
}

void Biquad::setLowShelfParams(double frequency, double dbGain) {
 // Clip frequencies to between 0 and 1, inclusive.
 frequency = std::max(0.0, std::min(frequency, 1.0));

 double A = fdlibm_pow(10.0, dbGain / 40);

 if (frequency == 1) {
   // The z-transform is a constant gain.
   setNormalizedCoefficients(A * A, 0, 0, 1, 0, 0);
 } else if (frequency > 0) {
   double w0 = M_PI * frequency;
   double S = 1;  // filter slope (1 is max value)
   double alpha = 0.5 * fdlibm_sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
   double k = fdlibm_cos(w0);
   double k2 = 2 * sqrt(A) * alpha;
   double aPlusOne = A + 1;
   double aMinusOne = A - 1;

   double b0 = A * (aPlusOne - aMinusOne * k + k2);
   double b1 = 2 * A * (aMinusOne - aPlusOne * k);
   double b2 = A * (aPlusOne - aMinusOne * k - k2);
   double a0 = aPlusOne + aMinusOne * k + k2;
   double a1 = -2 * (aMinusOne + aPlusOne * k);
   double a2 = aPlusOne + aMinusOne * k - k2;

   setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 } else {
   // When frequency is 0, the z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 }
}

void Biquad::setHighShelfParams(double frequency, double dbGain) {
 // Clip frequencies to between 0 and 1, inclusive.
 frequency = std::max(0.0, std::min(frequency, 1.0));

 double A = fdlibm_pow(10.0, dbGain / 40);

 if (frequency == 1) {
   // The z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 } else if (frequency > 0) {
   double w0 = M_PI * frequency;
   double S = 1;  // filter slope (1 is max value)
   double alpha = 0.5 * fdlibm_sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
   double k = fdlibm_cos(w0);
   double k2 = 2 * sqrt(A) * alpha;
   double aPlusOne = A + 1;
   double aMinusOne = A - 1;

   double b0 = A * (aPlusOne + aMinusOne * k + k2);
   double b1 = -2 * A * (aMinusOne + aPlusOne * k);
   double b2 = A * (aPlusOne + aMinusOne * k - k2);
   double a0 = aPlusOne - aMinusOne * k + k2;
   double a1 = 2 * (aMinusOne - aPlusOne * k);
   double a2 = aPlusOne - aMinusOne * k - k2;

   setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
 } else {
   // When frequency = 0, the filter is just a gain, A^2.
   setNormalizedCoefficients(A * A, 0, 0, 1, 0, 0);
 }
}

void Biquad::setPeakingParams(double frequency, double Q, double dbGain) {
 // Clip frequencies to between 0 and 1, inclusive.
 frequency = std::max(0.0, std::min(frequency, 1.0));

 // Don't let Q go negative, which causes an unstable filter.
 Q = std::max(0.0, Q);

 double A = fdlibm_pow(10.0, dbGain / 40);

 if (frequency > 0 && frequency < 1) {
   if (Q > 0) {
     double w0 = M_PI * frequency;
     double alpha = fdlibm_sin(w0) / (2 * Q);
     double k = fdlibm_cos(w0);

     double b0 = 1 + alpha * A;
     double b1 = -2 * k;
     double b2 = 1 - alpha * A;
     double a0 = 1 + alpha / A;
     double a1 = -2 * k;
     double a2 = 1 - alpha / A;

     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
   } else {
     // When Q = 0, the above formulas have problems. If we look at
     // the z-transform, we can see that the limit as Q->0 is A^2, so
     // set the filter that way.
     setNormalizedCoefficients(A * A, 0, 0, 1, 0, 0);
   }
 } else {
   // When frequency is 0 or 1, the z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 }
}

void Biquad::setAllpassParams(double frequency, double Q) {
 // Clip frequencies to between 0 and 1, inclusive.
 frequency = std::max(0.0, std::min(frequency, 1.0));

 // Don't let Q go negative, which causes an unstable filter.
 Q = std::max(0.0, Q);

 if (frequency > 0 && frequency < 1) {
   if (Q > 0) {
     double w0 = M_PI * frequency;
     double alpha = fdlibm_sin(w0) / (2 * Q);
     double k = fdlibm_cos(w0);

     double b0 = 1 - alpha;
     double b1 = -2 * k;
     double b2 = 1 + alpha;
     double a0 = 1 + alpha;
     double a1 = -2 * k;
     double a2 = 1 - alpha;

     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
   } else {
     // When Q = 0, the above formulas have problems. If we look at
     // the z-transform, we can see that the limit as Q->0 is -1, so
     // set the filter that way.
     setNormalizedCoefficients(-1, 0, 0, 1, 0, 0);
   }
 } else {
   // When frequency is 0 or 1, the z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 }
}

void Biquad::setNotchParams(double frequency, double Q) {
 // Clip frequencies to between 0 and 1, inclusive.
 frequency = std::max(0.0, std::min(frequency, 1.0));

 // Don't let Q go negative, which causes an unstable filter.
 Q = std::max(0.0, Q);

 if (frequency > 0 && frequency < 1) {
   if (Q > 0) {
     double w0 = M_PI * frequency;
     double alpha = fdlibm_sin(w0) / (2 * Q);
     double k = fdlibm_cos(w0);

     double b0 = 1;
     double b1 = -2 * k;
     double b2 = 1;
     double a0 = 1 + alpha;
     double a1 = -2 * k;
     double a2 = 1 - alpha;

     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
   } else {
     // When Q = 0, the above formulas have problems. If we look at
     // the z-transform, we can see that the limit as Q->0 is 0, so
     // set the filter that way.
     setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
   }
 } else {
   // When frequency is 0 or 1, the z-transform is 1.
   setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
 }
}

void Biquad::setBandpassParams(double frequency, double Q) {
 // No negative frequencies allowed.
 frequency = std::max(0.0, frequency);

 // Don't let Q go negative, which causes an unstable filter.
 Q = std::max(0.0, Q);

 if (frequency > 0 && frequency < 1) {
   double w0 = M_PI * frequency;
   if (Q > 0) {
     double alpha = fdlibm_sin(w0) / (2 * Q);
     double k = fdlibm_cos(w0);

     double b0 = alpha;
     double b1 = 0;
     double b2 = -alpha;
     double a0 = 1 + alpha;
     double a1 = -2 * k;
     double a2 = 1 - alpha;

     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
   } else {
     // When Q = 0, the above formulas have problems. If we look at
     // the z-transform, we can see that the limit as Q->0 is 1, so
     // set the filter that way.
     setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
   }
 } else {
   // When the cutoff is zero, the z-transform approaches 0, if Q
   // > 0. When both Q and cutoff are zero, the z-transform is
   // pretty much undefined. What should we do in this case?
   // For now, just make the filter 0. When the cutoff is 1, the
   // z-transform also approaches 0.
   setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
 }
}

void Biquad::setZeroPolePairs(const Complex& zero, const Complex& pole) {
 double b0 = 1;
 double b1 = -2 * zero.real();

 double zeroMag = abs(zero);
 double b2 = zeroMag * zeroMag;

 double a1 = -2 * pole.real();

 double poleMag = abs(pole);
 double a2 = poleMag * poleMag;
 setNormalizedCoefficients(b0, b1, b2, 1, a1, a2);
}

void Biquad::setAllpassPole(const Complex& pole) {
 Complex zero = Complex(1, 0) / pole;
 setZeroPolePairs(zero, pole);
}

void Biquad::getFrequencyResponse(int nFrequencies, const float* frequency,
                                 float* magResponse, float* phaseResponse) {
 // Evaluate the Z-transform of the filter at given normalized
 // frequency from 0 to 1.  (1 corresponds to the Nyquist
 // frequency.)
 //
 // The z-transform of the filter is
 //
 // H(z) = (b0 + b1*z^(-1) + b2*z^(-2))/(1 + a1*z^(-1) + a2*z^(-2))
 //
 // Evaluate as
 //
 // b0 + (b1 + b2*z1)*z1
 // --------------------
 // 1 + (a1 + a2*z1)*z1
 //
 // with z1 = 1/z and z = exp(j*pi*frequency). Hence z1 = exp(-j*pi*frequency)

 // Make local copies of the coefficients as a micro-optimization.
 double b0 = m_b0;
 double b1 = m_b1;
 double b2 = m_b2;
 double a1 = m_a1;
 double a2 = m_a2;

 for (int k = 0; k < nFrequencies; ++k) {
   double omega = -M_PI * frequency[k];
   Complex z = Complex(fdlibm_cos(omega), fdlibm_sin(omega));
   Complex numerator = b0 + (b1 + b2 * z) * z;
   Complex denominator = Complex(1, 0) + (a1 + a2 * z) * z;
   // Strangely enough, using complex division:
   // e.g. Complex response = numerator / denominator;
   // fails on our test machines, yielding infinities and NaNs, so we do
   // things the long way here.
   double n = norm(denominator);
   double r = (real(numerator) * real(denominator) +
               imag(numerator) * imag(denominator)) /
              n;
   double i = (imag(numerator) * real(denominator) -
               real(numerator) * imag(denominator)) /
              n;
   std::complex<double> response = std::complex<double>(r, i);

   magResponse[k] =
       static_cast<float>(fdlibm_hypot(real(response), imag(response)));
   phaseResponse[k] =
       static_cast<float>(fdlibm_atan2(imag(response), real(response)));
 }
}

}  // namespace WebCore