cfee9cfd9527baf98c44b883745ef1ad36499559
[WebKit-https.git] / Source / WebCore / platform / audio / Biquad.cpp
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 "config.h"
30
31 #if ENABLE(WEB_AUDIO)
32
33 #include "Biquad.h"
34
35 #include "DenormalDisabler.h"
36 #include <algorithm>
37 #include <stdio.h>
38 #include <wtf/MathExtras.h>
39
40 #if OS(DARWIN)
41 #include <Accelerate/Accelerate.h>
42 #endif
43
44 namespace WebCore {
45
46 const int kBufferSize = 1024;
47
48 Biquad::Biquad()
49 {
50 #if OS(DARWIN)
51     // Allocate two samples more for filter history
52     m_inputBuffer.allocate(kBufferSize + 2);
53     m_outputBuffer.allocate(kBufferSize + 2);
54 #endif
55
56     // Initialize as pass-thru (straight-wire, no filter effect)
57     m_b0 = 1;
58     m_b1 = 0;
59     m_b2 = 0;
60     m_a1 = 0;
61     m_a2 = 0;
62
63     reset(); // clear filter memory
64 }
65
66 void Biquad::process(const float* sourceP, float* destP, size_t framesToProcess)
67 {
68 #if OS(DARWIN)
69     // Use vecLib if available
70     processFast(sourceP, destP, framesToProcess);
71 #else
72     int n = framesToProcess;
73
74     // Create local copies of member variables
75     double x1 = m_x1;
76     double x2 = m_x2;
77     double y1 = m_y1;
78     double y2 = m_y2;
79
80     double b0 = m_b0;
81     double b1 = m_b1;
82     double b2 = m_b2;
83     double a1 = m_a1;
84     double a2 = m_a2;
85
86     while (n--) {
87         // FIXME: this can be optimized by pipelining the multiply adds...
88         float x = *sourceP++;
89         float y = b0*x + b1*x1 + b2*x2 - a1*y1 - a2*y2;
90
91         *destP++ = y;
92
93         // Update state variables
94         x2 = x1;
95         x1 = x;
96         y2 = y1;
97         y1 = y;
98     }
99
100     // Local variables back to member. Flush denormals here so we
101     // don't slow down the inner loop above.
102     m_x1 = DenormalDisabler::flushDenormalFloatToZero(x1);
103     m_x2 = DenormalDisabler::flushDenormalFloatToZero(x2);
104     m_y1 = DenormalDisabler::flushDenormalFloatToZero(y1);
105     m_y2 = DenormalDisabler::flushDenormalFloatToZero(y2);
106
107     m_b0 = b0;
108     m_b1 = b1;
109     m_b2 = b2;
110     m_a1 = a1;
111     m_a2 = a2;
112 #endif
113 }
114
115 #if OS(DARWIN)
116
117 // Here we have optimized version using Accelerate.framework
118
119 void Biquad::processFast(const float* sourceP, float* destP, size_t framesToProcess)
120 {
121     double filterCoefficients[5];
122     filterCoefficients[0] = m_b0;
123     filterCoefficients[1] = m_b1;
124     filterCoefficients[2] = m_b2;
125     filterCoefficients[3] = m_a1;
126     filterCoefficients[4] = m_a2;
127
128     double* inputP = m_inputBuffer.data();
129     double* outputP = m_outputBuffer.data();
130
131     double* input2P = inputP + 2;
132     double* output2P = outputP + 2;
133
134     // Break up processing into smaller slices (kBufferSize) if necessary.
135
136     int n = framesToProcess;
137
138     while (n > 0) {
139         int framesThisTime = n < kBufferSize ? n : kBufferSize;
140
141         // Copy input to input buffer
142         for (int i = 0; i < framesThisTime; ++i)
143             input2P[i] = *sourceP++;
144
145         processSliceFast(inputP, outputP, filterCoefficients, framesThisTime);
146
147         // Copy output buffer to output (converts float -> double).
148         for (int i = 0; i < framesThisTime; ++i)
149             *destP++ = static_cast<float>(output2P[i]);
150
151         n -= framesThisTime;
152     }
153 }
154
155 void Biquad::processSliceFast(double* sourceP, double* destP, double* coefficientsP, size_t framesToProcess)
156 {
157     // Use double-precision for filter stability
158     vDSP_deq22D(sourceP, 1, coefficientsP, destP, 1, framesToProcess);
159
160     // Save history.  Note that sourceP and destP reference m_inputBuffer and m_outputBuffer respectively.
161     // These buffers are allocated (in the constructor) with space for two extra samples so it's OK to access
162     // array values two beyond framesToProcess.
163     sourceP[0] = sourceP[framesToProcess - 2 + 2];
164     sourceP[1] = sourceP[framesToProcess - 1 + 2];
165     destP[0] = destP[framesToProcess - 2 + 2];
166     destP[1] = destP[framesToProcess - 1 + 2];
167 }
168
169 #endif // OS(DARWIN)
170
171
172 void Biquad::reset()
173 {
174     m_x1 = m_x2 = m_y1 = m_y2 = 0;
175
176 #if OS(DARWIN)
177     // Two extra samples for filter history
178     double* inputP = m_inputBuffer.data();
179     inputP[0] = 0;
180     inputP[1] = 0;
181
182     double* outputP = m_outputBuffer.data();
183     outputP[0] = 0;
184     outputP[1] = 0;
185 #endif
186 }
187
188 void Biquad::setLowpassParams(double cutoff, double resonance)
189 {
190     resonance = std::max(0.0, resonance); // can't go negative
191     // Limit cutoff to 0 to 1.
192     cutoff = std::max(0.0, std::min(cutoff, 1.0));
193     
194     double g = pow(10.0, 0.05 * resonance);
195     double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
196
197     if (cutoff == 1) {
198         // When cutoff is 1, the z-transform is 1.
199         m_b0 = 1;
200         m_b1 = 0;
201         m_b2 = 0;
202         m_a1 = 0;
203         m_a2 = 0;
204     } else if (cutoff > 0) {
205         // Compute biquad coefficients for lowpass filter
206         double theta = piDouble * cutoff;
207         double sn = 0.5 * d * sin(theta);
208         double beta = 0.5 * (1 - sn) / (1 + sn);
209         double gamma = (0.5 + beta) * cos(theta);
210         double alpha = 0.25 * (0.5 + beta - gamma);
211
212         m_b0 = 2 * alpha;
213         m_b1 = 2 * 2 * alpha;
214         m_b2 = 2 * alpha;
215         m_a1 = 2 * -gamma;
216         m_a2 = 2 * beta;
217     } else {
218         // When cutoff is zero, nothing gets through the filter, so set
219         // coefficients up correctly.
220         m_b0 = 0;
221         m_b1 = 0;
222         m_b2 = 0;
223         m_a1 = 0;
224         m_a2 = 0;
225     }
226 }
227
228 void Biquad::setHighpassParams(double cutoff, double resonance)
229 {
230     resonance = std::max(0.0, resonance); // can't go negative
231
232     // Limit cutoff to 0 to 1.
233     cutoff = std::max(0.0, std::min(cutoff, 1.0));
234
235     double g = pow(10.0, 0.05 * resonance);
236     double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
237
238     if (cutoff == 1) {
239         // The z-transform is 0.
240         m_b0 = 0;
241         m_b1 = 0;
242         m_b2 = 0;
243         m_a1 = 0;
244         m_a2 = 0;
245     } else if (cutoff > 0) {
246         // Compute biquad coefficients for highpass filter
247         double theta = piDouble * cutoff;
248         double sn = 0.5 * d * sin(theta);
249         double beta = 0.5 * (1 - sn) / (1 + sn);
250         double gamma = (0.5 + beta) * cos(theta);
251         double alpha = 0.25 * (0.5 + beta + gamma);
252
253         m_b0 = 2 * alpha;
254         m_b1 = 2 * -2 * alpha;
255         m_b2 = 2 * alpha;
256         m_a1 = 2 * -gamma;
257         m_a2 = 2 * beta;
258     } else {
259       // When cutoff is zero, we need to be careful because the above
260       // gives a quadratic divided by the same quadratic, with poles
261       // and zeros on the unit circle in the same place. When cutoff
262       // is zero, the z-transform is 1.
263       m_b0 = 1;
264       m_b1 = 0;
265       m_b2 = 0;
266       m_a1 = 0;
267       m_a2 = 0;
268     }
269 }
270
271 void Biquad::setNormalizedCoefficients(double b0, double b1, double b2, double a0, double a1, double a2)
272 {
273     double a0Inverse = 1 / a0;
274     
275     m_b0 = b0 * a0Inverse;
276     m_b1 = b1 * a0Inverse;
277     m_b2 = b2 * a0Inverse;
278     m_a1 = a1 * a0Inverse;
279     m_a2 = a2 * a0Inverse;
280 }
281
282 void Biquad::setLowShelfParams(double frequency, double dbGain)
283 {
284     // Clip frequencies to between 0 and 1, inclusive.
285     frequency = std::max(0.0, std::min(frequency, 1.0));
286     
287     double A = pow(10.0, dbGain / 40);
288
289     if (frequency == 1) {
290         // The z-transform is a constant gain.
291         setNormalizedCoefficients(A * A, 0, 0,
292                                   1, 0, 0);
293     } else if (frequency > 0) {
294         double w0 = piDouble * frequency;
295         double S = 1; // filter slope (1 is max value)
296         double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
297         double k = cos(w0);
298         double k2 = 2 * sqrt(A) * alpha;
299         double aPlusOne = A + 1;
300         double aMinusOne = A - 1;
301
302         double b0 = A * (aPlusOne - aMinusOne * k + k2);
303         double b1 = 2 * A * (aMinusOne - aPlusOne * k);
304         double b2 = A * (aPlusOne - aMinusOne * k - k2);
305         double a0 = aPlusOne + aMinusOne * k + k2;
306         double a1 = -2 * (aMinusOne + aPlusOne * k);
307         double a2 = aPlusOne + aMinusOne * k - k2;
308
309         setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
310     } else {
311         // When frequency is 0, the z-transform is 1.
312         setNormalizedCoefficients(1, 0, 0,
313                                   1, 0, 0);
314     }
315 }
316
317 void Biquad::setHighShelfParams(double frequency, double dbGain)
318 {
319     // Clip frequencies to between 0 and 1, inclusive.
320     frequency = std::max(0.0, std::min(frequency, 1.0));
321
322     double A = pow(10.0, dbGain / 40);
323
324     if (frequency == 1) {
325         // The z-transform is 1.
326         setNormalizedCoefficients(1, 0, 0,
327                                   1, 0, 0);
328     } else if (frequency > 0) {
329         double w0 = piDouble * frequency;
330         double S = 1; // filter slope (1 is max value)
331         double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
332         double k = cos(w0);
333         double k2 = 2 * sqrt(A) * alpha;
334         double aPlusOne = A + 1;
335         double aMinusOne = A - 1;
336
337         double b0 = A * (aPlusOne + aMinusOne * k + k2);
338         double b1 = -2 * A * (aMinusOne + aPlusOne * k);
339         double b2 = A * (aPlusOne + aMinusOne * k - k2);
340         double a0 = aPlusOne - aMinusOne * k + k2;
341         double a1 = 2 * (aMinusOne - aPlusOne * k);
342         double a2 = aPlusOne - aMinusOne * k - k2;
343
344         setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
345     } else {
346         // When frequency = 0, the filter is just a gain, A^2.
347         setNormalizedCoefficients(A * A, 0, 0,
348                                   1, 0, 0);
349     }
350 }
351
352 void Biquad::setPeakingParams(double frequency, double Q, double dbGain)
353 {
354     // Clip frequencies to between 0 and 1, inclusive.
355     frequency = std::max(0.0, std::min(frequency, 1.0));
356
357     // Don't let Q go negative, which causes an unstable filter.
358     Q = std::max(0.0, Q);
359
360     double A = pow(10.0, dbGain / 40);
361
362     if (frequency > 0 && frequency < 1) {
363         if (Q > 0) {
364             double w0 = piDouble * frequency;
365             double alpha = sin(w0) / (2 * Q);
366             double k = cos(w0);
367
368             double b0 = 1 + alpha * A;
369             double b1 = -2 * k;
370             double b2 = 1 - alpha * A;
371             double a0 = 1 + alpha / A;
372             double a1 = -2 * k;
373             double a2 = 1 - alpha / A;
374
375             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
376         } else {
377             // When Q = 0, the above formulas have problems. If we look at
378             // the z-transform, we can see that the limit as Q->0 is A^2, so
379             // set the filter that way.
380             setNormalizedCoefficients(A * A, 0, 0,
381                                       1, 0, 0);
382         }
383     } else {
384         // When frequency is 0 or 1, the z-transform is 1.
385         setNormalizedCoefficients(1, 0, 0,
386                                   1, 0, 0);
387     }
388 }
389
390 void Biquad::setAllpassParams(double frequency, double Q)
391 {
392     // Clip frequencies to between 0 and 1, inclusive.
393     frequency = std::max(0.0, std::min(frequency, 1.0));
394
395     // Don't let Q go negative, which causes an unstable filter.
396     Q = std::max(0.0, Q);
397
398     if (frequency > 0 && frequency < 1) {
399         if (Q > 0) {
400             double w0 = piDouble * frequency;
401             double alpha = sin(w0) / (2 * Q);
402             double k = cos(w0);
403
404             double b0 = 1 - alpha;
405             double b1 = -2 * k;
406             double b2 = 1 + alpha;
407             double a0 = 1 + alpha;
408             double a1 = -2 * k;
409             double a2 = 1 - alpha;
410
411             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
412         } else {
413             // When Q = 0, the above formulas have problems. If we look at
414             // the z-transform, we can see that the limit as Q->0 is -1, so
415             // set the filter that way.
416             setNormalizedCoefficients(-1, 0, 0,
417                                       1, 0, 0);
418         }
419     } else {
420         // When frequency is 0 or 1, the z-transform is 1.
421         setNormalizedCoefficients(1, 0, 0,
422                                   1, 0, 0);
423     }
424 }
425
426 void Biquad::setNotchParams(double frequency, double Q)
427 {
428     // Clip frequencies to between 0 and 1, inclusive.
429     frequency = std::max(0.0, std::min(frequency, 1.0));
430
431     // Don't let Q go negative, which causes an unstable filter.
432     Q = std::max(0.0, Q);
433
434     if (frequency > 0 && frequency < 1) {
435         if (Q > 0) {
436             double w0 = piDouble * frequency;
437             double alpha = sin(w0) / (2 * Q);
438             double k = cos(w0);
439
440             double b0 = 1;
441             double b1 = -2 * k;
442             double b2 = 1;
443             double a0 = 1 + alpha;
444             double a1 = -2 * k;
445             double a2 = 1 - alpha;
446
447             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
448         } else {
449             // When Q = 0, the above formulas have problems. If we look at
450             // the z-transform, we can see that the limit as Q->0 is 0, so
451             // set the filter that way.
452             setNormalizedCoefficients(0, 0, 0,
453                                       1, 0, 0);
454         }
455     } else {
456         // When frequency is 0 or 1, the z-transform is 1.
457         setNormalizedCoefficients(1, 0, 0,
458                                   1, 0, 0);
459     }
460 }
461
462 void Biquad::setBandpassParams(double frequency, double Q)
463 {
464     // No negative frequencies allowed.
465     frequency = std::max(0.0, frequency);
466
467     // Don't let Q go negative, which causes an unstable filter.
468     Q = std::max(0.0, Q);
469
470     if (frequency > 0 && frequency < 1) {
471         double w0 = piDouble * frequency;
472         if (Q > 0) {
473             double alpha = sin(w0) / (2 * Q);
474             double k = cos(w0);
475     
476             double b0 = alpha;
477             double b1 = 0;
478             double b2 = -alpha;
479             double a0 = 1 + alpha;
480             double a1 = -2 * k;
481             double a2 = 1 - alpha;
482
483             setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
484         } else {
485             // When Q = 0, the above formulas have problems. If we look at
486             // the z-transform, we can see that the limit as Q->0 is 1, so
487             // set the filter that way.
488             setNormalizedCoefficients(1, 0, 0,
489                                       1, 0, 0);
490         }
491     } else {
492         // When the cutoff is zero, the z-transform approaches 0, if Q
493         // > 0. When both Q and cutoff are zero, the z-transform is
494         // pretty much undefined. What should we do in this case?
495         // For now, just make the filter 0. When the cutoff is 1, the
496         // z-transform also approaches 0.
497         setNormalizedCoefficients(0, 0, 0,
498                                   1, 0, 0);
499     }
500 }
501
502 void Biquad::setZeroPolePairs(const Complex &zero, const Complex &pole)
503 {
504     m_b0 = 1;
505     m_b1 = -2 * zero.real();
506
507     double zeroMag = abs(zero);
508     m_b2 = zeroMag * zeroMag;
509
510     m_a1 = -2 * pole.real();
511
512     double poleMag = abs(pole);
513     m_a2 = poleMag * poleMag;
514 }
515
516 void Biquad::setAllpassPole(const Complex &pole)
517 {
518     Complex zero = Complex(1, 0) / pole;
519     setZeroPolePairs(zero, pole);
520 }
521
522 void Biquad::getFrequencyResponse(int nFrequencies,
523                                   const float* frequency,
524                                   float* magResponse,
525                                   float* phaseResponse)
526 {
527     // Evaluate the Z-transform of the filter at given normalized
528     // frequency from 0 to 1.  (1 corresponds to the Nyquist
529     // frequency.)
530     //
531     // The z-transform of the filter is
532     //
533     // H(z) = (b0 + b1*z^(-1) + b2*z^(-2))/(1 + a1*z^(-1) + a2*z^(-2))
534     //
535     // Evaluate as
536     //
537     // b0 + (b1 + b2*z1)*z1
538     // --------------------
539     // 1 + (a1 + a2*z1)*z1
540     //
541     // with z1 = 1/z and z = exp(j*pi*frequency). Hence z1 = exp(-j*pi*frequency)
542
543     // Make local copies of the coefficients as a micro-optimization.
544     double b0 = m_b0;
545     double b1 = m_b1;
546     double b2 = m_b2;
547     double a1 = m_a1;
548     double a2 = m_a2;
549     
550     for (int k = 0; k < nFrequencies; ++k) {
551         double omega = -piDouble * frequency[k];
552         Complex z = Complex(cos(omega), sin(omega));
553         Complex numerator = b0 + (b1 + b2 * z) * z;
554         Complex denominator = Complex(1, 0) + (a1 + a2 * z) * z;
555         Complex response = numerator / denominator;
556         magResponse[k] = static_cast<float>(abs(response));
557         phaseResponse[k] = static_cast<float>(atan2(imag(response), real(response)));
558     }
559 }
560
561 } // namespace WebCore
562
563 #endif // ENABLE(WEB_AUDIO)