1 // LowPass2 module
2 // ---------------
3 // by Daniel Moreno - ComaC (2001) < comac2k@teleline.es >
4 //
5 // This is a straight-forward implementation of a 2nd order resonant
6 // lowpass filter. Well... in fact there are two lowpass filters being
7 // calculated at the same time, since the old convolution filter cannot
8 // be emulated with only one.
9 //
10 // The only optimization i've made is to change doubles for fixed point
11 // integers.
12
13 #include <math.h>
14 #include "burner.h"
15 #include "lowpass2.h"
16
17 #define PI 3.141592653589793238462643383279502884197169399375105820974944592307816
18 #define FixBits 15
19 #define Fixed(A) ( (int)((A) * (1 << FixBits)) )
20
21
22 #define SATURATE(Min, Max, Val) ( (Val) <= (Min) ? (Min) : (Val) > (Max) ? (Max) : (Val) )
23
24 // In case we know we will not run out of range (speeds up by 5% on gcc2.95.3):
25 //#define SATURATE(Min, Max, Val) (Val)
26
27
28
LowPass2(void)29 LowPass2::LowPass2(void)
30 {
31 }
32
33
LowPass2(double Freq,double SampleRate,double Q,double Gain,double Freq2,double Q2,double Gain2)34 LowPass2::LowPass2(double Freq, double SampleRate, double Q, double Gain,
35 double Freq2, double Q2, double Gain2)
36 {
37 SetParam(Freq, SampleRate, Q, Gain, Freq2, Q2, Gain2);
38 }
39
Reset()40 void LowPass2::Reset()
41 {
42 i0 = 0;
43 i1 = 0;
44 i2 = 0;
45 o0 = 0;
46 o1 = 0;
47 o2 = 0;
48 o0b = 0;
49 o1b = 0;
50 o2b = 0;
51
52 }
53
Filter(INT16 * Buff,INT32 Tam)54 void LowPass2::Filter(INT16 *Buff, INT32 Tam)
55 {
56 INT32 a;
57 INT32 Tmp, Tmp2;
58
59 for (a = 0; a < Tam*2; a += 2)
60 {
61 Tmp = (b0*Buff[a] + b1*i1 + b2*i2
62 - a1*o1 - a2*o2) / (1 << FixBits);
63
64 Tmp2 = (b0b*Buff[a] + b1b*i1 + b2b*i2
65 - a1b*o1b - a2b*o2b) / (1 << FixBits);
66
67 i2 = i1;
68 i1 = Buff[a];
69 o2 = o1;
70 o1 = Tmp;
71
72 o2b = o1b;
73 o1b = Tmp2;
74
75 Buff[a] = (INT16)SATURATE(-32768, 32767, Tmp + Tmp2)*(1-bRunPause);
76 }
77 }
78
FilterMono(INT16 * Buff,INT32 Tam)79 void LowPass2::FilterMono(INT16 *Buff, INT32 Tam)
80 {
81 INT32 a;
82 INT32 Tmp, Tmp2;
83
84 for (a = 0; a < Tam; a++)
85 {
86 Tmp = (b0*Buff[a] + b1*i1 + b2*i2
87 - a1*o1 - a2*o2) / (1 << FixBits);
88
89 Tmp2 = (b0b*Buff[a] + b1b*i1 + b2b*i2
90 - a1b*o1b - a2b*o2b) / (1 << FixBits);
91
92 i2 = i1;
93 i1 = Buff[a];
94 o2 = o1;
95 o1 = Tmp;
96
97 o2b = o1b;
98 o1b = Tmp2;
99
100 Buff[a] = (INT16)SATURATE(-32768, 32767, Tmp + Tmp2)*(1-bRunPause);
101 }
102 }
103
104
105
SetParam(double Freq,double SampleRate,double Q,double Gain,double Freq2,double Q2,double Gain2)106 void LowPass2::SetParam(double Freq, double SampleRate, double Q, double Gain,
107 double Freq2, double Q2, double Gain2)
108 {
109 double omega, sn, cs, alpha;
110
111 if (Q < 0) { Q = 0; }
112
113 if (Freq < 0) { Freq = 0; }
114 if (Freq > SampleRate/2) { Freq = SampleRate/2; }
115
116 omega = PI * 2 * Freq / SampleRate;
117 sn = sin(omega);
118 cs = cos(omega);
119 alpha = sn / (2 * Q);
120
121 a0 = Fixed(1 + alpha);
122 b0 = Fixed( ((1 - cs) / 2) * Gain / (1 + alpha) );
123 b1 = Fixed( (1 - cs) * Gain / (1 + alpha) );
124 b2 = Fixed( ((1 - cs) / 2) * Gain / (1 + alpha) );
125 a1 = Fixed( (-2 * cs) / (1 + alpha) );
126 a2 = Fixed( (1 - alpha) / (1 + alpha) );
127
128
129 if (Q2 < 0) { Q2 = 0; }
130
131 if (Freq2 < 0) { Freq2 = 0; }
132 if (Freq2 > SampleRate/2) { Freq2 = SampleRate/2; }
133
134 omega = PI * 2 * Freq2 / SampleRate;
135 sn = sin(omega);
136 cs = cos(omega);
137 alpha = sn / (2 * Q2);
138
139 a0b = Fixed(1 + alpha);
140 b0b = Fixed( ((1 - cs) / 2) * Gain2 / (1 + alpha) );
141 b1b = Fixed( (1 - cs) * Gain2 / (1 + alpha) );
142 b2b = Fixed( ((1 - cs) / 2) * Gain2 / (1 + alpha) );
143 a1b = Fixed( (-2 * cs) / (1 + alpha) );
144 a2b = Fixed( (1 - alpha) / (1 + alpha) );
145
146 i0 = 0;
147 i1 = 0;
148 i2 = 0;
149 o0 = 0;
150 o1 = 0;
151 o2 = 0;
152 o0b = 0;
153 o1b = 0;
154 o2b = 0;
155 }
156