biquad-float.h

   1 /*
   2 Calf Box, an open source musical instrument.
   3 Copyright (C) 2010 Krzysztof Foltman
   4
   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.
   9
  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.
  14
  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/>.
  17 */
  18
  19 #ifndef CBOX_BIQUAD_FLOAT_H
  20 #define CBOX_BIQUAD_FLOAT_H
  21
  22 #include "dspmath.h"
  23
  24 struct cbox_biquadf_state
  25 {
  26     float x1;
  27     float y1;
  28     float x2;
  29     float y2;
  30 };
  31
  32 struct cbox_biquadf_coeffs
  33 {
  34     float a0;
  35     float a1;
  36     float a2;
  37     float b1;
  38     float b2;
  39 };
  40
  41 static inline void cbox_biquadf_reset(struct cbox_biquadf_state *state)
  42 {
  43     state->x1 = state->y1 = state->x2 = state->y2 = 0.f;
  44 }
  45
  46 static inline float cbox_biquadf_is_audible(struct cbox_biquadf_state *state, float level)
  47 {
  48     return fabs(state->x1) + fabs(state->x2) + fabs(state->y1) + fabs(state->y2) >= level;
  49 }
  50
  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)
  53 {
  54     float omega=(float)(2*M_PI*fc/sr);
  55     float sn=sin(omega);
  56     float cs=cos(omega);
  57     float alpha=(float)(sn/(2*q));
  58     float inv=(float)(1.0/(1.0+alpha));
  59
  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);
  64 }
  65
  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)
  68 {
  69     float omega=(float)(2*M_PI*fc/sr);
  70     float sn=sin(omega);
  71     float cs=cos(omega);
  72     float alpha=(float)(sn/(2*q));
  73     float inv=(float)(1.0/(1.0+alpha));
  74
  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);
  79 }
  80
  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)
  83 {
  84     float omega=(float)(2*M_PI*fc/sr);
  85     float sn=sin(omega);
  86     float cs=cos(omega);
  87     float alpha=(float)(sn/(2*q));
  88     float inv=(float)(1.0/(1.0+alpha));
  89
  90     coeffs->a0 = (float)(inv*alpha);
  91     coeffs->a1 = 0.f;
  92     coeffs->a2 = -coeffs->a0;
  93     coeffs->b1 =  (float)(-2*cs*inv);
  94     coeffs->b2 =  (float)((1 - alpha)*inv);
  95 }
  96
  97
  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)
 100 {
 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);
 109 }
 110
 111 static inline void cbox_biquadf_set_1plp(struct cbox_biquadf_coeffs *coeffs, float freq, float sr)
 112 {
 113     float w = hz2w(freq, sr);
 114     float x = tan (w * 0.5f);
 115     float q = 1 / (1 + x);
 116     float a01 = x*q;
 117     float b1 = a01 - q;
 118
 119     coeffs->a0 = a01;
 120     coeffs->a1 = a01;
 121     coeffs->b1 = b1;
 122     coeffs->a2 = 0;
 123     coeffs->b2 = 0;
 124 }
 125
 126 static inline void cbox_biquadf_set_1php(struct cbox_biquadf_coeffs *coeffs, float freq, float sr)
 127 {
 128     float w = hz2w(freq, sr);
 129     float x = tan (w * 0.5f);
 130     float q = 1 / (1 + x);
 131     float a01 = x*q;
 132     float b1 = a01 - q;
 133
 134     coeffs->a0 = q;
 135     coeffs->a1 = -q;
 136     coeffs->b1 = b1;
 137     coeffs->a2 = 0;
 138     coeffs->b2 = 0;
 139 }
 140
 141
 142
 143 static inline void cbox_biquadf_process(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer)
 144 {
 145     int i;
 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;
 153
 154     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 155     {
 156         float in = buffer[i];
 157         double out = a0 * in + a1 * state->x1 + a2 * state->x2 - b1 * y1 - b2 * y2;
 158
 159         buffer[i] = out;
 160         state->x2 = state->x1;
 161         state->x1 = in;
 162         y2 = y1;
 163         y1 = out;
 164     }
 165     state->y2 = sanef(y2);
 166     state->y1 = sanef(y1);
 167 }
 168
 169 static inline double cbox_biquadf_process_sample(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, double in)
 170 {
 171     double out = sanef(coeffs->a0 * sanef(in) + coeffs->a1 * state->x1 + coeffs->a2 * state->x2 - coeffs->b1 * state->y1 - coeffs->b2 * state->y2);
 172
 173     state->x2 = state->x1;
 174     state->x1 = in;
 175     state->y2 = state->y1;
 176     state->y1 = out;
 177
 178     return out;
 179 }
 180
 181 static inline void cbox_biquadf_process_to(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer_in, float *buffer_out)
 182 {
 183     int i;
 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;
 191
 192     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 193     {
 194         float in = buffer_in[i];
 195         double out = a0 * in + a1 * state->x1 + a2 * state->x2 - b1 * y1 - b2 * y2;
 196
 197         buffer_out[i] = out;
 198         state->x2 = state->x1;
 199         state->x1 = in;
 200         y2 = y1;
 201         y1 = out;
 202     }
 203     state->y2 = sanef(y2);
 204     state->y1 = sanef(y1);
 205 }
 206
 207 static inline void cbox_biquadf_process_adding(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer_in, float *buffer_out)
 208 {
 209     int i;
 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;
 217
 218     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 219     {
 220         float in = buffer_in[i];
 221         double out = a0 * in + a1 * state->x1 + a2 * state->x2 - b1 * y1 - b2 * y2;
 222
 223         buffer_out[i] += out;
 224         state->x2 = state->x1;
 225         state->x1 = in;
 226         y2 = y1;
 227         y1 = out;
 228     }
 229     state->y2 = sanef(y2);
 230     state->y1 = sanef(y1);
 231 }
 232
 233 #endif