1 /*******************************************************************************
2
3 "A Collection of Useful C++ Classes for Digital Signal Processing"
4 By Vinnie Falco
5
6 Official project location:
7 https://github.com/vinniefalco/DSPFilters
8
9 See Documentation.cpp for contact information, notes, and bibliography.
10
11 --------------------------------------------------------------------------------
12
13 License: MIT License (http://www.opensource.org/licenses/mit-license.php)
14 Copyright (c) 2009 by Vinnie Falco
15
16 Permission is hereby granted, free of charge, to any person obtaining a copy
17 of this software and associated documentation files (the "Software"), to deal
18 in the Software without restriction, including without limitation the rights
19 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
20 copies of the Software, and to permit persons to whom the Software is
21 furnished to do so, subject to the following conditions:
22
23 The above copyright notice and this permission notice shall be included in
24 all copies or substantial portions of the Software.
25
26 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
27 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
28 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
29 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
30 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
31 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
32 THE SOFTWARE.
33
34 *******************************************************************************/
35
36 #include "DspFilters/Common.h"
37 #include "DspFilters/MathSupplement.h"
38 #include "DspFilters/Biquad.h"
39
40 namespace Dsp {
41
BiquadPoleState(const BiquadBase & s)42 BiquadPoleState::BiquadPoleState (const BiquadBase& s)
43 {
44 const double a0 = s.getA0 ();
45 const double a1 = s.getA1 ();
46 const double a2 = s.getA2 ();
47 const double b0 = s.getB0 ();
48 const double b1 = s.getB1 ();
49 const double b2 = s.getB2 ();
50
51 if (a2 == 0 && b2 == 0)
52 {
53 // single pole
54 poles.first = -a1;
55 zeros.first = -b0 / b1;
56 poles.second = 0;
57 zeros.second = 0;
58 }
59 else
60 {
61 {
62 const complex_t c = sqrt (complex_t (a1 * a1 - 4 * a0 * a2, 0));
63 double d = 2. * a0;
64 poles.first = -(a1 + c) / d;
65 poles.second = (c - a1) / d;
66 assert (!poles.is_nan());
67 }
68
69 {
70 const complex_t c = sqrt (complex_t (
71 b1 * b1 - 4 * b0 * b2, 0));
72 double d = 2. * b0;
73 zeros.first = -(b1 + c) / d;
74 zeros.second = (c - b1) / d;
75 assert (!zeros.is_nan());
76 }
77 }
78
79 gain = b0 / a0;
80 }
81
82 //------------------------------------------------------------------------------
83
response(double normalizedFrequency) const84 complex_t BiquadBase::response (double normalizedFrequency) const
85 {
86 const double a0 = getA0 ();
87 const double a1 = getA1 ();
88 const double a2 = getA2 ();
89 const double b0 = getB0 ();
90 const double b1 = getB1 ();
91 const double b2 = getB2 ();
92
93 const double w = 2 * doublePi * normalizedFrequency;
94 const complex_t czn1 = std::polar (1., -w);
95 const complex_t czn2 = std::polar (1., -2 * w);
96 complex_t ch (1);
97 complex_t cbot (1);
98
99 complex_t ct (b0/a0);
100 complex_t cb (1);
101 ct = addmul (ct, b1/a0, czn1);
102 ct = addmul (ct, b2/a0, czn2);
103 cb = addmul (cb, a1/a0, czn1);
104 cb = addmul (cb, a2/a0, czn2);
105 ch *= ct;
106 cbot *= cb;
107
108 return ch / cbot;
109 }
110
getPoleZeros() const111 std::vector<PoleZeroPair> BiquadBase::getPoleZeros () const
112 {
113 std::vector<PoleZeroPair> vpz;
114 BiquadPoleState bps (*this);
115 vpz.push_back (bps);
116 return vpz;
117 }
118
setCoefficients(double a0,double a1,double a2,double b0,double b1,double b2)119 void BiquadBase::setCoefficients (double a0, double a1, double a2,
120 double b0, double b1, double b2)
121 {
122 assert (!Dsp::is_nan (a0) && !Dsp::is_nan (a1) && !Dsp::is_nan (a2) &&
123 !Dsp::is_nan (b0) && !Dsp::is_nan (b1) && !Dsp::is_nan (b2));
124
125 m_a0 = a0;
126 m_a1 = a1/a0;
127 m_a2 = a2/a0;
128 m_b0 = b0/a0;
129 m_b1 = b1/a0;
130 m_b2 = b2/a0;
131 }
132
setOnePole(complex_t pole,complex_t zero)133 void BiquadBase::setOnePole (complex_t pole, complex_t zero)
134 {
135 #if 0
136 pole = adjust_imag (pole);
137 zero = adjust_imag (zero);
138 #else
139 assert (pole.imag() == 0);
140 assert (zero.imag() == 0);
141 #endif
142
143 const double a0 = 1;
144 const double a1 = -pole.real();
145 const double a2 = 0;
146 const double b0 = -zero.real();
147 const double b1 = 1;
148 const double b2 = 0;
149
150 setCoefficients (a0, a1, a2, b0, b1, b2);
151 }
152
setTwoPole(complex_t pole1,complex_t zero1,complex_t pole2,complex_t zero2)153 void BiquadBase::setTwoPole (complex_t pole1, complex_t zero1,
154 complex_t pole2, complex_t zero2)
155 {
156 #if 0
157 pole1 = adjust_imag (pole1);
158 pole2 = adjust_imag (pole2);
159 zero1 = adjust_imag (zero1);
160 zero2 = adjust_imag (zero2);
161 #endif
162
163 const double a0 = 1;
164 double a1;
165 double a2;
166
167 if (pole1.imag() != 0)
168 {
169 assert (pole2 == std::conj (pole1));
170
171 a1 = -2 * pole1.real();
172 a2 = std::norm (pole1);
173 }
174 else
175 {
176 assert (pole2.imag() == 0);
177
178 a1 = -(pole1.real() + pole2.real());
179 a2 = pole1.real() * pole2.real();
180 }
181
182 const double b0 = 1;
183 double b1;
184 double b2;
185
186 if (zero1.imag() != 0)
187 {
188 assert (zero2 == std::conj (zero1));
189
190 b1 = -2 * zero1.real();
191 b2 = std::norm (zero1);
192 }
193 else
194 {
195 assert (zero2.imag() == 0);
196
197 b1 = -(zero1.real() + zero2.real());
198 b2 = zero1.real() * zero2.real();
199 }
200
201 setCoefficients (a0, a1, a2, b0, b1, b2);
202 }
203
setPoleZeroForm(const BiquadPoleState & bps)204 void BiquadBase::setPoleZeroForm (const BiquadPoleState& bps)
205 {
206 setPoleZeroPair (bps);
207 applyScale (bps.gain);
208 }
209
setIdentity()210 void BiquadBase::setIdentity ()
211 {
212 setCoefficients (1, 0, 0, 1, 0, 0);
213 }
214
applyScale(double scale)215 void BiquadBase::applyScale (double scale)
216 {
217 m_b0 *= scale;
218 m_b1 *= scale;
219 m_b2 *= scale;
220 }
221
222 //------------------------------------------------------------------------------
223
Biquad()224 Biquad::Biquad ()
225 {
226 }
227
228 // Construct a second order section from a pair of poles and zeroes
Biquad(const BiquadPoleState & bps)229 Biquad::Biquad (const BiquadPoleState& bps)
230 {
231 setPoleZeroForm (bps);
232 }
233
234 //------------------------------------------------------------------------------
235
236 }
237