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 "Biquad.h"
30
31 #include "DenormalDisabler.h"
32
33 #include <float.h>
34 #include <algorithm>
35 #include <math.h>
36
37 namespace WebCore {
38
Biquad()39 Biquad::Biquad() {
40 // Initialize as pass-thru (straight-wire, no filter effect)
41 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
42
43 reset(); // clear filter memory
44 }
45
46 Biquad::~Biquad() = default;
47
process(const float * sourceP,float * destP,size_t framesToProcess)48 void Biquad::process(const float* sourceP, float* destP,
49 size_t framesToProcess) {
50 // Create local copies of member variables
51 double x1 = m_x1;
52 double x2 = m_x2;
53 double y1 = m_y1;
54 double y2 = m_y2;
55
56 double b0 = m_b0;
57 double b1 = m_b1;
58 double b2 = m_b2;
59 double a1 = m_a1;
60 double a2 = m_a2;
61
62 for (size_t i = 0; i < framesToProcess; ++i) {
63 // FIXME: this can be optimized by pipelining the multiply adds...
64 double x = sourceP[i];
65 double y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
66
67 destP[i] = y;
68
69 // Update state variables
70 x2 = x1;
71 x1 = x;
72 y2 = y1;
73 y1 = y;
74 }
75
76 // Avoid introducing a stream of subnormals when input is silent and the
77 // tail approaches zero.
78 if (x1 == 0.0 && x2 == 0.0 && (y1 != 0.0 || y2 != 0.0) &&
79 fabs(y1) < FLT_MIN && fabs(y2) < FLT_MIN) {
80 // Flush future values to zero (until there is new input).
81 y1 = y2 = 0.0;
82 // Flush calculated values.
83 #ifndef HAVE_DENORMAL
84 for (int i = framesToProcess; i-- && fabsf(destP[i]) < FLT_MIN;) {
85 destP[i] = 0.0f;
86 }
87 #endif
88 }
89 // Local variables back to member.
90 m_x1 = x1;
91 m_x2 = x2;
92 m_y1 = y1;
93 m_y2 = y2;
94 }
95
reset()96 void Biquad::reset() { m_x1 = m_x2 = m_y1 = m_y2 = 0; }
97
setLowpassParams(double cutoff,double resonance)98 void Biquad::setLowpassParams(double cutoff, double resonance) {
99 // Limit cutoff to 0 to 1.
100 cutoff = std::max(0.0, std::min(cutoff, 1.0));
101
102 if (cutoff == 1) {
103 // When cutoff is 1, the z-transform is 1.
104 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
105 } else if (cutoff > 0) {
106 // Compute biquad coefficients for lowpass filter
107 double g = pow(10.0, -0.05 * resonance);
108 double w0 = M_PI * cutoff;
109 double cos_w0 = cos(w0);
110 double alpha = 0.5 * sin(w0) * g;
111
112 double b1 = 1.0 - cos_w0;
113 double b0 = 0.5 * b1;
114 double b2 = b0;
115 double a0 = 1.0 + alpha;
116 double a1 = -2.0 * cos_w0;
117 double a2 = 1.0 - alpha;
118
119 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
120 } else {
121 // When cutoff is zero, nothing gets through the filter, so set
122 // coefficients up correctly.
123 setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
124 }
125 }
126
setHighpassParams(double cutoff,double resonance)127 void Biquad::setHighpassParams(double cutoff, double resonance) {
128 // Limit cutoff to 0 to 1.
129 cutoff = std::max(0.0, std::min(cutoff, 1.0));
130
131 if (cutoff == 1) {
132 // The z-transform is 0.
133 setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
134 } else if (cutoff > 0) {
135 // Compute biquad coefficients for highpass filter
136 double g = pow(10.0, -0.05 * resonance);
137 double w0 = M_PI * cutoff;
138 double cos_w0 = cos(w0);
139 double alpha = 0.5 * sin(w0) * g;
140
141 double b1 = -1.0 - cos_w0;
142 double b0 = -0.5 * b1;
143 double b2 = b0;
144 double a0 = 1.0 + alpha;
145 double a1 = -2.0 * cos_w0;
146 double a2 = 1.0 - alpha;
147
148 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
149 } else {
150 // When cutoff is zero, we need to be careful because the above
151 // gives a quadratic divided by the same quadratic, with poles
152 // and zeros on the unit circle in the same place. When cutoff
153 // is zero, the z-transform is 1.
154 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
155 }
156 }
157
setNormalizedCoefficients(double b0,double b1,double b2,double a0,double a1,double a2)158 void Biquad::setNormalizedCoefficients(double b0, double b1, double b2,
159 double a0, double a1, double a2) {
160 double a0Inverse = 1 / a0;
161
162 m_b0 = b0 * a0Inverse;
163 m_b1 = b1 * a0Inverse;
164 m_b2 = b2 * a0Inverse;
165 m_a1 = a1 * a0Inverse;
166 m_a2 = a2 * a0Inverse;
167 }
168
setLowShelfParams(double frequency,double dbGain)169 void Biquad::setLowShelfParams(double frequency, double dbGain) {
170 // Clip frequencies to between 0 and 1, inclusive.
171 frequency = std::max(0.0, std::min(frequency, 1.0));
172
173 double A = pow(10.0, dbGain / 40);
174
175 if (frequency == 1) {
176 // The z-transform is a constant gain.
177 setNormalizedCoefficients(A * A, 0, 0, 1, 0, 0);
178 } else if (frequency > 0) {
179 double w0 = M_PI * frequency;
180 double S = 1; // filter slope (1 is max value)
181 double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
182 double k = cos(w0);
183 double k2 = 2 * sqrt(A) * alpha;
184 double aPlusOne = A + 1;
185 double aMinusOne = A - 1;
186
187 double b0 = A * (aPlusOne - aMinusOne * k + k2);
188 double b1 = 2 * A * (aMinusOne - aPlusOne * k);
189 double b2 = A * (aPlusOne - aMinusOne * k - k2);
190 double a0 = aPlusOne + aMinusOne * k + k2;
191 double a1 = -2 * (aMinusOne + aPlusOne * k);
192 double a2 = aPlusOne + aMinusOne * k - k2;
193
194 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
195 } else {
196 // When frequency is 0, the z-transform is 1.
197 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
198 }
199 }
200
setHighShelfParams(double frequency,double dbGain)201 void Biquad::setHighShelfParams(double frequency, double dbGain) {
202 // Clip frequencies to between 0 and 1, inclusive.
203 frequency = std::max(0.0, std::min(frequency, 1.0));
204
205 double A = pow(10.0, dbGain / 40);
206
207 if (frequency == 1) {
208 // The z-transform is 1.
209 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
210 } else if (frequency > 0) {
211 double w0 = M_PI * frequency;
212 double S = 1; // filter slope (1 is max value)
213 double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
214 double k = cos(w0);
215 double k2 = 2 * sqrt(A) * alpha;
216 double aPlusOne = A + 1;
217 double aMinusOne = A - 1;
218
219 double b0 = A * (aPlusOne + aMinusOne * k + k2);
220 double b1 = -2 * A * (aMinusOne + aPlusOne * k);
221 double b2 = A * (aPlusOne + aMinusOne * k - k2);
222 double a0 = aPlusOne - aMinusOne * k + k2;
223 double a1 = 2 * (aMinusOne - aPlusOne * k);
224 double a2 = aPlusOne - aMinusOne * k - k2;
225
226 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
227 } else {
228 // When frequency = 0, the filter is just a gain, A^2.
229 setNormalizedCoefficients(A * A, 0, 0, 1, 0, 0);
230 }
231 }
232
setPeakingParams(double frequency,double Q,double dbGain)233 void Biquad::setPeakingParams(double frequency, double Q, double dbGain) {
234 // Clip frequencies to between 0 and 1, inclusive.
235 frequency = std::max(0.0, std::min(frequency, 1.0));
236
237 // Don't let Q go negative, which causes an unstable filter.
238 Q = std::max(0.0, Q);
239
240 double A = pow(10.0, dbGain / 40);
241
242 if (frequency > 0 && frequency < 1) {
243 if (Q > 0) {
244 double w0 = M_PI * frequency;
245 double alpha = sin(w0) / (2 * Q);
246 double k = cos(w0);
247
248 double b0 = 1 + alpha * A;
249 double b1 = -2 * k;
250 double b2 = 1 - alpha * A;
251 double a0 = 1 + alpha / A;
252 double a1 = -2 * k;
253 double a2 = 1 - alpha / A;
254
255 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
256 } else {
257 // When Q = 0, the above formulas have problems. If we look at
258 // the z-transform, we can see that the limit as Q->0 is A^2, so
259 // set the filter that way.
260 setNormalizedCoefficients(A * A, 0, 0, 1, 0, 0);
261 }
262 } else {
263 // When frequency is 0 or 1, the z-transform is 1.
264 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
265 }
266 }
267
setAllpassParams(double frequency,double Q)268 void Biquad::setAllpassParams(double frequency, double Q) {
269 // Clip frequencies to between 0 and 1, inclusive.
270 frequency = std::max(0.0, std::min(frequency, 1.0));
271
272 // Don't let Q go negative, which causes an unstable filter.
273 Q = std::max(0.0, Q);
274
275 if (frequency > 0 && frequency < 1) {
276 if (Q > 0) {
277 double w0 = M_PI * frequency;
278 double alpha = sin(w0) / (2 * Q);
279 double k = cos(w0);
280
281 double b0 = 1 - alpha;
282 double b1 = -2 * k;
283 double b2 = 1 + alpha;
284 double a0 = 1 + alpha;
285 double a1 = -2 * k;
286 double a2 = 1 - alpha;
287
288 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
289 } else {
290 // When Q = 0, the above formulas have problems. If we look at
291 // the z-transform, we can see that the limit as Q->0 is -1, so
292 // set the filter that way.
293 setNormalizedCoefficients(-1, 0, 0, 1, 0, 0);
294 }
295 } else {
296 // When frequency is 0 or 1, the z-transform is 1.
297 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
298 }
299 }
300
setNotchParams(double frequency,double Q)301 void Biquad::setNotchParams(double frequency, double Q) {
302 // Clip frequencies to between 0 and 1, inclusive.
303 frequency = std::max(0.0, std::min(frequency, 1.0));
304
305 // Don't let Q go negative, which causes an unstable filter.
306 Q = std::max(0.0, Q);
307
308 if (frequency > 0 && frequency < 1) {
309 if (Q > 0) {
310 double w0 = M_PI * frequency;
311 double alpha = sin(w0) / (2 * Q);
312 double k = cos(w0);
313
314 double b0 = 1;
315 double b1 = -2 * k;
316 double b2 = 1;
317 double a0 = 1 + alpha;
318 double a1 = -2 * k;
319 double a2 = 1 - alpha;
320
321 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
322 } else {
323 // When Q = 0, the above formulas have problems. If we look at
324 // the z-transform, we can see that the limit as Q->0 is 0, so
325 // set the filter that way.
326 setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
327 }
328 } else {
329 // When frequency is 0 or 1, the z-transform is 1.
330 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
331 }
332 }
333
setBandpassParams(double frequency,double Q)334 void Biquad::setBandpassParams(double frequency, double Q) {
335 // No negative frequencies allowed.
336 frequency = std::max(0.0, frequency);
337
338 // Don't let Q go negative, which causes an unstable filter.
339 Q = std::max(0.0, Q);
340
341 if (frequency > 0 && frequency < 1) {
342 double w0 = M_PI * frequency;
343 if (Q > 0) {
344 double alpha = sin(w0) / (2 * Q);
345 double k = cos(w0);
346
347 double b0 = alpha;
348 double b1 = 0;
349 double b2 = -alpha;
350 double a0 = 1 + alpha;
351 double a1 = -2 * k;
352 double a2 = 1 - alpha;
353
354 setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
355 } else {
356 // When Q = 0, the above formulas have problems. If we look at
357 // the z-transform, we can see that the limit as Q->0 is 1, so
358 // set the filter that way.
359 setNormalizedCoefficients(1, 0, 0, 1, 0, 0);
360 }
361 } else {
362 // When the cutoff is zero, the z-transform approaches 0, if Q
363 // > 0. When both Q and cutoff are zero, the z-transform is
364 // pretty much undefined. What should we do in this case?
365 // For now, just make the filter 0. When the cutoff is 1, the
366 // z-transform also approaches 0.
367 setNormalizedCoefficients(0, 0, 0, 1, 0, 0);
368 }
369 }
370
setZeroPolePairs(const Complex & zero,const Complex & pole)371 void Biquad::setZeroPolePairs(const Complex& zero, const Complex& pole) {
372 double b0 = 1;
373 double b1 = -2 * zero.real();
374
375 double zeroMag = abs(zero);
376 double b2 = zeroMag * zeroMag;
377
378 double a1 = -2 * pole.real();
379
380 double poleMag = abs(pole);
381 double a2 = poleMag * poleMag;
382 setNormalizedCoefficients(b0, b1, b2, 1, a1, a2);
383 }
384
setAllpassPole(const Complex & pole)385 void Biquad::setAllpassPole(const Complex& pole) {
386 Complex zero = Complex(1, 0) / pole;
387 setZeroPolePairs(zero, pole);
388 }
389
getFrequencyResponse(int nFrequencies,const float * frequency,float * magResponse,float * phaseResponse)390 void Biquad::getFrequencyResponse(int nFrequencies, const float* frequency,
391 float* magResponse, float* phaseResponse) {
392 // Evaluate the Z-transform of the filter at given normalized
393 // frequency from 0 to 1. (1 corresponds to the Nyquist
394 // frequency.)
395 //
396 // The z-transform of the filter is
397 //
398 // H(z) = (b0 + b1*z^(-1) + b2*z^(-2))/(1 + a1*z^(-1) + a2*z^(-2))
399 //
400 // Evaluate as
401 //
402 // b0 + (b1 + b2*z1)*z1
403 // --------------------
404 // 1 + (a1 + a2*z1)*z1
405 //
406 // with z1 = 1/z and z = exp(j*pi*frequency). Hence z1 = exp(-j*pi*frequency)
407
408 // Make local copies of the coefficients as a micro-optimization.
409 double b0 = m_b0;
410 double b1 = m_b1;
411 double b2 = m_b2;
412 double a1 = m_a1;
413 double a2 = m_a2;
414
415 for (int k = 0; k < nFrequencies; ++k) {
416 double omega = -M_PI * frequency[k];
417 Complex z = Complex(cos(omega), sin(omega));
418 Complex numerator = b0 + (b1 + b2 * z) * z;
419 Complex denominator = Complex(1, 0) + (a1 + a2 * z) * z;
420 // Strangely enough, using complex division:
421 // e.g. Complex response = numerator / denominator;
422 // fails on our test machines, yielding infinities and NaNs, so we do
423 // things the long way here.
424 double n = norm(denominator);
425 double r = (real(numerator) * real(denominator) +
426 imag(numerator) * imag(denominator)) /
427 n;
428 double i = (imag(numerator) * real(denominator) -
429 real(numerator) * imag(denominator)) /
430 n;
431 std::complex<double> response = std::complex<double>(r, i);
432
433 magResponse[k] = static_cast<float>(abs(response));
434 phaseResponse[k] =
435 static_cast<float>(atan2(imag(response), real(response)));
436 }
437 }
438
439 } // namespace WebCore
440