2011-06-08 Chris Rogers <crogers@google.com>
[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 <algorithm>
36 #include <stdio.h>
37 #include <wtf/MathExtras.h>
38
39 #if OS(DARWIN)
40 #include <Accelerate/Accelerate.h>
41 #endif
42
43 namespace WebCore {
44
45 const int kBufferSize = 1024;
46
47 Biquad::Biquad()
48 {
49 #if OS(DARWIN)
50     // Allocate two samples more for filter history
51     m_inputBuffer.resize(kBufferSize + 2);
52     m_outputBuffer.resize(kBufferSize + 2);
53 #endif
54
55     // Initialize as pass-thru (straight-wire, no filter effect)
56     m_b0 = 1;
57     m_b1 = 0;
58     m_b2 = 0;
59     m_a1 = 0;
60     m_a2 = 0;
61
62     reset(); // clear filter memory
63 }
64
65 void Biquad::process(const float* sourceP, float* destP, size_t framesToProcess)
66 {
67 #if OS(DARWIN)
68     // Use vecLib if available
69     processFast(sourceP, destP, framesToProcess);
70 #else
71     int n = framesToProcess;
72
73     // Create local copies of member variables
74     double x1 = m_x1;
75     double x2 = m_x2;
76     double y1 = m_y1;
77     double y2 = m_y2;
78
79     double b0 = m_b0;
80     double b1 = m_b1;
81     double b2 = m_b2;
82     double a1 = m_a1;
83     double a2 = m_a2;
84
85     while (n--) {
86         // FIXME: this can be optimized by pipelining the multiply adds...
87         float x = *sourceP++;
88         float y = b0*x + b1*x1 + b2*x2 - a1*y1 - a2*y2;
89
90         *destP++ = y;
91
92         // Update state variables
93         x2 = x1;
94         x1 = x;
95         y2 = y1;
96         y1 = y;
97     }
98
99     // Local variables back to member
100     m_x1 = x1;
101     m_x2 = x2;
102     m_y1 = y1;
103     m_y2 = y2;
104
105     m_b0 = b0;
106     m_b1 = b1;
107     m_b2 = b2;
108     m_a1 = a1;
109     m_a2 = a2;
110 #endif
111 }
112
113 #if OS(DARWIN)
114
115 // Here we have optimized version using Accelerate.framework
116
117 void Biquad::processFast(const float* sourceP, float* destP, size_t framesToProcess)
118 {
119     double filterCoefficients[5];
120     filterCoefficients[0] = m_b0;
121     filterCoefficients[1] = m_b1;
122     filterCoefficients[2] = m_b2;
123     filterCoefficients[3] = m_a1;
124     filterCoefficients[4] = m_a2;
125
126     double* inputP = m_inputBuffer.data();
127     double* outputP = m_outputBuffer.data();
128
129     double* input2P = inputP + 2;
130     double* output2P = outputP + 2;
131
132     // Break up processing into smaller slices (kBufferSize) if necessary.
133
134     int n = framesToProcess;
135
136     while (n > 0) {
137         int framesThisTime = n < kBufferSize ? n : kBufferSize;
138
139         // Copy input to input buffer
140         for (int i = 0; i < framesThisTime; ++i)
141             input2P[i] = *sourceP++;
142
143         processSliceFast(inputP, outputP, filterCoefficients, framesThisTime);
144
145         // Copy output buffer to output (converts float -> double).
146         for (int i = 0; i < framesThisTime; ++i)
147             *destP++ = static_cast<float>(output2P[i]);
148
149         n -= framesThisTime;
150     }
151 }
152
153 void Biquad::processSliceFast(double* sourceP, double* destP, double* coefficientsP, size_t framesToProcess)
154 {
155     // Use double-precision for filter stability
156     vDSP_deq22D(sourceP, 1, coefficientsP, destP, 1, framesToProcess);
157
158     // Save history.  Note that sourceP and destP reference m_inputBuffer and m_outputBuffer respectively.
159     // These buffers are allocated (in the constructor) with space for two extra samples so it's OK to access
160     // array values two beyond framesToProcess.
161     sourceP[0] = sourceP[framesToProcess - 2 + 2];
162     sourceP[1] = sourceP[framesToProcess - 1 + 2];
163     destP[0] = destP[framesToProcess - 2 + 2];
164     destP[1] = destP[framesToProcess - 1 + 2];
165 }
166
167 #endif // OS(DARWIN)
168
169
170 void Biquad::reset()
171 {
172     m_x1 = m_x2 = m_y1 = m_y2 = 0;
173
174 #if OS(DARWIN)
175     // Two extra samples for filter history
176     double* inputP = m_inputBuffer.data();
177     inputP[0] = 0;
178     inputP[1] = 0;
179
180     double* outputP = m_outputBuffer.data();
181     outputP[0] = 0;
182     outputP[1] = 0;
183 #endif
184 }
185
186 void Biquad::setLowpassParams(double cutoff, double resonance)
187 {
188     resonance = std::max(0.0, resonance); // can't go negative
189
190     double g = pow(10.0, 0.05 * resonance);
191     double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
192
193     // Compute biquad coefficients for lopass filter
194     double theta = piDouble * cutoff;
195     double sn = 0.5 * d * sin(theta);
196     double beta = 0.5 * (1 - sn) / (1 + sn);
197     double gamma = (0.5 + beta) * cos(theta);
198     double alpha = 0.25 * (0.5 + beta - gamma);
199
200     m_b0 = 2 * alpha;
201     m_b1 = 2 * 2 * alpha;
202     m_b2 = 2 * alpha;
203     m_a1 = 2 * -gamma;
204     m_a2 = 2 * beta;
205 }
206
207 void Biquad::setHighpassParams(double cutoff, double resonance)
208 {
209     resonance = std::max(0.0, resonance); // can't go negative
210
211     double g = pow(10.0, 0.05 * resonance);
212     double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
213
214     // Compute biquad coefficients for highpass filter
215     double theta = piDouble * cutoff;
216     double sn = 0.5 * d * sin(theta);
217     double beta = 0.5 * (1 - sn) / (1 + sn);
218     double gamma = (0.5 + beta) * cos(theta);
219     double alpha = 0.25 * (0.5 + beta + gamma);
220
221     m_b0 = 2 * alpha;
222     m_b1 = 2 * -2 * alpha;
223     m_b2 = 2 * alpha;
224     m_a1 = 2 * -gamma;
225     m_a2 = 2 * beta;
226 }
227
228 void Biquad::setNormalizedCoefficients(double b0, double b1, double b2, double a0, double a1, double a2)
229 {
230     double a0Inverse = 1 / a0;
231     
232     m_b0 = b0 * a0Inverse;
233     m_b1 = b1 * a0Inverse;
234     m_b2 = b2 * a0Inverse;
235     m_a1 = a1 * a0Inverse;
236     m_a2 = a2 * a0Inverse;
237 }
238
239 void Biquad::setLowShelfParams(double frequency, double dbGain)
240 {
241     double w0 = piDouble * frequency;
242
243     double A = pow(10.0, dbGain / 40);
244     double S = 1; // filter slope (1 is max value)
245     double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
246
247     double k = cos(w0);
248     double k2 = 2 * sqrt(A) * alpha;
249
250     double aPlusOne = A + 1;
251     double aMinusOne = A - 1;
252     
253     double b0 = A * (aPlusOne - aMinusOne * k + k2);
254     double b1 = 2 * A * (aMinusOne - aPlusOne * k);
255     double b2 = A * (aPlusOne - aMinusOne * k - k2);
256     double a0 = aPlusOne + aMinusOne * k + k2;
257     double a1 = -2 * (aMinusOne + aPlusOne * k);
258     double a2 = aPlusOne + aMinusOne * k - k2;
259
260     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
261 }
262
263 void Biquad::setHighShelfParams(double frequency, double dbGain)
264 {
265     double w0 = piDouble * frequency;
266
267     double A = pow(10.0, dbGain / 40);
268     double S = 1; // filter slope (1 is max value)
269     double alpha = 0.5 * sin(w0) * sqrt((A + 1 / A) * (1 / S - 1) + 2);
270
271     double k = cos(w0);
272     double k2 = 2 * sqrt(A) * alpha;
273
274     double aPlusOne = A + 1;
275     double aMinusOne = A - 1;
276     
277     double b0 = A * (aPlusOne + aMinusOne * k + k2);
278     double b1 = -2 * A * (aMinusOne + aPlusOne * k);
279     double b2 = A * (aPlusOne + aMinusOne * k - k2);
280     double a0 = aPlusOne - aMinusOne * k + k2;
281     double a1 = 2 * (aMinusOne - aPlusOne * k);
282     double a2 = aPlusOne - aMinusOne * k - k2;
283
284     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
285 }
286
287 void Biquad::setPeakingParams(double frequency, double Q, double dbGain)
288 {
289     double w0 = piDouble * frequency;
290     double alpha = sin(w0) / (2 * Q);
291     double A = pow(10.0, dbGain / 40);
292
293     double k = cos(w0);
294
295     double b0 = 1 + alpha * A;
296     double b1 = -2 * k;
297     double b2 = 1 - alpha * A;
298     double a0 = 1 + alpha / A;
299     double a1 = -2 * k;
300     double a2 = 1 - alpha / A;
301
302     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
303 }
304
305 void Biquad::setAllpassParams(double frequency, double Q)
306 {
307     double w0 = piDouble * frequency;
308     double alpha = sin(w0) / (2 * Q);
309
310     double k = cos(w0);
311
312     double b0 = 1 - alpha;
313     double b1 = -2 * k;
314     double b2 = 1 + alpha;
315     double a0 = 1 + alpha;
316     double a1 = -2 * k;
317     double a2 = 1 - alpha;
318
319     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
320 }
321
322 void Biquad::setNotchParams(double frequency, double Q)
323 {
324     double w0 = piDouble * frequency;
325     double alpha = sin(w0) / (2 * Q);
326
327     double k = cos(w0);
328
329     double b0 = 1;
330     double b1 = -2 * k;
331     double b2 = 1;
332     double a0 = 1 + alpha;
333     double a1 = -2 * k;
334     double a2 = 1 - alpha;
335
336     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
337 }
338
339 void Biquad::setBandpassParams(double frequency, double Q)
340 {
341     double w0 = piDouble * frequency;
342     double alpha = sin(w0) / (2 * Q);
343
344     double k = cos(w0);
345     
346     double b0 = alpha;
347     double b1 = 0;
348     double b2 = -alpha;
349     double a0 = 1 + alpha;
350     double a1 = -2 * k;
351     double a2 = 1 - alpha;
352
353     setNormalizedCoefficients(b0, b1, b2, a0, a1, a2);
354 }
355
356 void Biquad::setZeroPolePairs(const Complex &zero, const Complex &pole)
357 {
358     m_b0 = 1;
359     m_b1 = -2 * zero.real();
360
361     double zeroMag = abs(zero);
362     m_b2 = zeroMag * zeroMag;
363
364     m_a1 = -2 * pole.real();
365
366     double poleMag = abs(pole);
367     m_a2 = poleMag * poleMag;
368 }
369
370 void Biquad::setAllpassPole(const Complex &pole)
371 {
372     Complex zero = Complex(1, 0) / pole;
373     setZeroPolePairs(zero, pole);
374 }
375
376 } // namespace WebCore
377
378 #endif // ENABLE(WEB_AUDIO)