1 /* SpiralSynth
2 * Copyleft (C) 2000 David Griffiths <dave@pawfal.org>
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 */
18
19 /*
20 Resonant low pass filter source code.
21 This code was originally written by baltrax@hotmail.com
22
23 See <http://www.harmony-central.com/Computer/Programming/resonant-lp-filter.c>
24 for the full version with explanatory comments, and the maths!
25 */
26
27 #include "iir_filter.h"
28
29 BIQUAD ProtoCoef[FILTER_SECTIONS];
30
iir_filter(float input,FILTER * iir)31 float iir_filter(float input,FILTER *iir)
32 {
33 unsigned int i;
34 float *hist1_ptr,*hist2_ptr,*coef_ptr;
35 float output,new_hist,history1,history2;
36
37 /* allocate history array if different size than last call */
38
39 if(!iir->history) {
40 iir->history = (float *) calloc(2*iir->length,sizeof(float));
41 if(!iir->history) {
42 printf("\nUnable to allocate history array in iir_filter\n");
43 exit(1);
44 }
45 }
46
47 coef_ptr = iir->coef; /* coefficient pointer */
48
49 hist1_ptr = iir->history; /* first history */
50 hist2_ptr = hist1_ptr + 1; /* next history */
51
52 /* 1st number of coefficients array is overall input scale factor,
53 * or filter gain */
54 output = input * (*coef_ptr++);
55
56 for (i = 0 ; i < iir->length; i++)
57 {
58 history1 = *hist1_ptr; /* history values */
59 history2 = *hist2_ptr;
60
61 output = output - history1 * (*coef_ptr++);
62 new_hist = output - history2 * (*coef_ptr++); /* poles */
63
64 output = new_hist + history1 * (*coef_ptr++);
65 output = output + history2 * (*coef_ptr++); /* zeros */
66
67 *hist2_ptr++ = *hist1_ptr;
68 *hist1_ptr++ = new_hist;
69 hist1_ptr++;
70 hist2_ptr++;
71 }
72
73 return(output);
74 }
75
prewarp(double * a0,double * a1,double * a2,double fc,double fs)76 void prewarp(
77 double *a0, double *a1, double *a2,
78 double fc, double fs)
79 {
80 double wp, pi;
81
82 pi = 4.0 * atan(1.0);
83 wp = 2.0 * fs * tan(pi * fc / fs);
84
85 *a2 = (*a2) / (wp * wp);
86 *a1 = (*a1) / wp;
87 }
88
bilinear(double a0,double a1,double a2,double b0,double b1,double b2,double * k,double fs,float * coef)89 void bilinear(
90 double a0, double a1, double a2, /* numerator coefficients */
91 double b0, double b1, double b2, /* denominator coefficients */
92 double *k, /* overall gain factor */
93 double fs, /* sampling rate */
94 float *coef /* pointer to 4 iir coefficients */
95 )
96 {
97 double ad, bd;
98
99 /* alpha (Numerator in s-domain) */
100 ad = 4. * a2 * fs * fs + 2. * a1 * fs + a0;
101 /* beta (Denominator in s-domain) */
102 bd = 4. * b2 * fs * fs + 2. * b1* fs + b0;
103
104 /* update gain constant for this section */
105 *k *= ad/bd;
106
107 /* Denominator */
108 *coef++ = (2. * b0 - 8. * b2 * fs * fs) / bd; /* beta1 */
109 *coef++ = (4. * b2 * fs * fs - 2. * b1 * fs + b0) / bd; /* beta2 */
110 /* Nominator */
111 *coef++ = (2. * a0 - 8. * a2 * fs * fs) / ad; /* alpha1 */
112 *coef = (4. * a2 * fs * fs - 2. * a1 * fs + a0) / ad; /* alpha2 */
113 }
114
szxform(double * a0,double * a1,double * a2,double * b0,double * b1,double * b2,double fc,double fs,double * k,float * coef)115 void szxform(
116 double *a0, double *a1, double *a2, /* numerator coefficients */
117 double *b0, double *b1, double *b2, /* denominator coefficients */
118 double fc, /* Filter cutoff frequency */
119 double fs, /* sampling rate */
120 double *k, /* overall gain factor */
121 float *coef) /* pointer to 4 iir coefficients */
122 {
123 /* Calculate a1 and a2 and overwrite the original values */
124 prewarp(a0, a1, a2, fc, fs);
125 prewarp(b0, b1, b2, fc, fs);
126 bilinear(*a0, *a1, *a2, *b0, *b1, *b2, k, fs, coef);
127 }
128
129