1 // (also appears in k054539.cpp, d_spectrum.cpp c/o dink) 2 // direct form II(transposed) biquadradic filter, needed for delay(echo) effect's filter taps -dink 3 enum { FILT_HIGHPASS = 0, FILT_LOWPASS = 1, FILT_LOWSHELF = 2, FILT_HIGHSHELF = 3, FILT_PEAK = 4, FILT_NOTCH = 5, FILT_BANDPASS }; 4 5 struct BIQ { 6 double a0; 7 double a1; 8 double a2; 9 double b1; 10 double b2; 11 double q; 12 double z1; 13 double z2; 14 double frequency; 15 double samplerate; 16 double output; 17 filterBIQ18 float filter(float input) { 19 output = input * a0 + z1; 20 z1 = input * a1 + z2 - b1 * output; 21 z2 = input * a2 - b2 * output; 22 return (float)output; 23 } 24 resetBIQ25 void reset() { 26 z1 = 0.0; z2 = 0.0; output = 0.0; 27 } 28 exitBIQ29 void exit() { 30 reset(); 31 } 32 initBIQ33 void init(INT32 type, INT32 sample_rate, INT32 freqhz, double q_, double gain) { 34 samplerate = sample_rate; 35 frequency = freqhz; 36 q = q_; 37 38 reset(); 39 40 double k = tan(3.14159265358979323846 * frequency / samplerate); 41 double norm = 1 / (1 + k / q + k * k); 42 double v = pow(10, fabs(gain) / 20); 43 44 switch (type) { 45 case FILT_HIGHPASS: 46 { 47 a0 = 1 * norm; 48 a1 = -2 * a0; 49 a2 = a0; 50 b1 = 2 * (k * k - 1) * norm; 51 b2 = (1 - k / q + k * k) * norm; 52 } 53 break; 54 case FILT_LOWPASS: 55 { 56 a0 = k * k * norm; 57 a1 = 2 * a0; 58 a2 = a0; 59 b1 = 2 * (k * k - 1) * norm; 60 b2 = (1 - k / q + k * k) * norm; 61 } 62 break; 63 case FILT_BANDPASS: 64 { 65 norm = 1 / (1 + k / q + k * k); 66 a0 = k / q * norm; 67 a1 = 0; 68 a2 = -a0; 69 b1 = 2 * (k * k - 1) * norm; 70 b2 = (1 - k / q + k * k) * norm; 71 } 72 break; 73 case FILT_LOWSHELF: 74 { 75 if (gain >= 0) { 76 norm = 1 / (1 + sqrt(2.0) * k + k * k); 77 a0 = (1 + sqrt(2*v) * k + v * k * k) * norm; 78 a1 = 2 * (v * k * k - 1) * norm; 79 a2 = (1 - sqrt(2*v) * k + v * k * k) * norm; 80 b1 = 2 * (k * k - 1) * norm; 81 b2 = (1 - sqrt(2.0) * k + k * k) * norm; 82 } else { 83 norm = 1 / (1 + sqrt(2*v) * k + v * k * k); 84 a0 = (1 + sqrt(2.0) * k + k * k) * norm; 85 a1 = 2 * (k * k - 1) * norm; 86 a2 = (1 - sqrt(2.0) * k + k * k) * norm; 87 b1 = 2 * (v * k * k - 1) * norm; 88 b2 = (1 - sqrt(2*v) * k + v * k * k) * norm; 89 } 90 } 91 break; 92 case FILT_HIGHSHELF: 93 { 94 if (gain >= 0) { 95 norm = 1 / (1 + sqrt(2.0) * k + k * k); 96 a0 = (v + sqrt(2*v) * k + k * k) * norm; 97 a1 = 2 * (k * k - v) * norm; 98 a2 = (v - sqrt(2*v) * k + k * k) * norm; 99 b1 = 2 * (k * k - 1) * norm; 100 b2 = (1 - sqrt(2.0) * k + k * k) * norm; 101 } else { 102 norm = 1 / (v + sqrt(2*v) * k + k * k); 103 a0 = (1 + sqrt(2.0) * k + k * k) * norm; 104 a1 = 2 * (k * k - 1) * norm; 105 a2 = (1 - sqrt(2.0) * k + k * k) * norm; 106 b1 = 2 * (k * k - v) * norm; 107 b2 = (v - sqrt(2*v) * k + k * k) * norm; 108 } 109 } 110 break; 111 case FILT_PEAK: 112 { 113 if (gain >= 0) { 114 norm = 1 / (1 + 1 / q * k + k * k); 115 a0 = (1 + v / q * k + k * k) * norm; 116 a1 = 2 * (k * k - 1) * norm; 117 a2 = (1 - v / q * k + k * k) * norm; 118 b1 = a1; 119 b2 = (1 - 1 / q * k + k * k) * norm; 120 } else { 121 norm = 1 / (1 + v / q * k + k * k); 122 a0 = (1 + 1 / q * k + k * k) * norm; 123 a1 = 2 * (k * k - 1) * norm; 124 a2 = (1 - 1 / q * k + k * k) * norm; 125 b1 = a1; 126 b2 = (1 - v / q * k + k * k) * norm; 127 } 128 } 129 break; 130 case FILT_NOTCH: 131 { 132 norm = 1 / (1 + k / q + k * k); 133 a0 = (1 + k * k) * norm; 134 a1 = 2 * (k * k - 1) * norm; 135 a2 = a0; 136 b1 = a1; 137 b2 = (1 - k / q + k * k) * norm; 138 } 139 break; 140 } 141 } 142 }; 143 // end biquad filter 144 145 struct BIQSTEREO { 146 BIQ L; 147 BIQ R; 148 initBIQSTEREO149 void init(INT32 type, INT32 sample_rate, INT32 freqhz, double q_, double gain) { 150 L.init(type, sample_rate, freqhz, q_, gain); 151 R.init(type, sample_rate, freqhz, q_, gain); 152 } 153 resetBIQSTEREO154 void reset() { 155 L.reset(); 156 R.reset(); 157 } 158 exitBIQSTEREO159 void exit() { 160 L.exit(); 161 R.exit(); 162 } 163 filter_bufferBIQSTEREO164 void filter_buffer(INT16 *buffer, INT32 buflen) { 165 for (INT32 i = 0; i < buflen; i++) { 166 INT32 idx = i * 2; 167 INT32 l = L.filter(buffer[idx + 0]); 168 INT32 r = R.filter(buffer[idx + 1]); 169 buffer[idx + 0] = BURN_SND_CLIP(l); 170 buffer[idx + 1] = BURN_SND_CLIP(r); 171 } 172 } 173 }; 174