1 /*
2 * Copyright (c) 2007, 2013, Oracle and/or its affiliates. All rights reserved.
3 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
4 *
5 * This code is free software; you can redistribute it and/or modify it
6 * under the terms of the GNU General Public License version 2 only, as
7 * published by the Free Software Foundation. Oracle designates this
8 * particular file as subject to the "Classpath" exception as provided
9 * by Oracle in the LICENSE file that accompanied this code.
10 *
11 * This code is distributed in the hope that it will be useful, but WITHOUT
12 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
13 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
14 * version 2 for more details (a copy is included in the LICENSE file that
15 * accompanied this code).
16 *
17 * You should have received a copy of the GNU General Public License version
18 * 2 along with this work; if not, write to the Free Software Foundation,
19 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
20 *
21 * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
22 * or visit www.oracle.com if you need additional information or have any
23 * questions.
24 */
25 package com.sun.media.sound;
26
27 /**
28 * Infinite impulse response (IIR) filter class.
29 *
30 * The filters where implemented and adapted using algorithms from musicdsp.org
31 * archive: 1-RC and C filter, Simple 2-pole LP LP and HP filter, biquad,
32 * tweaked butterworth RBJ Audio-EQ-Cookbook, EQ filter kookbook
33 *
34 * @author Karl Helgason
35 */
36 public final class SoftFilter {
37
38 public final static int FILTERTYPE_LP6 = 0x00;
39 public final static int FILTERTYPE_LP12 = 0x01;
40 public final static int FILTERTYPE_HP12 = 0x11;
41 public final static int FILTERTYPE_BP12 = 0x21;
42 public final static int FILTERTYPE_NP12 = 0x31;
43 public final static int FILTERTYPE_LP24 = 0x03;
44 public final static int FILTERTYPE_HP24 = 0x13;
45
46 //
47 // 0x0 = 1st-order, 6 dB/oct
48 // 0x1 = 2nd-order, 12 dB/oct
49 // 0x2 = 3rd-order, 18 dB/oct
50 // 0x3 = 4th-order, 24 db/oct
51 //
52 // 0x00 = LP, Low Pass Filter
53 // 0x10 = HP, High Pass Filter
54 // 0x20 = BP, Band Pass Filter
55 // 0x30 = NP, Notch or Band Elimination Filter
56 //
57 private int filtertype = FILTERTYPE_LP6;
58 private final float samplerate;
59 private float x1;
60 private float x2;
61 private float y1;
62 private float y2;
63 private float xx1;
64 private float xx2;
65 private float yy1;
66 private float yy2;
67 private float a0;
68 private float a1;
69 private float a2;
70 private float b1;
71 private float b2;
72 private float q;
73 private float gain = 1;
74 private float wet = 0;
75 private float last_wet = 0;
76 private float last_a0;
77 private float last_a1;
78 private float last_a2;
79 private float last_b1;
80 private float last_b2;
81 private float last_q;
82 private float last_gain;
83 private boolean last_set = false;
84 private double cutoff = 44100;
85 private double resonancedB = 0;
86 private boolean dirty = true;
87
88 public SoftFilter(float samplerate) {
89 this.samplerate = samplerate;
90 dirty = true;
91 }
92
93 public void setFrequency(double cent) {
94 if (cutoff == cent)
95 return;
96 cutoff = cent;
97 dirty = true;
98 }
99
100 public void setResonance(double db) {
101 if (resonancedB == db)
102 return;
103 resonancedB = db;
104 dirty = true;
105 }
106
107 public void reset() {
108 dirty = true;
109 last_set = false;
110 x1 = 0;
111 x2 = 0;
112 y1 = 0;
113 y2 = 0;
114 xx1 = 0;
115 xx2 = 0;
116 yy1 = 0;
117 yy2 = 0;
118 wet = 0.0f;
119 gain = 1.0f;
120 a0 = 0;
121 a1 = 0;
122 a2 = 0;
123 b1 = 0;
124 b2 = 0;
125 }
126
127 public void setFilterType(int filtertype) {
128 this.filtertype = filtertype;
129 }
130
131 public void processAudio(SoftAudioBuffer sbuffer) {
132 if (filtertype == FILTERTYPE_LP6)
133 filter1(sbuffer);
134 if (filtertype == FILTERTYPE_LP12)
135 filter2(sbuffer);
136 if (filtertype == FILTERTYPE_HP12)
137 filter2(sbuffer);
138 if (filtertype == FILTERTYPE_BP12)
139 filter2(sbuffer);
140 if (filtertype == FILTERTYPE_NP12)
141 filter2(sbuffer);
142 if (filtertype == FILTERTYPE_LP24)
143 filter4(sbuffer);
144 if (filtertype == FILTERTYPE_HP24)
145 filter4(sbuffer);
146 }
147
148 public void filter4(SoftAudioBuffer sbuffer) {
149
150 float[] buffer = sbuffer.array();
151
152 if (dirty) {
153 filter2calc();
154 dirty = false;
155 }
156 if (!last_set) {
157 last_a0 = a0;
158 last_a1 = a1;
159 last_a2 = a2;
160 last_b1 = b1;
161 last_b2 = b2;
162 last_gain = gain;
163 last_wet = wet;
164 last_set = true;
165 }
166
167 if (wet > 0 || last_wet > 0) {
168
169 int len = buffer.length;
170 float a0 = this.last_a0;
171 float a1 = this.last_a1;
172 float a2 = this.last_a2;
173 float b1 = this.last_b1;
174 float b2 = this.last_b2;
175 float gain = this.last_gain;
176 float wet = this.last_wet;
177 float a0_delta = (this.a0 - this.last_a0) / len;
178 float a1_delta = (this.a1 - this.last_a1) / len;
179 float a2_delta = (this.a2 - this.last_a2) / len;
180 float b1_delta = (this.b1 - this.last_b1) / len;
181 float b2_delta = (this.b2 - this.last_b2) / len;
182 float gain_delta = (this.gain - this.last_gain) / len;
183 float wet_delta = (this.wet - this.last_wet) / len;
184 float x1 = this.x1;
185 float x2 = this.x2;
186 float y1 = this.y1;
187 float y2 = this.y2;
188 float xx1 = this.xx1;
189 float xx2 = this.xx2;
190 float yy1 = this.yy1;
191 float yy2 = this.yy2;
192
193 if (wet_delta != 0) {
194 for (int i = 0; i < len; i++) {
195 a0 += a0_delta;
196 a1 += a1_delta;
197 a2 += a2_delta;
198 b1 += b1_delta;
199 b2 += b2_delta;
200 gain += gain_delta;
201 wet += wet_delta;
202 float x = buffer[i];
203 float y = (a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2);
204 float xx = (y * gain) * wet + (x) * (1 - wet);
205 x2 = x1;
206 x1 = x;
207 y2 = y1;
208 y1 = y;
209 float yy = (a0*xx + a1*xx1 + a2*xx2 - b1*yy1 - b2*yy2);
210 buffer[i] = (yy * gain) * wet + (xx) * (1 - wet);
211 xx2 = xx1;
212 xx1 = xx;
213 yy2 = yy1;
214 yy1 = yy;
215 }
216 } else if (a0_delta == 0 && a1_delta == 0 && a2_delta == 0
217 && b1_delta == 0 && b2_delta == 0) {
218 for (int i = 0; i < len; i++) {
219 float x = buffer[i];
220 float y = (a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2);
221 float xx = (y * gain) * wet + (x) * (1 - wet);
222 x2 = x1;
223 x1 = x;
224 y2 = y1;
225 y1 = y;
226 float yy = (a0*xx + a1*xx1 + a2*xx2 - b1*yy1 - b2*yy2);
227 buffer[i] = (yy * gain) * wet + (xx) * (1 - wet);
228 xx2 = xx1;
229 xx1 = xx;
230 yy2 = yy1;
231 yy1 = yy;
232 }
233 } else {
234 for (int i = 0; i < len; i++) {
235 a0 += a0_delta;
236 a1 += a1_delta;
237 a2 += a2_delta;
238 b1 += b1_delta;
239 b2 += b2_delta;
240 gain += gain_delta;
241 float x = buffer[i];
242 float y = (a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2);
243 float xx = (y * gain) * wet + (x) * (1 - wet);
244 x2 = x1;
245 x1 = x;
246 y2 = y1;
247 y1 = y;
248 float yy = (a0*xx + a1*xx1 + a2*xx2 - b1*yy1 - b2*yy2);
249 buffer[i] = (yy * gain) * wet + (xx) * (1 - wet);
250 xx2 = xx1;
251 xx1 = xx;
252 yy2 = yy1;
253 yy1 = yy;
254 }
255 }
256
257 if (Math.abs(x1) < 1.0E-8)
258 x1 = 0;
259 if (Math.abs(x2) < 1.0E-8)
260 x2 = 0;
261 if (Math.abs(y1) < 1.0E-8)
262 y1 = 0;
263 if (Math.abs(y2) < 1.0E-8)
264 y2 = 0;
265 this.x1 = x1;
266 this.x2 = x2;
267 this.y1 = y1;
268 this.y2 = y2;
269 this.xx1 = xx1;
270 this.xx2 = xx2;
271 this.yy1 = yy1;
272 this.yy2 = yy2;
273 }
274
275 this.last_a0 = this.a0;
276 this.last_a1 = this.a1;
277 this.last_a2 = this.a2;
278 this.last_b1 = this.b1;
279 this.last_b2 = this.b2;
280 this.last_gain = this.gain;
281 this.last_wet = this.wet;
282
283 }
284
285 private double sinh(double x) {
286 return (Math.exp(x) - Math.exp(-x)) * 0.5;
287 }
288
289 public void filter2calc() {
290
291 double resonancedB = this.resonancedB;
292 if (resonancedB < 0)
293 resonancedB = 0; // Negative dB are illegal.
294 if (resonancedB > 30)
295 resonancedB = 30; // At least 22.5 dB is needed.
296 if (filtertype == FILTERTYPE_LP24 || filtertype == FILTERTYPE_HP24)
297 resonancedB *= 0.6;
298
299 if (filtertype == FILTERTYPE_BP12) {
300 wet = 1;
301 double r = (cutoff / samplerate);
302 if (r > 0.45)
303 r = 0.45;
304
305 double bandwidth = Math.PI * Math.pow(10.0, -(resonancedB / 20));
306
307 double omega = 2 * Math.PI * r;
308 double cs = Math.cos(omega);
309 double sn = Math.sin(omega);
310 double alpha = sn * sinh((Math.log(2)*bandwidth*omega) / (sn * 2));
311
312 double b0 = alpha;
313 double b1 = 0;
314 double b2 = -alpha;
315 double a0 = 1 + alpha;
316 double a1 = -2 * cs;
317 double a2 = 1 - alpha;
318
319 double cf = 1.0 / a0;
320 this.b1 = (float) (a1 * cf);
321 this.b2 = (float) (a2 * cf);
322 this.a0 = (float) (b0 * cf);
323 this.a1 = (float) (b1 * cf);
324 this.a2 = (float) (b2 * cf);
325 }
326
327 if (filtertype == FILTERTYPE_NP12) {
328 wet = 1;
329 double r = (cutoff / samplerate);
330 if (r > 0.45)
331 r = 0.45;
332
333 double bandwidth = Math.PI * Math.pow(10.0, -(resonancedB / 20));
334
335 double omega = 2 * Math.PI * r;
336 double cs = Math.cos(omega);
337 double sn = Math.sin(omega);
338 double alpha = sn * sinh((Math.log(2)*bandwidth*omega) / (sn*2));
339
340 double b0 = 1;
341 double b1 = -2 * cs;
342 double b2 = 1;
343 double a0 = 1 + alpha;
344 double a1 = -2 * cs;
345 double a2 = 1 - alpha;
346
347 double cf = 1.0 / a0;
348 this.b1 = (float)(a1 * cf);
349 this.b2 = (float)(a2 * cf);
350 this.a0 = (float)(b0 * cf);
351 this.a1 = (float)(b1 * cf);
352 this.a2 = (float)(b2 * cf);
353 }
354
355 if (filtertype == FILTERTYPE_LP12 || filtertype == FILTERTYPE_LP24) {
356 double r = (cutoff / samplerate);
357 if (r > 0.45) {
358 if (wet == 0) {
359 if (resonancedB < 0.00001)
360 wet = 0.0f;
361 else
362 wet = 1.0f;
363 }
364 r = 0.45;
365 } else
366 wet = 1.0f;
367
368 double c = 1.0 / (Math.tan(Math.PI * r));
369 double csq = c * c;
370 double resonance = Math.pow(10.0, -(resonancedB / 20));
371 double q = Math.sqrt(2.0f) * resonance;
372 double a0 = 1.0 / (1.0 + (q * c) + (csq));
373 double a1 = 2.0 * a0;
374 double a2 = a0;
375 double b1 = (2.0 * a0) * (1.0 - csq);
376 double b2 = a0 * (1.0 - (q * c) + csq);
377
378 this.a0 = (float)a0;
379 this.a1 = (float)a1;
380 this.a2 = (float)a2;
381 this.b1 = (float)b1;
382 this.b2 = (float)b2;
383
384 }
385
386 if (filtertype == FILTERTYPE_HP12 || filtertype == FILTERTYPE_HP24) {
387 double r = (cutoff / samplerate);
388 if (r > 0.45)
389 r = 0.45;
390 if (r < 0.0001)
391 r = 0.0001;
392 wet = 1.0f;
393 double c = (Math.tan(Math.PI * (r)));
394 double csq = c * c;
395 double resonance = Math.pow(10.0, -(resonancedB / 20));
396 double q = Math.sqrt(2.0f) * resonance;
397 double a0 = 1.0 / (1.0 + (q * c) + (csq));
398 double a1 = -2.0 * a0;
399 double a2 = a0;
400 double b1 = (2.0 * a0) * (csq - 1.0);
401 double b2 = a0 * (1.0 - (q * c) + csq);
402
403 this.a0 = (float)a0;
404 this.a1 = (float)a1;
405 this.a2 = (float)a2;
406 this.b1 = (float)b1;
407 this.b2 = (float)b2;
408
409 }
410
411 }
412
413 public void filter2(SoftAudioBuffer sbuffer) {
414
415 float[] buffer = sbuffer.array();
416
417 if (dirty) {
418 filter2calc();
419 dirty = false;
420 }
421 if (!last_set) {
422 last_a0 = a0;
423 last_a1 = a1;
424 last_a2 = a2;
425 last_b1 = b1;
426 last_b2 = b2;
427 last_q = q;
428 last_gain = gain;
429 last_wet = wet;
430 last_set = true;
431 }
432
433 if (wet > 0 || last_wet > 0) {
434
435 int len = buffer.length;
436 float a0 = this.last_a0;
437 float a1 = this.last_a1;
438 float a2 = this.last_a2;
439 float b1 = this.last_b1;
440 float b2 = this.last_b2;
441 float gain = this.last_gain;
442 float wet = this.last_wet;
443 float a0_delta = (this.a0 - this.last_a0) / len;
444 float a1_delta = (this.a1 - this.last_a1) / len;
445 float a2_delta = (this.a2 - this.last_a2) / len;
446 float b1_delta = (this.b1 - this.last_b1) / len;
447 float b2_delta = (this.b2 - this.last_b2) / len;
448 float gain_delta = (this.gain - this.last_gain) / len;
449 float wet_delta = (this.wet - this.last_wet) / len;
450 float x1 = this.x1;
451 float x2 = this.x2;
452 float y1 = this.y1;
453 float y2 = this.y2;
454
455 if (wet_delta != 0) {
456 for (int i = 0; i < len; i++) {
457 a0 += a0_delta;
458 a1 += a1_delta;
459 a2 += a2_delta;
460 b1 += b1_delta;
461 b2 += b2_delta;
462 gain += gain_delta;
463 wet += wet_delta;
464 float x = buffer[i];
465 float y = (a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2);
466 buffer[i] = (y * gain) * wet + (x) * (1 - wet);
467 x2 = x1;
468 x1 = x;
469 y2 = y1;
470 y1 = y;
471 }
472 } else if (a0_delta == 0 && a1_delta == 0 && a2_delta == 0
473 && b1_delta == 0 && b2_delta == 0) {
474 for (int i = 0; i < len; i++) {
475 float x = buffer[i];
476 float y = (a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2);
477 buffer[i] = y * gain;
478 x2 = x1;
479 x1 = x;
480 y2 = y1;
481 y1 = y;
482 }
483 } else {
484 for (int i = 0; i < len; i++) {
485 a0 += a0_delta;
486 a1 += a1_delta;
487 a2 += a2_delta;
488 b1 += b1_delta;
489 b2 += b2_delta;
490 gain += gain_delta;
491 float x = buffer[i];
492 float y = (a0*x + a1*x1 + a2*x2 - b1*y1 - b2*y2);
493 buffer[i] = y * gain;
494 x2 = x1;
495 x1 = x;
496 y2 = y1;
497 y1 = y;
498 }
499 }
500
501 if (Math.abs(x1) < 1.0E-8)
502 x1 = 0;
503 if (Math.abs(x2) < 1.0E-8)
504 x2 = 0;
505 if (Math.abs(y1) < 1.0E-8)
506 y1 = 0;
507 if (Math.abs(y2) < 1.0E-8)
508 y2 = 0;
509 this.x1 = x1;
510 this.x2 = x2;
511 this.y1 = y1;
512 this.y2 = y2;
513 }
514
515 this.last_a0 = this.a0;
516 this.last_a1 = this.a1;
517 this.last_a2 = this.a2;
518 this.last_b1 = this.b1;
519 this.last_b2 = this.b2;
520 this.last_q = this.q;
521 this.last_gain = this.gain;
522 this.last_wet = this.wet;
523
524 }
525
526 public void filter1calc() {
527 if (cutoff < 120)
528 cutoff = 120;
529 double c = (7.0 / 6.0) * Math.PI * 2 * cutoff / samplerate;
530 if (c > 1)
531 c = 1;
532 a0 = (float)(Math.sqrt(1 - Math.cos(c)) * Math.sqrt(0.5 * Math.PI));
533 if (resonancedB < 0)
534 resonancedB = 0;
535 if (resonancedB > 20)
536 resonancedB = 20;
537 q = (float)(Math.sqrt(0.5) * Math.pow(10.0, -(resonancedB / 20)));
538 gain = (float)Math.pow(10, -((resonancedB)) / 40.0);
539 if (wet == 0.0f)
540 if (resonancedB > 0.00001 || c < 0.9999999)
541 wet = 1.0f;
542 }
543
544 public void filter1(SoftAudioBuffer sbuffer) {
545
546 if (dirty) {
547 filter1calc();
548 dirty = false;
549 }
550 if (!last_set) {
551 last_a0 = a0;
552 last_q = q;
553 last_gain = gain;
554 last_wet = wet;
555 last_set = true;
556 }
557
558 if (wet > 0 || last_wet > 0) {
559
560 float[] buffer = sbuffer.array();
561 int len = buffer.length;
562 float a0 = this.last_a0;
563 float q = this.last_q;
564 float gain = this.last_gain;
565 float wet = this.last_wet;
566 float a0_delta = (this.a0 - this.last_a0) / len;
567 float q_delta = (this.q - this.last_q) / len;
568 float gain_delta = (this.gain - this.last_gain) / len;
569 float wet_delta = (this.wet - this.last_wet) / len;
570 float y2 = this.y2;
571 float y1 = this.y1;
572
573 if (wet_delta != 0) {
574 for (int i = 0; i < len; i++) {
575 a0 += a0_delta;
576 q += q_delta;
577 gain += gain_delta;
578 wet += wet_delta;
579 float ga0 = (1 - q * a0);
580 y1 = ga0 * y1 + (a0) * (buffer[i] - y2);
581 y2 = ga0 * y2 + (a0) * y1;
582 buffer[i] = y2 * gain * wet + buffer[i] * (1 - wet);
583 }
584 } else if (a0_delta == 0 && q_delta == 0) {
585 float ga0 = (1 - q * a0);
586 for (int i = 0; i < len; i++) {
587 y1 = ga0 * y1 + (a0) * (buffer[i] - y2);
588 y2 = ga0 * y2 + (a0) * y1;
589 buffer[i] = y2 * gain;
590 }
591 } else {
592 for (int i = 0; i < len; i++) {
593 a0 += a0_delta;
594 q += q_delta;
595 gain += gain_delta;
596 float ga0 = (1 - q * a0);
597 y1 = ga0 * y1 + (a0) * (buffer[i] - y2);
598 y2 = ga0 * y2 + (a0) * y1;
599 buffer[i] = y2 * gain;
600 }
601 }
602
603 if (Math.abs(y2) < 1.0E-8)
604 y2 = 0;
605 if (Math.abs(y1) < 1.0E-8)
606 y1 = 0;
607 this.y2 = y2;
608 this.y1 = y1;
609 }
610
611 this.last_a0 = this.a0;
612 this.last_q = this.q;
613 this.last_gain = this.gain;
614 this.last_wet = this.wet;
615 }
616 }