Nsound  0.9.4
FilterStageIIR.cc
Go to the documentation of this file.
1 //-----------------------------------------------------------------------------
2 //
3 // $Id: FilterStageIIR.cc 874 2014-09-08 02:21:29Z weegreenblobbie $
4 //
5 // Copyright (c) 2008 Nick Hilton
6 //
7 // weegreenblobbie_yahoo_com (replace '_' with '@' and '.')
8 //
9 //-----------------------------------------------------------------------------
10 
11 //-----------------------------------------------------------------------------
12 //
13 // This program is free software; you can redistribute it and/or modify
14 // it under the terms of the GNU General Public License as published by
15 // the Free Software Foundation; either version 2 of the License, or
16 // (at your option) any later version.
17 //
18 // This program is distributed in the hope that it will be useful,
19 // but WITHOUT ANY WARRANTY; without even the implied warranty of
20 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 // GNU Library General Public License for more details.
22 //
23 // You should have received a copy of the GNU General Public License
24 // along with this program; if not, write to the Free Software
25 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
26 //
27 //-----------------------------------------------------------------------------
28 
29 #include <Nsound/AudioStream.h>
30 #include <Nsound/Buffer.h>
31 #include <Nsound/FilterStageIIR.h>
32 
33 #include <cmath>
34 #include <string.h>
35 #include <iostream>
36 
37 using namespace Nsound;
38 
39 using std::cout;
40 using std::endl;
41 
42 //-----------------------------------------------------------------------------
45  Type type,
46  const float64 & sample_rate,
47  uint32 n_poles,
48  const float64 & frequency,
49  const float64 & percent_ripple)
50  :
51  Filter(sample_rate),
52  type_(type),
53  n_poles_(n_poles),
54  frequency_(frequency),
55  percent_ripple_(percent_ripple),
56  a_(NULL),
57  b_(NULL),
58  x_history_(NULL),
59  x_ptr_(NULL),
60  x_end_ptr_(NULL),
61  y_history_(NULL),
62  y_ptr_(NULL),
63  y_end_ptr_(NULL),
64  kernel_cache_()
65 {
66  if(n_poles_ > 20)
67  {
68  n_poles_ = 20;
69  }
70  else if(n_poles_ < 2)
71  {
72  n_poles_ = 2;
73  }
74 
75  if(n_poles_ % 2 != 0)
76  {
77  ++n_poles_;
78  }
79 
80  x_history_ = new float64 [n_poles_ + 1];
83 
84  y_history_ = new float64 [n_poles_ + 1];
86  y_end_ptr_ = y_history_ + n_poles_ + 1;
87 
88  reset();
89 }
90 
91 //-----------------------------------------------------------------------------
94  :
95  Filter(copy.sample_rate_),
96  type_(copy.type_),
97  n_poles_(copy.n_poles_),
98  frequency_(copy.frequency_),
99  percent_ripple_(copy.percent_ripple_),
100  a_(NULL),
101  b_(NULL),
102  x_history_(NULL),
103  x_ptr_(NULL),
104  x_end_ptr_(NULL),
105  y_history_(NULL),
106  y_ptr_(NULL),
107  y_end_ptr_(NULL),
108  kernel_cache_()
109 {
110  *this = copy;
111 }
112 
113 //-----------------------------------------------------------------------------
116 {
117  delete [] x_history_;
118  delete [] y_history_;
119 
120  FilterStageIIR::KernelCache::iterator itor = kernel_cache_.begin();
121  FilterStageIIR::KernelCache::iterator end = kernel_cache_.end();
122 
123  while(itor != end)
124  {
125  delete [] itor->b_;
126  delete [] itor->a_;
127  ++itor;
128  }
129 }
130 
133 filter(const AudioStream & x)
134 {
135  return Filter::filter(x);
136 };
137 
138 
141 filter(const AudioStream & x, const float64 & f)
142 {
143  return Filter::filter(x, f);
144 }
145 
148 filter(const AudioStream & x, const Buffer & frequencies)
149 {
150  return Filter::filter(x, frequencies);
151 }
152 
153 Buffer
155 filter(const Buffer & x)
156 {
157  return Filter::filter(x);
158 }
159 
160 Buffer
162 filter(const Buffer & x, const float64 & f)
163 {
164  // Instead of calling Filter::filter(x,f), we implement this function here
165  // to avoid calling makeKernel(f) for each sample.
166 
167 //~ cout << "f = " << f << endl;
168 
171 
172  Buffer::const_iterator itor = x.begin();
173  Buffer::const_iterator end = x.end();
174 
175  Buffer y(x.getLength());
176  while(itor != end)
177  {
178  y << FilterStageIIR::filter(*itor);
179  ++itor;
180  }
181 
182  return y;
183 }
184 
185 Buffer
187 filter(const Buffer & x, const Buffer & frequencies)
188 {
189  return Filter::filter(x, frequencies);
190 }
191 
192 //-----------------------------------------------------------------------------
196 {
197  if(this == &rhs) return *this;
198 
199  uint32 n_poles_orig = n_poles_;
200 
201  type_ = rhs.type_;
202  n_poles_ = rhs.n_poles_;
203  frequency_ = rhs.frequency_;
205 
206  // Clear the kernal cache.
207  KernelCache::iterator itor = kernel_cache_.begin();
208 
209  while(itor != kernel_cache_.end())
210  {
211  delete [] itor->b_;
212  delete [] itor->a_;
213  ++itor;
214  }
215 
216  kernel_cache_.clear();
217 
218  // Copy kernal cache.
219  itor = rhs.kernel_cache_.begin();
220  while(itor != rhs.kernel_cache_.end())
221  {
222  Kernel new_kernel(static_cast<uint32>(itor->frequency_));
223 
224  // Allocate new kernel memory.
225  new_kernel.b_ = new float64 [n_poles_ + 1];
226  new_kernel.a_ = new float64 [n_poles_ + 1];
227 
228  // Copy values.
229  memcpy(new_kernel.b_, itor->b_, sizeof(float64) * (n_poles_ + 1));
230  memcpy(new_kernel.a_, itor->a_, sizeof(float64) * (n_poles_ + 1));
231 
232  // Insert into cache.
233  kernel_cache_.insert(new_kernel);
234  }
235 
236  // Setup history memory.
237  if(n_poles_orig != rhs.n_poles_)
238  {
239  delete [] x_history_;
240  delete [] y_history_;
241 
242  x_history_ = new float64 [n_poles_ + 1];
243  x_ptr_ = x_history_;
245 
246  y_history_ = new float64 [n_poles_ + 1];
247  y_ptr_ = y_history_;
248  y_end_ptr_ = y_history_ + n_poles_ + 1;
249  }
250 
251  reset();
252 
253  return *this;
254 }
255 
256 void
259 {
260  memset(x_history_, 0, sizeof(float64) * (n_poles_ + 1));
261  memset(y_history_, 0, sizeof(float64) * (n_poles_ + 1));
262 
263  x_ptr_ = x_history_;
264  y_ptr_ = y_history_;
265 
267 }
268 
269 float64
271 filter(const float64 & x)
272 {
273  // Write x to history
274  *x_ptr_ = x;
275 
276  // Increment history pointer
277  ++x_ptr_;
278 
279  // Bounds check
280  if(x_ptr_ >= x_end_ptr_)
281  {
282  x_ptr_ = x_history_;
283  }
284 
285  float64 y = 0.0;
286  float64 * x_hist = x_ptr_;
287  for(float64 * b = b_; b != b_ + n_poles_ + 1; ++b)
288  {
289  // When we enter this loop, x_hist is pointing at x[n + 1]
290  --x_hist;
291 
292  // Bounds check
293  if(x_hist < x_history_)
294  {
295  x_hist = x_end_ptr_ - 1;
296  }
297 
298  y += *b * *x_hist;
299  }
300 
301  float64 * y_hist = y_ptr_;
302  for(float64 * a = a_ + 1; a != a_ + n_poles_ + 1; ++a)
303  {
304  // When we enter this loop, y_hist is pointing at y[n + 1]
305  --y_hist;
306 
307  // Bounds check
308  if(y_hist < y_history_)
309  {
310  y_hist = y_end_ptr_ - 1;
311  }
312 
313  y += *a * *y_hist;
314 
315  }
316 
317  // insert output into history buffer
318  *y_ptr_ = y;
319 
320  // Increment history pointer
321  ++y_ptr_;
322 
323  // Bounds check
324  if(y_ptr_ >= y_end_ptr_)
325  {
326  y_ptr_ = y_history_;
327  }
328 
329  return y;
330 }
331 
332 float64
334 filter(const float64 & x, const float64 & frequency)
335 {
336  makeKernel(frequency);
337  return FilterStageIIR::filter(x);
338 }
339 
340 void
342 makeKernel(const float64 & frequency)
343 {
345  Kernel new_kernel(static_cast<uint32>(frequency));
346 
347  // See if the kernel is in the cache.
348 
349  FilterStageIIR::
350  KernelCache::const_iterator itor = kernel_cache_.find(new_kernel);
351 
352  if(itor != kernel_cache_.end())
353  {
354  b_ = itor->b_;
355  a_ = itor->a_;
356 
357  return;
358  }
359 
360  // Allocate new kernel.
361  new_kernel.b_ = new float64 [n_poles_ + 1];
362  new_kernel.a_ = new float64 [n_poles_ + 1];
363 
364  b_ = new_kernel.b_;
365  a_ = new_kernel.a_;
366 
367  // No kernel in cache, must make it.
368 
369  if(type_ == LOW_PASS && frequency < 1.0)
370  {
371  // create no pass filter
372  memset(new_kernel.b_, 0, sizeof(float64) * (n_poles_ + 1));
373  memset(new_kernel.a_, 0, sizeof(float64) * (n_poles_ + 1));
374  }
375 
376  else if(type_ == LOW_PASS && frequency >= (sample_rate_ / 2.0))
377  {
378  // create all pass
379  memset(new_kernel.b_, 0, sizeof(float64) * (n_poles_ + 1));
380  memset(new_kernel.a_, 0, sizeof(float64) * (n_poles_ + 1));
381 
382  b_[0] = 1.0;
383  }
384 
385  else if(type_ == HIGH_PASS && frequency < 1.0)
386  {
387  // create all pass
388  memset(new_kernel.b_, 0, sizeof(float64) * (n_poles_ + 1));
389  memset(new_kernel.a_, 0, sizeof(float64) * (n_poles_ + 1));
390 
391  b_[0] = 1.0;
392  }
393 
394  else if(type_ == HIGH_PASS && frequency >= (sample_rate_ / 2.0))
395  {
396  // create no pass filter
397  memset(new_kernel.b_, 0, sizeof(float64) * (n_poles_ + 1));
398  memset(new_kernel.a_, 0, sizeof(float64) * (n_poles_ + 1));
399  }
400 
401  else
402  {
403 
404  float64 B[23];
405  float64 A[23];
406 
407  float64 TB[23];
408  float64 TA[23];
409 
410  memset(B,0,sizeof(float64) * 23);
411  memset(A,0,sizeof(float64) * 23);
412  memset(TB,0,sizeof(float64) * 23);
413  memset(TA,0,sizeof(float64) * 23);
414 
415  B[2] = A[2] = 1.0;
416 
417  for(uint32 p = 1; p <= n_poles_ / 2; ++p)
418  {
419  float64 b[3];
420  float64 a[3];
421 
422  makeIIRKernelHelper(frequency, b, a, p);
423 
424  memcpy(TB,B, sizeof(float64) * 23);
425  memcpy(TA,A, sizeof(float64) * 23);
426 
427  for(uint32 i = 2; i <= 22; ++i)
428  {
429  B[i] = b[0] * TB[i] + b[1] * TB[i - 1] + b[2] * TB[i - 2];
430  A[i] = TA[i] - a[1] * TA[i - 1] - a[2] * TA[i - 2];
431  }
432  }
433 
434  A[2] = 0.0;
435  for(uint32 i = 0; i <= 20; ++i)
436  {
437  B[i] = B[i + 2];
438  A[i] = -1.0 * A[i + 2];
439  }
440 
441  float64 sumB = 0.0;
442  float64 sumA = 0.0;
443 
444  for(int32 i = 0; i <= 22; ++i)
445  {
446  if(type_ == LOW_PASS)
447  {
448  sumB += B[i];
449  sumA += A[i];
450  }
451  else if(type_ == HIGH_PASS)
452  {
453  sumB += B[i] * pow(-1.0,i);
454  sumA += A[i] * pow(-1.0,i);
455  }
456  }
457  float64 gain = sumB / (1.0 - sumA);
458 
459  for(uint32 i = 0; i <= 22; ++i)
460  {
461  B[i] /= gain;
462  }
463 
464  memcpy(b_, B, sizeof(float64) * (n_poles_ + 1));
465  memcpy(a_, A, sizeof(float64) * (n_poles_ + 1));
466  }
467 
468  // Add the new kernel to the cache.
469  kernel_cache_.insert(new_kernel);
470 }
471 
472 void
475  const float64 & frequency,
476  float64 * b,
477  float64 * a,
478  uint32 p)
479 {
480  float64 n_poles = static_cast<float64>(n_poles_);
481 
482  float64 RP = -1.0 * cos(M_PI / (n_poles * 2.0) + (p - 1) * (M_PI / n_poles));
483  float64 IP = sin(M_PI / (n_poles * 2.0) + (p - 1) * (M_PI / n_poles));
484 
485  float64 ES = 0.0;
486  float64 VX = 0.0;
487  float64 KX = 0.0;
488 
489  if(percent_ripple_ != 0.0)
490  {
491  float64 temp = (1.0 / (1.0 - percent_ripple_));
492 
493  ES = sqrt(temp * temp - 1.0);
494 
495  VX = (1.0 / n_poles) * log((1.0 / ES) + sqrt((1.0 /(ES * ES)) + 1.0));
496  KX = (1.0 / n_poles) * log((1.0 / ES) + sqrt((1.0 /(ES * ES)) - 1.0));
497  KX = (exp(KX) + exp(-KX)) / 2.0;
498  RP = RP * ( (exp(VX) - exp(-1.0 * VX) ) / 2.0) / KX;
499  IP = IP * ( (exp(VX) + exp(-1.0 * VX) ) / 2.0) / KX;
500  }
501 
502  /* DEBUG
503  if(p == 1)
504  {
505  std::cout << endl
506  << "RP = " << RP << endl
507  << "IP = " << IP << endl
508  << "ES = " << ES << endl
509  << "VX = " << VX << endl
510  << "KX = " << KX << endl << endl;
511  }
512  */
513 
514 
515  float64 T = 2.0 * tan(0.5);
516  float64 W = 2.0 * M_PI * (frequency / static_cast<float64>(sample_rate_));
517  float64 M = RP * RP + IP * IP;
518  float64 D = 4.0 - (4.0 * RP * T) + (M * T * T);
519  float64 X0 = T * T / D;
520  float64 X1 = 2.0 * X0;
521  float64 X2 = X0;
522  float64 Y1 = (8.0 - 2.0 * M * T * T) / D;
523  float64 Y2 = (-4.0 - (4.0 * RP * T) - M * T * T) / D;
524 
525  /* DEBUG
526  if(p == 1)
527  {
528  std::cout << "T = " << T << endl
529  << "W = " << W << endl
530  << "M = " << M << endl
531  << "D = " << D << endl
532  << "X0 = " << X0 << endl
533  << "X1 = " << X1 << endl
534  << "X2 = " << X2 << endl
535  << "Y1 = " << Y1 << endl
536  << "Y2 = " << Y2 << endl << endl;
537  }
538  */
539 
540 
541  // Low Pass
542  float64 K = 0.0;
543 
544  if(type_ == LOW_PASS)
545  {
546  K = sin(0.5 - W / 2.0) / sin(0.5 + W / 2.0);
547  }
548 
549  // High Pass
550  else if(type_ == HIGH_PASS)
551  {
552  K = -1.0 * cos(W / 2.0 + 0.5) / cos(W / 2.0 - 0.5);
553  }
554 
555  D = 1.0 + Y1 * K - Y2 * K * K;
556  b[0] = (X0 - X1 * K + X2 * K * K) / D;
557  b[1] = (-2.0 * X0 *K + X1 + X1 * K * K - 2.0 * X2 * K) / D;
558  b[2] = (X0 * K * K - X1 * K + X2) / D;
559  a[1] = (2.0 * K + Y1 + Y1 * K * K - 2.0 * Y2 * K) / D;
560  a[2] = (-1.0 * K * K - Y1 * K + Y2) / D;
561 
562  /* DEBUG
563  if(p == 1)
564  {
565  std::cout << "W = " << W << endl
566  << "K = " << K << endl
567  << "D = " << D << endl
568  << "A0 = " << a[0] << endl
569  << "A1 = " << a[1] << endl
570  << "A2 = " << a[2] << endl
571  << "B1 = " << b[1] << endl
572  << "B2 = " << b[2] << endl << endl;
573  }
574  */
575 
576  if(type_ == HIGH_PASS)
577  {
578  b[1] = -b[1];
579  a[1] = -a[1];
580  }
581 
582  /* DEBUG
583  if(p == 1)
584  {
585  std::cout << "A0 = " << a[0] << endl
586  << "A1 = " << a[1] << endl
587  << "A2 = " << a[2] << endl
588  << "B1 = " << b[1] << endl
589  << "B2 = " << b[2] << endl << endl;
590  }
591  */
592 }
593 
594 //-----------------------------------------------------------------------------
596 Kernel::
597 Kernel(const uint32 & frequency)
598  :
599  b_(NULL),
600  a_(NULL),
601  frequency_(frequency)
602 {
603 }
604 
605 bool
607 Kernel::
608 operator<(const Kernel & rhs) const
609 {
610  return frequency_ < rhs.frequency_;
611 }
612 
unsigned int uint32
Definition: Nsound.h:153
AudioStream filter(const AudioStream &x)
Kernel(const uint32 &frequency)
A class for filtering audio in the frequecy domain.
#define M_PI
Definition: Nsound.h:121
bool operator<(const Kernel &rhs) const
A class to store calculated kernels.
double float64
Definition: Nsound.h:146
uint32 getLength() const
Returns the number of samples in the Buffer.
Definition: Buffer.h:587
FilterStageIIR & operator=(const FilterStageIIR &rhs)
Base class for IIR Filters, defines the interface.
Definition: Filter.h:49
iterator end()
Retruns the itreator at the end of the Buffer.
Definition: Buffer.h:348
FilterStageIIR(Type type, const float64 &sample_rate, uint32 n_poles, const float64 &frequency, const float64 &percent_ripple)
FloatVector::const_iterator const_iterator
Definition: Buffer.h:70
iterator begin()
Retruns the itreator at the start of the Buffer.
Definition: Buffer.h:285
AudioStream filter(const AudioStream &x)
Definition: Filter.cc:53
A Buffer for storing audio samples.
Definition: Buffer.h:60
signed int int32
Definition: Nsound.h:142
void makeIIRKernelHelper(const float64 &frequency, float64 *a, float64 *b, uint32 p)
void makeKernel(const float64 &frequency)
float64 sample_rate_
Definition: Filter.h:113