1/**************************************************************************/
2/* audio_filter_sw.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "audio_filter_sw.h"
32
33void AudioFilterSW::set_mode(Mode p_mode) {
34 mode = p_mode;
35}
36
37void AudioFilterSW::set_cutoff(float p_cutoff) {
38 cutoff = p_cutoff;
39}
40
41void AudioFilterSW::set_resonance(float p_resonance) {
42 resonance = p_resonance;
43}
44
45void AudioFilterSW::set_gain(float p_gain) {
46 gain = p_gain;
47}
48
49void AudioFilterSW::set_sampling_rate(float p_srate) {
50 sampling_rate = p_srate;
51}
52
53void AudioFilterSW::prepare_coefficients(Coeffs *p_coeffs) {
54 int sr_limit = (sampling_rate / 2) + 512;
55
56 double final_cutoff = (cutoff > sr_limit) ? sr_limit : cutoff;
57 if (final_cutoff < 1) {
58 final_cutoff = 1; //don't allow less than this
59 }
60
61 double omega = Math_TAU * final_cutoff / sampling_rate;
62
63 double sin_v = Math::sin(omega);
64 double cos_v = Math::cos(omega);
65
66 double Q = resonance;
67 if (Q <= 0.0) {
68 Q = 0.0001;
69 }
70
71 if (mode == BANDPASS) {
72 Q *= 2.0;
73 } else if (mode == PEAK) {
74 Q *= 3.0;
75 }
76
77 double tmpgain = gain;
78
79 if (tmpgain < 0.001) {
80 tmpgain = 0.001;
81 }
82
83 if (stages > 1) {
84 Q = (Q > 1.0 ? Math::pow(Q, 1.0 / stages) : Q);
85 tmpgain = Math::pow(tmpgain, 1.0 / (stages + 1));
86 }
87 double alpha = sin_v / (2 * Q);
88
89 double a0 = 1.0 + alpha;
90
91 switch (mode) {
92 case LOWPASS: {
93 p_coeffs->b0 = (1.0 - cos_v) / 2.0;
94 p_coeffs->b1 = 1.0 - cos_v;
95 p_coeffs->b2 = (1.0 - cos_v) / 2.0;
96 p_coeffs->a1 = -2.0 * cos_v;
97 p_coeffs->a2 = 1.0 - alpha;
98 } break;
99
100 case HIGHPASS: {
101 p_coeffs->b0 = (1.0 + cos_v) / 2.0;
102 p_coeffs->b1 = -(1.0 + cos_v);
103 p_coeffs->b2 = (1.0 + cos_v) / 2.0;
104 p_coeffs->a1 = -2.0 * cos_v;
105 p_coeffs->a2 = 1.0 - alpha;
106 } break;
107
108 case BANDPASS: {
109 p_coeffs->b0 = alpha * sqrt(Q + 1);
110 p_coeffs->b1 = 0.0;
111 p_coeffs->b2 = -alpha * sqrt(Q + 1);
112 p_coeffs->a1 = -2.0 * cos_v;
113 p_coeffs->a2 = 1.0 - alpha;
114 } break;
115
116 case NOTCH: {
117 p_coeffs->b0 = 1.0;
118 p_coeffs->b1 = -2.0 * cos_v;
119 p_coeffs->b2 = 1.0;
120 p_coeffs->a1 = -2.0 * cos_v;
121 p_coeffs->a2 = 1.0 - alpha;
122 } break;
123 case PEAK: {
124 p_coeffs->b0 = (1.0 + alpha * tmpgain);
125 p_coeffs->b1 = (-2.0 * cos_v);
126 p_coeffs->b2 = (1.0 - alpha * tmpgain);
127 p_coeffs->a1 = -2 * cos_v;
128 p_coeffs->a2 = (1 - alpha / tmpgain);
129 } break;
130 case BANDLIMIT: {
131 //this one is extra tricky
132 double hicutoff = resonance;
133 double centercutoff = (cutoff + resonance) / 2.0;
134 double bandwidth = (Math::log(centercutoff) - Math::log(hicutoff)) / Math::log((double)2);
135 omega = Math_TAU * centercutoff / sampling_rate;
136 alpha = Math::sin(omega) * Math::sinh(Math::log((double)2) / 2 * bandwidth * omega / Math::sin(omega));
137 a0 = 1 + alpha;
138
139 p_coeffs->b0 = alpha;
140 p_coeffs->b1 = 0;
141 p_coeffs->b2 = -alpha;
142 p_coeffs->a1 = -2 * Math::cos(omega);
143 p_coeffs->a2 = 1 - alpha;
144
145 } break;
146 case LOWSHELF: {
147 double tmpq = Math::sqrt(Q);
148 if (tmpq <= 0) {
149 tmpq = 0.001;
150 }
151 double beta = Math::sqrt(tmpgain) / tmpq;
152
153 a0 = (tmpgain + 1.0) + (tmpgain - 1.0) * cos_v + beta * sin_v;
154 p_coeffs->b0 = tmpgain * ((tmpgain + 1.0) - (tmpgain - 1.0) * cos_v + beta * sin_v);
155 p_coeffs->b1 = 2.0 * tmpgain * ((tmpgain - 1.0) - (tmpgain + 1.0) * cos_v);
156 p_coeffs->b2 = tmpgain * ((tmpgain + 1.0) - (tmpgain - 1.0) * cos_v - beta * sin_v);
157 p_coeffs->a1 = -2.0 * ((tmpgain - 1.0) + (tmpgain + 1.0) * cos_v);
158 p_coeffs->a2 = ((tmpgain + 1.0) + (tmpgain - 1.0) * cos_v - beta * sin_v);
159
160 } break;
161 case HIGHSHELF: {
162 double tmpq = Math::sqrt(Q);
163 if (tmpq <= 0) {
164 tmpq = 0.001;
165 }
166 double beta = Math::sqrt(tmpgain) / tmpq;
167
168 a0 = (tmpgain + 1.0) - (tmpgain - 1.0) * cos_v + beta * sin_v;
169 p_coeffs->b0 = tmpgain * ((tmpgain + 1.0) + (tmpgain - 1.0) * cos_v + beta * sin_v);
170 p_coeffs->b1 = -2.0 * tmpgain * ((tmpgain - 1.0) + (tmpgain + 1.0) * cos_v);
171 p_coeffs->b2 = tmpgain * ((tmpgain + 1.0) + (tmpgain - 1.0) * cos_v - beta * sin_v);
172 p_coeffs->a1 = 2.0 * ((tmpgain - 1.0) - (tmpgain + 1.0) * cos_v);
173 p_coeffs->a2 = ((tmpgain + 1.0) - (tmpgain - 1.0) * cos_v - beta * sin_v);
174
175 } break;
176 }
177
178 p_coeffs->b0 /= a0;
179 p_coeffs->b1 /= a0;
180 p_coeffs->b2 /= a0;
181 p_coeffs->a1 /= 0.0 - a0;
182 p_coeffs->a2 /= 0.0 - a0;
183}
184
185void AudioFilterSW::set_stages(int p_stages) {
186 stages = p_stages;
187}
188
189/* Fourier transform kernel to obtain response */
190
191float AudioFilterSW::get_response(float p_freq, Coeffs *p_coeffs) {
192 float freq = p_freq / sampling_rate * Math_TAU;
193
194 float cx = p_coeffs->b0, cy = 0.0;
195
196 cx += cos(freq) * p_coeffs->b1;
197 cy -= sin(freq) * p_coeffs->b1;
198 cx += cos(2 * freq) * p_coeffs->b2;
199 cy -= sin(2 * freq) * p_coeffs->b2;
200
201 float H = cx * cx + cy * cy;
202 cx = 1.0;
203 cy = 0.0;
204
205 cx -= cos(freq) * p_coeffs->a1;
206 cy += sin(freq) * p_coeffs->a1;
207 cx -= cos(2 * freq) * p_coeffs->a2;
208 cy += sin(2 * freq) * p_coeffs->a2;
209
210 H = H / (cx * cx + cy * cy);
211 return H;
212}
213
214AudioFilterSW::Processor::Processor() {
215 set_filter(nullptr);
216}
217
218void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter, bool p_clear_history) {
219 if (p_clear_history) {
220 ha1 = ha2 = hb1 = hb2 = 0;
221 }
222 filter = p_filter;
223}
224
225void AudioFilterSW::Processor::update_coeffs(int p_interp_buffer_len) {
226 if (!filter) {
227 return;
228 }
229
230 if (p_interp_buffer_len) { //interpolate
231 Coeffs old_coeffs = coeffs;
232 filter->prepare_coefficients(&coeffs);
233 incr_coeffs.a1 = (coeffs.a1 - old_coeffs.a1) / p_interp_buffer_len;
234 incr_coeffs.a2 = (coeffs.a2 - old_coeffs.a2) / p_interp_buffer_len;
235 incr_coeffs.b0 = (coeffs.b0 - old_coeffs.b0) / p_interp_buffer_len;
236 incr_coeffs.b1 = (coeffs.b1 - old_coeffs.b1) / p_interp_buffer_len;
237 incr_coeffs.b2 = (coeffs.b2 - old_coeffs.b2) / p_interp_buffer_len;
238 coeffs = old_coeffs;
239 } else {
240 filter->prepare_coefficients(&coeffs);
241 }
242}
243
244void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride, bool p_interpolate) {
245 if (!filter) {
246 return;
247 }
248
249 if (p_interpolate) {
250 for (int i = 0; i < p_amount; i++) {
251 process_one_interp(*p_samples);
252 p_samples += p_stride;
253 }
254 } else {
255 for (int i = 0; i < p_amount; i++) {
256 process_one(*p_samples);
257 p_samples += p_stride;
258 }
259 }
260}
261