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