diff options
Diffstat (limited to 'trunk/main/libresample/src/filterkit.c')
| -rw-r--r-- | trunk/main/libresample/src/filterkit.c | 215 |
1 files changed, 215 insertions, 0 deletions
diff --git a/trunk/main/libresample/src/filterkit.c b/trunk/main/libresample/src/filterkit.c new file mode 100644 index 000000000..bc92285f2 --- /dev/null +++ b/trunk/main/libresample/src/filterkit.c @@ -0,0 +1,215 @@ +/********************************************************************** + + resamplesubs.c + + Real-time library interface by Dominic Mazzoni + + Based on resample-1.7: + http://www-ccrma.stanford.edu/~jos/resample/ + + License: LGPL - see the file LICENSE.txt for more information + + This file provides Kaiser-windowed low-pass filter support, + including a function to create the filter coefficients, and + two functions to apply the filter at a particular point. + +**********************************************************************/ + +/* Definitions */ +#include "resample_defs.h" + +#include "filterkit.h" + +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +/* LpFilter() + * + * reference: "Digital Filters, 2nd edition" + * R.W. Hamming, pp. 178-179 + * + * Izero() computes the 0th order modified bessel function of the first kind. + * (Needed to compute Kaiser window). + * + * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with + * the following characteristics: + * + * c[] = array in which to store computed coeffs + * frq = roll-off frequency of filter + * N = Half the window length in number of coeffs + * Beta = parameter of Kaiser window + * Num = number of coeffs before 1/frq + * + * Beta trades the rejection of the lowpass filter against the transition + * width from passband to stopband. Larger Beta means a slower + * transition and greater stopband rejection. See Rabiner and Gold + * (Theory and Application of DSP) under Kaiser windows for more about + * Beta. The following table from Rabiner and Gold gives some feel + * for the effect of Beta: + * + * All ripples in dB, width of transition band = D*N where N = window length + * + * BETA D PB RIP SB RIP + * 2.120 1.50 +-0.27 -30 + * 3.384 2.23 0.0864 -40 + * 4.538 2.93 0.0274 -50 + * 5.658 3.62 0.00868 -60 + * 6.764 4.32 0.00275 -70 + * 7.865 5.0 0.000868 -80 + * 8.960 5.7 0.000275 -90 + * 10.056 6.4 0.000087 -100 + */ + +#define IzeroEPSILON 1E-21 /* Max error acceptable in Izero */ + +static double Izero(double x) +{ + double sum, u, halfx, temp; + int n; + + sum = u = n = 1; + halfx = x/2.0; + do { + temp = halfx/(double)n; + n += 1; + temp *= temp; + u *= temp; + sum += u; + } while (u >= IzeroEPSILON*sum); + return(sum); +} + +void lrsLpFilter(double c[], int N, double frq, double Beta, int Num) +{ + double IBeta, temp, temp1, inm1; + int i; + + /* Calculate ideal lowpass filter impulse response coefficients: */ + c[0] = 2.0*frq; + for (i=1; i<N; i++) { + temp = PI*(double)i/(double)Num; + c[i] = sin(2.0*temp*frq)/temp; /* Analog sinc function, cutoff = frq */ + } + + /* + * Calculate and Apply Kaiser window to ideal lowpass filter. + * Note: last window value is IBeta which is NOT zero. + * You're supposed to really truncate the window here, not ramp + * it to zero. This helps reduce the first sidelobe. + */ + IBeta = 1.0/Izero(Beta); + inm1 = 1.0/((double)(N-1)); + for (i=1; i<N; i++) { + temp = (double)i * inm1; + temp1 = 1.0 - temp*temp; + temp1 = (temp1<0? 0: temp1); /* make sure it's not negative since + we're taking the square root - this + happens on Pentium 4's due to tiny + roundoff errors */ + c[i] *= Izero(Beta*sqrt(temp1)) * IBeta; + } +} + +float lrsFilterUp(float Imp[], /* impulse response */ + float ImpD[], /* impulse response deltas */ + UWORD Nwing, /* len of one wing of filter */ + BOOL Interp, /* Interpolate coefs using deltas? */ + float *Xp, /* Current sample */ + double Ph, /* Phase */ + int Inc) /* increment (1 for right wing or -1 for left) */ +{ + float *Hp, *Hdp = NULL, *End; + double a = 0; + float v, t; + + Ph *= Npc; /* Npc is number of values per 1/delta in impulse response */ + + v = 0.0; /* The output value */ + Hp = &Imp[(int)Ph]; + End = &Imp[Nwing]; + if (Interp) { + Hdp = &ImpD[(int)Ph]; + a = Ph - floor(Ph); /* fractional part of Phase */ + } + + if (Inc == 1) /* If doing right wing... */ + { /* ...drop extra coeff, so when Ph is */ + End--; /* 0.5, we don't do too many mult's */ + if (Ph == 0) /* If the phase is zero... */ + { /* ...then we've already skipped the */ + Hp += Npc; /* first sample, so we must also */ + Hdp += Npc; /* skip ahead in Imp[] and ImpD[] */ + } + } + + if (Interp) + while (Hp < End) { + t = *Hp; /* Get filter coeff */ + t += (*Hdp)*a; /* t is now interp'd filter coeff */ + Hdp += Npc; /* Filter coeff differences step */ + t *= *Xp; /* Mult coeff by input sample */ + v += t; /* The filter output */ + Hp += Npc; /* Filter coeff step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + else + while (Hp < End) { + t = *Hp; /* Get filter coeff */ + t *= *Xp; /* Mult coeff by input sample */ + v += t; /* The filter output */ + Hp += Npc; /* Filter coeff step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + + return v; +} + +float lrsFilterUD(float Imp[], /* impulse response */ + float ImpD[], /* impulse response deltas */ + UWORD Nwing, /* len of one wing of filter */ + BOOL Interp, /* Interpolate coefs using deltas? */ + float *Xp, /* Current sample */ + double Ph, /* Phase */ + int Inc, /* increment (1 for right wing or -1 for left) */ + double dhb) /* filter sampling period */ +{ + float a; + float *Hp, *Hdp, *End; + float v, t; + double Ho; + + v = 0.0; /* The output value */ + Ho = Ph*dhb; + End = &Imp[Nwing]; + if (Inc == 1) /* If doing right wing... */ + { /* ...drop extra coeff, so when Ph is */ + End--; /* 0.5, we don't do too many mult's */ + if (Ph == 0) /* If the phase is zero... */ + Ho += dhb; /* ...then we've already skipped the */ + } /* first sample, so we must also */ + /* skip ahead in Imp[] and ImpD[] */ + + if (Interp) + while ((Hp = &Imp[(int)Ho]) < End) { + t = *Hp; /* Get IR sample */ + Hdp = &ImpD[(int)Ho]; /* get interp bits from diff table*/ + a = Ho - floor(Ho); /* a is logically between 0 and 1 */ + t += (*Hdp)*a; /* t is now interp'd filter coeff */ + t *= *Xp; /* Mult coeff by input sample */ + v += t; /* The filter output */ + Ho += dhb; /* IR step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + else + while ((Hp = &Imp[(int)Ho]) < End) { + t = *Hp; /* Get IR sample */ + t *= *Xp; /* Mult coeff by input sample */ + v += t; /* The filter output */ + Ho += dhb; /* IR step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + + return v; +} |