2 Calf Box, an open source musical instrument.
3 Copyright (C) 2010 Krzysztof Foltman
5 This program is free software: you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation, either version 3 of the License, or
8 (at your option) any later version.
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
19 #ifndef CBOX_BIQUAD_FLOAT_H
20 #define CBOX_BIQUAD_FLOAT_H
24 struct cbox_biquadf_state
32 struct cbox_biquadf_coeffs
41 static inline void cbox_biquadf_reset(struct cbox_biquadf_state
*state
)
43 state
->x1
= state
->y1
= state
->x2
= state
->y2
= 0.f
;
46 static inline float cbox_biquadf_is_audible(struct cbox_biquadf_state
*state
, float level
)
48 return fabs(state
->x1
) + fabs(state
->x2
) + fabs(state
->y1
) + fabs(state
->y2
) >= level
;
51 // Based on filter coefficient equations by Robert Bristow-Johnson
52 static inline void cbox_biquadf_set_lp_rbj(struct cbox_biquadf_coeffs
*coeffs
, float fc
, float q
, float sr
)
54 float omega
=(float)(2*M_PI
*fc
/sr
);
57 float alpha
=(float)(sn
/(2*q
));
58 float inv
=(float)(1.0/(1.0+alpha
));
60 coeffs
->a2
= coeffs
->a0
= (float)(inv
*(1 - cs
)*0.5f
);
61 coeffs
->a1
= coeffs
->a0
+ coeffs
->a0
;
62 coeffs
->b1
= (float)(-2*cs
*inv
);
63 coeffs
->b2
= (float)((1 - alpha
)*inv
);
66 // Based on filter coefficient equations by Robert Bristow-Johnson
67 static inline void cbox_biquadf_set_hp_rbj(struct cbox_biquadf_coeffs
*coeffs
, float fc
, float q
, float sr
)
69 float omega
=(float)(2*M_PI
*fc
/sr
);
72 float alpha
=(float)(sn
/(2*q
));
73 float inv
=(float)(1.0/(1.0+alpha
));
75 coeffs
->a2
= coeffs
->a0
= (float)(inv
*(1 + cs
)*0.5f
);
76 coeffs
->a1
= -2 * coeffs
->a0
;
77 coeffs
->b1
= (float)(-2*cs
*inv
);
78 coeffs
->b2
= (float)((1 - alpha
)*inv
);
81 // Based on filter coefficient equations by Robert Bristow-Johnson
82 static inline void cbox_biquadf_set_bp_rbj(struct cbox_biquadf_coeffs
*coeffs
, float fc
, float q
, float sr
)
84 float omega
=(float)(2*M_PI
*fc
/sr
);
87 float alpha
=(float)(sn
/(2*q
));
88 float inv
=(float)(1.0/(1.0+alpha
));
90 coeffs
->a0
= (float)(inv
*alpha
);
92 coeffs
->a2
= -coeffs
->a0
;
93 coeffs
->b1
= (float)(-2*cs
*inv
);
94 coeffs
->b2
= (float)((1 - alpha
)*inv
);
98 // Based on filter coefficient equations by Robert Bristow-Johnson
99 static inline void cbox_biquadf_set_peakeq_rbj(struct cbox_biquadf_coeffs
*coeffs
, float freq
, float q
, float peak
, float sr
)
101 float A
= sqrt(peak
);
102 float w0
= freq
* 2 * M_PI
* (1.0 / sr
);
103 float alpha
= sin(w0
) / (2 * q
);
104 float ib0
= 1.0 / (1 + alpha
/A
);
105 coeffs
->a1
= coeffs
->b1
= -2*cos(w0
) * ib0
;
106 coeffs
->a0
= ib0
* (1 + alpha
*A
);
107 coeffs
->a2
= ib0
* (1 - alpha
*A
);
108 coeffs
->b2
= ib0
* (1 - alpha
/A
);
111 static inline void cbox_biquadf_set_1plp(struct cbox_biquadf_coeffs
*coeffs
, float freq
, float sr
)
113 float w
= hz2w(freq
, sr
);
114 float x
= tan (w
* 0.5f
);
115 float q
= 1 / (1 + x
);
126 static inline void cbox_biquadf_set_1php(struct cbox_biquadf_coeffs
*coeffs
, float freq
, float sr
)
128 float w
= hz2w(freq
, sr
);
129 float x
= tan (w
* 0.5f
);
130 float q
= 1 / (1 + x
);
143 static inline void cbox_biquadf_process(struct cbox_biquadf_state
*state
, struct cbox_biquadf_coeffs
*coeffs
, float *buffer
)
146 float a0
= coeffs
->a0
;
147 float a1
= coeffs
->a1
;
148 float a2
= coeffs
->a2
;
149 float b1
= coeffs
->b1
;
150 float b2
= coeffs
->b2
;
151 double y1
= state
->y1
;
152 double y2
= state
->y2
;
154 for (i
= 0; i
< CBOX_BLOCK_SIZE
; i
++)
156 float in
= buffer
[i
];
157 double out
= a0
* in
+ a1
* state
->x1
+ a2
* state
->x2
- b1
* y1
- b2
* y2
;
160 state
->x2
= state
->x1
;
165 state
->y2
= sanef(y2
);
166 state
->y1
= sanef(y1
);
169 static inline double cbox_biquadf_process_sample(struct cbox_biquadf_state
*state
, struct cbox_biquadf_coeffs
*coeffs
, double in
)
171 double out
= sanef(coeffs
->a0
* sanef(in
) + coeffs
->a1
* state
->x1
+ coeffs
->a2
* state
->x2
- coeffs
->b1
* state
->y1
- coeffs
->b2
* state
->y2
);
173 state
->x2
= state
->x1
;
175 state
->y2
= state
->y1
;
181 static inline void cbox_biquadf_process_to(struct cbox_biquadf_state
*state
, struct cbox_biquadf_coeffs
*coeffs
, float *buffer_in
, float *buffer_out
)
184 float a0
= coeffs
->a0
;
185 float a1
= coeffs
->a1
;
186 float a2
= coeffs
->a2
;
187 float b1
= coeffs
->b1
;
188 float b2
= coeffs
->b2
;
189 double y1
= state
->y1
;
190 double y2
= state
->y2
;
192 for (i
= 0; i
< CBOX_BLOCK_SIZE
; i
++)
194 float in
= buffer_in
[i
];
195 double out
= a0
* in
+ a1
* state
->x1
+ a2
* state
->x2
- b1
* y1
- b2
* y2
;
198 state
->x2
= state
->x1
;
203 state
->y2
= sanef(y2
);
204 state
->y1
= sanef(y1
);
207 static inline void cbox_biquadf_process_adding(struct cbox_biquadf_state
*state
, struct cbox_biquadf_coeffs
*coeffs
, float *buffer_in
, float *buffer_out
)
210 float a0
= coeffs
->a0
;
211 float a1
= coeffs
->a1
;
212 float a2
= coeffs
->a2
;
213 float b1
= coeffs
->b1
;
214 float b2
= coeffs
->b2
;
215 double y1
= state
->y1
;
216 double y2
= state
->y2
;
218 for (i
= 0; i
< CBOX_BLOCK_SIZE
; i
++)
220 float in
= buffer_in
[i
];
221 double out
= a0
* in
+ a1
* state
->x1
+ a2
* state
->x2
- b1
* y1
- b2
* y2
;
223 buffer_out
[i
] += out
;
224 state
->x2
= state
->x1
;
229 state
->y2
= sanef(y2
);
230 state
->y1
= sanef(y1
);