RTXI  2.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
filtfunc.cpp
Go to the documentation of this file.
1 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 //
3 // File = filtfunc.cpp
4 //
5 //
6 
7 #include "filtfunc.h"
8 #include "cmpxpoly.h"
9 #include "misdefs.h"
10 #include "unwrap.h"
11 #include <iostream>
12 #include <math.h>
13 #include <stdlib.h>
14 
15 #ifdef _DEBUG
16 extern std::ofstream DebugFile;
17 #endif
18 
19 //===========================================================
20 // constructor
21 
23 {
24  Num_Denorm_Poles = 0;
25  Num_Denorm_Zeros = 0;
26  Degree_Of_Denom = -1;
27  Degree_Of_Numer = -1;
29  return;
30 };
31 
32 //===========================================================
33 // constructor
34 
36 {
37  Filter_Order = order;
38  Num_Denorm_Poles = 0;
39  Num_Denorm_Zeros = 0;
40  Degree_Of_Denom = -1;
41  Degree_Of_Numer = -1;
43  return;
44 };
45 
46 //==========================================================
47 //
48 complex*
50 {
51  *num_poles = Num_Prototype_Poles;
52  return (Prototype_Pole_Locs);
53 };
54 
55 //==========================================================
56 //
57 complex*
59 {
61  *num_poles = Num_Denorm_Poles;
62  return (Denorm_Pole_Locs);
63  } else {
64  *num_poles = Num_Prototype_Poles;
65  return (Prototype_Pole_Locs);
66  }
67 };
68 //==========================================================
69 //
70 complex
72 {
74  if (pole_indx > Num_Denorm_Poles) {
75  return (complex(0.0, 0.0));
76  } else {
77  return (Denorm_Pole_Locs[pole_indx]);
78  }
79  } else {
80  if (pole_indx > Num_Prototype_Poles) {
81  return (complex(0.0, 0.0));
82  } else {
83  return (Prototype_Pole_Locs[pole_indx]);
84  }
85  }
86 };
87 //==========================================================
88 //
89 complex
91 {
93  if (zero_indx > Num_Denorm_Zeros) {
94  return (complex(0.0, 0.0));
95  } else {
96  return (Denorm_Zero_Locs[zero_indx]);
97  }
98  } else {
99  if (zero_indx > Num_Prototype_Zeros) {
100  return (complex(0.0, 0.0));
101  } else {
102  return (Prototype_Zero_Locs[zero_indx]);
103  }
104  }
105 };
106 
107 //========================================================
108 void
109 FilterTransFunc::FrequencyPrewarp(double sampling_interval)
110 {
111  int n;
112  double freq_scale, warped_analog_cutoff;
113  double desired_digital_cutoff;
114 
115  desired_digital_cutoff = Denorm_Cutoff_Freq_Rad;
116  warped_analog_cutoff = (2.0 / sampling_interval) *
117  tan(desired_digital_cutoff * sampling_interval / 2.0);
118  freq_scale = warped_analog_cutoff / desired_digital_cutoff;
119 
120 #ifdef _DEBUG
121  DebugFile << "freq_scale = " << freq_scale << std::endl;
122  DebugFile << "Num_Denorm_Poles = " << Num_Denorm_Poles << std::endl;
123  DebugFile << "Num_Denorm_Zeros = " << Num_Denorm_Zeros << std::endl;
124  DebugFile << "in prewarp, orig H_Sub_Zero = " << H_Sub_Zero << std::endl;
125 #endif
126 
127  for (n = 1; n <= Num_Denorm_Poles; n++) {
128  Denorm_Pole_Locs[n] *= freq_scale;
129  }
130  for (n = 1; n <= Num_Denorm_Zeros; n++) {
131  Denorm_Zero_Locs[n] *= freq_scale;
132  }
133  for (n = 1; n <= (Num_Denorm_Poles - Num_Denorm_Zeros); n++) {
134  H_Sub_Zero *= freq_scale;
135  }
136 #ifdef _DEBUG
137  DebugFile << "scaled H_Sub_Zero = " << H_Sub_Zero << std::endl;
138 #endif
139  return;
140 }
141 
142 //=========================================================
143 void
145 {
146  complex numer, denom;
147  complex transfer_function;
148  complex s_val, pole;
149  double delta_freq, magnitude, phase;
150  double peak_magnitude;
151  double *mag_resp, *phase_resp, *group_dly;
152  int i, k;
153 
154  delta_freq = 0.0125;
155  peak_magnitude = -1000.0;
156 
157  Response_File = new std::ofstream("anlg_rsp.txt", std::ios::out);
158  mag_resp = new double[800];
159  phase_resp = new double[800];
160  group_dly = new double[800];
161 
162  for (i = 1; i < 800; i++) {
163  numer = complex(1.0, 0.0);
164  denom = complex(1.0, 0.0);
165  s_val = complex(0.0, i * delta_freq);
166 
167  for (k = 1; k <= Num_Denorm_Zeros; k++) {
168  numer *= (s_val - Denorm_Zero_Locs[k]);
169  }
170 
171  for (k = 1; k <= Num_Denorm_Poles; k++) {
172  denom *= (s_val - Denorm_Pole_Locs[k]);
173  }
174  transfer_function = numer / denom;
175  magnitude = 10.0 * log10(mag_sqrd(transfer_function));
176  mag_resp[i] = magnitude;
177  if (magnitude > peak_magnitude) {
178  peak_magnitude = magnitude;
179  }
180  phase = 180.0 * arg(transfer_function) / PI;
181  phase_resp[i] = phase;
182  }
183  UnwrapPhase(0, &(phase_resp[1]));
184  for (i = 2; i < 800; i++) {
185  UnwrapPhase(1, &(phase_resp[i]));
186  }
187  group_dly[1] = PI * (phase_resp[1] - phase_resp[2]) / (180.0 * delta_freq);
188  for (i = 2; i < 800; i++) {
189  group_dly[i] =
190  PI * (phase_resp[i - 1] - phase_resp[i]) / (180.0 * delta_freq);
191  }
192  for (i = 1; i < 800; i++) {
193  (*Response_File) << i * delta_freq << ", "
194  << (mag_resp[i] - peak_magnitude) << ", " << phase_resp[i]
195  << ", " << group_dly[i] << std::endl;
196  }
197  Response_File->close();
198  delete[] phase_resp;
199  delete[] mag_resp;
200  delete[] group_dly;
201  return;
202 }
203 
204 //==========================================================
205 int
207 {
209  return (Num_Denorm_Poles);
210  } else {
211  return (Num_Prototype_Poles);
212  }
213 };
214 
215 //==========================================================
216 int
218 {
220  return (Num_Denorm_Zeros);
221  } else {
222  return (Num_Prototype_Zeros);
223  }
224 };
225 
226 //===============================================================
227 //
228 complex*
230 {
231  *num_zeros = Num_Prototype_Zeros;
232  return (Prototype_Zero_Locs);
233 }
234 
235 //===============================================================
236 //
237 complex*
239 {
241  *num_zeros = Num_Denorm_Zeros;
242  return (Denorm_Zero_Locs);
243  } else {
244  *num_zeros = Num_Prototype_Zeros;
245  return (Prototype_Zero_Locs);
246  }
247 }
248 
249 //==================================================================
250 //
251 
252 float
254 {
255  return ((float)H_Sub_Zero);
256 }
257 
258 //===============================================================
259 //
260 
261 void
262 FilterTransFunc::DumpBiquads(std::ofstream* output_stream)
263 {
264  (*output_stream) << "\nBiquad Coefficients\n" << std::endl;
265 
266  for (int i = 1; i <= Num_Biquad_Sects; i++) {
267  (*output_stream) << i << ") a = " << A_Biquad_Coef[i]
268  << " b = " << B_Biquad_Coef[i]
269  << " c = " << C_Biquad_Coef[i] << std::endl;
270  }
271  return;
272 }
273 //=======================================================
274 //
275 void
277 {
278  int j;
284  Denorm_Cutoff_Freq_Rad = cutoff_freq;
285 
286  for (j = 1; j <= Num_Denorm_Poles; j++) {
287  Denorm_Pole_Locs[j] = Prototype_Pole_Locs[j] * cutoff_freq;
288  }
289  for (j = 1; j <= Num_Denorm_Zeros; j++) {
290  Denorm_Zero_Locs[j] = Prototype_Zero_Locs[j] * cutoff_freq;
291  }
292  for (j = 0; j < (Num_Denorm_Poles - Num_Denorm_Zeros); j++) {
293  H_Sub_Zero *= cutoff_freq;
294  }
295 #ifdef _DEBUG
296  DebugFile << "in LP denorm, H_Sub_Zero scaled to " << H_Sub_Zero << std::endl;
297 #endif
298  return;
299 }
300 
301 //=========================================================
302 //
303 
306 {
307  //-----------------------------------------------------
308  // if denominator polynomial is not built yet,
309  // build it by multiplying together (s-p[i]) binomial
310  // factors where the p[i] are the poles of the filter
311 
312  if (Degree_Of_Denom < 0) {
313  CmplxPolynomial cmplx_denom_poly =
315  for (int ii = 2; ii <= Num_Prototype_Poles; ii++) {
316  cmplx_denom_poly *=
318  }
319 #ifdef _DEBUG
320  cmplx_denom_poly.DumpToStream(&DebugFile);
321 #endif
322 
323  Denom_Poly = Polynomial(cmplx_denom_poly);
324 
326 
327 #ifdef _DEBUG
328  DebugFile << "\nreal-valued version:" << std::endl;
330 #endif
331  }
332 
333  return (Denom_Poly);
334 }
335 
336 //================================================================
337 //
338 
341 {
342  //---------------------------------------------------
343  // if numerator polynomial is not built yet,
344  // build it by multiplying together (s-z[i]) binomial
345  // factors where the z[i] are the zeros of the filter.
346 
347  if (Degree_Of_Numer < 0) {
348  CmplxPolynomial cmplx_poly =
350  for (int ii = 2; ii <= Num_Prototype_Zeros; ii++) {
351  cmplx_poly *=
353  }
354 #ifdef _DEBUG
355  cmplx_poly.DumpToStream(&DebugFile);
356 #endif
357 
358  Numer_Poly = Polynomial(cmplx_poly);
359 
361 
362 #ifdef _DEBUG
363  DebugFile << "\nreal-valued version:" << std::endl;
365 #endif
366  }
367  return (Numer_Poly);
368 }
int Num_Biquad_Sects
Definition: filtfunc.h:59
std::ofstream * Response_File
Definition: filtfunc.h:73
Polynomial GetDenomPoly(void)
Definition: filtfunc.cpp:305
complex * GetPrototypePoles(int *num_poles)
Definition: filtfunc.cpp:49
int Num_Denorm_Poles
Definition: filtfunc.h:53
void DumpToStream(std::ofstream *output_stream)
Definition: poly.cpp:162
complex * Denorm_Zero_Locs
Definition: filtfunc.h:70
complex * GetPoles(int *num_poles)
Definition: filtfunc.cpp:58
complex * GetZeros(int *num_zeros)
Definition: filtfunc.cpp:238
double * C_Biquad_Coef
Definition: filtfunc.h:65
double * B_Biquad_Coef
Definition: filtfunc.h:64
complex * Denorm_Pole_Locs
Definition: filtfunc.h:69
void UnwrapPhase(int ix, double *phase)
Definition: unwrap.cpp:15
void FrequencyPrewarp(double sampling_interval)
Definition: filtfunc.cpp:109
complex GetPole(int pole_indx)
Definition: filtfunc.cpp:71
double arg(const complex _z)
Definition: complex.h:133
double * A_Biquad_Coef
Definition: filtfunc.h:63
Polynomial GetNumerPoly(void)
Definition: filtfunc.cpp:340
Polynomial Denom_Poly
Definition: filtfunc.h:71
#define PI
Definition: misdefs.h:9
logical Filter_Is_Denormalized
Definition: filtfunc.h:60
int Num_Denorm_Zeros
Definition: filtfunc.h:54
Polynomial Numer_Poly
Definition: filtfunc.h:72
double H_Sub_Zero
Definition: filtfunc.h:66
int GetNumPoles(void)
Definition: filtfunc.cpp:206
int Num_Prototype_Zeros
Definition: filtfunc.h:58
std::ofstream DebugFile
void FilterFrequencyResponse(void)
Definition: filtfunc.cpp:144
double Denorm_Cutoff_Freq_Rad
Definition: filtfunc.h:62
void LowpassDenorm(double cutoff_freq_hz)
Definition: filtfunc.cpp:276
int GetNumZeros(void)
Definition: filtfunc.cpp:217
void DumpToStream(std::ostream *output_stream)
Definition: cmpxpoly.cpp:302
double mag_sqrd(const complex _z)
Definition: complex.h:128
#define FALSE
Definition: misdefs.h:14
complex * Prototype_Pole_Locs
Definition: filtfunc.h:67
#define TRUE
Definition: misdefs.h:13
float GetHSubZero(void)
Definition: filtfunc.cpp:253
int Filter_Order
Definition: filtfunc.h:52
int Degree_Of_Numer
Definition: filtfunc.h:56
int GetDegree(void)
Definition: poly.cpp:176
complex GetZero(int zero_indx)
Definition: filtfunc.cpp:90
int Num_Prototype_Poles
Definition: filtfunc.h:57
void DumpBiquads(std::ofstream *output_stream)
Definition: filtfunc.cpp:262
complex * GetPrototypeZeros(int *num_zeros)
Definition: filtfunc.cpp:229
int Degree_Of_Denom
Definition: filtfunc.h:55
complex * Prototype_Zero_Locs
Definition: filtfunc.h:68
FilterTransFunc(void)
Definition: filtfunc.cpp:22