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