|
|
|
|
 |
Entries in this category
1 pole LPF for smooth parameter changes
1-RC and C filter
18dB/oct resonant 3 pole LPF with tanh() dist
1st and 2nd order pink noise filters
3 Band Equaliser
303 type filter with saturation
4th order Linkwitz-Riley filters
All-Pass Filters, a good explanation
Another 4-pole lowpass...
Bass Booster
Biquad C code
Biquad, Butterworth, Chebyshev N-order, M-channel optimized filters
Butterworth Optimized C++ Class
C++ class implementation of RBJ Filters
C-Weighed Filter
Cascaded resonant lp/hp filter
Cool Sounding Lowpass With Decibel Measured Resonance
DC filter
Delphi Class implementation of the RBJ filters
Digital RIAA equalization filter coefficients
Direct form II
Direct Form II biquad
Fast Downsampling With Antialiasing
Formant filter
frequency warped FIR lattice
Hilbert Filter Coefficient Calculation
Hiqh quality /2 decimators
Karlsen
Karlsen Fast Ladder
Lowpass filter for parameter edge filtering
LP and HP filter
LPF 24dB/Oct
MDCT and IMDCT based on FFTW3
Moog Filter
Moog VCF
Moog VCF, variation 1
Moog VCF, variation 2
Notch filter
One pole filter, LP and HP
One pole LP and HP
One pole, one zero LP/HP
One zero, LP/HP
One-Liner IIR Filters (1st order)
Output Limiter using Envelope Follower in C++
Peak/Notch filter
Perfect LP4 filter
Phase equalization
Pink noise filter
Plot Filter (Analyze filter characteristics)
Plotting R B-J Equalisers in Excel
Polyphase Filters
Polyphase Filters (Delphi)
Poor Man's FIWIZ
Prewarping
RBJ Audio-EQ-Cookbook
RBJ Audio-EQ-Cookbook
Remez Exchange Algorithm (Parks/McClellan)
Remez Remez (Parks/McClellan)
Resonant filter
Resonant IIR lowpass (12dB/oct)
Resonant low pass filter
Reverb Filter Generator
Simple biquad filter from apple.com
Simple Tilt equalizer
Spuc's open source filters
State variable
State Variable Filter (Chamberlin version)
State Variable Filter (Double Sampled, Stable)
Stilson's Moog filter code
Time domain convolution with O(n^log2(3))
Time domain convolution with O(n^log2(3))
Type : LPF 24dB/Oct
Various Biquad filters
Windowed Sinc FIR Generator
Zoelzer biquad filters
|
|
|
|
|
|
|
|
|
1 pole LPF for smooth parameter changes
Type : 1-pole LPF class
References : Posted by zioguido@gmail.com
Notes :
This is a very simple class that I'm using in my plugins for smoothing parameter changes that directly affect audio stream.
It's a 1-pole LPF, very easy on CPU.
Change the value of variable "a" (0~1) for slower or a faster response.
Of course you can also use it as a lowpass filter for audio signals.
Code :
class CParamSmooth
{
public:
CParamSmooth() { a = 0.99f; b = 1.f - a; z = 0; };
~CParamSmooth();
inline float Process(float in) { z = (in * b) + (z * a); return z; }
private:
float a, b, z;
};
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
1-RC and C filter
Type : Simple 2-pole LP
References : Posted by madbrain[AT]videotron[DOT]ca
Notes :
This filter is called 1-RC and C since it uses these two parameters. C and R correspond to raw cutoff and inverted resonance, and have a range from 0 to 1.
Code :
//Parameter calculation
//cutoff and resonance are from 0 to 127
c = pow(0.5, (128-cutoff) / 16.0);
r = pow(0.5, (resonance+24) / 16.0);
//Loop:
v0 = (1-r*c)*v0 - (c)*v1 + (c)*input;
v1 = (1-r*c)*v1 + (c)*v0;
output = v1;
2 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
1st and 2nd order pink noise filters
Type : Pink noise
References : Posted by umminger[AT]umminger[DOT]com
Notes :
Here are some new lower-order pink noise filter coefficients.
These have approximately equiripple error in decibels from 20hz to 20khz at a 44.1khz sampling rate.
1st order, ~ +/- 3 dB error (not recommended!)
num = [0.05338071119116 -0.03752455712906]
den = [1.00000000000000 -0.97712493947102]
2nd order, ~ +/- 0.9 dB error
num = [ 0.04957526213389 -0.06305581334498 0.01483220320740 ]
den = [ 1.00000000000000 -1.80116083982126 0.80257737639225 ]
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
3 Band Equaliser
References : Posted by Neil C
Notes :
Simple 3 band equaliser with adjustable low and high frequencies ...
Fairly fast algo, good quality output (seems to be accoustically transparent with all gains set to 1.0)
How to use ...
1. First you need to declare a state for your eq
EQSTATE eq;
2. Now initialise the state (we'll assume your output frequency is 48Khz)
set_3band_state(eq,880,5000,480000);
Your EQ bands are now as follows (approximatley!)
low band = 0Hz to 880Hz
mid band = 880Hz to 5000Hz
high band = 5000Hz to 24000Hz
3. Set the gains to some values ...
eq.lg = 1.5; // Boost bass by 50%
eq.mg = 0.75; // Cut mid by 25%
eq.hg = 1.0; // Leave high band alone
4. You can now EQ some samples
out_sample = do_3band(eq,in_sample)
Have fun and mail me if any problems ... etanza at lycos dot co dot uk
Neil C / Etanza Systems, 2006 :)
Code :
First the header file ....
//---------------------------------------------------------------------------
//
// 3 Band EQ :)
//
// EQ.H - Header file for 3 band EQ
//
// (c) Neil C / Etanza Systems / 2K6
//
// Shouts / Loves / Moans = etanza at lycos dot co dot uk
//
// This work is hereby placed in the public domain for all purposes, including
// use in commercial applications.
//
// The author assumes NO RESPONSIBILITY for any problems caused by the use of
// this software.
//
//----------------------------------------------------------------------------
#ifndef __EQ3BAND__
#define __EQ3BAND__
// ------------
//| Structures |
// ------------
typedef struct
{
// Filter #1 (Low band)
double lf; // Frequency
double f1p0; // Poles ...
double f1p1;
double f1p2;
double f1p3;
// Filter #2 (High band)
double hf; // Frequency
double f2p0; // Poles ...
double f2p1;
double f2p2;
double f2p3;
// Sample history buffer
double sdm1; // Sample data minus 1
double sdm2; // 2
double sdm3; // 3
// Gain Controls
double lg; // low gain
double mg; // mid gain
double hg; // high gain
} EQSTATE;
// ---------
//| Exports |
// ---------
extern void init_3band_state(EQSTATE* es, int lowfreq, int highfreq, int mixfreq);
extern double do_3band(EQSTATE* es, double sample);
#endif // #ifndef __EQ3BAND__
//---------------------------------------------------------------------------
Now the source ...
//----------------------------------------------------------------------------
//
// 3 Band EQ :)
//
// EQ.C - Main Source file for 3 band EQ
//
// (c) Neil C / Etanza Systems / 2K6
//
// Shouts / Loves / Moans = etanza at lycos dot co dot uk
//
// This work is hereby placed in the public domain for all purposes, including
// use in commercial applications.
//
// The author assumes NO RESPONSIBILITY for any problems caused by the use of
// this software.
//
//----------------------------------------------------------------------------
// NOTES :
//
// - Original filter code by Paul Kellet (musicdsp.pdf)
//
// - Uses 4 first order filters in series, should give 24dB per octave
//
// - Now with P4 Denormal fix :)
//----------------------------------------------------------------------------
// ----------
//| Includes |
// ----------
#include <math.h>
#include "eq.h"
// -----------
//| Constants |
// -----------
static double vsa = (1.0 / 4294967295.0); // Very small amount (Denormal Fix)
// ---------------
//| Initialise EQ |
// ---------------
// Recommended frequencies are ...
//
// lowfreq = 880 Hz
// highfreq = 5000 Hz
//
// Set mixfreq to whatever rate your system is using (eg 48Khz)
void init_3band_state(EQSTATE* es, int lowfreq, int highfreq, int mixfreq)
{
// Clear state
memset(es,0,sizeof(EQSTATE));
// Set Low/Mid/High gains to unity
es->lg = 1.0;
es->mg = 1.0;
es->hg = 1.0;
// Calculate filter cutoff frequencies
es->lf = 2 * sin(M_PI * ((double)lowfreq / (double)mixfreq));
es->hf = 2 * sin(M_PI * ((double)highfreq / (double)mixfreq));
}
// ---------------
//| EQ one sample |
// ---------------
// - sample can be any range you like :)
//
// Note that the output will depend on the gain settings for each band
// (especially the bass) so may require clipping before output, but you
// knew that anyway :)
double do_3band(EQSTATE* es, double sample)
{
// Locals
double l,m,h; // Low / Mid / High - Sample Values
// Filter #1 (lowpass)
es->f1p0 += (es->lf * (sample - es->f1p0)) + vsa;
es->f1p1 += (es->lf * (es->f1p0 - es->f1p1));
es->f1p2 += (es->lf * (es->f1p1 - es->f1p2));
es->f1p3 += (es->lf * (es->f1p2 - es->f1p3));
l = es->f1p3;
// Filter #2 (highpass)
es->f2p0 += (es->hf * (sample - es->f2p0)) + vsa;
es->f2p1 += (es->hf * (es->f2p0 - es->f2p1));
es->f2p2 += (es->hf * (es->f2p1 - es->f2p2));
es->f2p3 += (es->hf * (es->f2p2 - es->f2p3));
h = es->sdm3 - es->f2p3;
// Calculate midrange (signal - (low + high))
m = es->sdm3 - (h + l);
// Scale, Combine and store
l *= es->lg;
m *= es->mg;
h *= es->hg;
// Shuffle history buffer
es->sdm3 = es->sdm2;
es->sdm2 = es->sdm1;
es->sdm1 = sample;
// Return result
return(l + m + h);
}
//----------------------------------------------------------------------------
11 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
303 type filter with saturation
Type : Runge-Kutta Filters
References : Posted by Hans Mikelson
Linked file : filters001.txt
Notes :
I posted a filter to the Csound mailing list a couple of weeks ago that has a 303 flavor to it. It basically does wacky distortions to the sound. I used Runge-Kutta for the diff eq. simulation though which makes it somewhat sluggish.
This is a CSound score!!
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
4th order Linkwitz-Riley filters
Type : LP/HP - LR4
References : Posted by neolit123 at gmail dot com
Notes :
Original from T. Lossius - ttblue project
Optimized version in pseudo-code.
[! The filter is unstable for fast automation changes in the lower frequency range. Parameter interpolation and/or oversampling should fix this. !]
The sum of the Linkwitz-Riley (Butterworth squared) HP and LP outputs, will result an all-pass filter at Fc and flat magnitude response - close to ideal for crossovers.
Lubomir I. Ivanov
Code :
//-----------------------------------------
// [code]
//-----------------------------------------
//fc -> cutoff frequency
//pi -> 3.14285714285714
//srate -> sample rate
//================================================
// shared for both lp, hp; optimizations here
//================================================
wc=2*pi*fc;
wc2=wc*wc;
wc3=wc2*wc;
wc4=wc2*wc2;
k=wc/tan(pi*fc/srate);
k2=k*k;
k3=k2*k;
k4=k2*k2;
sqrt2=sqrt(2);
sq_tmp1=sqrt2*wc3*k;
sq_tmp2=sqrt2*wc*k3;
a_tmp=4*wc2*k2+2*sq_tmp1+k4+2*sq_tmp2+wc4;
b1=(4*(wc4+sq_tmp1-k4-sq_tmp2))/a_tmp;
b2=(6*wc4-8*wc2*k2+6*k4)/a_tmp;
b3=(4*(wc4-sq_tmp1+sq_tmp2-k4))/a_tmp;
b4=(k4-2*sq_tmp1+wc4-2*sq_tmp2+4*wc2*k2)/a_tmp;
//================================================
// low-pass
//================================================
a0=wc4/a_tmp;
a1=4*wc4/a_tmp;
a2=6*wc4/a_tmp;
a3=a1;
a4=a0;
//=====================================================
// high-pass
//=====================================================
a0=k4/a_tmp;
a1=-4*k4/a_tmp;
a2=6*k4/a_tmp;
a3=a1;
a4=a0;
//=====================================================
// sample loop - same for lp, hp
//=====================================================
tempx=input;
tempy=a0*tempx+a1*xm1+a2*xm2+a3*xm3+a4*xm4-b1*ym1-b2*ym2-b3*ym3-b4*ym4;
xm4=xm3;
xm3=xm2;
xm2=xm1;
xm1=tempx;
ym4=ym3;
ym3=ym2;
ym2=ym1;
ym1=tempy;
output=tempy;
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Another 4-pole lowpass...
Type : 4-pole LP/HP
References : Posted by fuzzpilz [AT] gmx [DOT] net
Notes :
Vaguely based on the Stilson/Smith Moog paper, but going in a rather different direction from others I've seen here.
The parameters are peak frequency and peak magnitude (g below); both are reasonably accurate for magnitudes above 1. DC gain is 1.
The filter has some undesirable properties - e.g. it's unstable for low peak freqs if implemented in single precision (haven't been able to cleanly separate it into biquads or onepoles to see if that helps), and it responds so strongly to parameter changes that it's not advisable to update the coefficients much more rarely than, say, every eight samples during sweeps, which makes it somewhat expensive.
I like the sound, however, and the accuracy is nice to have, since many filters are not very strong in that respect.
I haven't looked at the HP again for a while, but IIRC it had approximately the same good and bad sides.
Code :
double coef[9];
double d[4];
double omega; //peak freq
double g; //peak mag
// calculating coefficients:
double k,p,q,a;
double a0,a1,a2,a3,a4;
k=(4.0*g-3.0)/(g+1.0);
p=1.0-0.25*k;p*=p;
// LP:
a=1.0/(tan(0.5*omega)*(1.0+p));
p=1.0+a;
q=1.0-a;
a0=1.0/(k+p*p*p*p);
a1=4.0*(k+p*p*p*q);
a2=6.0*(k+p*p*q*q);
a3=4.0*(k+p*q*q*q);
a4= (k+q*q*q*q);
p=a0*(k+1.0);
coef[0]=p;
coef[1]=4.0*p;
coef[2]=6.0*p;
coef[3]=4.0*p;
coef[4]=p;
coef[5]=-a1*a0;
coef[6]=-a2*a0;
coef[7]=-a3*a0;
coef[8]=-a4*a0;
// or HP:
a=tan(0.5*omega)/(1.0+p);
p=a+1.0;
q=a-1.0;
a0=1.0/(p*p*p*p+k);
a1=4.0*(p*p*p*q-k);
a2=6.0*(p*p*q*q+k);
a3=4.0*(p*q*q*q-k);
a4= (q*q*q*q+k);
p=a0*(k+1.0);
coef[0]=p;
coef[1]=-4.0*p;
coef[2]=6.0*p;
coef[3]=-4.0*p;
coef[4]=p;
coef[5]=-a1*a0;
coef[6]=-a2*a0;
coef[7]=-a3*a0;
coef[8]=-a4*a0;
// per sample:
out=coef[0]*in+d[0];
d[0]=coef[1]*in+coef[5]*out+d[1];
d[1]=coef[2]*in+coef[6]*out+d[2];
d[2]=coef[3]*in+coef[7]*out+d[3];
d[3]=coef[4]*in+coef[8]*out;
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Bass Booster
Type : LP and SUM
References : Posted by Johny Dupej
Notes :
This function adds a low-passed signal to the original signal. The low-pass has a quite wide response.
Params:
selectivity - frequency response of the LP (higher value gives a steeper one) [70.0 to 140.0 sounds good]
ratio - how much of the filtered signal is mixed to the original
gain2 - adjusts the final volume to handle cut-offs (might be good to set dynamically)
Code :
#define saturate(x) __min(__max(-1.0,x),1.0)
float BassBoosta(float sample)
{
static float selectivity, gain1, gain2, ratio, cap;
gain1 = 1.0/(selectivity + 1.0);
cap= (sample + cap*selectivity )*gain1;
sample = saturate((sample + cap*ratio)*gain2);
return sample;
}
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Biquad, Butterworth, Chebyshev N-order, M-channel optimized filters
Type : LP, HP, BP, BS, Shelf, Notch, Boost
References : Posted by thevinn at yahoo dot com
Code :
/*
"A Collection of Useful C++ Classes for Digital Signal Processing"
By Vincent Falco
Please direct all comments to either the music-dsp mailing list or
the DSP and Plug-in Development forum:
http://music.columbia.edu/cmc/music-dsp/
http://www.kvraudio.com/forum/viewforum.php?f=33
http://www.kvraudio.com/forum/
Support is provided for performing N-order Dsp floating point filter
operations on M-channel data with a caller specified floating point type.
The implementation breaks a high order IIR filter down into a series of
cascaded second order stages. Tests conclude that numerical stability is
maintained even at higher orders. For example the Butterworth low pass
filter is stable at up to 53 poles.
Processing functions are provided to use either Direct Form I or Direct
Form II of the filter transfer function. Direct Form II is slightly faster
but can cause discontinuities in the output if filter parameters are changed
during processing. Direct Form I is slightly slower, but maintains fidelity
even when parameters are changed during processing.
To support fast parameter changes, filters provide two functions for
adjusting parameters. A high accuracy Setup() function, and a faster
form called SetupFast() that uses approximations for trigonometric
functions. The approximations work quite well and should be suitable for
most applications.
Channels are stored in an interleaved format with M samples per frame
arranged contiguously. A single class instance can process all M channels
simultaneously in an efficient manner. A 'skip' parameter causes the
processing function to advance by skip additional samples in the destination
buffer in between every frame. Through manipulation of the skip paramter it
is possible to exclude channels from processing (for example, only processing
the left half of stereo interleaved data). For multichannel data which is
not interleaved, it will be necessary to instantiate multiple instance of
the filter and set skip=0.
There are a few other utility classes and functions included that may prove useful.
Classes:
Complex
CascadeStages
Biquad
BiquadLowPass
BiquadHighPass
BiquadBandPass1
BiquadBandPass2
BiquadBandStop
BiquadAllPass
BiquadPeakEq
BiquadLowShelf
BiquadHighShelf
PoleFilter
Butterworth
ButterLowPass
ButterHighPass
ButterBandPass
ButterBandStop
Chebyshev1
Cheby1LowPass
Cheby1HighPass
Cheby1BandPass
Cheby1BandStop
Chebyshev2
Cheby2LowPass
Cheby2HighPass
Cheby2BandPass
Cheby2BandStop
EnvelopeFollower
AutoLimiter
Functions:
zero()
copy()
mix()
scale()
interleave()
deinterleave()
Order for PoleFilter derived classes is specified in the number of poles,
except for band pass and band stop filters, for which the number of pole pairs
is specified.
For some filters there are two versions of Setup(), the one called
SetupFast() uses approximations to trigonometric functions for speed.
This is an option if you are doing frequent parameter changes to the filter.
There is an example function at the bottom that shows how to use the classes.
Filter ideas are based on a java applet (http://www.falstad.com/dfilter/)
developed by Paul Falstad.
All of this code was written by the author Vincent Falco except where marked.
--------------------------------------------------------------------------------
License: MIT License (http://www.opensource.org/licenses/mit-license.php)
Copyright (c) 2009 by Vincent Falco
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*
*/
/*
To Do:
- Shelving, peak, all-pass for Butterworth, Chebyshev, and Elliptic.
The Biquads have these versions and I would like the others to have them
as well. It would also be super awesome if higher order filters could
have a "Q" parameter for resonance but I'm not expecting miracles, it
would require redistributing the poles and zeroes. But if there's
a research paper or code out there...I could incorporate it.
- Denormal testing and fixing
I'd like to know if denormals are a problem. And if so, it would be nice
to have a small function that can reproduce the denormal problem. This
way I can test the fix under all conditions. I will include the function
as a "unit test" object in the header file so anyone can verify its
correctness. But I'm a little lost.
- Optimized template specializations for stages=1, channels={1,2}
There are some pretty obvious optimizations I am saving for "the end".
I don't want to do them until the code is finalized.
- Optimized template specializations for SSE, other special instructions
- Optimized trigonometric functions for fast parameter changes
- Elliptic curve based filter coefficients
- More utility functions for manipulating sample buffers
- Need fast version of pow( 10, x )
*/
#ifndef __DSP_FILTER__
#define __DSP_FILTER__
#include <cmath>
#include <cfloat>
#include <assert.h>
#include <memory.h>
#include <stdlib.h>
//#define DSP_USE_STD_COMPLEX
#ifdef DSP_USE_STD_COMPLEX
#include <complex>
#endif
#define DSP_SSE3_OPTIMIZED
#ifdef DSP_SSE3_OPTIMIZED
//#include <xmmintrin.h>
//#include <emmintrin.h>
#include <pmmintrin.h>
#endif
namespace Dsp
{
//--------------------------------------------------------------------------
// WARNING: Here there be templates
//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
//
// Configuration
//
//--------------------------------------------------------------------------
// Regardless of the type of sample that the filter operates on (e.g.
// float or double), all calculations are performed using double (or
// better) for stability and accuracy. This controls the underlying
// type used for calculations:
typedef double CalcT;
typedef int Int32; // Must be 32 bits
typedef __int64 Int64; // Must be 64 bits
// This is used to prevent denormalization.
const CalcT vsa=1.0 / 4294967295.0; // for CalcT as float
// These constants are so important, I made my own copy. If you improve
// the resolution of CalcT be sure to add more significant digits to these.
const CalcT kPi =3.1415926535897932384626433832795;
const CalcT kPi_2 =1.57079632679489661923;
const CalcT kLn2 =0.693147180559945309417;
const CalcT kLn10 =2.30258509299404568402;
//--------------------------------------------------------------------------
// Some functions missing from <math.h>
template<typename T>
inline T acosh( T x )
{
return log( x+::sqrt(x*x-1) );
}
//--------------------------------------------------------------------------
//
// Fast Trigonometric Functions
//
//--------------------------------------------------------------------------
// Three approximations for both sine and cosine at a given angle.
// The faster the routine, the larger the error.
// From http://lab.polygonal.de/2007/07/18/fast-and-accurate-sinecosine-approximation/
// Tuned for maximum pole stability. r must be in the range 0..pi
// This one breaks down considerably at the higher angles. It is
// included only for educational purposes.
inline void fastestsincos( CalcT r, CalcT *sn, CalcT *cs )
{
const CalcT c=0.70710678118654752440; // sqrt(2)/2
CalcT v=(2-4*c)*r*r+c;
if(r<kPi_2)
{
*sn=v+r; *cs=v-r;
}
else
{
*sn=r+v; *cs=r-v;
}
}
// Lower precision than ::fastsincos() but still decent
inline void fastersincos( CalcT x, CalcT *sn, CalcT *cs )
{
//always wrap input angle to -PI..PI
if (x < -kPi) x += 2*kPi;
else if (x > kPi) x -= 2*kPi;
//compute sine
if (x < 0) *sn = 1.27323954 * x + 0.405284735 * x * x;
else *sn = 1.27323954 * x - 0.405284735 * x * x;
//compute cosine: sin(x + PI/2) = cos(x)
x += kPi_2;
if (x > kPi ) x -= 2*kPi;
if (x < 0) *cs = 1.27323954 * x + 0.405284735 * x * x;
else *cs = 1.27323954 * x - 0.405284735 * x * x;
}
// Slower than ::fastersincos() but still faster than
// sin(), and with the best accuracy of these routines.
inline void fastsincos( CalcT x, CalcT *sn, CalcT *cs )
{
CalcT s, c;
//always wrap input angle to -PI..PI
if (x < -kPi) x += 2*kPi;
else if (x > kPi) x -= 2*kPi;
//compute sine
if (x < 0)
{
s = 1.27323954 * x + .405284735 * x * x;
if (s < 0) s = .225 * (s * -s - s) + s;
else s = .225 * (s * s - s) + s;
}
else
{
s = 1.27323954 * x - 0.405284735 * x * x;
if (s < 0) s = .225 * (s * -s - s) + s;
else s = .225 * (s * s - s) + s;
}
*sn=s;
//compute cosine: sin(x + PI/2) = cos(x)
x += kPi_2;
if (x > kPi ) x -= 2*kPi;
if (x < 0)
{
c = 1.27323954 * x + 0.405284735 * x * x;
if (c < 0) c = .225 * (c * -c - c) + c;
else c = .225 * (c * c - c) + c;
}
else
{
c = 1.27323954 * x - 0.405284735 * x * x;
if (c < 0) c = .225 * (c * -c - c) + c;
else c = .225 * (c * c - c) + c;
}
*cs=c;
}
// Faster approximations to sqrt()
// From http://ilab.usc.edu/wiki/index.php/Fast_Square_Root
// The faster the routine, the more error in the approximation.
// Log Base 2 Approximation
// 5 times faster than sqrt()
inline float fastsqrt1( float x )
{
union { Int32 i; float x; } u;
u.x = x;
u.i = (Int32(1)<<29) + (u.i >> 1) - (Int32(1)<<22);
return u.x;
}
inline double fastsqrt1( double x )
{
union { Int64 i; double x; } u;
u.x = x;
u.i = (Int64(1)<<61) + (u.i >> 1) - (Int64(1)<<51);
return u.x;
}
// Log Base 2 Approximation with one extra Babylonian Step
// 2 times faster than sqrt()
inline float fastsqrt2( float x )
{
float v=fastsqrt1( x );
v = 0.5f * (v + x/v); // One Babylonian step
return v;
}
inline double fastsqrt2(const double x)
{
double v=fastsqrt1( x );
v = 0.5f * (v + x/v); // One Babylonian step
return v;
}
// Log Base 2 Approximation with two extra Babylonian Steps
// 50% faster than sqrt()
inline float fastsqrt3( float x )
{
float v=fastsqrt1( x );
v = v + x/v;
v = 0.25f* v + x/v; // Two Babylonian steps
return v;
}
inline double fastsqrt3(const double x)
{
double v=fastsqrt1( x );
v = v + x/v;
v = 0.25 * v + x/v; // Two Babylonian steps
return v;
}
//--------------------------------------------------------------------------
//
// Complex
//
//--------------------------------------------------------------------------
#ifdef DSP_USE_STD_COMPLEX
template<typename T>
inline std::complex<T> polar( const T &m, const T &a )
{
return std::polar( m, a );
}
template<typename T>
inline T norm( const std::complex<T> &c )
{
return std::norm( c );
}
template<typename T>
inline T abs( const std::complex<T> &c )
{
return std::abs(c);
}
template<typename T, typename To>
inline std::complex<T> addmul( const std::complex<T> &c, T v, const std::complex<To> &c1 )
{
return std::complex<T>( c.real()+v*c1.real(), c.imag()+v*c1.imag() );
}
template<typename T>
inline T arg( const std::complex<T> &c )
{
return std::arg( c );
}
template<typename T>
inline std::complex<T> recip( const std::complex<T> &c )
{
T n=1.0/Dsp::norm(c);
return std::complex<T>( n*c.real(), n*c.imag() );
}
template<typename T>
inline std::complex<T> sqrt( const std::complex<T> &c )
{
return std::sqrt( c );
}
typedef std::complex<CalcT> Complex;
#else
//--------------------------------------------------------------------------
// "Its always good to have a few extra wheels in case one goes flat."
template<typename T>
struct ComplexT
{
ComplexT();
ComplexT( T r_, T i_=0 );
template<typename To>
ComplexT( const ComplexT<To> &c );
T imag ( void ) const;
T real ( void ) const;
ComplexT & neg ( void );
ComplexT & conj ( void );
template<typename To>
ComplexT & add ( const ComplexT<To> &c );
template<typename To>
ComplexT & sub ( const ComplexT<To> &c );
template<typename To>
ComplexT & mul ( const ComplexT<To> &c );
template<typename To>
ComplexT & div ( const ComplexT<To> &c );
template<typename To>
ComplexT & addmul ( T v, const ComplexT<To> &c );
ComplexT operator- ( void ) const;
ComplexT operator+ ( T v ) const;
ComplexT operator- ( T v ) const;
ComplexT operator* ( T v ) const;
ComplexT operator/ ( T v ) const;
ComplexT & operator+= ( T v );
ComplexT & operator-= ( T v );
ComplexT & operator*= ( T v );
ComplexT & operator/= ( T v );
template<typename To>
ComplexT operator+ ( const ComplexT<To> &c ) const;
template<typename To>
ComplexT operator- ( const ComplexT<To> &c ) const;
template<typename To>
ComplexT operator* ( const ComplexT<To> &c ) const;
template<typename To>
ComplexT operator/ ( const ComplexT<To> &c ) const;
template<typename To>
ComplexT & operator+= ( const ComplexT<To> &c );
template<typename To>
ComplexT & operator-= ( const ComplexT<To> &c );
template<typename To>
ComplexT & operator*= ( const ComplexT<To> &c );
template<typename To>
ComplexT & operator/= ( const ComplexT<To> &c );
private:
ComplexT & add ( T v );
ComplexT & sub ( T v );
ComplexT & mul ( T c, T d );
ComplexT & mul ( T v );
ComplexT & div ( T v );
T r;
T i;
};
//--------------------------------------------------------------------------
template<typename T>
inline ComplexT<T>::ComplexT()
{
}
template<typename T>
inline ComplexT<T>::ComplexT( T r_, T i_ )
{
r=r_;
i=i_;
}
template<typename T>
template<typename To>
inline ComplexT<T>::ComplexT( const ComplexT<To> &c )
{
r=c.r;
i=c.i;
}
template<typename T>
inline T ComplexT<T>::imag( void ) const
{
return i;
}
template<typename T>
inline T ComplexT<T>::real( void ) const
{
return r;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::neg( void )
{
r=-r;
i=-i;
return *this;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::conj( void )
{
i=-i;
return *this;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::add( T v )
{
r+=v;
return *this;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::sub( T v )
{
r-=v;
return *this;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::mul( T c, T d )
{
T ac=r*c;
T bd=i*d;
// must do i first
i=(r+i)*(c+d)-(ac+bd);
r=ac-bd;
return *this;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::mul( T v )
{
r*=v;
i*=v;
return *this;
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::div( T v )
{
r/=v;
i/=v;
return *this;
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::add( const ComplexT<To> &c )
{
r+=c.r;
i+=c.i;
return *this;
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::sub( const ComplexT<To> &c )
{
r-=c.r;
i-=c.i;
return *this;
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::mul( const ComplexT<To> &c )
{
return mul( c.r, c.i );
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::div( const ComplexT<To> &c )
{
T s=1.0/norm(c);
return mul( c.r*s, -c.i*s );
}
template<typename T>
inline ComplexT<T> ComplexT<T>::operator-( void ) const
{
return ComplexT<T>(*this).neg();
}
template<typename T>
inline ComplexT<T> ComplexT<T>::operator+( T v ) const
{
return ComplexT<T>(*this).add( v );
}
template<typename T>
inline ComplexT<T> ComplexT<T>::operator-( T v ) const
{
return ComplexT<T>(*this).sub( v );
}
template<typename T>
inline ComplexT<T> ComplexT<T>::operator*( T v ) const
{
return ComplexT<T>(*this).mul( v );
}
template<typename T>
inline ComplexT<T> ComplexT<T>::operator/( T v ) const
{
return ComplexT<T>(*this).div( v );
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::operator+=( T v )
{
return add( v );
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::operator-=( T v )
{
return sub( v );
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::operator*=( T v )
{
return mul( v );
}
template<typename T>
inline ComplexT<T> &ComplexT<T>::operator/=( T v )
{
return div( v );
}
template<typename T>
template<typename To>
inline ComplexT<T> ComplexT<T>::operator+( const ComplexT<To> &c ) const
{
return ComplexT<T>(*this).add(c);
}
template<typename T>
template<typename To>
inline ComplexT<T> ComplexT<T>::operator-( const ComplexT<To> &c ) const
{
return ComplexT<T>(*this).sub(c);
}
template<typename T>
template<typename To>
inline ComplexT<T> ComplexT<T>::operator*( const ComplexT<To> &c ) const
{
return ComplexT<T>(*this).mul(c);
}
template<typename T>
template<typename To>
inline ComplexT<T> ComplexT<T>::operator/( const ComplexT<To> &c ) const
{
return ComplexT<T>(*this).div(c);
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::operator+=( const ComplexT<To> &c )
{
return add( c );
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::operator-=( const ComplexT<To> &c )
{
return sub( c );
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::operator*=( const ComplexT<To> &c )
{
return mul( c );
}
template<typename T>
template<typename To>
inline ComplexT<T> &ComplexT<T>::operator/=( const ComplexT<To> &c )
{
return div( c );
}
//--------------------------------------------------------------------------
template<typename T>
inline ComplexT<T> polar( const T &m, const T &a )
{
return ComplexT<T>( m*cos(a), m*sin(a) );
}
template<typename T>
inline T norm( const ComplexT<T> &c )
{
return c.real()*c.real()+c.imag()*c.imag();
}
template<typename T>
inline T abs( const ComplexT<T> &c )
{
return ::sqrt( c.real()*c.real()+c.imag()*c.imag() );
}
template<typename T, typename To>
inline ComplexT<T> addmul( const ComplexT<T> &c, T v, const ComplexT<To> &c1 )
{
return ComplexT<T>( c.real()+v*c1.real(), c.imag()+v*c1.imag() );
}
template<typename T>
inline T arg( const ComplexT<T> &c )
{
return atan2( c.imag(), c.real() );
}
template<typename T>
inline ComplexT<T> recip( const ComplexT<T> &c )
{
T n=1.0/norm(c);
return ComplexT<T>( n*c.real(), -n*c.imag() );
}
template<typename T>
inline ComplexT<T> sqrt( const ComplexT<T> &c )
{
return polar( ::sqrt(abs(c)), arg(c)*0.5 );
}
//--------------------------------------------------------------------------
typedef ComplexT<CalcT> Complex;
#endif
//--------------------------------------------------------------------------
//
// Numerical Analysis
//
//--------------------------------------------------------------------------
// Implementation of Brent's Method provided by
// John D. Cook (http://www.johndcook.com/)
// The return value of Minimize is the minimum of the function f.
// The location where f takes its minimum is returned in the variable minLoc.
// Notation and implementation based on Chapter 5 of Richard Brent's book
// "Algorithms for Minimization Without Derivatives".
template<class TFunction>
CalcT BrentMinimize
(
TFunction& f, // [in] objective function to minimize
CalcT leftEnd, // [in] smaller value of bracketing interval
CalcT rightEnd, // [in] larger value of bracketing interval
CalcT epsilon, // [in] stopping tolerance
CalcT& minLoc // [out] location of minimum
)
{
CalcT d, e, m, p, q, r, tol, t2, u, v, w, fu, fv, fw, fx;
static const CalcT c = 0.5*(3.0 - ::sqrt(5.0));
static const CalcT SQRT_DBL_EPSILON = ::sqrt(DBL_EPSILON);
CalcT& a = leftEnd; CalcT& b = rightEnd; CalcT& x = minLoc;
v = w = x = a + c*(b - a); d = e = 0.0;
fv = fw = fx = f(x);
int counter = 0;
loop:
counter++;
m = 0.5*(a + b);
tol = SQRT_DBL_EPSILON*::fabs(x) + epsilon; t2 = 2.0*tol;
// Check stopping criteria
if (::fabs(x - m) > t2 - 0.5*(b - a))
{
p = q = r = 0.0;
if (::fabs(e) > tol)
{
// fit parabola
r = (x - w)*(fx - fv);
q = (x - v)*(fx - fw);
p = (x - v)*q - (x - w)*r;
q = 2.0*(q - r);
(q > 0.0) ? p = -p : q = -q;
r = e; e = d;
}
if (::fabs(p) < ::fabs(0.5*q*r) && p < q*(a - x) && p < q*(b - x))
{
// A parabolic interpolation step
d = p/q;
u = x + d;
// f must not be evaluated too close to a or b
if (u - a < t2 || b - u < t2)
d = (x < m) ? tol : -tol;
}
else
{
// A golden section step
e = (x < m) ? b : a;
e -= x;
d = c*e;
}
// f must not be evaluated too close to x
if (::fabs(d) >= tol)
u = x + d;
else if (d > 0.0)
u = x + tol;
else
u = x - tol;
fu = f(u);
// Update a, b, v, w, and x
if (fu <= fx)
{
(u < x) ? b = x : a = x;
v = w; fv = fw;
w = x; fw = fx;
x = u; fx = fu;
}
else
{
(u < x) ? a = u : b = u;
if (fu <= fw || w == x)
{
v = w; fv = fw;
w = u; fw = fu;
}
else if (fu <= fv || v == x || v == w)
{
v = u; fv = fu;
}
}
goto loop; // Yes, the dreaded goto statement. But the code
// here is faithful to Brent's orginal pseudocode.
}
return fx;
}
//--------------------------------------------------------------------------
//
// Infinite Impulse Response Filters
//
//--------------------------------------------------------------------------
// IIR filter implementation using multiple second-order stages.
class CascadeFilter
{
public:
// Process data in place using Direct Form I
// skip is added after each frame.
// Direct Form I is more suitable when the filter parameters
// are changed often. However, it is slightly slower.
template<typename T>
void ProcessI( size_t frames, T *dest, int skip=0 );
// Process data in place using Direct Form II
// skip is added after each frame.
// Direct Form II is slightly faster than Direct Form I,
// but changing filter parameters on stream can result
// in discontinuities in the output. It is best suited
// for a filter whose parameters are set only once.
template<typename T>
void ProcessII( size_t frames, T *dest, int skip=0 );
// Convenience function that just calls ProcessI.
// Feel free to change the implementation.
template<typename T>
void Process( size_t frames, T *dest, int skip=0 );
// Determine response at angular frequency (0<=w<=kPi)
Complex Response( CalcT w );
// Clear the history buffer.
void Clear( void );
protected:
struct Hist;
struct Stage;
// for m_nchan==2
#ifdef DSP_SSE3_OPTIMIZED
template<typename T>
void ProcessISSEStageStereo( size_t frames, T *dest, Stage *stage, Hist *h, int skip );
template<typename T>
void ProcessISSEStereo( size_t frames, T *dest, int skip );
#endif
protected:
void Reset ( void );
void Normalize ( CalcT scale );
void SetAStage ( CalcT x1, CalcT x2 );
void SetBStage ( CalcT x0, CalcT x1, CalcT x2 );
void SetStage ( CalcT a1, CalcT a2, CalcT b0, CalcT b1, CalcT b2 );
protected:
struct Hist
{
CalcT v[4];
};
struct Stage
{
CalcT a[3]; // a[0] unused
CalcT b[3];
void Reset( void );
};
struct ResponseFunctor
{
CascadeFilter *m_filter;
CalcT operator()( CalcT w );
ResponseFunctor( CascadeFilter *filter );
};
int m_nchan;
int m_nstage;
Stage * m_stagep;
Hist * m_histp;
};
//--------------------------------------------------------------------------
template<typename T>
void CascadeFilter::ProcessI( size_t frames, T *dest, int skip )
{
#ifdef DSP_SSE3_OPTIMIZED
if( m_nchan==2 )
ProcessISSEStereo( frames, dest, skip );
else
#endif
while( frames-- )
{
Hist *h=m_histp;
for( int j=m_nchan;j;j-- )
{
CalcT in=CalcT(*dest);
Stage *s=m_stagep;
for( int i=m_nstage;i;i--,h++,s++ )
{
CalcT out;
out=s->b[0]*in + s->b[1]*h->v[0] + s->b[2]*h->v[1] +
s->a[1]*h->v[2] + s->a[2]*h->v[3];
h->v[1]=h->v[0]; h->v[0]=in;
h->v[3]=h->v[2]; h->v[2]=out;
in=out;
}
*dest++=T(in);
}
dest+=skip;
}
}
// A good compiler already produces code that is optimized even for
// the general case. The only way to make it go faster is to
// to implement it in assembler or special instructions. Like this:
#ifdef DSP_SSE3_OPTIMIZED
// ALL SSE OPTIMIZATIONS ASSUME CalcT as double
template<typename T>
inline void CascadeFilter::ProcessISSEStageStereo(
size_t frames, T *dest, Stage *s, Hist *h, int skip )
{
assert( m_nchan==2 );
#if 1
CalcT b0=s->b[0];
__m128d m0=_mm_loadu_pd( &s->a[1] ); // a1 , a2
__m128d m1=_mm_loadu_pd( &s->b[1] ); // b1 , b2
__m128d m2=_mm_loadu_pd( &h[0].v[0] ); // h->v[0] , h->v[1]
__m128d m3=_mm_loadu_pd( &h[0].v[2] ); // h->v[2] , h->v[3]
__m128d m4=_mm_loadu_pd( &h[1].v[0] ); // h->v[0] , h->v[1]
__m128d m5=_mm_loadu_pd( &h[1].v[2] ); // h->v[2] , h->v[3]
while( frames-- )
{
CalcT in, b0in, out;
__m128d m6;
__m128d m7;
in=CalcT(*dest);
b0in=b0*in;
m6=_mm_mul_pd ( m1, m2 ); // b1*h->v[0] , b2*h->v[1]
m7=_mm_mul_pd ( m0, m3 ); // a1*h->v[2] , a2*h->v[3]
m6=_mm_add_pd ( m6, m7 ); // b1*h->v[0] + a1*h->v[2], b2*h->v[1] + a2*h->v[3]
m7=_mm_load_sd( &b0in ); // b0*in , 0
m6=_mm_add_sd ( m6, m7 ); // b1*h->v[0] + a1*h->v[2] + in*b0 , b2*h->v[1] + a2*h->v[3] + 0
m6=_mm_hadd_pd( m6, m7 ); // b1*h->v[0] + a1*h->v[2] + in*b0 + b2*h->v[1] + a2*h->v[3], in*b0
_mm_store_sd( &out, m6 );
m6=_mm_loadh_pd( m6, &in ); // out , in
m2=_mm_shuffle_pd( m6, m2, _MM_SHUFFLE2( 0, 1 ) ); // h->v[0]=in , h->v[1]=h->v[0]
m3=_mm_shuffle_pd( m6, m3, _MM_SHUFFLE2( 0, 0 ) ); // h->v[2]=out, h->v[3]=h->v[2]
*dest++=T(out);
in=CalcT(*dest);
b0in=b0*in;
m6=_mm_mul_pd ( m1, m4 ); // b1*h->v[0] , b2*h->v[1]
m7=_mm_mul_pd ( m0, m5 ); // a1*h->v[2] , a2*h->v[3]
m6=_mm_add_pd ( m6, m7 ); // b1*h->v[0] + a1*h->v[2], b2*h->v[1] + a2*h->v[3]
m7=_mm_load_sd( &b0in ); // b0*in , 0
m6=_mm_add_sd ( m6, m7 ); // b1*h->v[0] + a1*h->v[2] + in*b0 , b2*h->v[1] + a2*h->v[3] + 0
m6=_mm_hadd_pd( m6, m7 ); // b1*h->v[0] + a1*h->v[2] + in*b0 + b2*h->v[1] + a2*h->v[3], in*b0
_mm_store_sd( &out, m6 );
m6=_mm_loadh_pd( m6, &in ); // out , in
m4=_mm_shuffle_pd( m6, m4, _MM_SHUFFLE2( 0, 1 ) ); // h->v[0]=in , h->v[1]=h->v[0]
m5=_mm_shuffle_pd( m6, m5, _MM_SHUFFLE2( 0, 0 ) ); // h->v[2]=out, h->v[3]=h->v[2]
*dest++=T(out);
dest+=skip;
}
// move history from registers back to state
_mm_storeu_pd( &h[0].v[0], m2 );
_mm_storeu_pd( &h[0].v[2], m3 );
_mm_storeu_pd( &h[1].v[0], m4 );
_mm_storeu_pd( &h[1].v[2], m5 );
#else
// Template-specialized version from which the assembly was modeled
CalcT a1=s->a[1];
CalcT a2=s->a[2];
CalcT b0=s->b[0];
CalcT b1=s->b[1];
CalcT b2=s->b[2];
while( frames-- )
{
CalcT in, out;
in=CalcT(*dest);
out=b0*in+b1*h[0].v[0]+b2*h[0].v[1] +a1*h[0].v[2]+a2*h[0].v[3];
h[0].v[1]=h[0].v[0]; h[0].v[0]=in;
h[0].v[3]=h[0].v[2]; h[0].v[2]=out;
in=out;
*dest++=T(in);
in=CalcT(*dest);
out=b0*in+b1*h[1].v[0]+b2*h[1].v[1] +a1*h[1].v[2]+a2*h[1].v[3];
h[1].v[1]=h[1].v[0]; h[1].v[0]=in;
h[1].v[3]=h[1].v[2]; h[1].v[2]=out;
in=out;
*dest++=T(in);
dest+=skip;
}
#endif
}
// Note there could be a loss of accuracy here. Unlike the original version
// of Process...() we are applying each stage to all of the input data.
// Since the underlying type T could be float, the results from this function
// may be different than the unoptimized version. However, it is much faster.
template<typename T>
void CascadeFilter::ProcessISSEStereo( size_t frames, T *dest, int skip )
{
assert( m_nchan==2 );
Stage *s=m_stagep;
Hist *h=m_histp;
for( int i=m_nstage;i;i--,h+=2,s++ )
{
ProcessISSEStageStereo( frames, dest, s, h, skip );
}
}
#endif
template<typename T>
void CascadeFilter::ProcessII( size_t frames, T *dest, int skip )
{
while( frames-- )
{
Hist *h=m_histp;
for( int j=m_nchan;j;j-- )
{
CalcT in=CalcT(*dest);
Stage *s=m_stagep;
for( int i=m_nstage;i;i--,h++,s++ )
{
CalcT d2=h->v[2]=h->v[1];
CalcT d1=h->v[1]=h->v[0];
CalcT d0=h->v[0]=
in+s->a[1]*d1 + s->a[2]*d2;
in=s->b[0]*d0 + s->b[1]*d1 + s->b[2]*d2;
}
*dest++=T(in);
}
dest+=skip;
}
}
template<typename T>
inline void CascadeFilter::Process( size_t frames, T *dest, int skip )
{
ProcessI( frames, dest, skip );
}
inline Complex CascadeFilter::Response( CalcT w )
{
Complex ch( 1 );
Complex cbot( 1 );
Complex czn1=polar( 1., -w );
Complex czn2=polar( 1., -2*w );
Stage *s=m_stagep;
for( int i=m_nstage;i;i-- )
{
Complex ct( s->b[0] );
Complex cb( 1 );
ct=addmul( ct, s->b[1], czn1 );
cb=addmul( cb, -s->a[1], czn1 );
ct=addmul( ct, s->b[2], czn2 );
cb=addmul( cb, -s->a[2], czn2 );
ch*=ct;
cbot*=cb;
s++;
}
return ch/cbot;
}
inline void CascadeFilter::Clear( void )
{
::memset( m_histp, 0, m_nstage*m_nchan*sizeof(m_histp[0]) );
}
inline void CascadeFilter::Stage::Reset( void )
{
a[1]=0; a[2]=0;
b[0]=1; b[1]=0; b[2]=0;
}
inline void CascadeFilter::Reset( void )
{
Stage *s=m_stagep;
for( int i=m_nstage;i;i--,s++ )
s->Reset();
}
// Apply scale factor to stage coefficients.
inline void CascadeFilter::Normalize( CalcT scale )
{
// We are throwing the normalization into the first
// stage. In theory it might be nice to spread it around
// to preserve numerical accuracy.
Stage *s=m_stagep;
s->b[0]*=scale; s->b[1]*=scale; s->b[2]*=scale;
}
inline void CascadeFilter::SetAStage( CalcT x1, CalcT x2 )
{
Stage *s=m_stagep;
for( int i=m_nstage;i;i-- )
{
if( s->a[1]==0 && s->a[2]==0 )
{
s->a[1]=x1;
s->a[2]=x2;
s=0;
break;
}
if( s->a[2]==0 && x2==0 )
{
s->a[2]=-s->a[1]*x1;
s->a[1]+=x1;
s=0;
break;
}
s++;
}
assert( s==0 );
}
inline void CascadeFilter::SetBStage( CalcT x0, CalcT x1, CalcT x2 )
{
Stage *s=m_stagep;
for( int i=m_nstage;i;i-- )
{
if( s->b[1]==0 && s->b[2]==0 )
{
s->b[0]=x0;
s->b[1]=x1;
s->b[2]=x2;
s=0;
break;
}
if( s->b[2]==0 && x2==0 )
{
// (b0 + z b1)(x0 + z x1) = (b0 x0 + (b1 x0+b0 x1) z + b1 x1 z^2)
s->b[2]=s->b[1]*x1;
s->b[1]=s->b[1]*x0+s->b[0]*x1;
s->b[0]*=x0;
s=0;
break;
}
s++;
}
assert( s==0 );
}
// Optimized version for Biquads
inline void CascadeFilter::SetStage(
CalcT a1, CalcT a2, CalcT b0, CalcT b1, CalcT b2 )
{
assert( m_nstage==1 );
Stage *s=&m_stagep[0];
s->a[1]=a1; s->a[2]=a2;
s->b[0]=b0; s->b[1]=b1; s->b[2]=b2;
}
inline CalcT CascadeFilter::ResponseFunctor::operator()( CalcT w )
{
return -Dsp::abs(m_filter->Response( w ));
}
inline CascadeFilter::ResponseFunctor::ResponseFunctor( CascadeFilter *filter )
{
m_filter=filter;
}
//--------------------------------------------------------------------------
template<int stages, int channels>
class CascadeStages : public CascadeFilter
{
public:
CascadeStages();
private:
Hist m_hist [stages*channels];
Stage m_stage [stages];
};
//--------------------------------------------------------------------------
template<int stages, int channels>
CascadeStages<stages, channels>::CascadeStages( void )
{
m_nchan=channels;
m_nstage=stages;
m_stagep=m_stage;
m_histp=m_hist;
Clear();
}
//--------------------------------------------------------------------------
//
// Biquad Second Order IIR Filters
//
//--------------------------------------------------------------------------
// Formulas from http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
template<int channels>
class Biquad : public CascadeStages<1, channels>
{
protected:
void Setup( const CalcT a[3], const CalcT b[3] );
};
//--------------------------------------------------------------------------
template<int channels>
inline void Biquad<channels>::Setup( const CalcT a[3], const CalcT b[3] )
{
Reset();
// transform Biquad coefficients
CalcT ra0=1/a[0];
SetAStage( -a[1]*ra0, -a[2]*ra0 );
SetBStage( b[0]*ra0, b[1]*ra0, b[2]*ra0 );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadLowPass : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT q );
void SetupFast ( CalcT normFreq, CalcT q );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT q );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadLowPass<channels>::SetupCommon( CalcT sn, CalcT cs, CalcT q )
{
CalcT alph = sn / ( 2 * q );
CalcT a0 = 1 / ( 1 + alph );
CalcT b1 = 1 - cs;
CalcT b0 = a0 * b1 * 0.5;
CalcT a1 = 2 * cs;
CalcT a2 = alph - 1;
SetStage( a1*a0, a2*a0, b0, b1*a0, b0 );
}
template<int channels>
void BiquadLowPass<channels>::Setup( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
SetupCommon( sn, cs, q );
}
template<int channels>
void BiquadLowPass<channels>::SetupFast( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
SetupCommon( sn, cs, q );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadHighPass : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT q );
void SetupFast ( CalcT normFreq, CalcT q );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT q );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadHighPass<channels>::SetupCommon( CalcT sn, CalcT cs, CalcT q )
{
CalcT alph = sn / ( 2 * q );
CalcT a0 = -1 / ( 1 + alph );
CalcT b1 = -( 1 + cs );
CalcT b0 = a0 * b1 * -0.5;
CalcT a1 = -2 * cs;
CalcT a2 = 1 - alph;
SetStage( a1*a0, a2*a0, b0, b1*a0, b0 );
}
template<int channels>
void BiquadHighPass<channels>::Setup( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
SetupCommon( sn, cs, q );
}
template<int channels>
void BiquadHighPass<channels>::SetupFast( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
SetupCommon( sn, cs, q );
}
//--------------------------------------------------------------------------
// Constant skirt gain, peak gain=Q
template<int channels>
class BiquadBandPass1 : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT q );
void SetupFast ( CalcT normFreq, CalcT q );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT q );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadBandPass1<channels>::SetupCommon( CalcT sn, CalcT cs, CalcT q )
{
CalcT alph = sn / ( 2 * q );
CalcT a0 = -1 / ( 1 + alph );
CalcT b0 = a0 * ( sn * -0.5 );
CalcT a1 = -2 * cs;
CalcT a2 = 1 - alph;
SetStage( a1*a0, a2*a0, b0, 0, -b0 );
}
template<int channels>
void BiquadBandPass1<channels>::Setup( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
SetupCommon( sn, cs, q );
}
template<int channels>
void BiquadBandPass1<channels>::SetupFast( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
SetupCommon( sn, cs, q );
}
//--------------------------------------------------------------------------
// Constant 0dB peak gain
template<int channels>
class BiquadBandPass2 : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT q );
void SetupFast ( CalcT normFreq, CalcT q );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT q );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadBandPass2<channels>::SetupCommon( CalcT sn, CalcT cs, CalcT q )
{
CalcT alph = sn / ( 2 * q );
CalcT b0 = -alph;
CalcT b2 = alph;
CalcT a0 = -1 / ( 1 + alph );
CalcT a1 = -2 * cs;
CalcT a2 = 1 - alph;
SetStage( a1*a0, a2*a0, b0*a0, 0, b2*a0 );
}
template<int channels>
void BiquadBandPass2<channels>::Setup( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
SetupCommon( sn, cs, q );
}
template<int channels>
void BiquadBandPass2<channels>::SetupFast( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
SetupCommon( sn, cs, q );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadBandStop : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT q );
void SetupFast ( CalcT normFreq, CalcT q );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT q );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadBandStop<channels>::SetupCommon( CalcT sn, CalcT cs, CalcT q )
{
CalcT alph = sn / ( 2 * q );
CalcT a0 = 1 / ( 1 + alph );
CalcT b1 = a0 * ( -2 * cs );
CalcT a2 = alph - 1;
SetStage( -b1, a2*a0, a0, b1, a0 );
}
template<int channels>
void BiquadBandStop<channels>::Setup( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
SetupCommon( sn, cs, q );
}
template<int channels>
void BiquadBandStop<channels>::SetupFast( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
SetupCommon( sn, cs, q );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadAllPass: public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT q );
void SetupFast ( CalcT normFreq, CalcT q );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT q );
};
//--------------------------------------------------------------------------
template<int channels>
void BiquadAllPass<channels>::SetupCommon( CalcT sn, CalcT cs, CalcT q )
{
CalcT alph = sn / ( 2 * q );
CalcT b2 = 1 + alph;
CalcT a0 = 1 / b2;
CalcT b0 =( 1 - alph ) * a0;
CalcT b1 = -2 * cs * a0;
SetStage( -b1, -b0, b0, b1, b2*a0 );
}
template<int channels>
void BiquadAllPass<channels>::Setup( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
SetupCommon( sn, cs, q );
}
template<int channels>
void BiquadAllPass<channels>::SetupFast( CalcT normFreq, CalcT q )
{
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
SetupCommon( sn, cs, q );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadPeakEq: public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT dB, CalcT bandWidth );
void SetupFast ( CalcT normFreq, CalcT dB, CalcT bandWidth );
protected:
void SetupCommon ( CalcT sn, CalcT cs, CalcT alph, CalcT A );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadPeakEq<channels>::SetupCommon(
CalcT sn, CalcT cs, CalcT alph, CalcT A )
{
CalcT t=alph*A;
CalcT b0 = 1 - t;
CalcT b2 = 1 + t;
t=alph/A;
CalcT a0 = 1 / ( 1 + t );
CalcT a2 = t - 1;
CalcT b1 = a0 * ( -2 * cs );
CalcT a1 = -b1;
SetStage( a1, a2*a0, b0*a0, b1, b2*a0 );
}
template<int channels>
void BiquadPeakEq<channels>::Setup( CalcT normFreq, CalcT dB, CalcT bandWidth )
{
CalcT A = pow( 10, dB/40 );
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
CalcT alph = sn * sinh( kLn2/2 * bandWidth * w0/sn );
SetupCommon( sn, cs, alph, A );
}
template<int channels>
void BiquadPeakEq<channels>::SetupFast( CalcT normFreq, CalcT dB, CalcT bandWidth )
{
CalcT A = pow( 10, dB/40 );
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
CalcT alph = sn * sinh( kLn2/2 * bandWidth * w0/sn );
SetupCommon( sn, cs, alph, A );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadLowShelf : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT dB, CalcT shelfSlope=1.0 );
void SetupFast ( CalcT normFreq, CalcT dB, CalcT shelfSlope=1.0 );
protected:
void SetupCommon ( CalcT cs, CalcT A, CalcT sa );
};
//--------------------------------------------------------------------------
template<int channels>
inline void BiquadLowShelf<channels>::SetupCommon(
CalcT cs, CalcT A, CalcT sa )
{
CalcT An = A-1;
CalcT Ap = A+1;
CalcT Ancs = An*cs;
CalcT Apcs = Ap*cs;
CalcT b0 = A * (Ap - Ancs + sa );
CalcT b2 = A * (Ap - Ancs - sa );
CalcT b1 = 2 * A * (An - Apcs);
CalcT a2 = sa - (Ap + Ancs);
CalcT a0 = 1 / (Ap + Ancs + sa );
CalcT a1 = 2 * (An + Apcs);
SetStage( a1*a0, a2*a0, b0*a0, b1*a0, b2*a0 );
}
template<int channels>
void BiquadLowShelf<channels>::Setup( CalcT normFreq, CalcT dB, CalcT shelfSlope )
{
CalcT A = pow( 10, dB/40 );
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
CalcT al = sn / 2 * ::sqrt( (A + 1/A) * (1/shelfSlope - 1) + 2 );
CalcT sa = 2 * ::sqrt( A ) * al;
SetupCommon( cs, A, sa );
}
// This could be optimized further
template<int channels>
void BiquadLowShelf<channels>::SetupFast( CalcT normFreq, CalcT dB, CalcT shelfSlope )
{
CalcT A = pow( 10, dB/40 );
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
CalcT al = sn / 2 * fastsqrt1( (A + 1/A) * (1/shelfSlope - 1) + 2 );
CalcT sa = 2 * fastsqrt1( A ) * al;
SetupCommon( cs, A, sa );
}
//--------------------------------------------------------------------------
template<int channels>
class BiquadHighShelf : public Biquad<channels>
{
public:
void Setup ( CalcT normFreq, CalcT dB, CalcT shelfSlope=1.0 );
void SetupFast ( CalcT normFreq, CalcT dB, CalcT shelfSlope=1.0 );
protected:
void SetupCommon ( CalcT cs, CalcT A, CalcT sa );
};
//--------------------------------------------------------------------------
template<int channels>
void BiquadHighShelf<channels>::SetupCommon(
CalcT cs, CalcT A, CalcT sa )
{
CalcT An = A-1;
CalcT Ap = A+1;
CalcT Ancs = An*cs;
CalcT Apcs = Ap*cs;
CalcT b0 = A * (Ap + Ancs + sa );
CalcT b1 = -2 * A * (An + Apcs);
CalcT b2 = A * (Ap + Ancs - sa );
CalcT a0 = (Ap - Ancs + sa );
CalcT a2 = Ancs + sa - Ap;
CalcT a1 = -2 * (An - Apcs);
SetStage( a1/a0, a2/a0, b0/a0, b1/a0, b2/a0 );
}
template<int channels>
void BiquadHighShelf<channels>::Setup( CalcT normFreq, CalcT dB, CalcT shelfSlope )
{
CalcT A = pow( 10, dB/40 );
CalcT w0 = 2 * kPi * normFreq;
CalcT cs = cos(w0);
CalcT sn = sin(w0);
CalcT alph = sn / 2 * ::sqrt( (A + 1/A) * (1/shelfSlope - 1) + 2 );
CalcT sa = 2 * ::sqrt( A ) * alph;
SetupCommon( cs, A, sa );
}
template<int channels>
void BiquadHighShelf<channels>::SetupFast( CalcT normFreq, CalcT dB, CalcT shelfSlope )
{
CalcT A = pow( 10, dB/40 );
CalcT w0 = 2 * kPi * normFreq;
CalcT sn, cs;
fastsincos( w0, &sn, &cs );
CalcT alph = sn / 2 * fastsqrt1( (A + 1/A) * (1/shelfSlope - 1) + 2 );
CalcT sa = 2 * fastsqrt1( A ) * alph;
SetupCommon( cs, A, sa );
}
//--------------------------------------------------------------------------
//
// General N-Pole IIR Filter
//
//--------------------------------------------------------------------------
template<int stages, int channels>
class PoleFilter : public CascadeStages<stages, channels>
{
public:
PoleFilter();
virtual int CountPoles ( void )=0;
virtual int CountZeroes ( void )=0;
virtual Complex GetPole ( int i )=0;
virtual Complex GetZero ( int i )=0;
protected:
virtual Complex GetSPole ( int i, CalcT wc )=0;
protected:
// Determines the method of obtaining
// unity gain coefficients in the passband.
enum Hint
{
// No normalizating
hintNone,
// Use Brent's method to find the maximum
hintBrent,
// Use the response at a given frequency
hintPassband
};
Complex BilinearTransform ( const Complex &c );
Complex BandStopTransform ( int i, const Complex &c );
Complex BandPassTransform ( int i, const Complex &c );
Complex GetBandStopPole ( int i );
Complex GetBandStopZero ( int i );
Complex GetBandPassPole ( int i );
Complex GetBandPassZero ( int i );
void Normalize ( void );
void Prepare ( void );
virtual void BrentHint ( CalcT *w0, CalcT *w1 );
virtual CalcT PassbandHint( void );
protected:
Hint m_hint;
int m_n;
CalcT m_wc;
CalcT m_wc2;
};
//--------------------------------------------------------------------------
template<int stages, int channels>
inline PoleFilter<stages, channels>::PoleFilter( void )
{
m_hint=hintNone;
}
template<int stages, int channels>
inline Complex PoleFilter<stages, channels>::BilinearTransform( const Complex &c )
{
return (c+1.)/(-c+1.);
}
template<int stages, int channels>
inline Complex PoleFilter<stages, channels>::BandStopTransform( int i, const Complex &c )
{
CalcT a=cos((m_wc+m_wc2)*.5) /
cos((m_wc-m_wc2)*.5);
CalcT b=tan((m_wc-m_wc2)*.5);
Complex c2(0);
c2=addmul( c2, 4*(b*b+a*a-1), c );
c2+=8*(b*b-a*a+1);
c2*=c;
c2+=4*(a*a+b*b-1);
c2=Dsp::sqrt( c2 );
c2*=((i&1)==0)?.5:-.5;
c2+=a;
c2=addmul( c2, -a, c );
Complex c3( b+1 );
c3=addmul( c3, b-1, c );
return c2/c3;
}
template<int stages, int channels>
inline Complex PoleFilter<stages, channels>::BandPassTransform( int i, const Complex &c )
{
CalcT a= cos((m_wc+m_wc2)*0.5)/
cos((m_wc-m_wc2)*0.5);
CalcT b=1/tan((m_wc-m_wc2)*0.5);
Complex c2(0);
c2=addmul( c2, 4*(b*b*(a*a-1)+1), c );
c2+=8*(b*b*(a*a-1)-1);
c2*=c;
c2+=4*(b*b*(a*a-1)+1);
c2=Dsp::sqrt( c2 );
if ((i & 1) == 0)
c2=-c2;
c2=addmul( c2, 2*a*b, c );
c2+=2*a*b;
Complex c3(0);
c3=addmul( c3, 2*(b-1), c );
c3+=2*(1+b);
return c2/c3;
}
template<int stages, int channels>
Complex PoleFilter<stages, channels>::GetBandStopPole( int i )
{
Complex c=GetSPole( i/2, kPi_2 );
c=BilinearTransform( c );
c=BandStopTransform( i, c );
return c;
}
template<int stages, int channels>
Complex PoleFilter<stages, channels>::GetBandStopZero( int i )
{
return BandStopTransform( i, Complex( -1 ) );
}
template<int stages, int channels>
Complex PoleFilter<stages, channels>::GetBandPassPole( int i )
{
Complex c=GetSPole( i/2, kPi_2 );
c=BilinearTransform( c );
c=BandPassTransform( i, c );
return c;
}
template<int stages, int channels>
Complex PoleFilter<stages, channels>::GetBandPassZero( int i )
{
return Complex( (i>=m_n)?1:-1 );
}
template<int stages, int channels>
void PoleFilter<stages, channels>::Normalize( void )
{
switch( m_hint )
{
default:
case hintNone:
break;
case hintPassband:
{
CalcT w=PassbandHint();
ResponseFunctor f(this);
CalcT mag=-f(w);
CascadeStages::Normalize( 1/mag );
}
break;
case hintBrent:
{
ResponseFunctor f(this);
CalcT w0, w1, wmin, mag;
BrentHint( &w0, &w1 );
mag=-BrentMinimize( f, w0, w1, 1e-4, wmin );
CascadeStages::Normalize( 1/mag );
}
break;
}
}
template<int stages, int channels>
void PoleFilter<stages, channels>::Prepare( void )
{
if( m_wc2<1e-8 )
m_wc2=1e-8;
if( m_wc >kPi-1e-8 )
m_wc =kPi-1e-8;
Reset();
Complex c;
int poles=CountPoles();
for( int i=0;i<poles;i++ )
{
c=GetPole( i );
if( ::abs(c.imag())<1e-6 )
c=Complex( c.real(), 0 );
if( c.imag()==0 )
SetAStage( c.real(), 0 );
else if( c.imag()>0 )
SetAStage( 2*c.real(), -Dsp::norm(c) );
}
int zeroes=CountZeroes();
for( int i=0;i<zeroes;i++ )
{
c=GetZero( i );
if( ::abs(c.imag())<1e-6 )
c=Complex( c.real(), 0 );
if( c.imag()==0 )
SetBStage( -c.real(), 1, 0 );
else if( c.imag()>0 )
SetBStage( Dsp::norm(c), -2*c.real(), 1 );
}
Normalize();
}
template<int stages, int channels>
void PoleFilter<stages, channels>::BrentHint( CalcT *w0, CalcT *w1 )
{
// best that this never executes
*w0=1e-4;
*w1=kPi-1e-4;
}
template<int stages, int channels>
CalcT PoleFilter<stages, channels>::PassbandHint( void )
{
// should never get here
assert( 0 );
return kPi_2;
}
//--------------------------------------------------------------------------
//
// Butterworth Response IIR Filter
//
//--------------------------------------------------------------------------
// Butterworth filter response characteristic.
// Maximally flat magnitude response in the passband at the
// expense of a more shallow rolloff in comparison to other types.
template<int poles, int channels>
class Butterworth : public PoleFilter<int((poles+1)/2), channels>
{
public:
Butterworth();
// cutoffFreq = freq / sampleRate
void Setup ( CalcT cutoffFreq );
virtual int CountPoles ( void );
virtual int CountZeroes ( void );
virtual Complex GetPole ( int i );
protected:
Complex GetSPole ( int i, CalcT wc );
};
//--------------------------------------------------------------------------
template<int poles, int channels>
Butterworth<poles, channels>::Butterworth( void )
{
m_hint=hintPassband;
}
template<int poles, int channels>
void Butterworth<poles, channels>::Setup( CalcT cutoffFreq )
{
m_n=poles;
m_wc=2*kPi*cutoffFreq;
Prepare();
}
template<int poles, int channels>
int Butterworth<poles, channels>::CountPoles( void )
{
return poles;
}
template<int poles, int channels>
int Butterworth<poles, channels>::CountZeroes( void )
{
return poles;
}
template<int poles, int channels>
Complex Butterworth<poles, channels>::GetPole( int i )
{
return BilinearTransform( GetSPole( i, m_wc ) );
}
template<int poles, int channels>
Complex Butterworth<poles, channels>::GetSPole( int i, CalcT wc )
{
return polar( tan(wc*0.5), kPi_2+(2*i+1)*kPi/(2*m_n) );
}
//--------------------------------------------------------------------------
// Low Pass Butterworth filter
// Stable up to 53 poles (frequency min=0.13% of Nyquist)
template<int poles, int channels>
class ButterLowPass : public Butterworth<poles, channels>
{
public:
Complex GetZero ( int i );
protected:
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int poles, int channels>
Complex ButterLowPass<poles, channels>::GetZero( int i )
{
return Complex( -1 );
}
template<int poles, int channels>
CalcT ButterLowPass<poles, channels>::PassbandHint( void )
{
return 0;
}
//--------------------------------------------------------------------------
// High Pass Butterworth filter
// Maximally flat magnitude response in the passband.
// Stable up to 110 poles (frequency max=97% of Nyquist)
template<int poles, int channels>
class ButterHighPass : public Butterworth<poles, channels>
{
public:
Complex GetZero( int i );
protected:
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int poles, int channels>
Complex ButterHighPass<poles, channels>::GetZero( int i )
{
return Complex( 1 );
}
template<int poles, int channels>
CalcT ButterHighPass<poles, channels>::PassbandHint( void )
{
return kPi;
}
//--------------------------------------------------------------------------
// Band Pass Butterworth filter
// Stable up to 80 pairs
template<int pairs, int channels>
class ButterBandPass : public Butterworth<pairs*2, channels>
{
public:
// centerFreq = freq / sampleRate
// normWidth = freqWidth / sampleRate
void Setup ( CalcT centerFreq, CalcT normWidth );
virtual int CountPoles ( void );
virtual int CountZeroes ( void );
virtual Complex GetPole ( int i );
virtual Complex GetZero ( int i );
protected:
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int pairs, int channels>
void ButterBandPass<pairs, channels>::Setup( CalcT centerFreq, CalcT normWidth )
{
m_n=pairs;
CalcT angularWidth=2*kPi*normWidth;
m_wc2=2*kPi*centerFreq-(angularWidth/2);
m_wc =m_wc2+angularWidth;
Prepare();
}
template<int pairs, int channels>
int ButterBandPass<pairs, channels>::CountPoles( void )
{
return pairs*2;
}
template<int pairs, int channels>
int ButterBandPass<pairs, channels>::CountZeroes( void )
{
return pairs*2;
}
template<int pairs, int channels>
Complex ButterBandPass<pairs, channels>::GetPole( int i )
{
return GetBandPassPole( i );
}
template<int pairs, int channels>
Complex ButterBandPass<pairs, channels>::GetZero( int i )
{
return GetBandPassZero( i );
}
template<int poles, int channels>
CalcT ButterBandPass<poles, channels>::PassbandHint( void )
{
return (m_wc+m_wc2)/2;
}
//--------------------------------------------------------------------------
// Band Stop Butterworth filter
// Stable up to 109 pairs
template<int pairs, int channels>
class ButterBandStop : public Butterworth<pairs*2, channels>
{
public:
// centerFreq = freq / sampleRate
// normWidth = freqWidth / sampleRate
void Setup ( CalcT centerFreq, CalcT normWidth );
virtual int CountPoles ( void );
virtual int CountZeroes ( void );
virtual Complex GetPole ( int i );
virtual Complex GetZero ( int i );
protected:
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int pairs, int channels>
void ButterBandStop<pairs, channels>::Setup( CalcT centerFreq, CalcT normWidth )
{
m_n=pairs;
CalcT angularWidth=2*kPi*normWidth;
m_wc2=2*kPi*centerFreq-(angularWidth/2);
m_wc =m_wc2+angularWidth;
Prepare();
}
template<int pairs, int channels>
int ButterBandStop<pairs, channels>::CountPoles( void )
{
return pairs*2;
}
template<int pairs, int channels>
int ButterBandStop<pairs, channels>::CountZeroes( void )
{
return pairs*2;
}
template<int pairs, int channels>
Complex ButterBandStop<pairs, channels>::GetPole( int i )
{
return GetBandStopPole( i );
}
template<int pairs, int channels>
Complex ButterBandStop<pairs, channels>::GetZero( int i )
{
return GetBandStopZero( i );
}
template<int poles, int channels>
CalcT ButterBandStop<poles, channels>::PassbandHint( void )
{
if( (m_wc+m_wc2)/2<kPi_2 )
return kPi;
else
return 0;
}
//--------------------------------------------------------------------------
//
// Chebyshev Response IIR Filter
//
//--------------------------------------------------------------------------
// Type I Chebyshev filter characteristic.
// Minimum error between actual and ideal response at the expense of
// a user-definable amount of ripple in the passband.
template<int poles, int channels>
class Chebyshev1 : public PoleFilter<int((poles+1)/2), channels>
{
public:
Chebyshev1();
// cutoffFreq = freq / sampleRate
virtual void Setup ( CalcT cutoffFreq, CalcT rippleDb );
virtual int CountPoles ( void );
virtual int CountZeroes ( void );
virtual Complex GetPole ( int i );
virtual Complex GetZero ( int i );
protected:
void SetupCommon ( CalcT rippleDb );
virtual Complex GetSPole ( int i, CalcT wc );
protected:
CalcT m_sgn;
CalcT m_eps;
};
//--------------------------------------------------------------------------
template<int poles, int channels>
Chebyshev1<poles, channels>::Chebyshev1()
{
m_hint=hintBrent;
}
template<int poles, int channels>
void Chebyshev1<poles, channels>::Setup( CalcT cutoffFreq, CalcT rippleDb )
{
m_n=poles;
m_wc=2*kPi*cutoffFreq;
SetupCommon( rippleDb );
}
template<int poles, int channels>
void Chebyshev1<poles, channels>::SetupCommon( CalcT rippleDb )
{
m_eps=::sqrt( 1/::exp( -rippleDb*0.1*kLn10 )-1 );
Prepare();
// This moves the bottom of the ripples to 0dB gain
//CascadeStages::Normalize( pow( 10, rippleDb/20.0 ) );
}
template<int poles, int channels>
int Chebyshev1<poles, channels>::CountPoles( void )
{
return poles;
}
template<int poles, int channels>
int Chebyshev1<poles, channels>::CountZeroes( void )
{
return poles;
}
template<int poles, int channels>
Complex Chebyshev1<poles, channels>::GetPole( int i )
{
return BilinearTransform( GetSPole( i, m_wc ) )*m_sgn;
}
template<int poles, int channels>
Complex Chebyshev1<poles, channels>::GetZero( int i )
{
return Complex( -m_sgn );
}
template<int poles, int channels>
Complex Chebyshev1<poles, channels>::GetSPole( int i, CalcT wc )
{
int n = m_n;
CalcT ni = 1.0/n;
CalcT alpha = 1/m_eps+::sqrt(1+1/(m_eps*m_eps));
CalcT pn = pow( alpha, ni );
CalcT nn = pow( alpha, -ni );
CalcT a = 0.5*( pn - nn );
CalcT b = 0.5*( pn + nn );
CalcT theta = kPi_2 + (2*i+1) * kPi/(2*n);
Complex c = polar( tan( 0.5*(m_sgn==-1?(kPi-wc):wc) ), theta );
return Complex( a*c.real(), b*c.imag() );
}
//--------------------------------------------------------------------------
// Low Pass Chebyshev Type I filter
template<int poles, int channels>
class Cheby1LowPass : public Chebyshev1<poles, channels>
{
public:
Cheby1LowPass();
void Setup ( CalcT cutoffFreq, CalcT rippleDb );
protected:
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int poles, int channels>
Cheby1LowPass<poles, channels>::Cheby1LowPass()
{
m_sgn=1;
m_hint=hintPassband;
}
template<int poles, int channels>
void Cheby1LowPass<poles, channels>::Setup( CalcT cutoffFreq, CalcT rippleDb )
{
Chebyshev1::Setup( cutoffFreq, rippleDb );
// move peak of ripple down to 0dB
if( !(poles&1) )
CascadeStages::Normalize( pow( 10, -rippleDb/20.0 ) );
}
template<int poles, int channels>
CalcT Cheby1LowPass<poles, channels>::PassbandHint( void )
{
return 0;
}
//--------------------------------------------------------------------------
// High Pass Chebyshev Type I filter
template<int poles, int channels>
class Cheby1HighPass : public Chebyshev1<poles, channels>
{
public:
Cheby1HighPass();
void Setup ( CalcT cutoffFreq, CalcT rippleDb );
protected:
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int poles, int channels>
Cheby1HighPass<poles, channels>::Cheby1HighPass()
{
m_sgn=-1;
m_hint=hintPassband;
}
template<int poles, int channels>
void Cheby1HighPass<poles, channels>::Setup( CalcT cutoffFreq, CalcT rippleDb )
{
Chebyshev1::Setup( cutoffFreq, rippleDb );
// move peak of ripple down to 0dB
if( !(poles&1) )
CascadeStages::Normalize( pow( 10, -rippleDb/20.0 ) );
}
template<int poles, int channels>
CalcT Cheby1HighPass<poles, channels>::PassbandHint( void )
{
return kPi;
}
//--------------------------------------------------------------------------
// Band Pass Chebyshev Type I filter
template<int pairs, int channels>
class Cheby1BandPass : public Chebyshev1<pairs*2, channels>
{
public:
Cheby1BandPass();
void Setup ( CalcT centerFreq, CalcT normWidth, CalcT rippleDb );
int CountPoles ( void );
int CountZeroes ( void );
Complex GetPole ( int i );
Complex GetZero ( int i );
protected:
void BrentHint ( CalcT *w0, CalcT *w1 );
//CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int pairs, int channels>
Cheby1BandPass<pairs, channels>::Cheby1BandPass()
{
m_sgn=1;
m_hint=hintBrent;
}
template<int pairs, int channels>
void Cheby1BandPass<pairs, channels>::Setup( CalcT centerFreq, CalcT normWidth, CalcT rippleDb )
{
m_n=pairs;
CalcT angularWidth=2*kPi*normWidth;
m_wc2=2*kPi*centerFreq-(angularWidth/2);
m_wc =m_wc2+angularWidth;
SetupCommon( rippleDb );
}
template<int pairs, int channels>
int Cheby1BandPass<pairs, channels>::CountPoles( void )
{
return pairs*2;
}
template<int pairs, int channels>
int Cheby1BandPass<pairs, channels>::CountZeroes( void )
{
return pairs*2;
}
template<int pairs, int channels>
Complex Cheby1BandPass<pairs, channels>::GetPole( int i )
{
return GetBandPassPole( i );
}
template<int pairs, int channels>
Complex Cheby1BandPass<pairs, channels>::GetZero( int i )
{
return GetBandPassZero( i );
}
template<int poles, int channels>
void Cheby1BandPass<poles, channels>::BrentHint( CalcT *w0, CalcT *w1 )
{
CalcT d=1e-4*(m_wc-m_wc2)/2;
*w0=m_wc2+d;
*w1=m_wc-d;
}
/*
// Unfortunately, this doesn't work at the frequency extremes
// Maybe we can inverse pre-warp the center point to make sure
// it stays put after bilinear and bandpass transformation.
template<int poles, int channels>
CalcT Cheby1BandPass<poles, channels>::PassbandHint( void )
{
return (m_wc+m_wc2)/2;
}
*/
//--------------------------------------------------------------------------
// Band Stop Chebyshev Type I filter
template<int pairs, int channels>
class Cheby1BandStop : public Chebyshev1<pairs*2, channels>
{
public:
Cheby1BandStop();
void Setup ( CalcT centerFreq, CalcT normWidth, CalcT rippleDb );
int CountPoles ( void );
int CountZeroes ( void );
Complex GetPole ( int i );
Complex GetZero ( int i );
protected:
void BrentHint ( CalcT *w0, CalcT *w1 );
CalcT PassbandHint ( void );
};
//--------------------------------------------------------------------------
template<int pairs, int channels>
Cheby1BandStop<pairs, channels>::Cheby1BandStop()
{
m_sgn=1;
m_hint=hintPassband;
}
template<int pairs, int channels>
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Butterworth Optimized C++ Class
Type : 24db Resonant Lowpass
References : Posted by neotec
Notes :
This ist exactly the same as posted by "Zxform" (filters004.txt). The only difference is, that this version is an optimized one.
Parameters:
Cutoff [0.f -> Nyquist.f]
Resonance [0.f -> 1.f]
There are some minima and maxima defined, to make ist sound nice in all situations. This class is part of some of my VST Plugins, and works well and executes fast.
Code :
// FilterButterworth24db.h
#pragma once
class CFilterButterworth24db
{
public:
CFilterButterworth24db(void);
~CFilterButterworth24db(void);
void SetSampleRate(float fs);
void Set(float cutoff, float q);
float Run(float input);
private:
float t0, t1, t2, t3;
float coef0, coef1, coef2, coef3;
float history1, history2, history3, history4;
float gain;
float min_cutoff, max_cutoff;
};
// FilterButterworth24db.cpp
#include <math.h>
#define BUDDA_Q_SCALE 6.f
#include "FilterButterworth24db.h"
CFilterButterworth24db::CFilterButterworth24db(void)
{
this->history1 = 0.f;
this->history2 = 0.f;
this->history3 = 0.f;
this->history4 = 0.f;
this->SetSampleRate(44100.f);
this->Set(22050.f, 0.0);
}
CFilterButterworth24db::~CFilterButterworth24db(void)
{
}
void CFilterButterworth24db::SetSampleRate(float fs)
{
float pi = 4.f * atanf(1.f);
this->t0 = 4.f * fs * fs;
this->t1 = 8.f * fs * fs;
this->t2 = 2.f * fs;
this->t3 = pi / fs;
this->min_cutoff = fs * 0.01f;
this->max_cutoff = fs * 0.45f;
}
void CFilterButterworth24db::Set(float cutoff, float q)
{
if (cutoff < this->min_cutoff)
cutoff = this->min_cutoff;
else if(cutoff > this->max_cutoff)
cutoff = this->max_cutoff;
if(q < 0.f)
q = 0.f;
else if(q > 1.f)
q = 1.f;
float wp = this->t2 * tanf(this->t3 * cutoff);
float bd, bd_tmp, b1, b2;
q *= BUDDA_Q_SCALE;
q += 1.f;
b1 = (0.765367f / q) / wp;
b2 = 1.f / (wp * wp);
bd_tmp = this->t0 * b2 + 1.f;
bd = 1.f / (bd_tmp + this->t2 * b1);
this->gain = bd * 0.5f;
this->coef2 = (2.f - this->t1 * b2);
this->coef0 = this->coef2 * bd;
this->coef1 = (bd_tmp - this->t2 * b1) * bd;
b1 = (1.847759f / q) / wp;
bd = 1.f / (bd_tmp + this->t2 * b1);
this->gain *= bd;
this->coef2 *= bd;
this->coef3 = (bd_tmp - this->t2 * b1) * bd;
}
float CFilterButterworth24db::Run(float input)
{
float output = input * this->gain;
float new_hist;
output -= this->history1 * this->coef0;
new_hist = output - this->history2 * this->coef1;
output = new_hist + this->history1 * 2.f;
output += this->history2;
this->history2 = this->history1;
this->history1 = new_hist;
output -= this->history3 * this->coef2;
new_hist = output - this->history4 * this->coef3;
output = new_hist + this->history3 * 2.f;
output += this->history4;
this->history4 = this->history3;
this->history3 = new_hist;
return output;
}
12 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
C-Weighed Filter
Type : digital implementation (after bilinear transform)
References : Posted by Christian@savioursofsoul.de
Notes :
unoptimized version!
Code :
First prewarp the frequency of both poles:
K1 = tan(0.5*Pi*20.6 / SampleRate) // for 20.6Hz
K2 = tan(0.5*Pi*12200 / SampleRate) // for 12200Hz
Then calculate the both biquads:
b0 = 1
b1 = 0
b2 =-1
a0 = ((K1+1)*(K1+1)*(K2+1)*(K2+1));
a1 =-4*(K1*K1*K2*K2+K1*K1*K2+K1*K2*K2-K1-K2-1)*t;
a2 =- ((K1-1)*(K1-1)*(K2-1)*(K2-1))*t;
and:
b3 = 1
b4 = 0
b5 =-1
a3 = ((K1+1)*(K1+1)*(K2+1)*(K2+1));
a4 =-4*(K1*K1*K2*K2+K1*K1*K2+K1*K2*K2-K1-K2-1)*t;
a5 =- ((K1-1)*(K1-1)*(K2-1)*(K2-1))*t;
Now use an equation for calculating the biquads like this:
Stage1 = b0*Input + State0;
State0 = + a1/a0*Stage1 + State1;
State1 = b2*Input + a2/a0*Stage1;
Output = b3*Stage1 + State2;
State2 = + a4/a3*Output + State2;
State3 = b5*Stage1 + a5/a3*Output;
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Cascaded resonant lp/hp filter
Type : lp+hp
References : Posted by tobybear[AT]web[DOT]de
Notes :
// Cascaded resonant lowpass/hipass combi-filter
// The original source for this filter is from Paul Kellet from
// the archive. This is a cascaded version in Delphi where the
// output of the lowpass is fed into the highpass filter.
// Cutoff frequencies are in the range of 0<=x<1 which maps to
// 0..nyquist frequency
// input variables are:
// cut_lp: cutoff frequency of the lowpass (0..1)
// cut_hp: cutoff frequency of the hipass (0..1)
// res_lp: resonance of the lowpass (0..1)
// res_hp: resonance of the hipass (0..1)
Code :
var n1,n2,n3,n4:single; // filter delay, init these with 0!
fb_lp,fb_hp:single; // storage for calculated feedback
const p4=1.0e-24; // Pentium 4 denormal problem elimination
function dofilter(inp,cut_lp,res_lp,cut_hp,res_hp:single):single;
begin
fb_lp:=res_lp+res_lp/(1-cut_lp);
fb_hp:=res_hp+res_hp/(1-cut_lp);
n1:=n1+cut_lp*(inp-n1+fb_lp*(n1-n2))+p4;
n2:=n2+cut_lp*(n1-n2);
n3:=n3+cut_hp*(n2-n3+fb_hp*(n3-n4))+p4;
n4:=n4+cut_hp*(n3-n4);
result:=i-n4;
end;
3 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Cool Sounding Lowpass With Decibel Measured Resonance
Type : LP 2-pole resonant tweaked butterworth
References : Posted by daniel_jacob_werner [AT] yaho [DOT] com [DOT] au
Notes :
This algorithm is a modified version of the tweaked butterworth lowpass filter by Patrice Tarrabia posted on musicdsp.org's archives. It calculates the coefficients for a second order IIR filter. The resonance is specified in decibels above the DC gain. It can be made suitable to use as a SoundFont 2.0 filter by scaling the output so the overall gain matches the specification (i.e. if resonance is 6dB then you should scale the output by -3dB). Note that you can replace the sqrt(2) values in the standard butterworth highpass algorithm with my "q =" line of code to get a highpass also. How it works: normally q is the constant sqrt(2), and this value controls resonance. At sqrt(2) resonance is 0dB, smaller values increase resonance. By multiplying sqrt(2) by a power ratio we can specify the resonant gain at the cutoff frequency. The resonance power ratio is calculated with a standard formula to convert between decibels and power ratios (the powf statement...).
Good Luck,
Daniel Werner
http://experimentalscene.com/
Code :
float c, csq, resonance, q, a0, a1, a2, b1, b2;
c = 1.0f / (tanf(pi * (cutoff / samplerate)));
csq = c * c;
resonance = powf(10.0f, -(resonancedB * 0.1f));
q = sqrt(2.0f) * resonance;
a0 = 1.0f / (1.0f + (q * c) + (csq));
a1 = 2.0f * a0;
a2 = a0;
b1 = (2.0f * a0) * (1.0f - csq);
b2 = a0 * (1.0f - (q * c) + csq);
4 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
DC filter
Type : 1-pole/1-zero DC filter
References : Posted by andy[DOT]rossol[AT]bluewin[DOT]ch
Notes :
This is based on code found in the document:
"Introduction to Digital Filters (DRAFT)"
Julius O. Smith III (jos@ccrma.stanford.edu)
(http://www-ccrma.stanford.edu/~jos/filters/)
---
Some audio algorithms (asymmetric waveshaping, cascaded filters, ...) can produce DC offset. This offset can accumulate and reduce the signal/noise ratio.
So, how to fix it? The example code from Julius O. Smith's document is:
...
y(n) = x(n) - x(n-1) + R * y(n-1)
// "R" between 0.9 .. 1
// n=current (n-1)=previous in/out value
...
"R" depends on sampling rate and the low frequency point. Do not set "R" to a fixed value (e.g. 0.99) if you don't know the sample rate. Instead set R to:
(-3dB @ 40Hz): R = 1-(250/samplerate)
(-3dB @ 30Hz): R = 1-(190/samplerate)
(-3dB @ 20Hz): R = 1-(126/samplerate)
2 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Delphi Class implementation of the RBJ filters
Type : Delphi class implementation of the RBJ filters
References : Posted by veryangrymobster@hotmail.com
Notes :
I haven't tested this code thoroughly as it's pretty much a straight conversion from Arguru c++ implementation.
Code :
{
RBJ Audio EQ Cookbook Filters
A pascal conversion of arguru[AT]smartelectronix[DOT]com's
c++ implementation.
WARNING:This code is not FPU undernormalization safe.
Filter Types
0-LowPass
1-HiPass
2-BandPass CSG
3-BandPass CZPG
4-Notch
5-AllPass
6-Peaking
7-LowShelf
8-HiShelf
}
unit uRbjEqFilters;
interface
uses math;
type
TRbjEqFilter=class
private
b0a0,b1a0,b2a0,a1a0,a2a0:single;
in1,in2,ou1,ou2:single;
fSampleRate:single;
fMaxBlockSize:integer;
fFilterType:integer;
fFreq,fQ,fDBGain:single;
fQIsBandWidth:boolean;
procedure SetQ(NewQ:single);
public
out1:array of single;
constructor create(SampleRate:single;MaxBlockSize:integer);
procedure CalcFilterCoeffs(pFilterType:integer;pFreq,pQ,pDBGain:single;pQIsBandWidth:boolean);overload;
procedure CalcFilterCoeffs;overload;
function Process(input:single):single; overload;
procedure Process(Input:psingle;sampleframes:integer); overload;
property FilterType:integer read fFilterType write fFilterType;
property Freq:single read fFreq write fFreq;
property q:single read fQ write SetQ;
property DBGain:single read fDBGain write fDBGain;
property QIsBandWidth:boolean read fQIsBandWidth write fQIsBandWidth;
end;
implementation
constructor TRbjEqFilter.create(SampleRate:single;MaxBlockSize:integer);
begin
fMaxBlockSize:=MaxBlockSize;
setLength(out1,fMaxBlockSize);
fSampleRate:=SampleRate;
fFilterType:=0;
fFreq:=500;
fQ:=0.3;
fDBGain:=0;
fQIsBandWidth:=true;
in1:=0;
in2:=0;
ou1:=0;
ou2:=0;
end;
procedure TRbjEqFilter.SetQ(NewQ:single);
begin
fQ:=(1-NewQ)*0.98;
end;
procedure TRbjEqFilter.CalcFilterCoeffs(pFilterType:integer;pFreq,pQ,pDBGain:single;pQIsBandWidth:boolean);
begin
FilterType:=pFilterType;
Freq:=pFreq;
Q:=pQ;
DBGain:=pDBGain;
QIsBandWidth:=pQIsBandWidth;
CalcFilterCoeffs;
end;
procedure TRbjEqFilter.CalcFilterCoeffs;
var
alpha,a0,a1,a2,b0,b1,b2:single;
A,beta,omega,tsin,tcos:single;
begin
//peaking, LowShelf or HiShelf
if fFilterType>=6 then
begin
A:=power(10.0,(DBGain/40.0));
omega:=2*pi*fFreq/fSampleRate;
tsin:=sin(omega);
tcos:=cos(omega);
if fQIsBandWidth then
alpha:=tsin*sinh(log2(2.0)/2.0*fQ*omega/tsin)
else
alpha:=tsin/(2.0*fQ);
beta:=sqrt(A)/fQ;
// peaking
if fFilterType=6 then
begin
b0:=1.0+alpha*A;
b1:=-2.0*tcos;
b2:=1.0-alpha*A;
a0:=1.0+alpha/A;
a1:=-2.0*tcos;
a2:=1.0-alpha/A;
end else
// lowshelf
if fFilterType=7 then
begin
b0:=(A*((A+1.0)-(A-1.0)*tcos+beta*tsin));
b1:=(2.0*A*((A-1.0)-(A+1.0)*tcos));
b2:=(A*((A+1.0)-(A-1.0)*tcos-beta*tsin));
a0:=((A+1.0)+(A-1.0)*tcos+beta*tsin);
a1:=(-2.0*((A-1.0)+(A+1.0)*tcos));
a2:=((A+1.0)+(A-1.0)*tcos-beta*tsin);
end;
// hishelf
if fFilterType=8 then
begin
b0:=(A*((A+1.0)+(A-1.0)*tcos+beta*tsin));
b1:=(-2.0*A*((A-1.0)+(A+1.0)*tcos));
b2:=(A*((A+1.0)+(A-1.0)*tcos-beta*tsin));
a0:=((A+1.0)-(A-1.0)*tcos+beta*tsin);
a1:=(2.0*((A-1.0)-(A+1.0)*tcos));
a2:=((A+1.0)-(A-1.0)*tcos-beta*tsin);
end;
end else //other filter types
begin
omega:=2*pi*fFreq/fSampleRate;
tsin:=sin(omega);
tcos:=cos(omega);
if fQIsBandWidth then
alpha:=tsin*sinh(log2(2)/2*fQ*omega/tsin)
else
alpha:=tsin/(2*fQ);
//lowpass
if fFilterType=0 then
begin
b0:=(1-tcos)/2;
b1:=1-tcos;
b2:=(1-tcos)/2;
a0:=1+alpha;
a1:=-2*tcos;
a2:=1-alpha;
end else //hipass
if fFilterType=1 then
begin
b0:=(1+tcos)/2;
b1:=-(1+tcos);
b2:=(1+tcos)/2;
a0:=1+alpha;
a1:=-2*tcos;
a2:=1-alpha;
end else //bandpass CSG
if fFilterType=2 then
begin
b0:=tsin/2;
b1:=0;
b2:=-tsin/2;
a0:=1+alpha;
a1:=-1*tcos;
a2:=1-alpha;
end else //bandpass CZPG
if fFilterType=3 then
begin
b0:=alpha;
b1:=0.0;
b2:=-alpha;
a0:=1.0+alpha;
a1:=-2.0*tcos;
a2:=1.0-alpha;
end else //notch
if fFilterType=4 then
begin
b0:=1.0;
b1:=-2.0*tcos;
b2:=1.0;
a0:=1.0+alpha;
a1:=-2.0*tcos;
a2:=1.0-alpha;
end else //allpass
if fFilterType=5 then
begin
b0:=1.0-alpha;
b1:=-2.0*tcos;
b2:=1.0+alpha;
a0:=1.0+alpha;
a1:=-2.0*tcos;
a2:=1.0-alpha;
end;
end;
b0a0:=b0/a0;
b1a0:=b1/a0;
b2a0:=b2/a0;
a1a0:=a1/a0;
a2a0:=a2/a0;
end;
function TRbjEqFilter.Process(input:single):single;
var
LastOut:single;
begin
// filter
LastOut:= b0a0*input + b1a0*in1 + b2a0*in2 - a1a0*ou1 - a2a0*ou2;
// push in/out buffers
in2:=in1;
in1:=input;
ou2:=ou1;
ou1:=LastOut;
// return output
result:=LastOut;
end;
{
the process method is overloaded.
use Process(input:single):single;
for per sample processing
use Process(Input:psingle;sampleframes:integer);
for block processing. The input is a pointer to
the start of an array of single which contains
the audio data.
i.e.
RBJFilter.Process(@WaveData[0],256);
}
procedure TRbjEqFilter.Process(Input:psingle;sampleframes:integer);
var
i:integer;
LastOut:single;
begin
for i:=0 to SampleFrames-1 do
begin
// filter
LastOut:= b0a0*(input^)+ b1a0*in1 + b2a0*in2 - a1a0*ou1 - a2a0*ou2;
//LastOut:=input^;
// push in/out buffers
in2:=in1;
in1:=input^;
ou2:=ou1;
ou1:=LastOut;
Out1[i]:=LastOut;
inc(input);
end;
end;
end.
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Digital RIAA equalization filter coefficients
Type : RIAA
References : Posted by Frederick Umminger
Notes :
Use at your own risk. Confirm correctness before using. Don't assume I didn't goof something up.
-Frederick Umminger
Code :
The "turntable-input software" thread inspired me to generate some coefficients for a digital RIAA equalization filter. These coefficients were found by matching the magnitude response of the s-domain transfer function using some proprietary Matlab scripts. The phase response may or may not be totally whacked.
The s-domain transfer function is
R3(1+R1*C1*s)(1+R2*C2*s)/(R1(1+R2*C2*s) + R2(1+R1*C1*s) + R3(1+R1*C1*s)(1+R2*C2*s))
where
R1 = 883.3k
R2 = 75k
R3 = 604
C1 = 3.6n
C2 = 1n
This is based on the reference circuit found in http://www.hagtech.com/pdf/riaa.pdf
The coefficients of the digital transfer function b(z^-1)/a(z^-1) in descending powers of z, are:
44.1kHz
b = [ 0.02675918611906 -0.04592084787595 0.01921229297239]
a = [ 1.00000000000000 -0.73845850035973 -0.17951755477430]
error +/- 0.25dB
48kHz
b = [ 0.02675918611906 -0.04592084787595 0.01921229297239]
a = [ 1.00000000000000 -0.73845850035973 -0.17951755477430]
error +/- 0.15dB
88.2kHz
b = [ 0.04872204977233 -0.09076930609195 0.04202280710877]
a = [ 1.00000000000000 -0.85197860443215 -0.10921171201431]
error +/- 0.01dB
96kHz
b = [ 0.05265477122714 -0.09864197097385 0.04596474352090 ]
a = [ 1.00000000000000 -0.85835597216218 -0.10600020417219 ]
error +/- 0.006dB
10 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Direct form II
Type : generic
References : Posted by Fuzzpilz
Notes :
I've noticed there's no code for direct form II filters in general here, though probably many of the filter examples use it. I haven't looked at them all to verify that, but there certainly doesn't seem to be a snippet describing this.
This is a simple direct form II implementation of a k-pole, k-zero filter. It's a little faster than (a naive, real-time implementation of) direct form I, as well as more numerically accurate.
Code :
Direct form I pseudocode:
y[n] = a[0]*x[n] + a[1]*x[n-1] + .. + a[k]*x[n-k]
- b[1]*y[n-1] - .. - b[k]*y[n-k];
Simple equivalent direct form II pseudocode:
y[n] = a[0]*x[n] + d[0];
d[0] = a[1]*x[n] - b[1]*y[n] + d[1];
d[1] = a[2]*x[n] - b[2]*y[n] + d[2];
.
.
d[k-2] = a[k-1]*x[n] - b[k-1]*y[n] + d[k-1];
d[k-1] = a[k]*x[n] - b[k]*y[n];
For example, a biquad:
out = a0*in + a1*h0 + a2*h1 - b1*h2 - b2*h3;
h1 = h0;
h0 = in;
h3 = h2;
h2 = out;
becomes
out = a0*in + d0;
d0 = a1*in - b1*out + d1;
d1 = a2*in - b2*out;
4 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Direct Form II biquad
References : Posted by robert.bielik at xponaut dot se
Notes :
The nominal implementation for biquads is the Direct Form I variant. But the Direct Form II is actually more suited for calculating the biquad since it needs only 2 memory locations, and the multiplications can be pipelined better by the compiler. In release build, I've noted a considerable speedup when compared to DF I. When processing stereo, the code below was ~ 2X faster. Until I develop a SIMD biquad that is faster, this will do.
Code :
// b0, b1, b2, a1, a2 calculated via r.b-j's cookbook
// formulae.
// m1, m2 are the memory locations
// dn is the de-denormal coeff (=1.0e-20f)
void processBiquad(const float* in, float* out, unsigned length)
{
for(unsigned i = 0; i < length; ++i)
{
register float w = in[i] - a1*m1 - a2*m2 + dn;
out[i] = b1*m1 + b2*m2 + b0*w;
m2 = m1; m1 = w;
}
dn = -dn;
}
void processBiquadStereo(const float* inL,
const float* inR,
float* outL,
float* outR,
unsigned length)
{
for(unsigned i = 0; i < length; ++i)
{
register float wL = inL[i] - a1*m1L - a2*m2L + dn;
register float wR = inR[i] - a1*m1R - a2*m2R + dn;
outL[i] = b1*m1L + b2*m2L + b0*wL;
m2L = m1L; m1L = wL;
outR[i] = b1*m1R + b2*m2R + b0*wR;
m2R = m1R; m1R = wR;
}
dn = -dn;
}
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Fast Downsampling With Antialiasing
References : Posted by mumart[AT]gmail[DOT]com
Notes :
A quick and simple method of downsampling a signal by a factor of two with a useful amount of antialiasing. Each source sample is convolved with { 0.25, 0.5, 0.25 } before downsampling.
Code :
int filter_state;
/* input_buf can be equal to output_buf */
void downsample( int *input_buf, int *output_buf, int output_count ) {
int input_idx, input_end, output_idx, output_sam;
input_idx = output_idx = 0;
input_end = output_count * 2;
while( input_idx < input_end ) {
output_sam = filter_state + ( input_buf[ input_idx++ ] >> 1 );
filter_state = input_buf[ input_idx++ ] >> 2;
output_buf[ output_idx++ ] = output_sam + filter_state;
}
}
7 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Formant filter
References : Posted by Alex
Code :
/*
Public source code by alex@smartelectronix.com
Simple example of implementation of formant filter
Vowelnum can be 0,1,2,3,4 <=> A,E,I,O,U
Good for spectral rich input like saw or square
*/
//-------------------------------------------------------------VOWEL COEFFICIENTS
const double coeff[5][11]= {
{ 8.11044e-06,
8.943665402, -36.83889529, 92.01697887, -154.337906, 181.6233289,
-151.8651235, 89.09614114, -35.10298511, 8.388101016, -0.923313471 ///A
},
{4.36215e-06,
8.90438318, -36.55179099, 91.05750846, -152.422234, 179.1170248, ///E
-149.6496211,87.78352223, -34.60687431, 8.282228154, -0.914150747
},
{ 3.33819e-06,
8.893102966, -36.49532826, 90.96543286, -152.4545478, 179.4835618,
-150.315433, 88.43409371, -34.98612086, 8.407803364, -0.932568035 ///I
},
{1.13572e-06,
8.994734087, -37.2084849, 93.22900521, -156.6929844, 184.596544, ///O
-154.3755513, 90.49663749, -35.58964535, 8.478996281, -0.929252233
},
{4.09431e-07,
8.997322763, -37.20218544, 93.11385476, -156.2530937, 183.7080141, ///U
-153.2631681, 89.59539726, -35.12454591, 8.338655623, -0.910251753
}
};
//---------------------------------------------------------------------------------
static double memory[10]={0,0,0,0,0,0,0,0,0,0};
//---------------------------------------------------------------------------------
float formant_filter(float *in, int vowelnum)
{
res= (float) ( coeff[vowelnum][0] *in +
coeff[vowelnum][1] *memory[0] +
coeff[vowelnum][2] *memory[1] +
coeff[vowelnum][3] *memory[2] +
coeff[vowelnum][4] *memory[3] +
coeff[vowelnum][5] *memory[4] +
coeff[vowelnum][6] *memory[5] +
coeff[vowelnum][7] *memory[6] +
coeff[vowelnum][8] *memory[7] +
coeff[vowelnum][9] *memory[8] +
coeff[vowelnum][10] *memory[9] );
memory[9]= memory[8];
memory[8]= memory[7];
memory[7]= memory[6];
memory[6]= memory[5];
memory[5]= memory[4];
memory[4]= memory[3];
memory[3]= memory[2];
memory[2]= memory[1];
memory[1]= memory[0];
memory[0]=(double) res;
return res;
}
13 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
frequency warped FIR lattice
Type : FIR using allpass chain
References : Posted by mail[AT]mutagene[DOT]net
Notes :
Not at all optimized and pretty hungry in terms of arrays and overhead (function requires two arrays containing lattice filter's internal state and ouputs to another two arrays with their next states). In this implementation I think you'll have to juggle taps1/newtaps in your processing loop, alternating between one set of arrays and the other for which to send to wfirlattice).
A frequency-warped lattice filter is just a lattice filter where every delay has been replaced with an allpass filter. By adjusting the allpass filters, the frequency response of the filter can be adjusted (e.g., design an FIR that approximates some filter. Play with with warping coefficient to "sweep" the FIR up and down without changing any other coefficients). Much more on warped filters can be found on Aki Harma's website ( http://www.acoustics.hut.fi/~aqi/ )
Code :
float wfirlattice(float input, float *taps1, float *taps2, float *reflcof, float lambda, float *newtaps1, float *newtaps2, int P)
// input is filter input
// taps1,taps2 are previous filter states (init to 0)
// reflcof are reflection coefficients. abs(reflcof) < 1 for stable filter
// lamba is warping (0 = no warping, 0.75 is close to bark scale at 44.1 kHz)
// newtaps1, newtaps2 are new filter states
// P is the order of the filter
{
float forward;
float topline;
forward = input;
topline = forward;
for (int i=0;i<P;i++)
{
newtaps2[i] = topline;
newtaps1[i] = float(lambda)*(-topline + taps1[i]) + taps2[i];
topline = newtaps1[i]+forward*(reflcof[i]);
forward += newtaps1[i]*(reflcof[i]);
taps1[i]=newtaps1[i];
taps2[i]=newtaps2[i];
}
return forward;
}
3 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Hilbert Filter Coefficient Calculation
Type : Uncle Hilbert
References : Posted by Christian[at]savioursofsoul[dot]de
Notes :
This is the delphi code to create the filter coefficients, which are needed to phaseshift a signal by 90°
This may be useful for an evelope detector...
By windowing the filter coefficients you can trade phase response flatness with magnitude response flatness.
I had problems checking its response by using a dirac impulse. White noise works fine.
Also this introduces a latency of N/2!
Code :
type TSingleArray = Array of Single;
procedure UncleHilbert(var FilterCoefficients: TSingleArray; N : Integer);
var i,j : Integer;
begin
SetLength(FilterCoefficients,N);
for i:=0 to (N div 4) do
begin
FilterCoefficients[(N div 2)+(2*i-1)]:=+2/(PI*(2*i-1));
FilterCoefficients[(N div 2)-(2*i-1)]:=-2/(PI*(2*i-1));
end;
end;
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Hiqh quality /2 decimators
Type : Decimators
References : Posted by Paul Sernine
Notes :
These are /2 decimators,
Just instanciate one of them and use the Calc method to obtain one sample while inputing two. There is 5,7 and 9 tap versions.
They are extracted/adapted from a tutorial code by Thierry Rochebois. The optimal coefficients are excerpts of Traitement numérique du signal, 5eme edition, M Bellanger, Masson pp. 339-346.
Code :
//Filtres décimateurs
// T.Rochebois
// Based on
//Traitement numérique du signal, 5eme edition, M Bellanger, Masson pp. 339-346
class Decimateur5
{
private:
float R1,R2,R3,R4,R5;
const float h0;
const float h1;
const float h3;
const float h5;
public:
Decimateur5::Decimateur5():h0(346/692.0f),h1(208/692.0f),h3(-44/692.0f),h5(9/692.0f)
{
R1=R2=R3=R4=R5=0.0f;
}
float Calc(const float x0,const float x1)
{
float h5x0=h5*x0;
float h3x0=h3*x0;
float h1x0=h1*x0;
float R6=R5+h5x0;
R5=R4+h3x0;
R4=R3+h1x0;
R3=R2+h1x0+h0*x1;
R2=R1+h3x0;
R1=h5x0;
return R6;
}
};
class Decimateur7
{
private:
float R1,R2,R3,R4,R5,R6,R7;
const float h0,h1,h3,h5,h7;
public:
Decimateur7::Decimateur7():h0(802/1604.0f),h1(490/1604.0f),h3(-116/1604.0f),h5(33/1604.0f),h7(-6/1604.0f)
{
R1=R2=R3=R4=R5=R6=R7=0.0f;
}
float Calc(const float x0,const float x1)
{
float h7x0=h7*x0;
float h5x0=h5*x0;
float h3x0=h3*x0;
float h1x0=h1*x0;
float R8=R7+h7x0;
R7=R6+h5x0;
R6=R5+h3x0;
R5=R4+h1x0;
R4=R3+h1x0+h0*x1;
R3=R2+h3x0;
R2=R1+h5x0;
R1=h7x0;
return R8;
}
};
class Decimateur9
{
private:
float R1,R2,R3,R4,R5,R6,R7,R8,R9;
const float h0,h1,h3,h5,h7,h9;
public:
Decimateur9::Decimateur9():h0(8192/16384.0f),h1(5042/16384.0f),h3(-1277/16384.0f),h5(429/16384.0f),h7(-116/16384.0f),h9(18/16384.0f)
{
R1=R2=R3=R4=R5=R6=R7=R8=R9=0.0f;
}
float Calc(const float x0,const float x1)
{
float h9x0=h9*x0;
float h7x0=h7*x0;
float h5x0=h5*x0;
float h3x0=h3*x0;
float h1x0=h1*x0;
float R10=R9+h9x0;
R9=R8+h7x0;
R8=R7+h5x0;
R7=R6+h3x0;
R6=R5+h1x0;
R5=R4+h1x0+h0*x1;
R4=R3+h3x0;
R3=R2+h5x0;
R2=R1+h7x0;
R1=h9x0;
return R10;
}
};
3 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Karlsen
Type : 24-dB (4-pole) lowpass
References : Posted by Best Regards,Ove Karlsen
Notes :
There's really not much voodoo going on in the filter itself, it's a simple as possible:
pole1 = (in * frequency) + (pole1 * (1 - frequency));
Most of you can probably understand that math, it's very similar to how an analog condenser works.
Although, I did have to do some JuJu to add resonance to it.
While studing the other filters, I found that the feedback phase is very important to how the overall
resonance level will be, and so I made a dynamic feedback path, and constant Q approximation by manipulation
of the feedback phase.
A bonus with this filter, is that you can "overdrive" it... Try high input levels..
Code :
// Karlsen 24dB Filter by Ove Karlsen / Synergy-7 in the year 2003.
// b_f = frequency 0..1
// b_q = resonance 0..50
// b_in = input
// to do bandpass, subtract poles from eachother, highpass subtract with input.
float b_inSH = b_in // before the while statement.
while (b_oversample < 2) { //2x oversampling (@44.1khz)
float prevfp;
prevfp = b_fp;
if (prevfp > 1) {prevfp = 1;} // Q-limiter
b_fp = (b_fp * 0.418) + ((b_q * pole4) * 0.582); // dynamic feedback
float intfp;
intfp = (b_fp * 0.36) + (prevfp * 0.64); // feedback phase
b_in = b_inSH - intfp; // inverted feedback
pole1 = (b_in * b_f) + (pole1 * (1 - b_f)); // pole 1
if (pole1 > 1) {pole1 = 1;} else if (pole1 < -1) {pole1 = -1;} // pole 1 clipping
pole2 = (pole1 * b_f) + (pole2 * (1 - b_f)); // pole 2
pole3 = (pole2 * b_f) + (pole3 * (1 - b_f)); // pole 3
pole4 = (pole3 * b_f) + (pole4 * (1 - b_f)); // pole 4
b_oversample++;
}
lowpassout = b_in;
16 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Karlsen Fast Ladder
Type : 4 pole ladder emulation
References : Posted by arifovekarlsen[AT]hotmail[DOT]com
Notes :
ATTN Admin: You should remove the old version named "Karlsen" on your website, and rather include this one instead.
Code :
// An updated version of "Karlsen 24dB Filter"
// This time, the fastest incarnation possible.
// The very best greetings, Arif Ove Karlsen.
// arifovekarlsen->hotmail.com
b_rscl = b_buf4; if (b_rscl > 1) {b_rscl = 1;}
b_in = (-b_rscl * b_rez) + b_in;
b_buf1 = ((-b_buf1 + b_in1) * b_cut) + b_buf1;
b_buf2 = ((-b_buf2 + b_buf1) * b_cut) + b_buf2;
b_buf3 = ((-b_buf3 + b_buf2) * b_cut) + b_buf3;
b_buf4 = ((-b_buf4 + b_buf3) * b_cut) + b_buf4;
b_lpout = b_buf4;
5 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Lowpass filter for parameter edge filtering
References : Olli Niemitalo
Linked file : filter001.gif
Notes :
use this filter to smooth sudden parameter changes
(see linkfile!)
Code :
/* - Three one-poles combined in parallel
* - Output stays within input limits
* - 18 dB/oct (approx) frequency response rolloff
* - Quite fast, 2x3 parallel multiplications/sample, no internal buffers
* - Time-scalable, allowing use with different samplerates
* - Impulse and edge responses have continuous differential
* - Requires high internal numerical precision
*/
{
/* Parameters */
// Number of samples from start of edge to halfway to new value
const double scale = 100;
// 0 < Smoothness < 1. High is better, but may cause precision problems
const double smoothness = 0.999;
/* Precalc variables */
double a = 1.0-(2.4/scale); // Could also be set directly
double b = smoothness; // -"-
double acoef = a;
double bcoef = a*b;
double ccoef = a*b*b;
double mastergain = 1.0 / (-1.0/(log(a)+2.0*log(b))+2.0/
(log(a)+log(b))-1.0/log(a));
double again = mastergain;
double bgain = mastergain * (log(a*b*b)*(log(a)-log(a*b)) /
((log(a*b*b)-log(a*b))*log(a*b))
- log(a)/log(a*b));
double cgain = mastergain * (-(log(a)-log(a*b)) /
(log(a*b*b)-log(a*b)));
/* Runtime variables */
long streamofs;
double areg = 0;
double breg = 0;
double creg = 0;
/* Main loop */
for (streamofs = 0; streamofs < streamsize; streamofs++)
{
/* Update filters */
areg = acoef * areg + fromstream [streamofs];
breg = bcoef * breg + fromstream [streamofs];
creg = ccoef * creg + fromstream [streamofs];
/* Combine filters in parallel */
long temp = again * areg
+ bgain * breg
+ cgain * creg;
/* Check clipping */
if (temp > 32767)
{
temp = 32767;
}
else if (temp < -32768)
{
temp = -32768;
}
/* Store new value */
tostream [streamofs] = temp;
}
}
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
LP and HP filter
Type : biquad, tweaked butterworth
References : Posted by Patrice Tarrabia
Code :
r = rez amount, from sqrt(2) to ~ 0.1
f = cutoff frequency
(from ~0 Hz to SampleRate/2 - though many
synths seem to filter only up to SampleRate/4)
The filter algo:
out(n) = a1 * in + a2 * in(n-1) + a3 * in(n-2) - b1*out(n-1) - b2*out(n-2)
Lowpass:
c = 1.0 / tan(pi * f / sample_rate);
a1 = 1.0 / ( 1.0 + r * c + c * c);
a2 = 2* a1;
a3 = a1;
b1 = 2.0 * ( 1.0 - c*c) * a1;
b2 = ( 1.0 - r * c + c * c) * a1;
Hipass:
c = tan(pi * f / sample_rate);
a1 = 1.0 / ( 1.0 + r * c + c * c);
a2 = -2*a1;
a3 = a1;
b1 = 2.0 * ( c*c - 1.0) * a1;
b2 = ( 1.0 - r * c + c * c) * a1;
13 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
LPF 24dB/Oct
Type : Chebyshev
References : Posted by Christian[AT]savioursofsoul[DOT]de
Code :
First calculate the prewarped digital frequency:
K = tan(Pi * Frequency / Samplerate);
Now we calc some Coefficients:
sg = Sinh(PassbandRipple);
cg = Cosh(PassbandRipple);
cg *= cg;
Coeff[0] = 1 / (cg-0.85355339059327376220042218105097);
Coeff[1] = K * Coeff[0]*sg*1.847759065022573512256366378792;
Coeff[2] = 1 / (cg-0.14644660940672623779957781894758);
Coeff[3] = K * Coeff[2]*sg*0.76536686473017954345691996806;
K *= K; // (just to optimize it a little bit)
Calculate the first biquad:
A0 = (Coeff[1]+K+Coeff[0]);
A1 = 2*(Coeff[0]-K)*t;
A2 = (Coeff[1]-K-Coeff[0])*t;
B0 = t*K;
B1 = 2*B0;
B2 = B0;
Calculate the second biquad:
A3 = (Coeff[3]+K+Coeff[2]);
A4 = 2*(Coeff[2]-K)*t;
A5 = (Coeff[3]-K-Coeff[2])*t;
B3 = t*K;
B4 = 2*B3;
B5 = B3;
Then calculate the output as follows:
Stage1 = B0*Input + State0;
State0 = B1*Input + A1/A0*Stage1 + State1;
State1 = B2*Input + A2/A0*Stage1;
Output = B3*Stage1 + State2;
State2 = B4*Stage1 + A4/A3*Output + State2;
State3 = B5*Stage1 + A5/A3*Output;
3 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
MDCT and IMDCT based on FFTW3
Type : analysis and synthesis filterbank
References : Posted by shuhua dot zhang at gmail dot com
Notes :
MDCT/IMDCT is the most widely used filterbank in digital audio coding, e.g. MP3, AAC, WMA, OGG Vorbis, ATRAC.
suppose input x and N=size(x,1)/2. the MDCT transform matrix is
C=cos(pi/N*([0:2*N-1]'+.5+.5*N)*([0:N-1]+.5));
then MDCT spectrum for input x is
y=C'*x;
A well known fast algorithm is based on FFT :
(1) fold column-wisely the 2*N rows into N rows
(2) complex arrange the N rows into N/2 rows
(3) pre-twiddle, N/2-point complex fft, post-twiddle
(4) reorder to form the MDCT spectrum
in fact, (2)-(4) is a fast DCT-IV algorithm.
Implementation of the algorithm can be found in faac, but a little bit mess to extract for standalone use, and I ran into that problem. So I wrote some c codes to implement MDCT/IMDCT for any length that is a multiple of 4. Hopefully they will be useful to people here.
I benchmarked the codes using 3 FFT routines, FFT in faac, kiss_fft, and the awful FFTW. MDCT based on FFTW is the fastest, 2048-point MDCT single precision 10^5 times in 1.54s, about 50% of FFT in faac on my Petium IV 3G Hz.
An author of the FFTW, Steven G. Johnson, has a hard-coded fixed size MDCT of 256 input samples(http://jdj.mit.edu/~stevenj/mdct_128nr.c). My code is 13% slower than his.
Using the codes is very simple:
(1) init (declare first "extern void* mdctf_init(int)")
void* m_plan = mdctf_init(N);
(2) run mdct/imdct as many times as you wish
mdctf(freq, time, m_plan);
(3) free
mdctf_free(m_plan);
Of course you need the the fftw library. On Linux, gcc options are "-O2 -lfftw3f -lm". This is single precision.
Enjoy :)
Code :
/*********************************************************
MDCT/IMDCT of 4x length, Single Precision, based on FFTW
shuhua dot zhang at gmail dot com
Dept. of E.E., Tsinghua University
*********************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <fftw3.h>
typedef struct {
int N; // Number of time data points
float* twiddle; // Twiddle factor
fftwf_complex* fft_in; // fft workspace, input
fftwf_complex* fft_out; // fft workspace, output
fftwf_plan fft_plan; // fft configuration
} mdctf_plan;
mdctf_plan* mdctf_init(int N);
void mdctf_free(mdctf_plan* m_plan);
void mdctf(float* mdct_line, float* time_signal, mdctf_plan* m_plan);
void imdctf(float* time_signal, float* mdct_line, mdctf_plan* m_plan);
mdctf_plan* mdctf_init(int N)
{
mdctf_plan* m_plan;
double alpha, omiga, scale;
int n;
if( 0x00 != (N & 0x03))
{
fprintf(stderr, " Expecting N a multiple of 4\n");
return NULL;
}
m_plan = (mdctf_plan*) malloc(sizeof(mdctf_plan));
m_plan->N = N;
m_plan->twiddle = (float*) malloc(sizeof(float) * N >> 1);
alpha = 2.f * M_PI / (8.f * N);
omiga = 2.f * M_PI / N;
scale = sqrt(sqrt(2.f / N));
for(n = 0; n < (N >> 2); n++)
{
m_plan->twiddle[2*n+0] = (float) (scale * cos(omiga * n + alpha));
m_plan->twiddle[2*n+1] = (float) (scale * sin(omiga * n + alpha));
}
m_plan->fft_in = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * N >> 2);
m_plan->fft_out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * N >> 2);
m_plan->fft_plan = fftwf_plan_dft_1d(N >> 2,
m_plan->fft_in,
m_plan->fft_out,
FFTW_FORWARD,
FFTW_MEASURE);
return m_plan;
}
void mdctf_free(mdctf_plan* m_plan)
{
fftwf_destroy_plan(m_plan->fft_plan);
fftwf_free(m_plan->fft_in);
fftwf_free(m_plan->fft_out);
free(m_plan->twiddle);
free(m_plan);
}
void mdctf(float* mdct_line, float* time_signal, mdctf_plan* m_plan)
{
float *xr, *xi, r0, i0;
float *cos_tw, *sin_tw, c, s;
int N4, N2, N34, N54, n;
N4 = (m_plan->N) >> 2;
N2 = 2 * N4;
N34 = 3 * N4;
N54 = 5 * N4;
cos_tw = m_plan->twiddle;
sin_tw = cos_tw + 1;
/* odd/even folding and pre-twiddle */
xr = (float*) m_plan->fft_in;
xi = xr + 1;
for(n = 0; n < N4; n += 2)
{
r0 = time_signal[N34-1-n] + time_signal[N34+n];
i0 = time_signal[N4+n] - time_signal[N4-1-n];
c = cos_tw[n];
s = sin_tw[n];
xr[n] = r0 * c + i0 * s;
xi[n] = i0 * c - r0 * s;
}
for(; n < N2; n += 2)
{
r0 = time_signal[N34-1-n] - time_signal[-N4+n];
i0 = time_signal[N4+n] + time_signal[N54-1-n];
c = cos_tw[n];
s = sin_tw[n];
xr[n] = r0 * c + i0 * s;
xi[n] = i0 * c - r0 * s;
}
/* complex FFT of N/4 long */
fftwf_execute(m_plan->fft_plan);
/* post-twiddle */
xr = (float*) m_plan->fft_out;
xi = xr + 1;
for(n = 0; n < N2; n += 2)
{
r0 = xr[n];
i0 = xi[n];
c = cos_tw[n];
s = sin_tw[n];
mdct_line[n] = - r0 * c - i0 * s;
mdct_line[N2-1-n] = - r0 * s + i0 * c;
}
}
void imdctf(float* time_signal, float* mdct_line, mdctf_plan* m_plan)
{
float *xr, *xi, r0, i0, r1, i1;
float *cos_tw, *sin_tw, c, s;
int N4, N2, N34, N54, n;
N4 = (m_plan->N) >> 2;
N2 = 2 * N4;
N34 = 3 * N4;
N54 = 5 * N4;
cos_tw = m_plan->twiddle;
sin_tw = cos_tw + 1;
/* pre-twiddle */
xr = (float*) m_plan->fft_in;
xi = xr + 1;
for(n = 0; n < N2; n += 2)
{
r0 = mdct_line[n];
i0 = mdct_line[N2-1-n];
c = cos_tw[n];
s = sin_tw[n];
xr[n] = -2.f * (i0 * s + r0 * c);
xi[n] = -2.f * (i0 * c - r0 * s);
}
/* complex FFT of N/4 long */
fftwf_execute(m_plan->fft_plan);
/* odd/even expanding and post-twiddle */
xr = (float*) m_plan->fft_out;
xi = xr + 1;
for(n = 0; n < N4; n += 2)
{
r0 = xr[n];
i0 = xi[n];
c = cos_tw[n];
s = sin_tw[n];
r1 = r0 * c + i0 * s;
i1 = r0 * s - i0 * c;
time_signal[N34-1-n] = r1;
time_signal[N34+n] = r1;
time_signal[N4+n] = i1;
time_signal[N4-1-n] = -i1;
}
for(; n < N2; n += 2)
{
r0 = xr[n];
i0 = xi[n];
c = cos_tw[n];
s = sin_tw[n];
r1 = r0 * c + i0 * s;
i1 = r0 * s - i0 * c;
time_signal[N34-1-n] = r1;
time_signal[-N4+n] = -r1;
time_signal[N4+n] = i1;
time_signal[N54-1-n] = i1;
}
}
2 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Moog Filter
Type : Antti's version (nonlinearities)
References : Posted by Christian[at]savioursofsoul[dot]de
Notes :
Here is a Delphi/Object Pascal translation of Antti's Moog Filter.
Antti wrote:
"At last DAFX I published a paper presenting a non-linear model of the Moog ladder. For that, see http://dafx04.na.infn.it/WebProc/Proc/P_061.pdf
I used quite different approach in that one. A half-sample delay ([0.5 0.5] FIR filter basically) is inserted in the feedback loop. The remaining tuning and resonance error are corrected with polynomials. This approach depends on using at least 2X oversampling - the response after nyquist/2 is abysmal but that's taken care of by the oversampling.
Victor Lazzarini has implemented my model in CSound:
http://www.csounds.com/udo/displayOpcode.php?opcode_id=32
In summary: You can use various methods, but you will need some numerically derived correction to realize exact tuning and resonance control. If you can afford 2X oversampling, use Victor's CSound code - the tuning has been tested to be very close ideal.
Ps. Remember to use real oversampling instead of the "double sampling" the CSound code uses."
I did not implemented real oversampling, but i inserted additional noise, which simulates the resistance noise and also avoids denormal problems...
Code :
http://www.savioursofsoul.de/Christian/MoogFilter.pas
14 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Moog VCF
Type : 24db resonant lowpass
References : CSound source code, Stilson/Smith CCRMA paper.
Notes :
Digital approximation of Moog VCF. Fairly easy to calculate coefficients, fairly easy to process algorithm, good sound.
Code :
//Init
cutoff = cutoff freq in Hz
fs = sampling frequency //(e.g. 44100Hz)
res = resonance [0 - 1] //(minimum - maximum)
f = 2 * cutoff / fs; //[0 - 1]
k = 3.6*f - 1.6*f*f -1; //(Empirical tunning)
p = (k+1)*0.5;
scale = e^((1-p)*1.386249;
r = res*scale;
y4 = output;
y1=y2=y3=y4=oldx=oldy1=oldy2=oldy3=0;
//Loop
//--Inverted feed back for corner peaking
x = input - r*y4;
//Four cascaded onepole filters (bilinear transform)
y1=x*p + oldx*p - k*y1;
y2=y1*p+oldy1*p - k*y2;
y3=y2*p+oldy2*p - k*y3;
y4=y3*p+oldy3*p - k*y4;
//Clipper band limited sigmoid
y4 = y4 - (y4^3)/6;
oldx = x;
oldy1 = y1;
oldy2 = y2;
oldy3 = y3;
6 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Moog VCF, variation 1
Type : 24db resonant lowpass
References : CSound source code, Stilson/Smith CCRMA paper., Paul Kellett version
Notes :
The second "q =" line previously used exp() - I'm not sure if what I've done is any faster, but this line needs playing with anyway as it controls which frequencies will self-oscillate. I
think it could be tweaked to sound better than it currently does.
Highpass / Bandpass :
They are only 6dB/oct, but still seem musically useful - the 'fruity' sound of the 24dB/oct lowpass is retained.
Code :
// Moog 24 dB/oct resonant lowpass VCF
// References: CSound source code, Stilson/Smith CCRMA paper.
// Modified by paul.kellett@maxim.abel.co.uk July 2000
float f, p, q; //filter coefficients
float b0, b1, b2, b3, b4; //filter buffers (beware denormals!)
float t1, t2; //temporary buffers
// Set coefficients given frequency & resonance [0.0...1.0]
q = 1.0f - frequency;
p = frequency + 0.8f * frequency * q;
f = p + p - 1.0f;
q = resonance * (1.0f + 0.5f * q * (1.0f - q + 5.6f * q * q));
// Filter (in [-1.0...+1.0])
in -= q * b4; //feedback
t1 = b1; b1 = (in + b0) * p - b1 * f;
t2 = b2; b2 = (b1 + t1) * p - b2 * f;
t1 = b3; b3 = (b2 + t2) * p - b3 * f;
b4 = (b3 + t1) * p - b4 * f;
b4 = b4 - b4 * b4 * b4 * 0.166667f; //clipping
b0 = in;
// Lowpass output: b4
// Highpass output: in - b4;
// Bandpass output: 3.0f * (b3 - b4);
6 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Moog VCF, variation 2
Type : 24db resonant lowpass
References : CSound source code, Stilson/Smith CCRMA paper., Timo Tossavainen (?) version
Notes :
in[x] and out[x] are member variables, init to 0.0 the controls:
fc = cutoff, nearly linear [0,1] -> [0, fs/2]
res = resonance [0, 4] -> [no resonance, self-oscillation]
Code :
Tdouble MoogVCF::run(double input, double fc, double res)
{
double f = fc * 1.16;
double fb = res * (1.0 - 0.15 * f * f);
input -= out4 * fb;
input *= 0.35013 * (f*f)*(f*f);
out1 = input + 0.3 * in1 + (1 - f) * out1; // Pole 1
in1 = input;
out2 = out1 + 0.3 * in2 + (1 - f) * out2; // Pole 2
in2 = out1;
out3 = out2 + 0.3 * in3 + (1 - f) * out3; // Pole 3
in3 = out2;
out4 = out3 + 0.3 * in4 + (1 - f) * out4; // Pole 4
in4 = out3;
return out4;
}
10 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Notch filter
Type : 2 poles 2 zeros IIR
References : Posted by Olli Niemitalo
Notes :
Creates a muted spot in the spectrum with adjustable steepness. A complex conjugate pair of zeros on the z- plane unit circle and neutralizing poles approaching at the same angles from inside the unit circle.
Code :
Parameters:
0 =< freq =< samplerate/2
0 =< q < 1 (The higher, the narrower)
AlgoAlgo=double pi = 3.141592654;
double sqrt2 = sqrt(2.0);
double freq = 2050; // Change! (zero & pole angle)
double q = 0.4; // Change! (pole magnitude)
double z1x = cos(2*pi*freq/samplerate);
double a0a2 = (1-q)*(1-q)/(2*(fabs(z1x)+1)) + q;
double a1 = -2*z1x*a0a2;
double b1 = -2*z1x*q;
double b2 = q*q;
double reg0, reg1, reg2;
unsigned int streamofs;
reg1 = 0;
reg2 = 0;
/* Main loop */
for (streamofs = 0; streamofs < streamsize; streamofs++)
{
reg0 = a0a2 * ((double)fromstream[streamofs]
+ fromstream[streamofs+2])
+ a1 * fromstream[streamofs+1]
- b1 * reg1
- b2 * reg2;
reg2 = reg1;
reg1 = reg0;
int temp = reg0;
/* Check clipping */
if (temp > 32767) {
temp = 32767;
} else if (temp < -32768) temp = -32768;
/* Store new value */
tostream[streamofs] = temp;
}
4 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
One pole filter, LP and HP
Type : Simple 1 pole LP and HP filter
References : Posted by scoofy[AT]inf[DOT]elte[DOT]hu
Notes :
Slope: 6dB/Oct
Reference: www.dspguide.com
Code :
Process loop (lowpass):
out = a0*in - b1*tmp;
tmp = out;
Simple HP version: subtract lowpass output from the input (has strange behaviour towards nyquist):
out = a0*in - b1*tmp;
tmp = out;
hp = in-out;
Coefficient calculation:
x = exp(-2.0*pi*freq/samplerate);
a0 = 1.0-x;
b1 = -x;
5 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
One pole LP and HP
References : Posted by Bram
Code :
LP:
recursion: tmp = (1-p)*in + p*tmp with output = tmp
coefficient: p = (2-cos(x)) - sqrt((2-cos(x))^2 - 1) with x = 2*pi*cutoff/samplerate
coeficient approximation: p = (1 - 2*cutoff/samplerate)^2
HP:
recursion: tmp = (p-1)*in - p*tmp with output = tmp
coefficient: p = (2+cos(x)) - sqrt((2+cos(x))^2 - 1) with x = 2*pi*cutoff/samplerate
coeficient approximation: p = (2*cutoff/samplerate)^2
4 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
One pole, one zero LP/HP
References : Posted by mistert[AT]inwind[DOT]it
Code :
void SetLPF(float fCut, float fSampling)
{
float w = 2.0 * fSampling;
float Norm;
fCut *= 2.0F * PI;
Norm = 1.0 / (fCut + w);
b1 = (w - fCut) * Norm;
a0 = a1 = fCut * Norm;
}
void SetHPF(float fCut, float fSampling)
{
float w = 2.0 * fSampling;
float Norm;
fCut *= 2.0F * PI;
Norm = 1.0 / (fCut + w);
a0 = w * Norm;
a1 = -a0;
b1 = (w - fCut) * Norm;
}
Where
out[n] = in[n]*a0 + in[n-1]*a1 + out[n-1]*b1;
6 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
One zero, LP/HP
References : Posted by Bram
Notes :
LP is only 'valid' for cutoffs > samplerate/4
HP is only 'valid' for cutoffs < samplerate/4
Code :
theta = cutoff*2*pi / samplerate
LP:
H(z) = (1+p*z^(-1)) / (1+p)
out[i] = 1/(1+p) * in[i] + p/(1+p) * in[i-1];
p = (1-2*cos(theta)) - sqrt((1-2*cos(theta))^2 - 1)
Pi/2 < theta < Pi
HP:
H(z) = (1-p*z^(-1)) / (1+p)
out[i] = 1/(1+p) * in[i] - p/(1+p) * in[i-1];
p = (1+2*cos(theta)) - sqrt((1+2*cos(theta))^2 - 1)
0 < theta < Pi/2
4 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
One-Liner IIR Filters (1st order)
Type : IIR 1-pole
References : Posted by chris at ariescode dot com
Notes :
Here is a collection of one liner IIR filters.
Each filter has been transformed into a single C++ expression.
The filter parameter is f or g, and the state variable that needs to be kept around between interations is s.
- Christian
Code :
101 Leaky Integrator
a0 = 1
b1 = 1 - f
out = s += in - f * s;
102 Basic Lowpass (all-pole)
A first order lowpass filter, by finite difference appoximation (differentials --> differences).
a0 = f
b1 = 1 - f
out = s += f * ( in - s );
103 Lowpass with inverted control
Same as above, except for different filter parameter is now inverted.
In this case, g equals the location of the pole.
a0 = g - 1
b1 = g
out = s = in + g * ( s - in );
104 Lowpass with zero at Nyquist
A first order lowpass filter, by via the conformal map of the z-plane (0..infinity --> 0..Nyquist).
a0 = f
a1 = f
b1 = 1 - 2 * f
s = temp + ( out = s + ( temp = f * ( in - s ) ) );
105 Basic Highpass (DC-blocker)
Input complement to basic lowpass, yields a finite difference highpass filter.
a0 = 1 - f
a1 = f - 1
b1 = 1 - f
out = in - ( s += f * ( in - s ) );
106 Highpass with forced unity gain at Nyquist
Input complement to filter 104, yields a conformal map highpass filter.
a0 = 1 - f
a1 = f - 1
b1 = 1 - 2 * f
out = in + temp - ( s += 2 * ( temp = f * ( in - s ) ) );
107 Basic Allpass
This corresponds to a first order allpass filter,
where g is the location of the pole in the range -1..1.
a0 = -g
a1 = 1
b1 = g
s = in + g * ( out = s - g * in );
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Output Limiter using Envelope Follower in C++
References : Posted by thevinn at yahoo dot com
Notes :
Here's a Limiter class that will automatically compress a signal if it would cause clipping.
You can control the attack and decay parameters of the limiter. The attack determines how quickly the limiter will respond to a sudden increase in output level. I have found that attack=10ms and decay=500ms works very well for my application.
This C++ example demonstrates the use of template parameters to allow the same piece of code to work with either floats or doubles (without needing to make a duplicate of the code). As well as allowing the same code to work with interleaved audio data (any number of channels) or linear, via the "skip" parameter. Note that even in this case, the compiler produces fully optimized output in the case where the template is instantiated for a compile-time constant value of skip.
In Limiter::Process() you can see the envelope class getting called for one sample, this shows how even calling a function for a single sample can get fully optimized out by the compiler if code is structured correctly.
While this is a fairly simple algorithm, I wanted to share the technique for using template parameters to develop routines that can work with any size floating point representation or multichannel audio data, while still remaining fully optimized.
These classes were based on ideas found in the musicdsp.org archives.
Code :
class EnvelopeFollower
{
public:
EnvelopeFollower();
void Setup( double attackMs, double releaseMs, int sampleRate );
template<class T, int skip>
void Process( size_t count, const T *src );
double envelope;
protected:
double a;
double r;
};
//----------
inline EnvelopeFollower::EnvelopeFollower()
{
envelope=0;
}
inline void EnvelopeFollower::Setup( double attackMs, double releaseMs, int sampleRate )
{
a = pow( 0.01, 1.0 / ( attackMs * sampleRate * 0.001 ) );
r = pow( 0.01, 1.0 / ( releaseMs * sampleRate * 0.001 ) );
}
template<class T, int skip>
void EnvelopeFollower::Process( size_t count, const T *src )
{
while( count-- )
{
double v=::fabs( *src );
src+=skip;
if( v>envelope )
envelope = a * ( envelope - v ) + v;
else
envelope = r * ( envelope - v ) + v;
}
}
//----------
struct Limiter
{
void Setup( double attackMs, double releaseMs, int sampleRate );
template<class T, int skip>
void Process( size_t nSamples, T *dest );
private:
EnvelopeFollower e;
};
//----------
inline void Limiter::Setup( double attackMs, double releaseMs, int sampleRate )
{
e.Setup( attackMs, releaseMs, sampleRate );
}
template<class T, int skip>
void Limiter::Process( size_t count, T *dest )
{
while( count-- )
{
T v=*dest;
// don't worry, this should get optimized
e.Process<T, skip>( 1, &v );
if( e.envelope>1 )
*dest=*dest/e.envelope;
dest+=skip;
}
}
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Peak/Notch filter
Type : peak/notch
References : Posted by tobybear[AT]web[DOT]de
Notes :
// Peak/Notch filter
// I don't know anymore where this came from, just found it on
// my hard drive :-)
// Seems to be a peak/notch filter with adjustable slope
// steepness, though slope gets rather wide the lower the
// frequency is.
// "cut" and "steep" range is from 0..1
// Try to feed it with white noise, then the peak output does
// rather well eliminate all other frequencies except the given
// frequency in higher frequency ranges.
Code :
var f,r:single;
outp,outp1,outp2:single; // init these with 0!
const p4=1.0e-24; // Pentium 4 denormal problem elimination
function PeakNotch(inp,cut,steep:single;ftype:integer):single;
begin
r:=steep*0.99609375;
f:=cos(pi*cut);
a0:=(1-r)*sqrt(r*(r-4*(f*f)+2)+1);
b1:=2*f*r;
b2:=-(r*r);
outp:=a0*inp+b1*outp1+b2*outp2+p4;
outp2:=outp1;
outp1:=outp;
if ftype=0 then
result:=outp //peak
else
result:=inp-outp; //notch
end;
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Perfect LP4 filter
Type : LP
References : Posted by azertopia at free dot fr
Notes :
hacked from the exemple of user script in FL Edison
Code :
TLP24DB = class
constructor create;
procedure process(inp,Frq,Res:single;SR:integer);
private
t, t2, x, f, k, p, r, y1, y2, y3, y4, oldx, oldy1, oldy2, oldy3: Single;
public outlp:single;
end;
----------------------------------------
implementation
constructor TLP24DB.create;
begin
y1:=0;
y2:=0;
y3:=0;
y4:=0;
oldx:=0;
oldy1:=0;
oldy2:=0;
oldy3:=0;
end;
procedure TLP24DB.process(inp: Single; Frq: Single; Res: Single; SR: Integer);
begin
f := (Frq+Frq) / SR;
p:=f*(1.8-0.8*f);
k:=p+p-1.0;
t:=(1.0-p)*1.386249;
t2:=12.0+t*t;
r := res*(t2+6.0*t)/(t2-6.0*t);
x := inp - r*y4;
y1:=x*p + oldx*p - k*y1;
y2:=y1*p+oldy1*p - k*y2;
y3:=y2*p+oldy2*p - k*y3;
y4:=y3*p+oldy3*p - k*y4;
y4 := y4 - ((y4*y4*y4)/6.0);
oldx := x;
oldy1 := y1+_kd;
oldy2 := y2+_kd;;
oldy3 := y3+_kd;;
outlp := y4;
end;
// the result is in outlp
// 1/ call MyTLP24DB.Process
// 2/then get the result from outlp.
// this filter have a fantastic sound w/a very special res
// _kd is the denormal killer value.
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Phase equalization
Type : Allpass
References : Posted by Uli the Grasso
Notes :
The idea is simple: One can equalize the phase response of a system, for example of a loudspeaker, by approximating its phase response by an FIR filter and then turn around the coefficients of the filter. At http://grassomusic.de/english/phaseeq.htm you find more info and an Octave script.
4 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Plot Filter (Analyze filter characteristics)
Type : Test
References : Posted by scanner598 at yahoo dot co dot uk
Notes :
As a newbe, and one that has very, very little mathematical background, I wanted to see what all the filters posted here were doing to get a feeling of what was going on here. So with what I picked up from this site, I wrote a little filter test program. Hope it is of any use to you.
Code :
//
// plotFilter.cpp
//
// Simple test program to plot filter characteristics of a particular
// filter to stdout. Nice to see how the filter behaves under various
// conditions (cutoff/resonance/samplerate/etc.).
//
// Should work on any platform that supports C++ and should work on C
// as well with a little rework. It justs prints text, so no graphical
// stuff is used.
//
// Filter input and filter output should be between -1 and 1 (floating point)
//
// Output is a plotted graph (as text) with a logarithmic scale
// (so half a plotted bar is half of what the human ear can hear).
// If you dont like the vertical output, just print it and turn the paper :-)
//
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#define myPI 3.1415926535897932384626433832795
#define FP double
#define DWORD unsigned long
#define CUTOFF 5000
#define SAMPLERATE 48000
// take enough samples to test the 20 herz frequency 2 times
#define TESTSAMPLES (SAMPLERATE/20) * 2
//
// Any filter can be tested, as long as it outputs
// between -1 and 1 (floating point). This filter
// can be replaced with any filter you would like
// to test.
//
class Filter {
FP sdm1; // sample data minus 1
FP a0; // multiply factor on current sample
FP b1; // multiply factor on sdm1
public:
Filter (void) {
sdm1 = 0;
// init on no filtering
a0 = 1;
b1 = 0;
}
void init(FP freq, FP samplerate) {
FP x;
x = exp(-2.0 * myPI * freq / samplerate);
sdm1 = 0;
a0 = 1.0 - x;
b1 = -x;
}
FP getSample(FP sample) {
FP out;
out = (a0 * sample) - (b1 * sdm1);
sdm1 = out;
return out;
}
};
int
main(void)
{
DWORD freq;
DWORD spos;
double sIn;
double sOut;
double tIn;
double tOut;
double dB;
DWORD tmp;
// define the test filter
Filter filter;
printf(" 9bB 6dB 3dB 0dB\n");
printf(" freq dB | | | | \n");
// test frequencies 20 - 20020 with 100 herz steps
for (freq=20; freq<20020; freq+=100) {
// (re)initialize the filter
filter.init(CUTOFF, SAMPLERATE);
// let the filter do it's thing here
tIn = tOut = 0;
for (spos=0; spos<TESTSAMPLES; spos++) {
sIn = sin((2 * myPI * spos * freq) / SAMPLERATE);
sOut = filter.getSample(sIn);
if ((sOut>1) || (sOut<-1)) {
// If filter is no good, stop the test
printf("Error! Clipping!\n");
return(1);
}
if (sIn >0) tIn += sIn;
if (sIn <0) tIn -= sIn;
if (sOut>0) tOut += sOut;
if (sOut<0) tOut -= sOut;
}
// analyse the results
dB = 20*log(tIn/tOut);
printf("%5d %5.1f ", freq, dB);
tmp = (DWORD)(60.0/pow(2, (dB/3)));
while (tmp) {
putchar('#');
tmp--;
}
putchar('\n');
}
return 0;
}
1 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Plotting R B-J Equalisers in Excel
References : Posted by Web Surf
Linked file : rbj_eq.xls
Notes :
Interactive XL sheet that shows frequency response of the R B-K high pass/low pass, Peaking and Shelf filters
Useful if --
--You want to validate your implementation against mine
--You want to convert given Biquad coefficients into Fo/Q/dBgain by visual curve fitting.
--You want the Coefficients to implement a particular fixed filter
-- Educational aid
Many thanks to R B-J for his personal support !!
Web Surf WebsurffATgmailDOTcom
Code :
see attached file
3 comment(s) | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Polyphase Filters (Delphi)
Type : polyphase filters, used for up and down-sampling
References : Posted by veryangrymobster[AT]hotmail[DOT]com
Notes :
Pascal conversion of C++ code by Dave from Muon Software. Conversion by Shannon Faulkner.
Code :
{
polyphase filters, used for up and down-sampling
original c++ code by Dave from Muon Software found
at MusicDSP.
rewritten in pascal by Shannon Faulkner, 4/7/06.
}
unit uPolyphase;
interface
type
TAllPass=class
private
a,x0,x1,x2,y0,y1,y2:single;
public
constructor create(coefficient:single);
function Process(input:single):single;
end;
TAllPassFilterCascade=class
private
AllPassFilters:array of TAllPass;
fOrder:integer;
public
constructor create(coefficients:psingle;Order:integer);
function Process(input:single):single;
end;
THalfBandFilter=class
private
fOrder:integer;
OldOut:single;
aCoeffs,bCoeffs:array of single;
FilterA,FilterB:TAllPassFilterCascade;
public
constructor create(order:integer;Steep:boolean);
function process(input:single):single;
end;
implementation
//----------- AllPass Filter -------------------
//----------- AllPass Filter ----------------------------------------------
//----------- AllPass Filter ----------------------------------------------
constructor TAllPass.create(coefficient:single);
begin
a:=coefficient;
x0:=0;
x1:=0;
x2:=0;
y0:=0;
y1:=0;
y2:=0;
end;
function TAllPass.Process(input:single):single;
var output:single;
begin
//shuffle inputs
x2:=x1;
x1:=x0;
x0:=input;
//shuffle outputs
y2:=y1;
y1:=y0;
//allpass filter 1
output:=x2+((input-y2)*a);
y0:=output;
result:=output;
end;
//------------ AllPass Filter Cascade -------------
//------------ AllPass Filter Cascade -------------
//------------ AllPass Filter Cascade -------------
constructor TAllPassFilterCascade.create(coefficients:psingle;Order:integer);
var i:integer;
begin
fOrder:=Order;
setlength(AllPassFilters,fOrder);
for i:=0 to fOrder-1 do
begin
AllPassFilters[i]:=TAllPass.create(coefficients^);
inc(coefficients);
end;
end;
function TAllPassFilterCascade.Process(input:single):single;
var
output:single;
i:integer;
begin
output:=input;
for i:=0 to fOrder-1 do
begin
output:=allpassfilters[i].Process(output);
end;
result:=output;
end;
//-------------- Halfband Filter -------------------------------------------
//-------------- Halfband Filter ---------------------
//-------------- Halfband Filter -------------------------------------------
constructor THalfBandFilter.create(order:integer;Steep:boolean);
begin
fOrder:=order;
setlength(aCoeffs,Order div 2);
setlength(bCoeffs,Order div 2);
if steep=true then
begin
if (order=12) then //rejection=104dB, transition band=0.01
begin
aCoeffs[0]:=0.036681502163648017;
aCoeffs[1]:=0.2746317593794541;
aCoeffs[2]:=0.56109896978791948;
aCoeffs[3]:=0.769741833862266;
aCoeffs[4]:=0.8922608180038789;
aCoeffs[5]:=0.962094548378084;
bCoeffs[0]:=0.13654762463195771;
bCoeffs[1]:=0.42313861743656667;
bCoeffs[2]:=0.6775400499741616;
bCoeffs[3]:=0.839889624849638;
bCoeffs[4]:=0.9315419599631839;
bCoeffs[5]:=0.9878163707328971;
end
else if (order=10) then //rejection=86dB, transition band=0.01
begin
aCoeffs[0]:=0.051457617441190984;
aCoeffs[1]:=0.35978656070567017;
aCoeffs[2]:=0.6725475931034693;
aCoeffs[3]:=0.8590884928249939;
aCoeffs[4]:=0.9540209867860787;
bCoeffs[0]:=0.18621906251989334;
bCoeffs[1]:=0.529951372847964;
bCoeffs[2]:=0.7810257527489514;
bCoeffs[3]:=0.9141815687605308;
bCoeffs[4]:=0.985475023014907;
end
else if (order=8) then //rejection=69dB, transition band=0.01
begin
aCoeffs[0]:=0.07711507983241622;
aCoeffs[1]:=0.4820706250610472;
aCoeffs[2]:=0.7968204713315797;
aCoeffs[3]:=0.9412514277740471;
bCoeffs[0]:=0.2659685265210946;
bCoeffs[1]:=0.6651041532634957;
bCoeffs[2]:=0.8841015085506159;
bCoeffs[3]:=0.9820054141886075;
end
else if (order=6) then //rejection=51dB, transition band=0.01
begin
aCoeffs[0]:=0.1271414136264853;
aCoeffs[1]:=0.6528245886369117;
aCoeffs[2]:=0.9176942834328115;
bCoeffs[0]:=0.40056789819445626;
bCoeffs[1]:=0.8204163891923343;
bCoeffs[2]:=0.9763114515836773;
end
else if (order=4) then //rejection=53dB,transition band=0.05
begin
aCoeffs[0]:=0.12073211751675449;
aCoeffs[1]:=0.6632020224193995;
bCoeffs[0]:=0.3903621872345006;
bCoeffs[1]:=0.890786832653497;
end
else //order=2, rejection=36dB, transition band=0.1
begin
aCoeffs[0]:=0.23647102099689224;
bCoeffs[0]:=0.7145421497126001;
end;
end else //softer slopes, more attenuation and less stopband ripple
begin
if (order=12) then //rejection=104dB, transition band=0.01
begin
aCoeffs[0]:=0.01677466677723562;
aCoeffs[1]:=0.13902148819717805;
aCoeffs[2]:=0.3325011117394731;
aCoeffs[3]:=0.53766105314488;
aCoeffs[4]:=0.7214184024215805;
aCoeffs[5]:=0.8821858402078155;
bCoeffs[0]:=0.06501319274445962;
bCoeffs[1]:=0.23094129990840923;
bCoeffs[2]:=0.4364942348420355;
//bug fix - coefficient changed,
//rob[DOT]belcham[AT]zen[DOT]co[DOT]uk
//bCoeffs[3]:=0.06329609551399348; //original coefficient
bCoeffs[3]:=0.6329609551399348; //correct coefficient
bCoeffs[4]:=0.80378086794111226;
bCoeffs[5]:=0.9599687404800694;
end
else if (order=10) then //rejection=86dB, transition band=0.01
begin
aCoeffs[0]:=0.02366831419883467;
aCoeffs[1]:=0.18989476227180174;
aCoeffs[2]:=0.43157318062118555;
aCoeffs[3]:=0.6632020224193995;
aCoeffs[4]:=0.860015542499582;
bCoeffs[0]:=0.09056555904993387;
bCoeffs[1]:=0.3078575723749043;
bCoeffs[2]:=0.5516782402507934;
bCoeffs[3]:=0.7652146863779808;
bCoeffs[4]:=0.95247728378667541;
end
else if (order=8) then //rejection=69dB, transition band=0.01
begin
aCoeffs[0]:=0.03583278843106211;
aCoeffs[1]:=0.2720401433964576;
aCoeffs[2]:=0.5720571972357003;
aCoeffs[3]:=0.827124761997324;
bCoeffs[0]:=0.1340901419430669;
bCoeffs[1]:=0.4243248712718685;
bCoeffs[2]:=0.7062921421386394;
bCoeffs[3]:=0.9415030941737551;
end
else if (order=6) then //rejection=51dB, transition band=0.01
begin
aCoeffs[0]:=0.06029739095712437;
aCoeffs[1]:=0.4125907203610563;
aCoeffs[2]:=0.7727156537429234;
bCoeffs[0]:=0.21597144456092948;
bCoeffs[1]:=0.6043586264658363;
bCoeffs[2]:=0.9238861386532906;
end
else if (order=4) then //rejection=53dB,transition band=0.05
begin
aCoeffs[0]:=0.07986642623635751;
aCoeffs[1]:=0.5453536510711322;
bCoeffs[0]:=0.28382934487410993;
bCoeffs[1]:=0.8344118914807379;
end
else //order=2, rejection=36dB, transition band=0.1
begin
aCoeffs[0]:=0.23647102099689224;
bCoeffs[0]:=0.7145421497126001;
end;
end;
FilterA:=TAllPassFilterCascade.create(@aCoeffs[0],fOrder div 2);
FilterB:=TAllPassFilterCascade.create(@bCoeffs[0],fOrder div 2);
oldout:=0;
end;
function THalfBandFilter.process(input:single):single;
begin
result:=(FilterA.Process(input)+oldout)*0.5;
oldout:=FilterB.Process(input);
end;
end.
no comments on this item | add a comment | nofrills version |
|
|
|
|
|
|
|
|
Poor Man's FIWIZ
Type : Filter Design Utility
References : Posted by mailbjl[at]yahoo[dot]com
Notes :
FIWIZ is a neat little filter design utility that uses a genetic programming technique called Differential Evolution. As useful as it is, it looks like it took about a week to write, and is thus very undeserving of the $70 license fee. So I decided to write my own. There's a freely available optimizer class that uses Differential Evolution and I patched it together with some filter-specific logic.
Use of the utility requires the ability to do simple coding in C, but you need only revise a single function, which basically describes your filter specification. There's a big comment in the main source file that clarifies things a bit more.
Although it's not as easy to use as FIWIZ, it's arguably more powerful because your specifications are limited only by what you can express in C, whereas FIWIZ is completely GUI based.
Another thing: I'm afraid that due to the use of _kbhit and _getch, the code is a bit Microsofty, but you can take those out and the code will still be basically usable.
Code :
// First File: DESolver.cpp
#include <stdio.h>
#include <memory.h>
#include <conio.h>
#include "DESolver.h"
#define Element(a,b,c) a[b*nDim+c]
#define RowVector(a,b) (&a[b*nDim])
#define CopyVector(a,b) memcpy((a),(b),nDim*sizeof(double))
DESolver::DESolver(int dim,int popSize) :
nDim(dim), nPop(popSize),
generations(0), strategy(stRand1Exp),
scale(0.7), probability(0.5), bestEnergy(0.0),
trialSolution(0), bestSolution(0),
popEnergy(0), population(0)
{
trialSolution = new double[nDim];
bestSolution = new double[nDim];
popEnergy = new double[nPop];
population = new double[nPop * nDim];
return;
}
DESolver::~DESolver(void)
{
if (trialSolution) delete trialSolution;
if (bestSolution) delete bestSolution;
if (popEnergy) delete popEnergy;
if (population) delete population;
trialSolution = bestSolution = popEnergy = population = 0;
return;
}
void DESolver::Setup(double *min,double *max,
int deStrategy,double diffScale,double crossoverProb)
{
int i;
strategy = deStrategy;
scale = diffScale;
probability = crossoverProb;
for (i=0; i < nPop; i++)
{
for (int j=0; j < nDim; j++)
Element(population,i,j) = RandomUniform(min[j],max[j]);
popEnergy[i] = 1.0E20;
}
for (i=0; i < nDim; i++)
bestSolution[i] = 0.0;
switch (strategy)
{
case stBest1Exp:
calcTrialSolution = &DESolver::Best1Exp;
break;
case stRand1Exp:
calcTrialSolution = &DESolver::Rand1Exp;
break;
case stRandToBest1Exp:
calcTrialSolution = &DESolver::RandToBest1Exp;
break;
case stBest2Exp:
calcTrialSolution = &DESolver::Best2Exp;
break;
case stRand2Exp:
calcTrialSolution = &DESolver::Rand2Exp;
break;
case stBest1Bin:
calcTrialSolution = &DESolver::Best1Bin;
break;
case stRand1Bin:
calcTrialSolution = &DESolver::Rand1Bin;
break;
case stRandToBest1Bin:
calcTrialSolution = &DESolver::RandToBest1Bin;
break;
case stBest2Bin:
calcTrialSolution = &DESolver::Best2Bin;
break;
case stRand2Bin:
calcTrialSolution = &DESolver::Rand2Bin;
break;
}
return;
}
bool DESolver::Solve(int maxGenerations)
{
int generation;
int candidate;
bool bAtSolution;
int generationsPerLoop = 10;
bestEnergy = 1.0E20;
bAtSolution = false;
for (generation=0;
(generation < maxGenerations) && !bAtSolution && (0 == _kbhit());
generation++)
{
for (candidate=0; candidate < nPop; candidate++)
{
(this->*calcTrialSolution)(candidate);
trialEnergy = EnergyFunction(trialSolution,bAtSolution);
if (trialEnergy < popEnergy[candidate])
{
// New low for this candidate
popEnergy[candidate] = trialEnergy;
CopyVector(RowVector(population,candidate),trialSolution);
// Check if all-time low
if (trialEnergy < bestEnergy)
{
bestEnergy = trialEnergy;
CopyVector(bestSolution,trialSolution);
}
}
}
if ((generation % generationsPerLoop) == (generationsPerLoop - 1))
{
printf("Gens %u Cost %.15g\n", generation+1, Energy());
}
}
if (0 != _kbhit())
{
_getch();
}
generations = generation;
return(bAtSolution);
}
void DESolver::Best1Exp(int candidate)
{
int r1, r2;
int n;
SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = bestSolution[n]
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Rand1Exp(int candidate)
{
int r1, r2, r3;
int n;
SelectSamples(candidate,&r1,&r2,&r3);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
- Element(population,r3,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::RandToBest1Exp(int candidate)
{
int r1, r2;
int n;
SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] += scale * (bestSolution[n] - trialSolution[n])
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Best2Exp(int candidate)
{
int r1, r2, r3, r4;
int n;
SelectSamples(candidate,&r1,&r2,&r3,&r4);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = bestSolution[n] +
scale * (Element(population,r1,n)
+ Element(population,r2,n)
- Element(population,r3,n)
- Element(population,r4,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Rand2Exp(int candidate)
{
int r1, r2, r3, r4, r5;
int n;
SelectSamples(candidate,&r1,&r2,&r3,&r4,&r5);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
+ Element(population,r3,n)
- Element(population,r4,n)
- Element(population,r5,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Best1Bin(int candidate)
{
int r1, r2;
int n;
SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = bestSolution[n]
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Rand1Bin(int candidate)
{
int r1, r2, r3;
int n;
SelectSamples(candidate,&r1,&r2,&r3);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
- Element(population,r3,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::RandToBest1Bin(int candidate)
{
int r1, r2;
int n;
SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] += scale * (bestSolution[n] - trialSolution[n])
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Best2Bin(int candidate)
{
int r1, r2, r3, r4;
int n;
SelectSamples(candidate,&r1,&r2,&r3,&r4);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = bestSolution[n]
+ scale * (Element(population,r1,n)
+ Element(population,r2,n)
- Element(population,r3,n)
- Element(population,r4,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::Rand2Bin(int candidate)
{
int r1, r2, r3, r4, r5;
int n;
SelectSamples(candidate,&r1,&r2,&r3,&r4,&r5);
n = (int)RandomUniform(0.0,(double)nDim);
CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
+ Element(population,r3,n)
- Element(population,r4,n)
- Element(population,r5,n));
n = (n + 1) % nDim;
}
return;
}
void DESolver::SelectSamples(int candidate,int *r1,int *r2,
int *r3,int *r4,int *r5)
{
if (r1)
{
do
{
*r1 = (int)RandomUniform(0.0,(double)nPop);
}
while (*r1 == candidate);
}
if (r2)
{
do
{
*r2 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r2 == candidate) || (*r2 == *r1));
}
if (r3)
{
do
{
*r3 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r3 == candidate) || (*r3 == *r2) || (*r3 == *r1));
}
if (r4)
{
do
{
*r4 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r4 == candidate) || (*r4 == *r3) || (*r4 == *r2) || (*r4 == *r1));
}
if (r5)
{
do
{
*r5 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r5 == candidate) || (*r5 == *r4) || (*r5 == *r3)
|| (*r5 == *r2) || (*r5 == *r1));
}
return;
}
/*------Constants for RandomUniform()---------------------------------------*/
#define SEED 3
#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)
double DESolver::RandomUniform(double minValue,double maxValue)
{
long j;
long k;
static long idum;
static long idum2=123456789;
static long iy=0;
static long iv[NTAB];
double result;
if (iy == 0)
idum = SEED;
if (idum <= 0)
{
if (-idum < 1)
idum = 1;
else
idum = -idum;
idum2 = idum;
for (j=NTAB+7; j>=0; j--)
{
k = idum / IQ1;
idum = IA1 * (idum - k*IQ1) - k*IR1;
if (idum < 0) idum += IM1;
if (j < NTAB) iv[j] = idum;
}
iy = iv[0];
}
k = idum / IQ1;
idum = IA1 * (idum - k*IQ1) - k*IR1;
if (idum < 0)
idum += IM1;
k = idum2 / IQ2;
idum2 = IA2 * (idum2 - k*IQ2) - k*IR2;
if (idum2 < 0)
idum2 += IM2;
j = iy / NDIV;
iy = iv[j] - idum2;
iv[j] = idum;
if (iy < 1)
iy += IMM1;
result = AM * iy;
if (result > RNMX)
result = RNMX;
result = minValue + result * (maxValue - minValue);
return(result);
}
// END FIRST FILE
// BEGIN SECOND FILE: DESolver.h
// Differential Evolution Solver Class
// Based on algorithms developed by Dr. Rainer Storn & Kenneth Price
// Written By: Lester E. Godwin
// PushCorp, Inc.
// Dallas, Texas
// 972-840-0208 x102
// godwin@pushcorp.com
// Created: 6/8/98
// Last Modified: 6/8/98
// Revision: 1.0
#if !defined(_DESOLVER_H)
#define _DESOLVER_H
#define stBest1Exp 0
#define stRand1Exp 1
#define stRandToBest1Exp 2
#define stBest2Exp 3
#define stRand2Exp 4
#define stBest1Bin 5
#define stRand1Bin 6
#define stRandToBest1Bin 7
#define stBest2Bin 8
#define stRand2Bin 9
class DESolver;
typedef void (DESolver::*StrategyFunction)(int);
class DESolver
{
public:
DESolver(int dim,int popSize);
~DESolver(void);
// Setup() must be called before solve to set min, max, strategy etc.
void Setup(double min[],double max[],int deStrategy,
double diffScale,double crossoverProb);
// Solve() returns true if EnergyFunction() returns true.
// Otherwise it runs maxGenerations generations and returns false.
virtual bool Solve(int maxGenerations);
// EnergyFunction must be overridden for problem to solve
// testSolution[] is nDim array for a candidate solution
// setting bAtSolution = true indicates solution is found
// and Solve() immediately returns true.
virtual double EnergyFunction(double testSolution[],bool &bAtSolution) = 0;
int Dimension(void) { return(nDim); }
int Population(void) { return(nPop); }
// Call these functions after Solve() to get results.
double Energy(void) { return(bestEnergy); }
double *Solution(void) { return(bestSolution); }
int Generations(void) { return(generations); }
protected:
void SelectSamples(int candidate,int *r1,int *r2=0,int *r3=0,
int *r4=0,int *r5=0);
double RandomUniform(double min,double max);
int nDim;
int nPop;
int generations;
int strategy;
StrategyFunction calcTrialSolution;
double scale;
double probability;
double trialEnergy;
double bestEnergy;
double *trialSolution;
double *bestSolution;
double *popEnergy;
double *population;
private:
void Best1Exp(int candidate);
void Rand1Exp(int candidate);
void RandToBest1Exp(int candidate);
void Best2Exp(int candidate);
void Rand2Exp(int candidate);
void Best1Bin(int candidate);
void Rand1Bin(int candidate);
void RandToBest1Bin(int candidate);
void Best2Bin(int candidate);
void Rand2Bin(int candidate);
};
// I added the following stuff 19 March 2007
// Brent Lehman
struct ASpectrum
{
unsigned mNumValues;
double* mReals;
double* mImags;
};
bool ComputeSpectrum(double* evenZeros, unsigned numEvenZeros, double* oddZero,
double* evenPoles, unsigned numEvenPoles, double* oddPole,
double gain, ASpectrum* spectrum);
class FilterSolver : public DESolver
{
public:
FilterSolver(int dim, int popSize, int spectrumSize,
unsigned numZeros, unsigned numPoles, bool minimumPhase) :
DESolver(dim, popSize)
{
mSpectrum.mNumValues = spectrumSize;
mSpectrum.mReals = new double[spectrumSize];
mSpectrum.mImags = new double[spectrumSize];
mNumZeros = numZeros;
mNumPoles = numPoles;
mMinimumPhase = minimumPhase;
}
virtual ~FilterSolver()
{
delete[] mSpectrum.mReals;
delete[] mSpectrum.mImags;
}
virtual double EnergyFunction(double testSolution[], bool& bAtSolution);
virtual ASpectrum* Spectrum() {return &mSpectrum;}
private:
unsigned mNumZeros;
unsigned mNumPoles;
bool mMinimumPhase;
ASpectrum mSpectrum;
};
#endif // _DESOLVER_H
// END SECOND FILE DESolver.h
// BEGIN FINAL FILE: FilterDesign.cpp
/*
*
* Filter Design Utility
* Source
*
* Brent Lehman
* 16 March 2007
*
*
*/
////////////////////////////////////////////////////////////////////
// //
// The idea is that an optimization algorithm passes a bunch of //
// different filter specifications to the function //
// "EnergyFunction" below. That function is supposed to //
// compute an "error" or "cost" value for each specification //
// it receives, which the algorithm uses to decide on other //
// filter specifications to try. Over the course of several //
// thousand different specifications, the algorithm will //
// eventually converge on a single best one. This one has the //
// lowest error value of all possible specifications. Thus, //
// you effectively tell the optimization algorithm what it's //
// looking for through code that you put into EnergyFunction. //
// //
// Look for a note in the code like this one to see what part //
// you need to change for your own uses. //
// //
////////////////////////////////////////////////////////////////////
#include <stdlib.h>
#include <stdio.h>
#include <memory.h>
#include <conio.h>
#include <math.h>
#include <time.h>
#include "DESolver.h"
#define kIntIsOdd(x) (((x) & 0x00000001) == 1)
double FilterSolver::EnergyFunction(double testSolution[], bool& bAtSolution)
{
unsigned i;
double tempReal;
double tempImag;
// You probably will want to keep this if statement and its contents
if (mMinimumPhase)
{
// Make sure there are no zeros outside the unit circle
unsigned lastEvenZero = (mNumZeros & 0xfffffffe) - 1;
for (i = 0; i <= lastEvenZero; i+=2)
{
tempReal = testSolution[i];
tempImag = testSolution[i+1];
if ((tempReal*tempReal + tempImag*tempImag) > 1.0)
{
return 1.0e+300;
}
}
if (kIntIsOdd(mNumZeros))
{
tempReal = testSolution[mNumZeros - 1];
if ((tempReal * tempReal) > 1.0)
{
return 1.0e+300;
}
}
}
// Make sure there are no poles on or outside the unit circle
// You probably will want to keep this too
unsigned lastEvenPole = mNumZeros + (mNumPoles & 0xfffffffe) - 2;
for (i = mNumZeros; i <= lastEvenPole; i+=2)
{
tempReal = testSolution[i];
tempImag = testSolution[i+1];
if ((tempReal*tempReal + tempImag*tempImag) > 0.999999999)
{
return 1.0e+300;
}
}
// If you keep the for loop above, keep this too
if (kIntIsOdd(mNumPoles))
{
tempReal = testSolution[mNumZeros + mNumPoles - 1];
if ((tempReal * tempReal) > 1.0)
{
return 1.0e+300;
}
}
double* evenZeros = &(testSolution[0]);
double* evenPoles = &(testSolution[mNumZeros]);
double* oddZero = NULL;
double* oddPole = NULL;
double gain = testSolution[mNumZeros + mNumPoles];
if (kIntIsOdd(mNumZeros))
{
oddZero = &(testSolution[mNumZeros - 1]);
}
if (kIntIsOdd(mNumPoles))
{
oddPole = &(testSolution[mNumZeros + mNumPoles - 1]);
}
ComputeSpectrum(evenZeros, mNumZeros & 0xfffffffe, oddZero,
evenPoles, mNumPoles & 0xfffffffe, oddPole,
gain, &mSpectrum);
unsigned numPoints = mSpectrum.mNumValues;
/////////////////////////////////////////////////////////////////
// //
// Use the impulse response, held in the variable //
// "mSpectrum", to compute a score for the solution that //
// has been passed into this function. You probably don't //
// want to touch any of the code above this point, but //
// from here to the end of this function, it's all you! //
// //
/////////////////////////////////////////////////////////////////
#define kLnTwoToThe127 88.02969193111305
#define kRecipLn10 0.4342944819032518
// Compute square sum of errors for magnitude
double magnitudeError = 0.0;
double magnitude = 0.0;
double logMagnitude = 0.0;
tempReal = mSpectrum.mReals[0];
tempImag = mSpectrum.mImags[0];
magnitude = tempReal*tempReal + tempImag*tempImag;
double baseMagnitude = 0.0;
if (0.0 == magnitude)
{
baseMagnitude = -kLnTwoToThe127;
}
else
{
baseMagnitude = log(magnitude) * kRecipLn10;
baseMagnitude *= 0.5;
}
for (i = 0; i < numPoints; i++)
{
tempReal = mSpectrum.mReals[i];
tempImag = mSpectrum.mImags[i];
magnitude = tempReal*tempReal + tempImag*tempImag;
if (0.0 == magnitude)
{
logMagnitude = -kLnTwoToThe127;
}
else
{
logMagnitude = log(magnitude) * kRecipLn10;
logMagnitude *= 0.5; // Half the log because it's mag squared
}
logMagnitude -= baseMagnitude;
magnitudeError += logMagnitude * logMagnitude;
}
// Compute errors for phase
double phaseError = 0.0;
double phase = 0.0;
double componentError = 0.0;
double degree = 1;//((mNumZeros + 1) & 0xfffffffe) - 1;
double angleSpacing = -3.141592653589793 * 0.5 / numPoints * degree;
double targetPhase = 0.0;
double oldPhase = 0.0;
double phaseDifference = 0;
double totalPhaseTraversal = 0.0;
double traversalError = 0.0;
for (i = 0; i < (numPoints - 5); i++)
{
tempReal = mSpectrum.mReals[i];
tempImag = mSpectrum.mImags[i];
oldPhase = phase;
phase = atan2(tempImag, tempReal);
phaseDifference = phase - oldPhase;
if (phaseDifference > 3.141592653589793)
{
phaseDifference -= 3.141592653589793;
phaseDifference -= 3.141592653589793;
}
else if (phaseDifference < -3.141592653589793)
{
phaseDifference += 3.141592653589793;
phaseDifference += 3.141592653589793;
}
totalPhaseTraversal += phaseDifference;
componentError = cosh(200.0*(phaseDifference - angleSpacing)) - 0.5;
phaseError += componentError * componentError;
targetPhase += angleSpacing;
if (targetPhase < -3.141592653589793)
{
targetPhase += 3.141592653589793;
targetPhase += 3.141592653589793;
}
}
traversalError = totalPhaseTraversal - angleSpacing * numPoints;
traversalError *= traversalError;
double baseMagnitudeError = baseMagnitude * baseMagnitude;
// Compute weighted sum of the two subtotals
// Take square root
return sqrt(baseMagnitudeError*1.0 + magnitudeError*100.0 +
phaseError*400.0 + traversalError*4000000.0);
}
///////////////////////////////
int main(int argc, char** argv)
{
srand((unsigned)time(NULL));
unsigned numZeros;
unsigned numPoles;
bool minimumPhase;
if (argc < 4)
{
printf("Usage: FilterDesign.exe <minimumPhase?> <numZeros> <numPoles>\n");
return 0;
}
else
{
if (0 == atoi(argv[1]))
{
minimumPhase = false;
}
else
{
minimumPhase = true;
}
numZeros = (unsigned)atoi(argv[2]);
if (0 == numZeros)
{
numZeros = 1;
}
numPoles = (unsigned)atoi(argv[3]);
}
unsigned vectorLength = numZeros + numPoles + 1;
unsigned populationSize = vectorLength * 10;
FilterSolver theSolver(vectorLength, populationSize, 200,
numZeros, numPoles, minimumPhase);
double* minimumSolution = new double[vectorLength];
unsigned i;
if (minimumPhase)
{
for (i = 0; i < numZeros; i++)
{
minimumSolution[i] = -sqrt(0.5);
}
}
else
{
for (i = 0; i < numZeros; i++)
{
minimumSolution[i] = -10.0;
}
}
for (; i < (vectorLength - 1); i++)
{
minimumSolution[i] = -sqrt(0.5);
}
minimumSolution[vectorLength - 1] = 0.0;
double* maximumSolution = new double[vectorLength];
if (minimumPhase)
{
for (i = 0; i < numZeros; i++)
{
maximumSolution[i] = sqrt(0.5);
}
}
else
{
for (i = 0; i < numZeros; i++)
{
maximumSolution[i] = 10.0;
}
}
for (i = 0; i < (vectorLength - 1); i++)
{
maximumSolution[i] = sqrt(0.5);
}
maximumSolution[vectorLength - 1] = 2.0;
theSolver.Setup(minimumSolution, maximumSolution, 0, 0.5, 0.75);
theSolver.Solve(1000000);
double* bestSolution = theSolver.Solution();
printf("\nZeros:\n");
unsigned numEvenZeros = numZeros & 0xfffffffe;
for (i = 0; i < numEvenZeros; i+=2)
{
printf("%.10f +/- %.10fi\n", bestSolution[i], bestSolution[i+1]);
}
if (kIntIsOdd(numZeros))
{
printf("%.10f\n", bestSolution[numZeros-1]);
}
printf("Poles:\n");
unsigned lastEvenPole = numZeros + (numPoles & 0xfffffffe) - 2;
for (i = numZeros; i <= lastEvenPole; i+=2)
{
printf("%.10f +/- %.10fi\n", bestSolution[i], bestSolution[i+1]);
}
unsigned numRoots = numZeros + numPoles;
if (kIntIsOdd(numPoles))
{
printf("%.10f\n", bestSolution[numRoots-1]);
}
double gain = bestSolution[numRoots];
printf("Gain: %.10f\n", gain);
_getch();
unsigned j;
ASpectrum* spectrum = theSolver.Spectrum();
double logMagnitude;
printf("Magnitude Response, millibels:\n");
for (i = 0; i < 20; i++)
{
for (j = 0; j < 10; j++)
{
logMagnitude = kRecipLn10 *
log(spectrum->mReals[i*10 + j] * spectrum->mReals[i*10 + j] +
spectrum->mImags[i*10 + j] * spectrum->mImags[i*10 + j]);
if (logMagnitude < -9.999)
{
logMagnitude = -9.999;
}
printf("%+5.0f ", logMagnitude*1000);
}
printf("\n");
}
_getch();
double phase;
printf("Phase Response, milliradians:\n");
for (i = 0; i < 20; i++)
{
for (j = 0; j < 10; j++)
{
phase = atan2(spectrum->mImags[i*10 + j], spectrum->mReals[i*10 + j]);
printf("%+5.0f ", phase*1000);
}
printf("\n");
}
_getch();
printf("Biquad Sections:\n");
unsigned numBiquadSections =
(numZeros > numPoles) ? ((numZeros + 1) >> 1) : ((numPoles + 1) >> 1);
double x0, x1, x2;
double y0, y1, y2;
if (numZeros >=2)
{
x0 = (bestSolution[0]*bestSolution[0] + bestSolution[1]*bestSolution[1]) *
gain;
x1 = 2.0 * bestSolution[0] * gain;
x2 = gain;
}
else if (1 == numZeros)
{
x0 = bestSolution[0] * gain;
x1 = gain;
x2 = 0.0;
}
else
{
x0 = gain;
x1 = 0.0;
x2 = 0.0;
}
if (numPoles >= 2)
{
y0 = (bestSolution[numZeros]*bestSolution[numZeros] +
bestSolution[numZeros+1]*bestSolution[numZeros+1]);
y1 = 2.0 * bestSolution[numZeros];
y2 = 1.0;
}
else if (1 == numPoles)
{
y0 = bestSolution[numZeros];
y1 = 1.0;
y2 = 0.0;
}
else
{
y0 = 1.0;
y1 = 0.0;
y2 = 0.0;
}
x0 /= y0;
x1 /= y0;
x2 /= y0;
y1 /= y0;
y2 /= y0;
printf("y[n] = %.10fx[n]", x0);
if (numZeros > 0)
{
printf(" + %.10fx[n-1]", x1);
}
if (numZeros > 1)
{
printf(" + %.10fx[n-2]", x2);
}
printf("\n");
if (numPoles > 0)
{
printf(" + %.10fy[n-1]", y1);
}
if (numPoles > 1)
{
printf(" + &.10fy[n-2]", y2);
}
if (numPoles > 0)
{
printf("\n");
}
int numRemainingZeros = numZeros - 2;
int numRemainingPoles = numPoles - 2;
for (i = 1; i < numBiquadSections; i++)
{
if (numRemainingZeros >= 2)
{
x0 = (bestSolution[i*2] * bestSolution[i*2] +
bestSolution[i*2+1] * bestSolution[i*2+1]);
x1 = -2.0 * bestSolution[i*2];
x2 = 1.0;
}
else if (numRemainingZeros >= 1)
{
x0 = bestSolution[i*2];
x1 = 1.0;
x2 = 0.0;
}
else
{
x0 = 1.0;
x1 = 0.0;
x2 = 0.0;
}
if (numRemainingPoles >= 2)
{
y0 = (bestSolution[i*2+numZeros] * bestSolution[i*2+numZeros] +
bestSolution[i*2+numZeros+1] * bestSolution[i*2+numZeros+1]);
y1 = -2.0 * bestSolution[i*2+numZeros];
y2 = 1.0;
}
else if (numRemainingPoles >= 1)
{
y0 = bestSolution[i*2+numZeros];
y1 = 1.0;
y2 = 0.0;
}
else
{
y0 = 1.0;
y1 = 0.0;
y2 = 0.0;
}
x0 /= y0;
x1 /= y0;
x2 /= y0;
y1 /= y0;
y2 /= y0;
printf("y[n] = %.10fx[n]", x0);
if (numRemainingZeros > 0)
{
printf(" + %.10fx[n-1]", x1);
}
if (numRemainingZeros > 1)
{
printf(" + %.10fx[n-2]", x2);
}
printf("\n");
if (numRemainingPoles > 0)
{
printf(" + %.10fy[n-1]", -y1);
}
if (numRemainingPoles > 1)
{
printf(" + %.10fy[n-2]", -y2);
}
if (numRemainingPoles > 0)
{
printf("\n");
}
numRemainingZeros -= 2;
numRemainingPoles -= 2;
}
_getch();
printf("Full Expansion:\n");
double* xpolynomial = new double[numRoots + 1];
memset(xpolynomial, 0, sizeof(double) * (numRoots + 1));
xpolynomial[0] = 1.0;
if (numZeros >= 2)
{
xpolynomial[0] = bestSolution[0] * bestSolution[0] +
bestSolution[1] * bestSolution[1];
xpolynomial[1] = -2.0 * bestSolution[0];
xpolynomial[2] = 1.0;
}
else if (numZeros == 1)
{
xpolynomial[0] = bestSolution[0];
xpolynomial[1] = 1.0;
}
else
{
xpolynomial[0] = 1.0;
}
for (i = 2, numRemainingZeros = numZeros; numRemainingZeros >= 2;
i += 2, numRemainingZeros-=2)
{
x2 = 1.0;
x1 = -2.0 * bestSolution[i];
x0 = bestSolution[i] * bestSolution[i] +
bestSolution[i+1] * bestSolution[i+1];
for (j = numRoots; j > 1; j--)
{
xpolynomial[j] = xpolynomial[j-2] + xpolynomial[j-1] * x1 +
xpolynomial[j] * x0;
}
xpolynomial[1] = xpolynomial[0] * x1 + xpolynomial[1] * x0;
xpolynomial[0] *= x0;
}
if (numRemainingZeros > 0)
{
x1 = 1.0;
x0 = bestSolution[numZeros-1];
for (j = numRoots; j > 0; j--)
{
xpolynomial[j] = xpolynomial[j-1] + xpolynomial[j] * x0;
}
xpolynomial[0] *= x0;
}
double* ypolynomial = new double[numRoots + 1];
memset(ypolynomial, 0, sizeof(double) * (numRoots + 1));
ypolynomial[0] = 1.0;
if (numPoles >= 2)
{
ypolynomial[0] = bestSolution[numZeros] * bestSolution[numZeros] +
bestSolution[numZeros+1] * bestSolution[numZeros+1];
ypolynomial[1] = -2.0 * bestSolution[numZeros];
ypolynomial[2] = 1.0;
}
else if (numPoles == 1)
{
ypolynomial[0] = bestSolution[numZeros];
ypolynomial[1] = 1.0;
}
else
{
xpolynomial[0] = 1.0;
}
for (i = 2, numRemainingPoles = numPoles; numRemainingPoles >= 2;
i += 2, numRemainingPoles-=2)
{
y2 = 1.0;
y1 = -2.0 * bestSolution[numZeros+i];
y0 = bestSolution[numZeros+i] * bestSolution[numZeros+i] +
bestSolution[numZeros+i+1] * bestSolution[numZeros+i+1];
for (j = numRoots; j > 1; j--)
{
ypolynomial[j] = ypolynomial[j-2] + ypolynomial[j-1] * y1 +
ypolynomial[j] * y0;
}
ypolynomial[1] = ypolynomial[0] * y1 + ypolynomial[1] * y0;
ypolynomial[0] *= y0;
}
if (numRemainingPoles > 0)
{
y1 = 1.0;
y0 = bestSolution[numZeros+numPoles-1];
for (j = numRoots; j > 0; j--)
{
ypolynomial[j] = ypolynomial[j-1] + ypolynomial[j] * y0;
}
ypolynomial[0] *= y0;
}
y0 = ypolynomial[0];
for (i = 0; i <= numRoots; i++)
{
xpolynomial[i] /= y0;
ypolynomial[i] /= y0;
}
printf("y[n] = %.10fx[n]", xpolynomial[0]*gain);
for (i = 1; i <= numZeros; i++)
{
printf(" + %.10fx[n-%d]", xpolynomial[i]*gain, i);
if ((i % 3) == 2)
{
printf("\n");
}
}
if ((i % 3) != 0)
{
printf("\n");
}
if (numPoles > 0)
{
printf(" ");
}
for (i = 1; i <= numPoles; i++)
{
printf(" + %.10fy[n-%d]", -ypolynomial[i], i);
if ((i % 3) == 2)
{
printf("\n");
}
}
if ((i % 3) != 0)
{
printf("\n");
}
delete[] minimumSolution;
delete[] maximumSolution;
delete[] xpolynomial;
delete[] ypolynomial;
}
bool ComputeSpectrum(double* evenZeros, unsigned numEvenZeros, double* oddZero,
double* evenPoles, unsigned numEvenPoles, double* oddPole,
double gain, ASpectrum* spectrum)
{
unsigned i, j;
// For equally spaced points on the unit circle
unsigned numPoints = spectrum->mNumValues;
double spacingAngle = 3.141592653589793 / (numPoints - 1);
double pointArgument = 0.0;
double pointReal = 0.0;
double pointImag = 0.0;
double rootReal = 0.0;
double rootImag = 0.0;
double differenceReal = 0.0;
double differenceImag = 0.0;
double responseReal = 1.0;
double responseImag = 0.0;
double recipSquareMagnitude = 0.0;
double recipReal = 0.0;
double recipImag = 0.0;
double tempRealReal = 0.0;
double tempRealImag = 0.0;
double tempImagReal = 0.0;
double tempImagImag = 0.0;
for (i = 0; i < numPoints; i++)
{
responseReal = 1.0;
responseImag = 0.0;
// The imaginary component is negated because we're using 1/z, not z
pointReal = cos(pointArgument);
pointImag = -sin(pointArgument);
// For each even zero
for (j = 0; j < numEvenZeros; j+=2)
{
rootReal = evenZeros[j];
rootImag = evenZeros[j + 1];
// Compute distance from that zero to that point
differenceReal = pointReal - rootReal;
differenceImag = pointImag - rootImag;
// Multiply that distance by the accumulating product
tempRealReal = responseReal * differenceReal;
tempRealImag = responseReal * differenceImag;
tempImagReal = responseImag * differenceReal;
tempImagImag = responseImag * differenceImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
// Do the same with the conjugate root
differenceImag = pointImag + rootImag;
tempRealReal = responseReal * differenceReal;
tempRealImag = responseReal * differenceImag;
tempImagReal = responseImag * differenceReal;
tempImagImag = responseImag * differenceImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
// The following way is little faster, if any
// response *= (1/z - r) * (1/z - conj(r))
// *= r*conj(r) - (r + conj(r))/z + 1/(z*z)
// *= real(r)*real(r) + imag(r)*imag(r) - 2*real(r)/z + 1/(z*z)
// *= ... - 2*real(r)*conj(z) + conj(z)*conj(z)
// *= ... - 2*real(r)*real(z) + 2i*real(r)*imag(z) +
// real(z)*real(z) - 2i*real(z)*imag(z) + imag(z)*imag(z)
// *= real(r)*real(r) + imag(r)*imag(r) - 2*real(r)*real(z) +
// real(z)*real(z) + imag(z)*imag(z) +
// 2i * imag(z) * (real(r) - real(z))
// *= (real(r) - real(z))^2 + imag(r)^2 + imag(z)^2 +
// 2i * imag(z) * (real(r) - real(z))
// This ends up being 8 multiplications, 6 additions
}
if (NULL != oddZero)
{
rootReal = *oddZero;
// Compute distance from that zero to that point
differenceReal = pointReal - rootReal;
differenceImag = pointImag;
// Multiply that distance by the accumulating product
tempRealReal = responseReal * differenceReal;
tempRealImag = responseReal * differenceImag;
tempImagReal = responseImag * differenceReal;
tempImagImag = responseImag * differenceImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
}
// For each pole
for (j = 0; j < numEvenPoles; j+=2)
{
rootReal = evenPoles[j];
rootImag = evenPoles[j + 1];
// Compute distance from that pole to that point
differenceReal = pointReal - rootReal;
differenceImag = pointImag - rootImag;
// Multiply the reciprocal of that distance by the accumulating product
recipSquareMagnitude = 1.0 / (differenceReal * differenceReal +
differenceImag * differenceImag);
recipReal = differenceReal * recipSquareMagnitude;
recipImag = -differenceImag * recipSquareMagnitude;
tempRealReal = responseReal * recipReal;
tempRealImag = responseReal * recipImag;
tempImagReal = responseImag * recipReal;
tempImagImag = responseImag * recipImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
// Do the same with the conjugate root
differenceImag = pointImag + rootImag;
recipSquareMagnitude = 1.0 / (differenceReal * differenceReal +
differenceImag * differenceImag);
recipReal = differenceReal * recipSquareMagnitude;
recipImag = -differenceImag * recipSquareMagnitude;
tempRealReal = responseReal * recipReal;
tempRealImag = responseReal * recipImag;
tempImagReal = responseImag * recipReal;
tempImagImag = responseImag * recipImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
}
if (NULL != oddPole)
{
rootReal = *oddPole;
// Compute distance from that point to that zero
differenceReal = pointReal - rootReal;
differenceImag = pointImag;
// Multiply the reciprocal of that distance by the accumulating product
recipSquareMagnitude = 1.0 / (differenceReal * differenceReal +
differenceImag * differenceImag);
recipReal = differenceReal * recipSquareMagnitude;
recipImag = -differenceImag * recipSquareMagnitude;
tempRealReal = responseReal * recipReal;
tempRealImag = responseReal * recipImag;
tempImagReal = responseImag * recipReal;
tempImagImag = responseImag * recipImag;
| | | |
