1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/content/media/webaudio/blink/Biquad.cpp Wed Dec 31 06:09:35 2014 +0100
1.3 @@ -0,0 +1,471 @@
1.4 +/*
1.5 + * Copyright (C) 2010 Google Inc. All rights reserved.
1.6 + *
1.7 + * Redistribution and use in source and binary forms, with or without
1.8 + * modification, are permitted provided that the following conditions
1.9 + * are met:
1.10 + *
1.11 + * 1. Redistributions of source code must retain the above copyright
1.12 + * notice, this list of conditions and the following disclaimer.
1.13 + * 2. Redistributions in binary form must reproduce the above copyright
1.14 + * notice, this list of conditions and the following disclaimer in the
1.15 + * documentation and/or other materials provided with the distribution.
1.16 + * 3. Neither the name of Apple Computer, Inc. ("Apple") nor the names of
1.17 + * its contributors may be used to endorse or promote products derived
1.18 + * from this software without specific prior written permission.
1.19 + *
1.20 + * THIS SOFTWARE IS PROVIDED BY APPLE AND ITS CONTRIBUTORS "AS IS" AND ANY
1.21 + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
1.22 + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
1.23 + * DISCLAIMED. IN NO EVENT SHALL APPLE OR ITS CONTRIBUTORS BE LIABLE FOR ANY
1.24 + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
1.25 + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
1.26 + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
1.27 + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
1.28 + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
1.29 + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
1.30 + */
1.31 +
1.32 +// For M_PI from cmath
1.33 +#ifdef _MSC_VER
1.34 +# define _USE_MATH_DEFINES
1.35 +#endif
1.36 +
1.37 +#include "Biquad.h"
1.38 +
1.39 +#include <cmath>
1.40 +#include <float.h>
1.41 +#include <algorithm>
1.42 +
1.43 +namespace WebCore {
1.44 +
1.45 +Biquad::Biquad()
1.46 +{
1.47 + // Initialize as pass-thru (straight-wire, no filter effect)
1.48 + setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
1.49 +
1.50 + reset(); // clear filter memory
1.51 +}
1.52 +
1.53 +Biquad::~Biquad()
1.54 +{
1.55 +}
1.56 +
1.57 +void Biquad::process(const float* sourceP, float* destP, size_t framesToProcess)
1.58 +{
1.59 + // Create local copies of member variables
1.60 + double x1 = m_x1;
1.61 + double x2 = m_x2;
1.62 + double y1 = m_y1;
1.63 + double y2 = m_y2;
1.64 +
1.65 + double b0 = m_b0;
1.66 + double b1 = m_b1;
1.67 + double b2 = m_b2;
1.68 + double a1 = m_a1;
1.69 + double a2 = m_a2;
1.70 +
1.71 + for (size_t i = 0; i < framesToProcess; ++i) {
1.72 + // FIXME: this can be optimized by pipelining the multiply adds...
1.73 + double x = sourceP[i];
1.74 + double y = b0*x + b1*x1 + b2*x2 - a1*y1 - a2*y2;
1.75 +
1.76 + destP[i] = y;
1.77 +
1.78 + // Update state variables
1.79 + x2 = x1;
1.80 + x1 = x;
1.81 + y2 = y1;
1.82 + y1 = y;
1.83 + }
1.84 +
1.85 + // Avoid introducing a stream of subnormals when input is silent and the
1.86 + // tail approaches zero.
1.87 + if (x1 == 0.0 && x2 == 0.0 && (y1 != 0.0 || y2 != 0.0) &&
1.88 + fabs(y1) < FLT_MIN && fabs(y2) < FLT_MIN) {
1.89 + // Flush future values to zero (until there is new input).
1.90 + y1 = y2 = 0.0;
1.91 + // Flush calculated values.
1.92 + for (int i = framesToProcess; i-- && fabsf(destP[i]) < FLT_MIN; ) {
1.93 + destP[i] = 0.0f;
1.94 + }
1.95 + }
1.96 + // Local variables back to member.
1.97 + m_x1 = x1;
1.98 + m_x2 = x2;
1.99 + m_y1 = y1;
1.100 + m_y2 = y2;
1.101 +}
1.102 +
1.103 +void Biquad::reset()
1.104 +{
1.105 + m_x1 = m_x2 = m_y1 = m_y2 = 0;
1.106 +}
1.107 +
1.108 +void Biquad::setLowpassParams(double cutoff, double resonance)
1.109 +{
1.110 + // Limit cutoff to 0 to 1.
1.111 + cutoff = std::max(0.0, std::min(cutoff, 1.0));
1.112 +
1.113 + if (cutoff == 1) {
1.114 + // When cutoff is 1, the z-transform is 1.
1.115 + setNormalizedCoefficients(1, 0, 0,
1.116 + 1, 0, 0);
1.117 + } else if (cutoff > 0) {
1.118 + // Compute biquad coefficients for lowpass filter
1.119 + resonance = std::max(0.0, resonance); // can't go negative
1.120 + double g = pow(10.0, 0.05 * resonance);
1.121 + double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
1.122 +
1.123 + double theta = M_PI * cutoff;
1.124 + double sn = 0.5 * d * sin(theta);
1.125 + double beta = 0.5 * (1 - sn) / (1 + sn);
1.126 + double gamma = (0.5 + beta) * cos(theta);
1.127 + double alpha = 0.25 * (0.5 + beta - gamma);
1.128 +
1.129 + double b0 = 2 * alpha;
1.130 + double b1 = 2 * 2 * alpha;
1.131 + double b2 = 2 * alpha;
1.132 + double a1 = 2 * -gamma;
1.133 + double a2 = 2 * beta;
1.134 +
1.135 + setNormalizedCoefficients(b0, b1, b2, 1, a1, a2);
1.136 + } else {
1.137 + // When cutoff is zero, nothing gets through the filter, so set
1.138 + // coefficients up correctly.
1.139 + setNormalizedCoefficients(0, 0, 0,
1.140 + 1, 0, 0);
1.141 + }
1.142 +}
1.143 +
1.144 +void Biquad::setHighpassParams(double cutoff, double resonance)
1.145 +{
1.146 + // Limit cutoff to 0 to 1.
1.147 + cutoff = std::max(0.0, std::min(cutoff, 1.0));
1.148 +
1.149 + if (cutoff == 1) {
1.150 + // The z-transform is 0.
1.151 + setNormalizedCoefficients(0, 0, 0,
1.152 + 1, 0, 0);
1.153 + } else if (cutoff > 0) {
1.154 + // Compute biquad coefficients for highpass filter
1.155 + resonance = std::max(0.0, resonance); // can't go negative
1.156 + double g = pow(10.0, 0.05 * resonance);
1.157 + double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
1.158 +
1.159 + double theta = M_PI * cutoff;
1.160 + double sn = 0.5 * d * sin(theta);
1.161 + double beta = 0.5 * (1 - sn) / (1 + sn);
1.162 + double gamma = (0.5 + beta) * cos(theta);
1.163 + double alpha = 0.25 * (0.5 + beta + gamma);
1.164 +
1.165 + double b0 = 2 * alpha;
1.166 + double b1 = 2 * -2 * alpha;
1.167 + double b2 = 2 * alpha;
1.168 + double a1 = 2 * -gamma;
1.169 + double a2 = 2 * beta;
1.170 +
1.171 + setNormalizedCoefficients(b0, b1, b2, 1, a1, a2);
1.172 + } else {
1.173 + // When cutoff is zero, we need to be careful because the above
1.174 + // gives a quadratic divided by the same quadratic, with poles
1.175 + // and zeros on the unit circle in the same place. When cutoff
1.176 + // is zero, the z-transform is 1.
1.177 + setNormalizedCoefficients(1, 0, 0,
1.178 + 1, 0, 0);
1.179 + }
1.180 +}
1.181 +
1.182 +void Biquad::setNormalizedCoefficients(double b0, double b1, double b2, double a0, double a1, double a2)
1.183 +{
1.184 + double a0Inverse = 1 / a0;
1.185 +
1.186 + m_b0 = b0 * a0Inverse;
1.187 + m_b1 = b1 * a0Inverse;
1.188 + m_b2 = b2 * a0Inverse;
1.189 + m_a1 = a1 * a0Inverse;
1.190 + m_a2 = a2 * a0Inverse;
1.191 +}
1.192 +
1.193 +void Biquad::setLowShelfParams(double frequency, double dbGain)
1.194 +{
1.195 + // Clip frequencies to between 0 and 1, inclusive.
1.196 + frequency = std::max(0.0, std::min(frequency, 1.0));
1.197 +
1.198 + double A = pow(10.0, dbGain / 40);
1.199 +
1.200 + if (frequency == 1) {
1.201 + // The z-transform is a constant gain.
1.202 + setNormalizedCoefficients(A * A, 0, 0,
1.203 + 1, 0, 0);
1.204 + } else if (frequency > 0) {
1.205 + double w0 = M_PI * frequency;
1.206 + double S = 1; // filter slope (1 is max value)
1.207 + double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
1.208 + double k = cos(w0);
1.209 + double k2 = 2 * sqrt(A) * alpha;
1.210 + double aPlusOne = A + 1;
1.211 + double aMinusOne = A - 1;
1.212 +
1.213 + double b0 = A * (aPlusOne - aMinusOne * k + k2);
1.214 + double b1 = 2 * A * (aMinusOne - aPlusOne * k);
1.215 + double b2 = A * (aPlusOne - aMinusOne * k - k2);
1.216 + double a0 = aPlusOne + aMinusOne * k + k2;
1.217 + double a1 = -2 * (aMinusOne + aPlusOne * k);
1.218 + double a2 = aPlusOne + aMinusOne * k - k2;
1.219 +
1.220 + setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
1.221 + } else {
1.222 + // When frequency is 0, the z-transform is 1.
1.223 + setNormalizedCoefficients(1, 0, 0,
1.224 + 1, 0, 0);
1.225 + }
1.226 +}
1.227 +
1.228 +void Biquad::setHighShelfParams(double frequency, double dbGain)
1.229 +{
1.230 + // Clip frequencies to between 0 and 1, inclusive.
1.231 + frequency = std::max(0.0, std::min(frequency, 1.0));
1.232 +
1.233 + double A = pow(10.0, dbGain / 40);
1.234 +
1.235 + if (frequency == 1) {
1.236 + // The z-transform is 1.
1.237 + setNormalizedCoefficients(1, 0, 0,
1.238 + 1, 0, 0);
1.239 + } else if (frequency > 0) {
1.240 + double w0 = M_PI * frequency;
1.241 + double S = 1; // filter slope (1 is max value)
1.242 + double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
1.243 + double k = cos(w0);
1.244 + double k2 = 2 * sqrt(A) * alpha;
1.245 + double aPlusOne = A + 1;
1.246 + double aMinusOne = A - 1;
1.247 +
1.248 + double b0 = A * (aPlusOne + aMinusOne * k + k2);
1.249 + double b1 = -2 * A * (aMinusOne + aPlusOne * k);
1.250 + double b2 = A * (aPlusOne + aMinusOne * k - k2);
1.251 + double a0 = aPlusOne - aMinusOne * k + k2;
1.252 + double a1 = 2 * (aMinusOne - aPlusOne * k);
1.253 + double a2 = aPlusOne - aMinusOne * k - k2;
1.254 +
1.255 + setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
1.256 + } else {
1.257 + // When frequency = 0, the filter is just a gain, A^2.
1.258 + setNormalizedCoefficients(A * A, 0, 0,
1.259 + 1, 0, 0);
1.260 + }
1.261 +}
1.262 +
1.263 +void Biquad::setPeakingParams(double frequency, double Q, double dbGain)
1.264 +{
1.265 + // Clip frequencies to between 0 and 1, inclusive.
1.266 + frequency = std::max(0.0, std::min(frequency, 1.0));
1.267 +
1.268 + // Don't let Q go negative, which causes an unstable filter.
1.269 + Q = std::max(0.0, Q);
1.270 +
1.271 + double A = pow(10.0, dbGain / 40);
1.272 +
1.273 + if (frequency > 0 && frequency < 1) {
1.274 + if (Q > 0) {
1.275 + double w0 = M_PI * frequency;
1.276 + double alpha = sin(w0) / (2 * Q);
1.277 + double k = cos(w0);
1.278 +
1.279 + double b0 = 1 + alpha * A;
1.280 + double b1 = -2 * k;
1.281 + double b2 = 1 - alpha * A;
1.282 + double a0 = 1 + alpha / A;
1.283 + double a1 = -2 * k;
1.284 + double a2 = 1 - alpha / A;
1.285 +
1.286 + setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
1.287 + } else {
1.288 + // When Q = 0, the above formulas have problems. If we look at
1.289 + // the z-transform, we can see that the limit as Q->0 is A^2, so
1.290 + // set the filter that way.
1.291 + setNormalizedCoefficients(A * A, 0, 0,
1.292 + 1, 0, 0);
1.293 + }
1.294 + } else {
1.295 + // When frequency is 0 or 1, the z-transform is 1.
1.296 + setNormalizedCoefficients(1, 0, 0,
1.297 + 1, 0, 0);
1.298 + }
1.299 +}
1.300 +
1.301 +void Biquad::setAllpassParams(double frequency, double Q)
1.302 +{
1.303 + // Clip frequencies to between 0 and 1, inclusive.
1.304 + frequency = std::max(0.0, std::min(frequency, 1.0));
1.305 +
1.306 + // Don't let Q go negative, which causes an unstable filter.
1.307 + Q = std::max(0.0, Q);
1.308 +
1.309 + if (frequency > 0 && frequency < 1) {
1.310 + if (Q > 0) {
1.311 + double w0 = M_PI * frequency;
1.312 + double alpha = sin(w0) / (2 * Q);
1.313 + double k = cos(w0);
1.314 +
1.315 + double b0 = 1 - alpha;
1.316 + double b1 = -2 * k;
1.317 + double b2 = 1 + alpha;
1.318 + double a0 = 1 + alpha;
1.319 + double a1 = -2 * k;
1.320 + double a2 = 1 - alpha;
1.321 +
1.322 + setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
1.323 + } else {
1.324 + // When Q = 0, the above formulas have problems. If we look at
1.325 + // the z-transform, we can see that the limit as Q->0 is -1, so
1.326 + // set the filter that way.
1.327 + setNormalizedCoefficients(-1, 0, 0,
1.328 + 1, 0, 0);
1.329 + }
1.330 + } else {
1.331 + // When frequency is 0 or 1, the z-transform is 1.
1.332 + setNormalizedCoefficients(1, 0, 0,
1.333 + 1, 0, 0);
1.334 + }
1.335 +}
1.336 +
1.337 +void Biquad::setNotchParams(double frequency, double Q)
1.338 +{
1.339 + // Clip frequencies to between 0 and 1, inclusive.
1.340 + frequency = std::max(0.0, std::min(frequency, 1.0));
1.341 +
1.342 + // Don't let Q go negative, which causes an unstable filter.
1.343 + Q = std::max(0.0, Q);
1.344 +
1.345 + if (frequency > 0 && frequency < 1) {
1.346 + if (Q > 0) {
1.347 + double w0 = M_PI * frequency;
1.348 + double alpha = sin(w0) / (2 * Q);
1.349 + double k = cos(w0);
1.350 +
1.351 + double b0 = 1;
1.352 + double b1 = -2 * k;
1.353 + double b2 = 1;
1.354 + double a0 = 1 + alpha;
1.355 + double a1 = -2 * k;
1.356 + double a2 = 1 - alpha;
1.357 +
1.358 + setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
1.359 + } else {
1.360 + // When Q = 0, the above formulas have problems. If we look at
1.361 + // the z-transform, we can see that the limit as Q->0 is 0, so
1.362 + // set the filter that way.
1.363 + setNormalizedCoefficients(0, 0, 0,
1.364 + 1, 0, 0);
1.365 + }
1.366 + } else {
1.367 + // When frequency is 0 or 1, the z-transform is 1.
1.368 + setNormalizedCoefficients(1, 0, 0,
1.369 + 1, 0, 0);
1.370 + }
1.371 +}
1.372 +
1.373 +void Biquad::setBandpassParams(double frequency, double Q)
1.374 +{
1.375 + // No negative frequencies allowed.
1.376 + frequency = std::max(0.0, frequency);
1.377 +
1.378 + // Don't let Q go negative, which causes an unstable filter.
1.379 + Q = std::max(0.0, Q);
1.380 +
1.381 + if (frequency > 0 && frequency < 1) {
1.382 + double w0 = M_PI * frequency;
1.383 + if (Q > 0) {
1.384 + double alpha = sin(w0) / (2 * Q);
1.385 + double k = cos(w0);
1.386 +
1.387 + double b0 = alpha;
1.388 + double b1 = 0;
1.389 + double b2 = -alpha;
1.390 + double a0 = 1 + alpha;
1.391 + double a1 = -2 * k;
1.392 + double a2 = 1 - alpha;
1.393 +
1.394 + setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
1.395 + } else {
1.396 + // When Q = 0, the above formulas have problems. If we look at
1.397 + // the z-transform, we can see that the limit as Q->0 is 1, so
1.398 + // set the filter that way.
1.399 + setNormalizedCoefficients(1, 0, 0,
1.400 + 1, 0, 0);
1.401 + }
1.402 + } else {
1.403 + // When the cutoff is zero, the z-transform approaches 0, if Q
1.404 + // > 0. When both Q and cutoff are zero, the z-transform is
1.405 + // pretty much undefined. What should we do in this case?
1.406 + // For now, just make the filter 0. When the cutoff is 1, the
1.407 + // z-transform also approaches 0.
1.408 + setNormalizedCoefficients(0, 0, 0,
1.409 + 1, 0, 0);
1.410 + }
1.411 +}
1.412 +
1.413 +void Biquad::setZeroPolePairs(const Complex &zero, const Complex &pole)
1.414 +{
1.415 + double b0 = 1;
1.416 + double b1 = -2 * zero.real();
1.417 +
1.418 + double zeroMag = abs(zero);
1.419 + double b2 = zeroMag * zeroMag;
1.420 +
1.421 + double a1 = -2 * pole.real();
1.422 +
1.423 + double poleMag = abs(pole);
1.424 + double a2 = poleMag * poleMag;
1.425 + setNormalizedCoefficients(b0, b1, b2, 1, a1, a2);
1.426 +}
1.427 +
1.428 +void Biquad::setAllpassPole(const Complex &pole)
1.429 +{
1.430 + Complex zero = Complex(1, 0) / pole;
1.431 + setZeroPolePairs(zero, pole);
1.432 +}
1.433 +
1.434 +void Biquad::getFrequencyResponse(int nFrequencies,
1.435 + const float* frequency,
1.436 + float* magResponse,
1.437 + float* phaseResponse)
1.438 +{
1.439 + // Evaluate the Z-transform of the filter at given normalized
1.440 + // frequency from 0 to 1. (1 corresponds to the Nyquist
1.441 + // frequency.)
1.442 + //
1.443 + // The z-transform of the filter is
1.444 + //
1.445 + // H(z) = (b0 + b1*z^(-1) + b2*z^(-2))/(1 + a1*z^(-1) + a2*z^(-2))
1.446 + //
1.447 + // Evaluate as
1.448 + //
1.449 + // b0 + (b1 + b2*z1)*z1
1.450 + // --------------------
1.451 + // 1 + (a1 + a2*z1)*z1
1.452 + //
1.453 + // with z1 = 1/z and z = exp(j*pi*frequency). Hence z1 = exp(-j*pi*frequency)
1.454 +
1.455 + // Make local copies of the coefficients as a micro-optimization.
1.456 + double b0 = m_b0;
1.457 + double b1 = m_b1;
1.458 + double b2 = m_b2;
1.459 + double a1 = m_a1;
1.460 + double a2 = m_a2;
1.461 +
1.462 + for (int k = 0; k < nFrequencies; ++k) {
1.463 + double omega = -M_PI * frequency[k];
1.464 + Complex z = Complex(cos(omega), sin(omega));
1.465 + Complex numerator = b0 + (b1 + b2 * z) * z;
1.466 + Complex denominator = Complex(1, 0) + (a1 + a2 * z) * z;
1.467 + Complex response = numerator / denominator;
1.468 + magResponse[k] = static_cast<float>(abs(response));
1.469 + phaseResponse[k] = static_cast<float>(atan2(imag(response), real(response)));
1.470 + }
1.471 +}
1.472 +
1.473 +} // namespace WebCore
1.474 +
