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