Debian Codegraph @

plugins/LadspaEffect/swh/util/iir.c

raw
1 /*
2 * iir.c
3 * Copyright (C) 2000-2003 Alexander Ehlert
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 2 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, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 *
19 * A port of my glame lowpass/highpass/bandpass filters due to public demand in #lad
20 * Try out the original at glame.sourceforge.net ;-)
21 */
22
23 #include <string.h>
24 #include "../config.h"
25 #include <math.h>
26 #include <stdlib.h>
27 #include "iir.h"
28
29 /* To get better filter accuracy I decided to compute the single
30 * stages of the filter seperatly and apply them one by one
31 * to the sample data. According to the DSPGUIDE chapter 20 pp339
32 * filters are more stable when applied in stages.
33 * Who doesn't like that can still combine
34 * all stages to one stage by just convoluting the single stages.
35 * But in the moment it's up to the user that he knows what he's doing.
36 * float accuracy can't be enough for certain parameters.
37 */
38
39 #define DPRINTF(x)
40
41 /* (hopefully) generic description of an iir filter */
42
43
44
45 iir_stage_t *init_iir_stage(int mode, int nstages, int na, int nb){
46 iir_stage_t *dum=NULL;
47 int i;
48 if ((dum=ALLOCN(1,iir_stage_t))){
49 dum->mode=mode;
50 dum->nstages=0;
51 dum->availst=nstages;
52 dum->na=na;
53 dum->nb=nb;
54 dum->fc=-1.0;
55 dum->coeff=(gliirt **)malloc(nstages*sizeof(gliirt *));
56 for(i=0;i<nstages;i++)
57 dum->coeff[i]=(gliirt *)malloc((na+nb)*sizeof(gliirt));
58 }
59 return dum;
60 }
61
62 /* be sure to combine stages with some na, nb count! */
63 void combine_iir_stages(int mode, iir_stage_t* gt, iir_stage_t *first, iir_stage_t *second, int upf, int ups){
64 int stages, i, j, cnt;
65
66 if ( (upf==-1) && (ups==-1))
67 return;
68
69 stages = first->nstages + second->nstages;
70 gt->nstages = stages;
71
72 cnt = first->na + first->nb;
73
74 /* copy coefficients */
75 if (upf!=-1)
76 for(i=0; i<first->nstages; i++)
77 for(j=0; j<cnt; j++)
78 gt->coeff[i][j]=first->coeff[i][j];
79
80 if (ups!=-1)
81 for(i=first->nstages; i<stages; i++)
82 for(j=0; j<cnt; j++)
83 gt->coeff[i][j]=second->coeff[i-first->nstages][j];
84 }
85
86 void free_iir_stage(iir_stage_t *gt){
87 int i;
88 for(i=0;i<gt->availst;i++)
89 if (gt->coeff[i]) free(gt->coeff[i]);
90 if (gt->coeff) free(gt->coeff);
91 if (gt) free(gt);
92 }
93
94 /* center: frequency already normalized between 0 and 0.5 of sampling
95 * bandwidth given in octaves between lower and upper -3dB point
96 */
97
98 void calc_2polebandpass(iirf_t* iirf, iir_stage_t* gt, float fc, float bw, long sample_rate)
99 {
100 double omega, alpha, bandwidth, center, gain;
101 int i;
102
103 if ( (gt->fc==fc) && (gt->bw==bw) )
104 return;
105
106 /*reset_iirf_t(iirf, gt, 1);*/
107 gt->fc = fc;
108 gt->bw = bw;
109 gt->nstages = 1;
110
111 fc = CLAMP(fc, 0.0, (float)sample_rate*0.45f); /* if i go all the way up to 0.5 it doesn't work */
112
113 center = fc/(float)sample_rate;
114 /* bandwidth is given in octaves */
115 bandwidth = log((fc+bw*0.5)/MAX(fc-bw*0.5,0.01))/log(2.0);
116
117 omega = 2.0*M_PI*center;
118 alpha = sin(omega)*sinh(log(2.0)/2.0*bandwidth*omega/sin(omega));
119
120 gt->coeff[0][0] = alpha;
121 gt->coeff[0][1] = 0.0;
122 gt->coeff[0][2] = -alpha;
123 gt->coeff[0][3] = 2.0 * cos(omega);
124 gt->coeff[0][4] = alpha - 1.0;
125 gain = 1.0 + alpha;
126 for(i=0;i<5;i++)
127 gt->coeff[0][i]/=gain;
128 }
129
130 /* chebyshev calculates coefficients for a chebyshev filter
131 * a,b coefficients
132 * n number of poles(2,4,6,...)
133 * m 0..lowpass, 1..highpass
134 * fc cutoff frequency in percent of samplerate
135 * pr percent ripple in passband (0.5 is optimal)
136 *
137 * Code from DSPGUIDE Chapter 20, pp341
138 * online version http://www.dspguide.com
139 */
140
141 #define chebtype double
142
143 int chebyshev_stage(iir_stage_t *gt, int n){
144 chebtype h,rp,ip,es,kx,vx,t,w,m,d,k,gain;
145 chebtype x[3], y[2], a[3], b[2];
146 int res=-1,i;
147
148 if (n>gt->availst)
149 goto _error;
150 if (gt->na+gt->nb!=5)
151 goto _error;
152
153 h=M_PI/((chebtype)gt->np*2.0)+n*M_PI/(chebtype)gt->np;
154 rp=-cos(h);
155 ip=sin(h);
156
157 if(gt->ppr>0.0) {
158 h=100.0/(100.0-gt->ppr);
159 es=sqrt(h*h-1.0);
160 h=1.0/es;
161 vx=1.0/(chebtype)gt->np*log(h+sqrt(h*h+1.0));
162 kx=1.0/(chebtype)gt->np*log(h+sqrt(h*h-1.0));
163 kx=(exp(kx)+exp(-kx))/2.0;
164 h=exp(vx);
165 rp*=(h-1.0/h)*0.5/kx;
166 ip*=(h+1.0/h)*0.5/kx;
167 }
168
169 t=2.0*tan(0.5);
170 w=2.0*M_PI*gt->fc;
171 m=rp*rp+ip*ip;
172 d=4.0-4.0*rp*t+m*t*t;
173 x[0]=t*t/d;
174 x[1]=2*x[0];
175 x[2]=x[0];
176 y[0]=(8.0-2.0*m*t*t)/d;
177 y[1]=(-4.0-4.0*rp*t-m*t*t)/d;
178
179 if (gt->mode==IIR_STAGE_HIGHPASS)
180 k=-cos(w*0.5+0.5)/cos(w*0.5-0.5);
181 else
182 k=sin(0.5-w*0.5)/sin(0.5+w*0.5);
183 d=1+y[0]*k-y[1]*k*k;
184 a[0]=(x[0]-x[1]*k+x[2]*k*k)/d;
185 a[1]=(-2.0*x[0]*k+x[1]+x[1]*k*k-2.0*x[2]*k)/d;
186 a[2]=(x[0]*k*k-x[1]*k+x[2])/d;
187 b[0]=(2.0*k+y[0]+y[0]*k*k-2.0*y[1]*k)/d;
188 b[1]=(-k*k-y[0]*k+y[1])/d;
189 if(gt->mode==IIR_STAGE_HIGHPASS){
190 a[1]=-a[1];
191 b[0]=-b[0];
192 }
193
194 if(gt->mode==IIR_STAGE_HIGHPASS)
195 gain=(a[0]-a[1]+a[2])/(1.0+b[0]-b[1]);
196 else
197 gain=(a[0]+a[1]+a[2])/(1.0-b[0]-b[1]);
198 for(i=0;i<3;i++) a[i]/=gain;
199
200 gt->coeff[n][0]=(gliirt)(a[0]);
201 gt->coeff[n][1]=(gliirt)(a[1]);
202 gt->coeff[n][2]=(gliirt)(a[2]);
203 gt->coeff[n][3]=(gliirt)(b[0]);
204 gt->coeff[n][4]=(gliirt)(b[1]);
205
206 res=0;
207 _error:
208 return res;
209 }
210
211 int chebyshev(iirf_t* iirf, iir_stage_t* gt, int n, int mode, float fc, float pr){
212 int i;
213
214 if ( (gt->fc==fc) && (gt->np==n) && (gt->ppr=pr) )
215 return -1;
216
217 if (n%2!=0)
218 return -1;
219
220 if ((mode!=IIR_STAGE_HIGHPASS) && (mode!=IIR_STAGE_LOWPASS))
221 return -1;
222
223 fc=CLAMP(fc, 0.0001f, 0.4999f);
224
225 if ((n/2)>gt->nstages)
226 reset_iirf_t(iirf,gt,n/2);
227
228 gt->ppr=pr;
229 gt->fc=fc;
230 gt->np=n;
231 gt->nstages=n/2;
232 for(i=0;i<n/2;i++)
233 chebyshev_stage(gt,i);
234
235 return 0;
236 }