1 /** @addtogroup biquad
2 * @{
3 */
4 /*
5 Copyright (C) 2016 D Levin (https://www.kfrlib.com)
6 This file is part of KFR
7
8 KFR is free software: you can redistribute it and/or modify
9 it under the terms of the GNU General Public License as published by
10 the Free Software Foundation, either version 2 of the License, or
11 (at your option) any later version.
12
13 KFR is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU General Public License for more details.
17
18 You should have received a copy of the GNU General Public License
19 along with KFR.
20
21 If GPL is not suitable for your project, you must purchase a commercial license to use KFR.
22 Buying a commercial license is mandatory as soon as you develop commercial activities without
23 disclosing the source code of your own applications.
24 See https://www.kfrlib.com for details.
25 */
26 #pragma once
27
28 #include "../base/filter.hpp"
29 #include "../base/pointer.hpp"
30 #include "../math/hyperbolic.hpp"
31 #include "../simd/impl/function.hpp"
32 #include "../simd/operators.hpp"
33 #include "../simd/vec.hpp"
34 #include "../testo/assert.hpp"
35 #include "biquad_design.hpp"
36
37 namespace kfr
38 {
39
40 template <typename T>
41 struct zpk
42 {
43 univector<complex<T>> z;
44 univector<complex<T>> p;
45 T k;
46 };
47
48 inline namespace CMT_ARCH_NAME
49 {
50
51 template <typename T>
chebyshev1(int N,identity<T> rp)52 KFR_FUNCTION zpk<T> chebyshev1(int N, identity<T> rp)
53 {
54 if (N <= 0)
55 {
56 return { {}, {}, 1 };
57 }
58
59 T eps = sqrt(exp10(0.1 * rp) - 1.0);
60 T mu = 1.0 / N * std::asinh(1 / eps);
61
62 univector<T> m = linspace(-N + 1, N + 1, N, false, true);
63
64 univector<T> theta = c_pi<T> * m / (2 * N);
65 univector<complex<T>> p = -csinh(make_complex(mu, theta));
66 T k = product(-p).real();
67 if (N % 2 == 0)
68 k = k / sqrt(1.0 + eps * eps);
69 return { {}, std::move(p), k };
70 }
71
72 template <typename T>
chebyshev2(int N,identity<T> rs)73 KFR_FUNCTION zpk<T> chebyshev2(int N, identity<T> rs)
74 {
75 if (N <= 0)
76 {
77 return { {}, {}, 1 };
78 }
79
80 T de = 1.0 / sqrt(exp10(0.1 * rs) - 1);
81 T mu = std::asinh(1.0 / de) / N;
82
83 univector<T> m;
84
85 if (N % 2)
86 {
87 m = concatenate(linspace(-N + 1, -2, N / 2, true, true), linspace(2, N - 1, N / 2, true, true));
88 }
89 else
90 {
91 m = linspace(-N + 1, N + 1, N, false, true);
92 }
93
94 univector<complex<T>> z = -cconj(complex<T>(0, 1) / sin(m * c_pi<T> / (2.0 * N)));
95
96 univector<complex<T>> p =
97 -cexp(complex<T>(0, 1) * c_pi<T> * linspace(-N + 1, N + 1, N, false, true) / (2 * N));
98 p = make_complex(sinh(mu) * real(p), cosh(mu) * imag(p));
99 p = 1.0 / p;
100
101 T k = (product(-p) / product(-z)).real();
102
103 return { std::move(z), std::move(p), k };
104 }
105
106 template <typename T>
butterworth(int N)107 KFR_FUNCTION zpk<T> butterworth(int N)
108 {
109 switch (N)
110 {
111 case 1:
112 return { {}, { complex<T>(-1., +0.) }, 1 };
113 case 2:
114 return { {},
115 { complex<T>(-0.7071067811865476, -0.7071067811865476),
116 complex<T>(-0.7071067811865476, +0.7071067811865476) },
117 1 };
118 case 3:
119 return { {},
120 { complex<T>(-0.5000000000000001, -0.8660254037844386), complex<T>(-1., +0.),
121 complex<T>(-0.5000000000000001, +0.8660254037844386) },
122 1 };
123 case 4:
124 return { {},
125 { complex<T>(-0.38268343236508984, -0.9238795325112867),
126 complex<T>(-0.9238795325112867, -0.3826834323650898),
127 complex<T>(-0.9238795325112867, +0.3826834323650898),
128 complex<T>(-0.38268343236508984, +0.9238795325112867) },
129 1 };
130 case 5:
131 return { {},
132 { complex<T>(-0.30901699437494745, -0.9510565162951535),
133 complex<T>(-0.8090169943749475, -0.5877852522924731), complex<T>(-1., +0.),
134 complex<T>(-0.8090169943749475, +0.5877852522924731),
135 complex<T>(-0.30901699437494745, +0.9510565162951535) },
136 1 };
137 case 6:
138 return { {},
139 { complex<T>(-0.25881904510252096, -0.9659258262890682),
140 complex<T>(-0.7071067811865476, -0.7071067811865476),
141 complex<T>(-0.9659258262890683, -0.25881904510252074),
142 complex<T>(-0.9659258262890683, +0.25881904510252074),
143 complex<T>(-0.7071067811865476, +0.7071067811865476),
144 complex<T>(-0.25881904510252096, +0.9659258262890682) },
145 1 };
146 case 7:
147 return { {},
148 { complex<T>(-0.22252093395631445, -0.9749279121818236),
149 complex<T>(-0.6234898018587336, -0.7818314824680298),
150 complex<T>(-0.9009688679024191, -0.4338837391175581), complex<T>(-1., +0.),
151 complex<T>(-0.9009688679024191, +0.4338837391175581),
152 complex<T>(-0.6234898018587336, +0.7818314824680298),
153 complex<T>(-0.22252093395631445, +0.9749279121818236) },
154 1 };
155 case 8:
156 return { {},
157 { complex<T>(-0.19509032201612833, -0.9807852804032304),
158 complex<T>(-0.5555702330196023, -0.8314696123025452),
159 complex<T>(-0.8314696123025452, -0.5555702330196022),
160 complex<T>(-0.9807852804032304, -0.19509032201612825),
161 complex<T>(-0.9807852804032304, +0.19509032201612825),
162 complex<T>(-0.8314696123025452, +0.5555702330196022),
163 complex<T>(-0.5555702330196023, +0.8314696123025452),
164 complex<T>(-0.19509032201612833, +0.9807852804032304) },
165 1 };
166 case 9:
167 return { {},
168 { complex<T>(-0.17364817766693041, -0.984807753012208),
169 complex<T>(-0.5000000000000001, -0.8660254037844386),
170 complex<T>(-0.766044443118978, -0.6427876096865393),
171 complex<T>(-0.9396926207859084, -0.3420201433256687), complex<T>(-1., +0.),
172 complex<T>(-0.9396926207859084, +0.3420201433256687),
173 complex<T>(-0.766044443118978, +0.6427876096865393),
174 complex<T>(-0.5000000000000001, +0.8660254037844386),
175 complex<T>(-0.17364817766693041, +0.984807753012208) },
176 1 };
177 case 10:
178 return { {},
179 { complex<T>(-0.15643446504023092, -0.9876883405951378),
180 complex<T>(-0.4539904997395468, -0.8910065241883678),
181 complex<T>(-0.7071067811865476, -0.7071067811865476),
182 complex<T>(-0.8910065241883679, -0.45399049973954675),
183 complex<T>(-0.9876883405951378, -0.15643446504023087),
184 complex<T>(-0.9876883405951378, +0.15643446504023087),
185 complex<T>(-0.8910065241883679, +0.45399049973954675),
186 complex<T>(-0.7071067811865476, +0.7071067811865476),
187 complex<T>(-0.4539904997395468, +0.8910065241883678),
188 complex<T>(-0.15643446504023092, +0.9876883405951378) },
189 1 };
190 case 11:
191 return { {},
192 { complex<T>(-0.14231483827328512, -0.9898214418809327),
193 complex<T>(-0.41541501300188644, -0.9096319953545183),
194 complex<T>(-0.654860733945285, -0.7557495743542583),
195 complex<T>(-0.8412535328311812, -0.5406408174555976),
196 complex<T>(-0.9594929736144974, -0.28173255684142967), complex<T>(-1., +0.),
197 complex<T>(-0.9594929736144974, +0.28173255684142967),
198 complex<T>(-0.8412535328311812, +0.5406408174555976),
199 complex<T>(-0.654860733945285, +0.7557495743542583),
200 complex<T>(-0.41541501300188644, +0.9096319953545183),
201 complex<T>(-0.14231483827328512, +0.9898214418809327) },
202 1 };
203 case 12:
204 return { {},
205 { complex<T>(-0.13052619222005193, -0.9914448613738104),
206 complex<T>(-0.38268343236508984, -0.9238795325112867),
207 complex<T>(-0.6087614290087207, -0.7933533402912352),
208 complex<T>(-0.7933533402912353, -0.6087614290087205),
209 complex<T>(-0.9238795325112867, -0.3826834323650898),
210 complex<T>(-0.9914448613738104, -0.13052619222005157),
211 complex<T>(-0.9914448613738104, +0.13052619222005157),
212 complex<T>(-0.9238795325112867, +0.3826834323650898),
213 complex<T>(-0.7933533402912353, +0.6087614290087205),
214 complex<T>(-0.6087614290087207, +0.7933533402912352),
215 complex<T>(-0.38268343236508984, +0.9238795325112867),
216 complex<T>(-0.13052619222005193, +0.9914448613738104) },
217 1 };
218 case 13:
219 return { {},
220 { complex<T>(-0.120536680255323, -0.992708874098054),
221 complex<T>(-0.35460488704253557, -0.9350162426854148),
222 complex<T>(-0.5680647467311558, -0.8229838658936565),
223 complex<T>(-0.7485107481711011, -0.6631226582407952),
224 complex<T>(-0.8854560256532099, -0.46472317204376856),
225 complex<T>(-0.970941817426052, -0.23931566428755777), complex<T>(-1., +0.),
226 complex<T>(-0.970941817426052, +0.23931566428755777),
227 complex<T>(-0.8854560256532099, +0.46472317204376856),
228 complex<T>(-0.7485107481711011, +0.6631226582407952),
229 complex<T>(-0.5680647467311558, +0.8229838658936565),
230 complex<T>(-0.35460488704253557, +0.9350162426854148),
231 complex<T>(-0.120536680255323, +0.992708874098054) },
232 1 };
233 case 14:
234 return { {},
235 { complex<T>(-0.11196447610330791, -0.9937122098932426),
236 complex<T>(-0.3302790619551673, -0.9438833303083675),
237 complex<T>(-0.5320320765153366, -0.8467241992282841),
238 complex<T>(-0.7071067811865476, -0.7071067811865476),
239 complex<T>(-0.8467241992282842, -0.5320320765153366),
240 complex<T>(-0.9438833303083676, -0.3302790619551671),
241 complex<T>(-0.9937122098932426, -0.11196447610330786),
242 complex<T>(-0.9937122098932426, +0.11196447610330786),
243 complex<T>(-0.9438833303083676, +0.3302790619551671),
244 complex<T>(-0.8467241992282842, +0.5320320765153366),
245 complex<T>(-0.7071067811865476, +0.7071067811865476),
246 complex<T>(-0.5320320765153366, +0.8467241992282841),
247 complex<T>(-0.3302790619551673, +0.9438833303083675),
248 complex<T>(-0.11196447610330791, +0.9937122098932426) },
249 1 };
250 case 15:
251 return { {},
252 { complex<T>(-0.10452846326765346, -0.9945218953682733),
253 complex<T>(-0.30901699437494745, -0.9510565162951535),
254 complex<T>(-0.5000000000000001, -0.8660254037844386),
255 complex<T>(-0.6691306063588582, -0.7431448254773941),
256 complex<T>(-0.8090169943749475, -0.5877852522924731),
257 complex<T>(-0.9135454576426009, -0.40673664307580015),
258 complex<T>(-0.9781476007338057, -0.20791169081775931), complex<T>(-1., +0.),
259 complex<T>(-0.9781476007338057, +0.20791169081775931),
260 complex<T>(-0.9135454576426009, +0.40673664307580015),
261 complex<T>(-0.8090169943749475, +0.5877852522924731),
262 complex<T>(-0.6691306063588582, +0.7431448254773941),
263 complex<T>(-0.5000000000000001, +0.8660254037844386),
264 complex<T>(-0.30901699437494745, +0.9510565162951535),
265 complex<T>(-0.10452846326765346, +0.9945218953682733) },
266 1 };
267 case 16:
268 return { {},
269 { complex<T>(-0.09801714032956077, -0.9951847266721968),
270 complex<T>(-0.29028467725446233, -0.9569403357322089),
271 complex<T>(-0.4713967368259978, -0.8819212643483549),
272 complex<T>(-0.6343932841636455, -0.7730104533627369),
273 complex<T>(-0.773010453362737, -0.6343932841636455),
274 complex<T>(-0.881921264348355, -0.47139673682599764),
275 complex<T>(-0.9569403357322088, -0.29028467725446233),
276 complex<T>(-0.9951847266721969, -0.0980171403295606),
277 complex<T>(-0.9951847266721969, +0.0980171403295606),
278 complex<T>(-0.9569403357322088, +0.29028467725446233),
279 complex<T>(-0.881921264348355, +0.47139673682599764),
280 complex<T>(-0.773010453362737, +0.6343932841636455),
281 complex<T>(-0.6343932841636455, +0.7730104533627369),
282 complex<T>(-0.4713967368259978, +0.8819212643483549),
283 complex<T>(-0.29028467725446233, +0.9569403357322089),
284 complex<T>(-0.09801714032956077, +0.9951847266721968) },
285 1 };
286 case 17:
287 return { {},
288 { complex<T>(-0.09226835946330202, -0.9957341762950345),
289 complex<T>(-0.273662990072083, -0.961825643172819),
290 complex<T>(-0.4457383557765383, -0.8951632913550623),
291 complex<T>(-0.6026346363792565, -0.7980172272802395),
292 complex<T>(-0.7390089172206591, -0.6736956436465572),
293 complex<T>(-0.8502171357296142, -0.5264321628773557),
294 complex<T>(-0.9324722294043558, -0.3612416661871529),
295 complex<T>(-0.9829730996839018, -0.18374951781657034), complex<T>(-1., +0.),
296 complex<T>(-0.9829730996839018, +0.18374951781657034),
297 complex<T>(-0.9324722294043558, +0.3612416661871529),
298 complex<T>(-0.8502171357296142, +0.5264321628773557),
299 complex<T>(-0.7390089172206591, +0.6736956436465572),
300 complex<T>(-0.6026346363792565, +0.7980172272802395),
301 complex<T>(-0.4457383557765383, +0.8951632913550623),
302 complex<T>(-0.273662990072083, +0.961825643172819),
303 complex<T>(-0.09226835946330202, +0.9957341762950345) },
304 1 };
305 case 18:
306 return { {},
307 { complex<T>(-0.08715574274765814, -0.9961946980917455),
308 complex<T>(-0.25881904510252096, -0.9659258262890682),
309 complex<T>(-0.42261826174069944, -0.9063077870366499),
310 complex<T>(-0.5735764363510463, -0.8191520442889917),
311 complex<T>(-0.7071067811865476, -0.7071067811865476),
312 complex<T>(-0.8191520442889918, -0.573576436351046),
313 complex<T>(-0.9063077870366499, -0.4226182617406994),
314 complex<T>(-0.9659258262890683, -0.25881904510252074),
315 complex<T>(-0.9961946980917455, -0.08715574274765817),
316 complex<T>(-0.9961946980917455, +0.08715574274765817),
317 complex<T>(-0.9659258262890683, +0.25881904510252074),
318 complex<T>(-0.9063077870366499, +0.4226182617406994),
319 complex<T>(-0.8191520442889918, +0.573576436351046),
320 complex<T>(-0.7071067811865476, +0.7071067811865476),
321 complex<T>(-0.5735764363510463, +0.8191520442889917),
322 complex<T>(-0.42261826174069944, +0.9063077870366499),
323 complex<T>(-0.25881904510252096, +0.9659258262890682),
324 complex<T>(-0.08715574274765814, +0.9961946980917455) },
325 1 };
326 case 19:
327 return { {},
328 { complex<T>(-0.0825793454723324, -0.9965844930066698),
329 complex<T>(-0.24548548714079924, -0.9694002659393304),
330 complex<T>(-0.40169542465296953, -0.9157733266550574),
331 complex<T>(-0.5469481581224269, -0.8371664782625285),
332 complex<T>(-0.6772815716257411, -0.7357239106731316),
333 complex<T>(-0.7891405093963936, -0.6142127126896678),
334 complex<T>(-0.8794737512064891, -0.4759473930370735),
335 complex<T>(-0.9458172417006346, -0.32469946920468346),
336 complex<T>(-0.9863613034027223, -0.1645945902807339), complex<T>(-1., +0.),
337 complex<T>(-0.9863613034027223, +0.1645945902807339),
338 complex<T>(-0.9458172417006346, +0.32469946920468346),
339 complex<T>(-0.8794737512064891, +0.4759473930370735),
340 complex<T>(-0.7891405093963936, +0.6142127126896678),
341 complex<T>(-0.6772815716257411, +0.7357239106731316),
342 complex<T>(-0.5469481581224269, +0.8371664782625285),
343 complex<T>(-0.40169542465296953, +0.9157733266550574),
344 complex<T>(-0.24548548714079924, +0.9694002659393304),
345 complex<T>(-0.0825793454723324, +0.9965844930066698) },
346 1 };
347 case 20:
348 return { {},
349 { complex<T>(-0.078459095727845, -0.996917333733128),
350 complex<T>(-0.23344536385590525, -0.9723699203976767),
351 complex<T>(-0.38268343236508984, -0.9238795325112867),
352 complex<T>(-0.5224985647159487, -0.8526401643540923),
353 complex<T>(-0.6494480483301837, -0.7604059656000309),
354 complex<T>(-0.7604059656000309, -0.6494480483301837),
355 complex<T>(-0.8526401643540923, -0.5224985647159488),
356 complex<T>(-0.9238795325112867, -0.3826834323650898),
357 complex<T>(-0.9723699203976766, -0.2334453638559054),
358 complex<T>(-0.996917333733128, -0.07845909572784494),
359 complex<T>(-0.996917333733128, +0.07845909572784494),
360 complex<T>(-0.9723699203976766, +0.2334453638559054),
361 complex<T>(-0.9238795325112867, +0.3826834323650898),
362 complex<T>(-0.8526401643540923, +0.5224985647159488),
363 complex<T>(-0.7604059656000309, +0.6494480483301837),
364 complex<T>(-0.6494480483301837, +0.7604059656000309),
365 complex<T>(-0.5224985647159487, +0.8526401643540923),
366 complex<T>(-0.38268343236508984, +0.9238795325112867),
367 complex<T>(-0.23344536385590525, +0.9723699203976767),
368 complex<T>(-0.078459095727845, +0.996917333733128) },
369 1 };
370 case 21:
371 return { {},
372 { complex<T>(-0.07473009358642439, -0.9972037971811801),
373 complex<T>(-0.22252093395631445, -0.9749279121818236),
374 complex<T>(-0.3653410243663952, -0.9308737486442041),
375 complex<T>(-0.5000000000000001, -0.8660254037844386),
376 complex<T>(-0.6234898018587336, -0.7818314824680298),
377 complex<T>(-0.7330518718298263, -0.6801727377709194),
378 complex<T>(-0.8262387743159949, -0.563320058063622),
379 complex<T>(-0.9009688679024191, -0.4338837391175581),
380 complex<T>(-0.9555728057861408, -0.29475517441090415),
381 complex<T>(-0.9888308262251285, -0.14904226617617441),
382 complex<T>(-1., +0.),
383 complex<T>(-0.9888308262251285, +0.14904226617617441),
384 complex<T>(-0.9555728057861408, +0.29475517441090415),
385 complex<T>(-0.9009688679024191, +0.4338837391175581),
386 complex<T>(-0.8262387743159949, +0.563320058063622),
387 complex<T>(-0.7330518718298263, +0.6801727377709194),
388 complex<T>(-0.6234898018587336, +0.7818314824680298),
389 complex<T>(-0.5000000000000001, +0.8660254037844386),
390 complex<T>(-0.3653410243663952, +0.9308737486442041),
391 complex<T>(-0.22252093395631445, +0.9749279121818236),
392 complex<T>(-0.07473009358642439, +0.9972037971811801) },
393 1 };
394 case 22:
395 return { {},
396 { complex<T>(-0.07133918319923235, -0.9974521146102535),
397 complex<T>(-0.21256528955297682, -0.9771468659711595),
398 complex<T>(-0.3494641795990982, -0.9369497249997618),
399 complex<T>(-0.479248986720057, -0.8776789895672555),
400 complex<T>(-0.5992776665113468, -0.8005412409243605),
401 complex<T>(-0.7071067811865477, -0.7071067811865475),
402 complex<T>(-0.8005412409243604, -0.5992776665113468),
403 complex<T>(-0.8776789895672557, -0.4792489867200568),
404 complex<T>(-0.9369497249997617, -0.34946417959909837),
405 complex<T>(-0.9771468659711595, -0.21256528955297668),
406 complex<T>(-0.9974521146102535, -0.07133918319923234),
407 complex<T>(-0.9974521146102535, +0.07133918319923234),
408 complex<T>(-0.9771468659711595, +0.21256528955297668),
409 complex<T>(-0.9369497249997617, +0.34946417959909837),
410 complex<T>(-0.8776789895672557, +0.4792489867200568),
411 complex<T>(-0.8005412409243604, +0.5992776665113468),
412 complex<T>(-0.7071067811865477, +0.7071067811865475),
413 complex<T>(-0.5992776665113468, +0.8005412409243605),
414 complex<T>(-0.479248986720057, +0.8776789895672555),
415 complex<T>(-0.3494641795990982, +0.9369497249997618),
416 complex<T>(-0.21256528955297682, +0.9771468659711595),
417 complex<T>(-0.07133918319923235, +0.9974521146102535) },
418 1 };
419 case 23:
420 return { {},
421 { complex<T>(-0.06824241336467123, -0.9976687691905392),
422 complex<T>(-0.20345601305263397, -0.9790840876823228),
423 complex<T>(-0.3348796121709863, -0.9422609221188204),
424 complex<T>(-0.4600650377311522, -0.8878852184023752),
425 complex<T>(-0.5766803221148672, -0.816969893010442),
426 complex<T>(-0.6825531432186541, -0.730835964278124),
427 complex<T>(-0.7757112907044199, -0.6310879443260528),
428 complex<T>(-0.8544194045464886, -0.5195839500354336),
429 complex<T>(-0.917211301505453, -0.39840108984624145),
430 complex<T>(-0.9629172873477994, -0.2697967711570243),
431 complex<T>(-0.9906859460363308, -0.1361666490962466),
432 complex<T>(-1., +0.),
433 complex<T>(-0.9906859460363308, +0.1361666490962466),
434 complex<T>(-0.9629172873477994, +0.2697967711570243),
435 complex<T>(-0.917211301505453, +0.39840108984624145),
436 complex<T>(-0.8544194045464886, +0.5195839500354336),
437 complex<T>(-0.7757112907044199, +0.6310879443260528),
438 complex<T>(-0.6825531432186541, +0.730835964278124),
439 complex<T>(-0.5766803221148672, +0.816969893010442),
440 complex<T>(-0.4600650377311522, +0.8878852184023752),
441 complex<T>(-0.3348796121709863, +0.9422609221188204),
442 complex<T>(-0.20345601305263397, +0.9790840876823228),
443 complex<T>(-0.06824241336467123, +0.9976687691905392) },
444 1 };
445 case 24:
446 return { {},
447 { complex<T>(-0.06540312923014327, -0.9978589232386035),
448 complex<T>(-0.19509032201612833, -0.9807852804032304),
449 complex<T>(-0.3214394653031617, -0.9469301294951056),
450 complex<T>(-0.44228869021900125, -0.8968727415326884),
451 complex<T>(-0.5555702330196024, -0.8314696123025451),
452 complex<T>(-0.6593458151000688, -0.7518398074789774),
453 complex<T>(-0.7518398074789775, -0.6593458151000687),
454 complex<T>(-0.8314696123025452, -0.5555702330196022),
455 complex<T>(-0.8968727415326884, -0.44228869021900125),
456 complex<T>(-0.9469301294951057, -0.32143946530316153),
457 complex<T>(-0.9807852804032304, -0.19509032201612825),
458 complex<T>(-0.9978589232386035, -0.06540312923014306),
459 complex<T>(-0.9978589232386035, +0.06540312923014306),
460 complex<T>(-0.9807852804032304, +0.19509032201612825),
461 complex<T>(-0.9469301294951057, +0.32143946530316153),
462 complex<T>(-0.8968727415326884, +0.44228869021900125),
463 complex<T>(-0.8314696123025452, +0.5555702330196022),
464 complex<T>(-0.7518398074789775, +0.6593458151000687),
465 complex<T>(-0.6593458151000688, +0.7518398074789774),
466 complex<T>(-0.5555702330196024, +0.8314696123025451),
467 complex<T>(-0.44228869021900125, +0.8968727415326884),
468 complex<T>(-0.3214394653031617, +0.9469301294951056),
469 complex<T>(-0.19509032201612833, +0.9807852804032304),
470 complex<T>(-0.06540312923014327, +0.9978589232386035) },
471 1 };
472 default:
473 return { {}, {}, 1.0 };
474 }
475 }
476
477 template <typename T>
bessel(int N)478 KFR_FUNCTION zpk<T> bessel(int N)
479 {
480 switch (N)
481 {
482 case 0:
483 return { {}, {}, 1.0 };
484 case 1:
485 return { {}, { complex<T>(-1., +0.) }, 1.0 };
486 case 2:
487 return { {},
488 { complex<T>(-0.8660254037844385, -0.4999999999999999),
489 complex<T>(-0.8660254037844385, +0.4999999999999999) },
490 1.0 };
491 case 3:
492 return { {},
493 { complex<T>(-0.7456403858480766, -0.7113666249728353), complex<T>(-0.9416000265332067, +0.),
494 complex<T>(-0.7456403858480766, +0.7113666249728353) },
495 1.0 };
496 case 4:
497 return { {},
498 { complex<T>(-0.6572111716718827, -0.830161435004873),
499 complex<T>(-0.904758796788245, -0.27091873300387465),
500 complex<T>(-0.904758796788245, +0.27091873300387465),
501 complex<T>(-0.6572111716718827, +0.830161435004873) },
502 1.0 };
503 case 5:
504 return { {},
505 { complex<T>(-0.5905759446119192, -0.9072067564574549),
506 complex<T>(-0.8515536193688396, -0.44271746394433276), complex<T>(-0.92644207738776, +0.),
507 complex<T>(-0.8515536193688396, +0.44271746394433276),
508 complex<T>(-0.5905759446119192, +0.9072067564574549) },
509 1.0 };
510 case 6:
511 return { {},
512 { complex<T>(-0.5385526816693108, -0.9616876881954276),
513 complex<T>(-0.7996541858328287, -0.5621717346937316),
514 complex<T>(-0.9093906830472273, -0.1856964396793047),
515 complex<T>(-0.9093906830472273, +0.1856964396793047),
516 complex<T>(-0.7996541858328287, +0.5621717346937316),
517 complex<T>(-0.5385526816693108, +0.9616876881954276) },
518 1.0 };
519 case 7:
520 return { {},
521 { complex<T>(-0.4966917256672317, -1.0025085084544205),
522 complex<T>(-0.7527355434093214, -0.6504696305522553),
523 complex<T>(-0.8800029341523379, -0.32166527623077407), complex<T>(-0.919487155649029, +0.),
524 complex<T>(-0.8800029341523379, +0.32166527623077407),
525 complex<T>(-0.7527355434093214, +0.6504696305522553),
526 complex<T>(-0.4966917256672317, +1.0025085084544205) },
527 1.0 };
528 case 8:
529 return { {},
530 { complex<T>(-0.46217404125321254, -1.0343886811269012),
531 complex<T>(-0.7111381808485397, -0.71865173141084),
532 complex<T>(-0.8473250802359334, -0.42590175382729345),
533 complex<T>(-0.9096831546652911, -0.1412437976671423),
534 complex<T>(-0.9096831546652911, +0.1412437976671423),
535 complex<T>(-0.8473250802359334, +0.42590175382729345),
536 complex<T>(-0.7111381808485397, +0.71865173141084),
537 complex<T>(-0.46217404125321254, +1.0343886811269012) },
538 1.0 };
539 case 9:
540 return { {},
541 { complex<T>(-0.43314155615536226, -1.0600736701359301),
542 complex<T>(-0.6743622686854763, -0.7730546212691185),
543 complex<T>(-0.8148021112269014, -0.50858156896315),
544 complex<T>(-0.8911217017079759, -0.25265809345821644),
545 complex<T>(-0.9154957797499037, +0.),
546 complex<T>(-0.8911217017079759, +0.25265809345821644),
547 complex<T>(-0.8148021112269014, +0.50858156896315),
548 complex<T>(-0.6743622686854763, +0.7730546212691185),
549 complex<T>(-0.43314155615536226, +1.0600736701359301) },
550 1.0 };
551 case 10:
552 return { {},
553 { complex<T>(-0.4083220732868863, -1.0812748428191246),
554 complex<T>(-0.6417513866988322, -0.8175836167191022),
555 complex<T>(-0.7837694413101445, -0.5759147538499948),
556 complex<T>(-0.8688459641284763, -0.34300082337663096),
557 complex<T>(-0.9091347320900505, -0.11395831373355114),
558 complex<T>(-0.9091347320900505, +0.11395831373355114),
559 complex<T>(-0.8688459641284763, +0.34300082337663096),
560 complex<T>(-0.7837694413101445, +0.5759147538499948),
561 complex<T>(-0.6417513866988322, +0.8175836167191022),
562 complex<T>(-0.4083220732868863, +1.0812748428191246) },
563 1.0 };
564 case 11:
565 return { {},
566 { complex<T>(-0.3868149510055096, -1.0991174667631216),
567 complex<T>(-0.6126871554915198, -0.8547813893314768),
568 complex<T>(-0.7546938934722305, -0.6319150050721849),
569 complex<T>(-0.8453044014712964, -0.4178696917801249),
570 complex<T>(-0.8963656705721169, -0.20804803750710327),
571 complex<T>(-0.9129067244518985, +0.),
572 complex<T>(-0.8963656705721169, +0.20804803750710327),
573 complex<T>(-0.8453044014712964, +0.4178696917801249),
574 complex<T>(-0.7546938934722305, +0.6319150050721849),
575 complex<T>(-0.6126871554915198, +0.8547813893314768),
576 complex<T>(-0.3868149510055096, +1.0991174667631216) },
577 1.0 };
578 case 12:
579 return { {},
580 { complex<T>(-0.3679640085526314, -1.1143735756415463),
581 complex<T>(-0.5866369321861475, -0.8863772751320727),
582 complex<T>(-0.7276681615395161, -0.6792961178764695),
583 complex<T>(-0.8217296939939074, -0.48102121151006755),
584 complex<T>(-0.880253434201683, -0.2871779503524227),
585 complex<T>(-0.9084478234140686, -0.09550636521345042),
586 complex<T>(-0.9084478234140686, +0.09550636521345042),
587 complex<T>(-0.880253434201683, +0.2871779503524227),
588 complex<T>(-0.8217296939939074, +0.48102121151006755),
589 complex<T>(-0.7276681615395161, +0.6792961178764695),
590 complex<T>(-0.5866369321861475, +0.8863772751320727),
591 complex<T>(-0.3679640085526314, +1.1143735756415463) },
592 1.0 };
593 case 13:
594 return { {},
595 { complex<T>(-0.35127923233898156, -1.1275915483177048),
596 complex<T>(-0.5631559842430193, -0.9135900338325103),
597 complex<T>(-0.7026234675721276, -0.7199611890171304),
598 complex<T>(-0.7987460692470972, -0.5350752120696802),
599 complex<T>(-0.8625094198260553, -0.35474137311729914),
600 complex<T>(-0.8991314665475196, -0.17683429561610436),
601 complex<T>(-0.9110914665984182, +0.),
602 complex<T>(-0.8991314665475196, +0.17683429561610436),
603 complex<T>(-0.8625094198260553, +0.35474137311729914),
604 complex<T>(-0.7987460692470972, +0.5350752120696802),
605 complex<T>(-0.7026234675721276, +0.7199611890171304),
606 complex<T>(-0.5631559842430193, +0.9135900338325103),
607 complex<T>(-0.35127923233898156, +1.1275915483177048) },
608 1.0 };
609 case 14:
610 return { {},
611 { complex<T>(-0.3363868224902031, -1.1391722978398593),
612 complex<T>(-0.5418766775112306, -0.9373043683516926),
613 complex<T>(-0.6794256425119227, -0.7552857305042031),
614 complex<T>(-0.7766591387063624, -0.581917067737761),
615 complex<T>(-0.8441199160909851, -0.41316538251026935),
616 complex<T>(-0.8869506674916446, -0.24700791787653334),
617 complex<T>(-0.9077932138396491, -0.08219639941940154),
618 complex<T>(-0.9077932138396491, +0.08219639941940154),
619 complex<T>(-0.8869506674916446, +0.24700791787653334),
620 complex<T>(-0.8441199160909851, +0.41316538251026935),
621 complex<T>(-0.7766591387063624, +0.581917067737761),
622 complex<T>(-0.6794256425119227, +0.7552857305042031),
623 complex<T>(-0.5418766775112306, +0.9373043683516926),
624 complex<T>(-0.3363868224902031, +1.1391722978398593) },
625 1.0 };
626 case 15:
627 return { {},
628 { complex<T>(-0.3229963059766445, -1.14941615458363),
629 complex<T>(-0.5224954069658334, -0.9581787261092526),
630 complex<T>(-0.6579196593111002, -0.7862895503722519),
631 complex<T>(-0.7556027168970723, -0.6229396358758262),
632 complex<T>(-0.8256631452587148, -0.46423487527343266),
633 complex<T>(-0.8731264620834984, -0.30823524705642674),
634 complex<T>(-0.9006981694176978, -0.1537681197278439), complex<T>(-0.9097482363849062, +0.),
635 complex<T>(-0.9006981694176978, +0.1537681197278439),
636 complex<T>(-0.8731264620834984, +0.30823524705642674),
637 complex<T>(-0.8256631452587148, +0.46423487527343266),
638 complex<T>(-0.7556027168970723, +0.6229396358758262),
639 complex<T>(-0.6579196593111002, +0.7862895503722519),
640 complex<T>(-0.5224954069658334, +0.9581787261092526),
641 complex<T>(-0.3229963059766445, +1.14941615458363) },
642 1.0 };
643 case 16:
644 return { {},
645 { complex<T>(-0.31087827556453856, -1.1585528411993304),
646 complex<T>(-0.504760644442476, -0.9767137477799086),
647 complex<T>(-0.6379502514039066, -0.8137453537108762),
648 complex<T>(-0.7356166304713119, -0.6591950877860395),
649 complex<T>(-0.8074790293236005, -0.5092933751171799),
650 complex<T>(-0.8584264231521322, -0.3621697271802063),
651 complex<T>(-0.8911723070323642, -0.2167089659900575),
652 complex<T>(-0.9072099595087003, -0.07214211304111734),
653 complex<T>(-0.9072099595087003, +0.07214211304111734),
654 complex<T>(-0.8911723070323642, +0.2167089659900575),
655 complex<T>(-0.8584264231521322, +0.3621697271802063),
656 complex<T>(-0.8074790293236005, +0.5092933751171799),
657 complex<T>(-0.7356166304713119, +0.6591950877860395),
658 complex<T>(-0.6379502514039066, +0.8137453537108762),
659 complex<T>(-0.504760644442476, +0.9767137477799086),
660 complex<T>(-0.31087827556453856, +1.1585528411993304) },
661 1.0 };
662 case 17:
663 return { {},
664 { complex<T>(-0.29984894599900724, -1.1667612729256673),
665 complex<T>(-0.48846293376727057, -0.9932971956316782),
666 complex<T>(-0.6193710717342136, -0.8382497252826987),
667 complex<T>(-0.7166893842372348, -0.6914936286393606),
668 complex<T>(-0.7897644147799701, -0.5493724405281085),
669 complex<T>(-0.8433414495836128, -0.41007592829100215),
670 complex<T>(-0.8801100704438625, -0.2725347156478803),
671 complex<T>(-0.9016273850787279, -0.13602679951730237),
672 complex<T>(-0.9087141161336388, +0.),
673 complex<T>(-0.9016273850787279, +0.13602679951730237),
674 complex<T>(-0.8801100704438625, +0.2725347156478803),
675 complex<T>(-0.8433414495836128, +0.41007592829100215),
676 complex<T>(-0.7897644147799701, +0.5493724405281085),
677 complex<T>(-0.7166893842372348, +0.6914936286393606),
678 complex<T>(-0.6193710717342136, +0.8382497252826987),
679 complex<T>(-0.48846293376727057, +0.9932971956316782),
680 complex<T>(-0.29984894599900724, +1.1667612729256673) },
681 1.0 };
682 case 18:
683 return { {},
684 { complex<T>(-0.28975920298804847, -1.1741830106000584),
685 complex<T>(-0.4734268069916154, -1.0082343003148009),
686 complex<T>(-0.6020482668090646, -0.8602708961893666),
687 complex<T>(-0.698782144500527, -0.7204696509726628),
688 complex<T>(-0.7726285030739557, -0.5852778162086639),
689 complex<T>(-0.8281885016242831, -0.45293856978159136),
690 complex<T>(-0.8681095503628832, -0.32242049251632576),
691 complex<T>(-0.8939764278132456, -0.19303746408947586),
692 complex<T>(-0.9067004324162776, -0.06427924106393067),
693 complex<T>(-0.9067004324162776, +0.06427924106393067),
694 complex<T>(-0.8939764278132456, +0.19303746408947586),
695 complex<T>(-0.8681095503628832, +0.32242049251632576),
696 complex<T>(-0.8281885016242831, +0.45293856978159136),
697 complex<T>(-0.7726285030739557, +0.5852778162086639),
698 complex<T>(-0.698782144500527, +0.7204696509726628),
699 complex<T>(-0.6020482668090646, +0.8602708961893666),
700 complex<T>(-0.4734268069916154, +1.0082343003148009),
701 complex<T>(-0.28975920298804847, +1.1741830106000584) },
702 1.0 };
703 case 19:
704 return { {},
705 { complex<T>(-0.2804866851439361, -1.1809316284532905),
706 complex<T>(-0.4595043449730983, -1.0217687769126707),
707 complex<T>(-0.5858613321217832, -0.8801817131014564),
708 complex<T>(-0.6818424412912442, -0.7466272357947761),
709 complex<T>(-0.7561260971541627, -0.6176483917970176),
710 complex<T>(-0.8131725551578203, -0.491536503556246),
711 complex<T>(-0.8555768765618422, -0.3672925896399872),
712 complex<T>(-0.8849290585034385, -0.24425907575498182),
713 complex<T>(-0.9021937639390656, -0.12195683818720263),
714 complex<T>(-0.9078934217899399, +0.),
715 complex<T>(-0.9021937639390656, +0.12195683818720263),
716 complex<T>(-0.8849290585034385, +0.24425907575498182),
717 complex<T>(-0.8555768765618422, +0.3672925896399872),
718 complex<T>(-0.8131725551578203, +0.491536503556246),
719 complex<T>(-0.7561260971541627, +0.6176483917970176),
720 complex<T>(-0.6818424412912442, +0.7466272357947761),
721 complex<T>(-0.5858613321217832, +0.8801817131014564),
722 complex<T>(-0.4595043449730983, +1.0217687769126707),
723 complex<T>(-0.2804866851439361, +1.1809316284532905) },
724 1.0 };
725 case 20:
726 return { {},
727 { complex<T>(-0.27192995802516506, -1.187099379810886),
728 complex<T>(-0.44657006982051484, -1.0340977025608422),
729 complex<T>(-0.5707026806915716, -0.8982829066468254),
730 complex<T>(-0.6658120544829932, -0.7703721701100759),
731 complex<T>(-0.7402780309646764, -0.6469975237605227),
732 complex<T>(-0.7984251191290602, -0.526494238881713),
733 complex<T>(-0.8427907479956664, -0.4078917326291931),
734 complex<T>(-0.8749560316673335, -0.2905559296567909),
735 complex<T>(-0.8959150941925766, -0.17403171759187044),
736 complex<T>(-0.9062570115576768, -0.0579617802778495),
737 complex<T>(-0.9062570115576768, +0.0579617802778495),
738 complex<T>(-0.8959150941925766, +0.17403171759187044),
739 complex<T>(-0.8749560316673335, +0.2905559296567909),
740 complex<T>(-0.8427907479956664, +0.4078917326291931),
741 complex<T>(-0.7984251191290602, +0.526494238881713),
742 complex<T>(-0.7402780309646764, +0.6469975237605227),
743 complex<T>(-0.6658120544829932, +0.7703721701100759),
744 complex<T>(-0.5707026806915716, +0.8982829066468254),
745 complex<T>(-0.44657006982051484, +1.0340977025608422),
746 complex<T>(-0.27192995802516506, +1.187099379810886) },
747 1.0 };
748 case 21:
749 return { {},
750 { complex<T>(-0.2640041595834027, -1.192762031948052),
751 complex<T>(-0.4345168906815268, -1.045382255856986),
752 complex<T>(-0.5564766488918566, -0.9148198405846728),
753 complex<T>(-0.6506315378609466, -0.7920349342629495),
754 complex<T>(-0.7250839687106612, -0.6737426063024383),
755 complex<T>(-0.7840287980408347, -0.5583186348022857),
756 complex<T>(-0.8299435470674444, -0.44481777394079575),
757 complex<T>(-0.8643915813643203, -0.33262585125221866),
758 complex<T>(-0.8883808106664449, -0.221306921508435),
759 complex<T>(-0.9025428073192694, -0.11052525727898564),
760 complex<T>(-0.9072262653142963, +0.),
761 complex<T>(-0.9025428073192694, +0.11052525727898564),
762 complex<T>(-0.8883808106664449, +0.221306921508435),
763 complex<T>(-0.8643915813643203, +0.33262585125221866),
764 complex<T>(-0.8299435470674444, +0.44481777394079575),
765 complex<T>(-0.7840287980408347, +0.5583186348022857),
766 complex<T>(-0.7250839687106612, +0.6737426063024383),
767 complex<T>(-0.6506315378609466, +0.7920349342629495),
768 complex<T>(-0.5564766488918566, +0.9148198405846728),
769 complex<T>(-0.4345168906815268, +1.045382255856986),
770 complex<T>(-0.2640041595834027, +1.192762031948052) },
771 1.0 };
772 case 22:
773 return { {},
774 { complex<T>(-0.2566376987939318, -1.1979824335552132),
775 complex<T>(-0.4232528745642629, -1.055755605227546),
776 complex<T>(-0.5430983056306306, -0.9299947824439877),
777 complex<T>(-0.6362427683267828, -0.811887504024635),
778 complex<T>(-0.7105305456418792, -0.6982266265924527),
779 complex<T>(-0.7700332930556816, -0.5874255426351151),
780 complex<T>(-0.8171682088462721, -0.47856194922027806),
781 complex<T>(-0.8534754036851689, -0.37103893194823206),
782 complex<T>(-0.8799661455640174, -0.26443630392015344),
783 complex<T>(-0.8972983138153532, -0.15843519122898653),
784 complex<T>(-0.9058702269930871, -0.05277490828999903),
785 complex<T>(-0.9058702269930871, +0.05277490828999903),
786 complex<T>(-0.8972983138153532, +0.15843519122898653),
787 complex<T>(-0.8799661455640174, +0.26443630392015344),
788 complex<T>(-0.8534754036851689, +0.37103893194823206),
789 complex<T>(-0.8171682088462721, +0.47856194922027806),
790 complex<T>(-0.7700332930556816, +0.5874255426351151),
791 complex<T>(-0.7105305456418792, +0.6982266265924527),
792 complex<T>(-0.6362427683267828, +0.811887504024635),
793 complex<T>(-0.5430983056306306, +0.9299947824439877),
794 complex<T>(-0.4232528745642629, +1.055755605227546),
795 complex<T>(-0.2566376987939318, +1.1979824335552132) },
796 1.0 };
797 case 23:
798 return { {},
799 { complex<T>(-0.24976972022089572, -1.2028131878706978),
800 complex<T>(-0.4126986617510148, -1.0653287944755134),
801 complex<T>(-0.5304922463810198, -0.9439760364018306),
802 complex<T>(-0.6225903228771341, -0.830155830281298),
803 complex<T>(-0.6965966033912708, -0.720734137475305),
804 complex<T>(-0.7564660146829886, -0.6141594859476034),
805 complex<T>(-0.8045561642053178, -0.5095305912227259),
806 complex<T>(-0.8423805948021129, -0.4062657948237603),
807 complex<T>(-0.8709469395587415, -0.3039581993950041),
808 complex<T>(-0.8909283242471254, -0.20230246993812237),
809 complex<T>(-0.9027564979912508, -0.10105343353140452),
810 complex<T>(-0.9066732476324991, +0.),
811 complex<T>(-0.9027564979912508, +0.10105343353140452),
812 complex<T>(-0.8909283242471254, +0.20230246993812237),
813 complex<T>(-0.8709469395587415, +0.3039581993950041),
814 complex<T>(-0.8423805948021129, +0.4062657948237603),
815 complex<T>(-0.8045561642053178, +0.5095305912227259),
816 complex<T>(-0.7564660146829886, +0.6141594859476034),
817 complex<T>(-0.6965966033912708, +0.720734137475305),
818 complex<T>(-0.6225903228771341, +0.830155830281298),
819 complex<T>(-0.5304922463810198, +0.9439760364018306),
820 complex<T>(-0.4126986617510148, +1.0653287944755134),
821 complex<T>(-0.24976972022089572, +1.2028131878706978) },
822 1.0 };
823 case 24:
824 return { {},
825 { complex<T>(-0.24334813375248746, -1.2072986837319728),
826 complex<T>(-0.4027853855197519, -1.0741951965186751),
827 complex<T>(-0.518591457482032, -0.9569048385259057),
828 complex<T>(-0.6096221567378332, -0.8470292433077199),
829 complex<T>(-0.6832565803536519, -0.7415032695091649),
830 complex<T>(-0.7433392285088533, -0.6388084216222569),
831 complex<T>(-0.7921695462343489, -0.5380628490968016),
832 complex<T>(-0.8312326466813242, -0.4386985933597306),
833 complex<T>(-0.8615278304016355, -0.34032021126186246),
834 complex<T>(-0.8837358034555707, -0.24263352344013836),
835 complex<T>(-0.8983105104397872, -0.14540561338736102),
836 complex<T>(-0.9055312363372773, -0.0484400665404787),
837 complex<T>(-0.9055312363372773, +0.0484400665404787),
838 complex<T>(-0.8983105104397872, +0.14540561338736102),
839 complex<T>(-0.8837358034555707, +0.24263352344013836),
840 complex<T>(-0.8615278304016355, +0.34032021126186246),
841 complex<T>(-0.8312326466813242, +0.4386985933597306),
842 complex<T>(-0.7921695462343489, +0.5380628490968016),
843 complex<T>(-0.7433392285088533, +0.6388084216222569),
844 complex<T>(-0.6832565803536519, +0.7415032695091649),
845 complex<T>(-0.6096221567378332, +0.8470292433077199),
846 complex<T>(-0.518591457482032, +0.9569048385259057),
847 complex<T>(-0.4027853855197519, +1.0741951965186751),
848 complex<T>(-0.24334813375248746, +1.2072986837319728) },
849 1.0 };
850 default:
851 return { {}, {}, 1.f };
852 }
853 }
854
855 // template <typename T>
856 // KFR_FUNCTION zpk<T> iirfilter(const zpk<T>& filter)
857 // {
858 // }
859
860 namespace internal
861 {
862
863 template <typename T>
bilinear(const zpk<T> & filter,identity<T> fs)864 KFR_FUNCTION zpk<T> bilinear(const zpk<T>& filter, identity<T> fs)
865 {
866 const T fs2 = 2.0 * fs;
867 zpk<T> result;
868 result.z = (fs2 + filter.z) / (fs2 - filter.z);
869 result.p = (fs2 + filter.p) / (fs2 - filter.p);
870 result.z.resize(result.p.size(), complex<T>(-1));
871 result.k = filter.k * kfr::real(product(fs2 - filter.z) / product(fs2 - filter.p));
872 return result;
873 }
874
875 template <typename T>
876 struct zero_pole_pairs
877 {
878 complex<T> p1, p2, z1, z2;
879 };
880
881 template <typename T>
zpk2tf_poly(const complex<T> & x,const complex<T> & y)882 KFR_FUNCTION vec<T, 3> zpk2tf_poly(const complex<T>& x, const complex<T>& y)
883 {
884 return { T(1), -(x.real() + y.real()), x.real() * y.real() - x.imag() * y.imag() };
885 }
886
887 template <typename T>
zpk2tf(const zero_pole_pairs<T> & pairs,identity<T> k)888 KFR_FUNCTION biquad_params<T> zpk2tf(const zero_pole_pairs<T>& pairs, identity<T> k)
889 {
890 vec<T, 3> zz = k * zpk2tf_poly(pairs.z1, pairs.z2);
891 vec<T, 3> pp = zpk2tf_poly(pairs.p1, pairs.p2);
892 // return { zz[0], zz[1], zz[2], pp[0], pp[1], pp[2] };
893 return { pp[0], pp[1], pp[2], zz[0], zz[1], zz[2] };
894 }
895
896 template <typename T>
cplxreal(const univector<complex<T>> & list)897 KFR_FUNCTION univector<complex<T>> cplxreal(const univector<complex<T>>& list)
898 {
899 univector<complex<T>> x = list;
900 std::sort(x.begin(), x.end(), [](const complex<T>& a, const complex<T>& b) { return a.real() < b.real(); });
901 T tol = std::numeric_limits<T>::epsilon() * 100;
902 univector<complex<T>> result = x;
903 for (size_t i = result.size(); i > 1; i--)
904 {
905 if (!isreal(result[i - 1]) && !isreal(result[i - 2]))
906 {
907 if (abs(result[i - 1].real() - result[i - 2].real()) < tol &&
908 abs(result[i - 1].imag() + result[i - 2].imag()) < tol)
909 {
910 result.erase(result.begin() + i - 1);
911 result[i - 2].imag(abs(result[i - 2].imag()));
912 }
913 }
914 }
915 return result;
916 }
917
918 template <typename T>
nearest_real_or_complex(const univector<complex<T>> & list,const complex<T> & val,bool mustbereal=true)919 KFR_FUNCTION size_t nearest_real_or_complex(const univector<complex<T>>& list, const complex<T>& val,
920 bool mustbereal = true)
921 {
922 univector<complex<T>> filtered;
923 for (complex<T> v : list)
924 {
925 if (isreal(v) == mustbereal)
926 {
927 filtered.push_back(v);
928 }
929 }
930 TESTO_ASSERT(!filtered.empty());
931 if (filtered.empty())
932 return std::numeric_limits<size_t>::max();
933
934 size_t minidx = 0;
935 T minval = cabs(val - filtered[0]);
936 for (size_t i = 1; i < list.size(); i++)
937 {
938 T newminval = cabs(val - filtered[i]);
939 if (newminval < minval)
940 {
941 minval = newminval;
942 minidx = i;
943 }
944 }
945 return minidx;
946 }
947
948 template <typename T>
countreal(const univector<complex<T>> & list)949 KFR_FUNCTION int countreal(const univector<complex<T>>& list)
950 {
951 int nreal = 0;
952 for (complex<T> c : list)
953 {
954 if (c.imag() == 0)
955 nreal++;
956 }
957 return nreal;
958 }
959
960 template <typename T>
lp2lp_zpk(const zpk<T> & filter,identity<T> wo)961 KFR_FUNCTION zpk<T> lp2lp_zpk(const zpk<T>& filter, identity<T> wo)
962 {
963 zpk<T> result;
964 result.z = wo * filter.z;
965 result.p = wo * filter.p;
966 result.k = filter.k * pow(wo, filter.p.size() - filter.z.size());
967 return result;
968 }
969
970 template <typename T>
lp2hp_zpk(const zpk<T> & filter,identity<T> wo)971 KFR_FUNCTION zpk<T> lp2hp_zpk(const zpk<T>& filter, identity<T> wo)
972 {
973 zpk<T> result;
974 result.z = wo / filter.z;
975 result.p = wo / filter.p;
976 result.z.resize(result.p.size(), T(0));
977 result.k = filter.k * kfr::real(product(-filter.z) / product(-filter.p));
978 return result;
979 }
980
981 template <typename T>
lp2bp_zpk(const zpk<T> & filter,identity<T> wo,identity<T> bw)982 KFR_FUNCTION zpk<T> lp2bp_zpk(const zpk<T>& filter, identity<T> wo, identity<T> bw)
983 {
984 zpk<T> lowpass;
985 lowpass.z = bw * 0.5 * filter.z;
986 lowpass.p = bw * 0.5 * filter.p;
987
988 zpk<T> result;
989 result.z = concatenate(lowpass.z + csqrt(csqr(lowpass.z) - wo * wo),
990 lowpass.z - csqrt(csqr(lowpass.z) - wo * wo));
991 result.p = concatenate(lowpass.p + csqrt(csqr(lowpass.p) - wo * wo),
992 lowpass.p - csqrt(csqr(lowpass.p) - wo * wo));
993
994 result.z.resize(result.z.size() + filter.p.size() - filter.z.size(), T(0));
995 result.k = filter.k * pow(bw, filter.p.size() - filter.z.size());
996
997 return result;
998 }
999
1000 template <typename T>
lp2bs_zpk(const zpk<T> & filter,identity<T> wo,identity<T> bw)1001 KFR_FUNCTION zpk<T> lp2bs_zpk(const zpk<T>& filter, identity<T> wo, identity<T> bw)
1002 {
1003 zpk<T> highpass;
1004 highpass.z = (bw * 0.5) / filter.z;
1005 highpass.p = (bw * 0.5) / filter.p;
1006
1007 zpk<T> result;
1008 result.z = concatenate(highpass.z + csqrt(csqr(highpass.z) - wo * wo),
1009 highpass.z - csqrt(csqr(highpass.z) - wo * wo));
1010 result.p = concatenate(highpass.p + csqrt(csqr(highpass.p) - wo * wo),
1011 highpass.p - csqrt(csqr(highpass.p) - wo * wo));
1012
1013 result.z.resize(result.z.size() + filter.p.size() - filter.z.size(), complex<T>(0, +wo));
1014 result.z.resize(result.z.size() + filter.p.size() - filter.z.size(), complex<T>(0, -wo));
1015 result.k = filter.k * kfr::real(product(-filter.z) / product(-filter.p));
1016
1017 return result;
1018 }
1019
1020 template <typename T>
warp_freq(T frequency,T fs)1021 KFR_FUNCTION T warp_freq(T frequency, T fs)
1022 {
1023 frequency = 2 * frequency / fs;
1024 fs = 2.0;
1025 T warped = 2 * fs * tan(c_pi<T> * frequency / fs);
1026 return warped;
1027 }
1028
1029 } // namespace internal
1030
1031 template <typename T>
iir_lowpass(const zpk<T> & filter,identity<T> frequency,identity<T> fs=2.0)1032 KFR_FUNCTION zpk<T> iir_lowpass(const zpk<T>& filter, identity<T> frequency, identity<T> fs = 2.0)
1033 {
1034 T warped = internal::warp_freq(frequency, fs);
1035
1036 zpk<T> result = filter;
1037 result = internal::lp2lp_zpk(result, warped);
1038 result = internal::bilinear(result, 2.0); // fs = 2.0
1039 return result;
1040 }
1041
1042 template <typename T>
iir_highpass(const zpk<T> & filter,identity<T> frequency,identity<T> fs=2.0)1043 KFR_FUNCTION zpk<T> iir_highpass(const zpk<T>& filter, identity<T> frequency, identity<T> fs = 2.0)
1044 {
1045 T warped = internal::warp_freq(frequency, fs);
1046
1047 zpk<T> result = filter;
1048 result = internal::lp2hp_zpk(result, warped);
1049 result = internal::bilinear(result, 2.0); // fs = 2.0
1050 return result;
1051 }
1052
1053 template <typename T>
iir_bandpass(const zpk<T> & filter,identity<T> lowfreq,identity<T> highfreq,identity<T> fs=2.0)1054 KFR_FUNCTION zpk<T> iir_bandpass(const zpk<T>& filter, identity<T> lowfreq, identity<T> highfreq,
1055 identity<T> fs = 2.0)
1056 {
1057 T warpedlow = internal::warp_freq(lowfreq, fs);
1058 T warpedhigh = internal::warp_freq(highfreq, fs);
1059
1060 zpk<T> result = filter;
1061 result = internal::lp2bp_zpk(result, std::sqrt(warpedlow * warpedhigh), warpedhigh - warpedlow);
1062 result = internal::bilinear(result, 2.0); // fs = 2.0
1063 return result;
1064 }
1065
1066 template <typename T>
iir_bandstop(const zpk<T> & filter,identity<T> lowfreq,identity<T> highfreq,identity<T> fs=2.0)1067 KFR_FUNCTION zpk<T> iir_bandstop(const zpk<T>& filter, identity<T> lowfreq, identity<T> highfreq,
1068 identity<T> fs = 2.0)
1069 {
1070 T warpedlow = internal::warp_freq(lowfreq, fs);
1071 T warpedhigh = internal::warp_freq(highfreq, fs);
1072
1073 zpk<T> result = filter;
1074 result = internal::lp2bs_zpk(result, std::sqrt(warpedlow * warpedhigh), warpedhigh - warpedlow);
1075 result = internal::bilinear(result, 2.0); // fs = 2.0
1076 return result;
1077 }
1078
1079 template <typename T>
to_sos(const zpk<T> & filter)1080 KFR_FUNCTION std::vector<biquad_params<T>> to_sos(const zpk<T>& filter)
1081 {
1082 if (filter.p.empty() && filter.z.empty())
1083 return { biquad_params<T>(filter.k, 0., 0., 1., 0., 0) };
1084
1085 zpk<T> filt = filter;
1086 size_t length = std::max(filter.p.size(), filter.z.size());
1087 filt.p.resize(length, complex<T>(0));
1088 filt.z.resize(length, complex<T>(0));
1089
1090 size_t n_sections = (length + 1) / 2;
1091 if (length & 1)
1092 {
1093 filt.z.push_back(complex<T>(0));
1094 filt.p.push_back(complex<T>(0));
1095 }
1096
1097 filt.z = internal::cplxreal(filt.z);
1098 filt.p = internal::cplxreal(filt.p);
1099 std::vector<internal::zero_pole_pairs<T>> pairs(n_sections);
1100
1101 for (size_t si = 0; si < n_sections; si++)
1102 {
1103 size_t worstidx = 0;
1104 T worstval = abs(1 - cabs(filt.p[0]));
1105 for (size_t i = 1; i < filt.p.size(); i++)
1106 {
1107 T val = abs(1 - cabs(filt.p[i]));
1108 if (val < worstval)
1109 {
1110 worstidx = i;
1111 worstval = val;
1112 }
1113 }
1114 complex<T> p1 = filt.p[worstidx];
1115 filt.p.erase(filt.p.begin() + worstidx);
1116
1117 complex<T> z1, p2, z2;
1118 if (isreal(p1) && internal::countreal(filt.p) == 0)
1119 {
1120 size_t z1_idx = internal::nearest_real_or_complex(filt.z, p1, true);
1121 z1 = filt.z[z1_idx];
1122 filt.z.erase(filt.z.begin() + z1_idx);
1123 p2 = z2 = 0;
1124 }
1125 else
1126 {
1127 size_t z1_idx;
1128 if (!isreal(p1) && internal::countreal(filt.z) == 1)
1129 {
1130 z1_idx = internal::nearest_real_or_complex(filt.z, p1, false);
1131 }
1132 else
1133 {
1134 size_t minidx = 0;
1135 T minval = cabs(p1 - filt.z[0]);
1136 for (size_t i = 1; i < filt.z.size(); i++)
1137 {
1138 T newminval = cabs(p1 - filt.z[i]);
1139 if (newminval < minval)
1140 {
1141 minidx = i;
1142 minval = newminval;
1143 }
1144 }
1145 z1_idx = minidx;
1146 }
1147 z1 = filt.z[z1_idx];
1148 filt.z.erase(filt.z.begin() + z1_idx);
1149 if (!isreal(p1))
1150 {
1151 if (!isreal(z1))
1152 {
1153 p2 = cconj(p1);
1154 z2 = cconj(z1);
1155 }
1156 else
1157 {
1158 p2 = cconj(p1);
1159 size_t z2_idx = internal::nearest_real_or_complex(filt.z, p1, true);
1160 z2 = filt.z[z2_idx];
1161 TESTO_ASSERT(isreal(z2));
1162 filt.z.erase(filt.z.begin() + z2_idx);
1163 }
1164 }
1165 else
1166 {
1167 size_t p2_idx;
1168 size_t z2_idx;
1169 if (!isreal(z1))
1170 {
1171 z2 = cconj(z1);
1172 p2_idx = internal::nearest_real_or_complex(filt.z, p1, true);
1173 p2 = filt.p[p2_idx];
1174 TESTO_ASSERT(isreal(p2));
1175 }
1176 else
1177 {
1178 size_t worstidx = 0;
1179 T worstval = abs(cabs(filt.p[0]) - 1);
1180 for (size_t i = 1; i < filt.p.size(); i++)
1181 {
1182 T val = abs(cabs(filt.p[i]) - 1);
1183 if (val < worstval)
1184 {
1185 worstidx = i;
1186 worstval = val;
1187 }
1188 }
1189 p2_idx = worstidx;
1190 p2 = filt.p[p2_idx];
1191
1192 TESTO_ASSERT(isreal(p2));
1193 z2_idx = internal::nearest_real_or_complex(filt.z, p2, true);
1194 z2 = filt.z[z2_idx];
1195 TESTO_ASSERT(isreal(z2));
1196 filt.z.erase(filt.z.begin() + z2_idx);
1197 }
1198 filt.p.erase(filt.p.begin() + p2_idx);
1199 }
1200 }
1201 pairs[si].p1 = p1;
1202 pairs[si].p2 = p2;
1203 pairs[si].z1 = z1;
1204 pairs[si].z2 = z2;
1205 }
1206
1207 std::vector<biquad_params<T>> result(n_sections);
1208 for (size_t si = 0; si < n_sections; si++)
1209 {
1210 result[si] = internal::zpk2tf(pairs[n_sections - 1 - si], si == 0 ? filt.k : T(1));
1211 }
1212 return result;
1213 }
1214 } // namespace CMT_ARCH_NAME
1215
1216 } // namespace kfr