Quelle  Biquad.cpp   Sprache: C

/*
 * 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 "DenormalDisabler.h"

#include <float.h>
#include <algorithm>
#include <math.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