This is filterkit.c in view mode; [Download] [Up]
/*
* filterkit.c (library "filterkit.a"): Kaiser-windowed low-pass filter support.
*
/* filterkit.c
*
* LpFilter() - Calculates the filter coeffs for a Kaiser-windowed low-pass
* filter with a given roll-off frequency. These coeffs
* are stored into a array of doubles.
* writeFilter() - Writes a filter to a file.
* makeFilter() - Calls LpFilter() to create a filter, then scales the double
* coeffs into an array of half words.
* readFilter() - Reads a filter from a file.
* FilterUp() - Applies a filter to a given sample when up-converting.
* FilterUD() - Applies a filter to a given sample when up- or down-
* converting.
* initZerox() - Initialization routine for the zerox() function. Must
* be called before zerox() is called. This routine loads
* the correct filter so zerox() can use it.
* zerox() - Given a pointer into a sample, finds a zero-crossing on the
* interval [pointer-1:pointer+2] by iteration.
* Query() - Ask the user for a yes/no question with prompt, default,
* and optional help.
* GetUShort() - Ask the user for a unsigned short with prompt, default,
* and optional help.
* GetDouble() - Ask the user for a double with prompt, default, and
* optional help.
* GetString() - Ask the user for a string with prompt, default, and
* optional help.
*/
#import "resample.h"
#import "filterkit.h"
#import <libc.h>
#import <math.h>
/*
* Getstr() will print the passed "prompt" as a message to the user, and
* wait for the user to type an input string. The string is
* then copied into "answer". If the user types just a carriage
* return, then the string "defalt" is copied into "answer".
* ???
* "Answer" and "defalt" may be the same string, in which case,
* the default value will be the original contents of "answer".
* ???
*/
static void getstr(char *prompt, char *defalt, char *answer)
{
char *p,s[200];
printf("%s (default = %s):",prompt,defalt);
strcpy(s, prompt);
if (!(*s))
strcpy(s, "input:");
p = s;
while (*p && *p != '(')
p++;
p--;
while (*p == ' ')
p--;
*(++p) = '\0';
gets(answer);
if (answer[0] == '\0') {
strcpy(answer, defalt);
printf("\t%s = %s\n",s,answer);
} else {
printf("\t%s set to %s\n",s,answer);
}
}
/* 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 LpFilter(double c[], int N, double frq, double Beta, int Num)
{
double IBeta, temp, 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;
c[i] *= Izero(Beta*sqrt(1.0-temp*temp)) * IBeta;
}
}
/* Write a filter to a file
* Filter file format:
* file name: "F" Nmult "T" Nhc ".filter"
* 1st line: the string "ScaleFactor" followed by its value.
* 2nd line: the string "Length" followed by Nwing's value.
* 3rd line: the string "Nmult" followed by Nmult's value.
* 4th line: the string "Coeffs:" on a separate line.
* following lines: Nwing number of 16-bit impulse response values
* in the right wing of the impulse response (the Imp[] array).
* (Nwing is equal to Npc*(Nmult+1)/2+1, where Npc is defined in the
* file "resample.h".) Each coefficient is on a separate line.
* next line: the string "Differences:" on a separate line.
* following lines: Nwing number of 16-bit impulse-response
* successive differences: ImpD[i] = Imp[i+1] - Imp[i].
* ERROR codes:
* 0 - no error
* 1 - could not open file
*/
int writeFilter(HWORD Imp[], HWORD ImpD[], UHWORD LpScl, UHWORD Nmult,
UHWORD Nwing)
{
char fname[30];
FILE *fp;
int i;
sprintf(fname, "F%dT%d.filter", Nmult, Nhc);
fp = fopen(fname, "w");
if (!fp)
return(1);
fprintf(fp, "ScaleFactor %d\n", LpScl);
fprintf(fp, "Length %d\n", Nwing);
fprintf(fp, "Nmult %d\n", Nmult);
fprintf(fp, "Coeffs:\n");
for (i=0; i<Nwing; i++) /* Put array of 16-bit filter coefficients */
fprintf(fp, "%d\n", Imp[i]);
fprintf(fp, "Differences:\n");
for (i=0; i<Nwing; i++) /* Put array of 16-bit filter coeff differences */
fprintf(fp, "%d\n", ImpD[i]);
fclose(fp);
printf("Wrote filter file '%s' in current directory.\n",fname);
return(0);
}
/* ERROR return codes:
* 0 - no error
* 1 - Nwing too large (Nwing is > MAXNWING)
* 2 - Froll is not in interval [0:1)
* 3 - Beta is < 1.0
* 4 - LpScl will not fit in 16-bits
*
* Made following global to avoid stack problems in Sun3 compilation: */
#define MAXNWING 8192
static double ImpR[MAXNWING];
int makeFilter(HWORD Imp[], HWORD ImpD[], UHWORD *LpScl, UHWORD Nwing,
double Froll, double Beta)
{
double DCgain, Scl, Maxh;
HWORD Dh;
int i, temp;
if (Nwing > MAXNWING) /* Check for valid parameters */
return(1);
if ((Froll<=0) || (Froll>1))
return(2);
if (Beta < 1)
return(3);
/*
* Design Kaiser-windowed sinc-function low-pass filter
*/
LpFilter(ImpR, (int)Nwing, 0.5*Froll, Beta, Npc);
/* Compute the DC gain of the lowpass filter, and its maximum coefficient
* magnitude. Scale the coefficients so that the maximum coeffiecient just
* fits in Nh-bit fixed-point, and compute LpScl as the NLpScl-bit (signed)
* scale factor which when multiplied by the output of the lowpass filter
* gives unity gain. */
DCgain = 0;
Dh = Npc; /* Filter sampling period for factors>=1 */
for (i=Dh; i<Nwing; i+=Dh)
DCgain += ImpR[i];
DCgain = 2*DCgain + ImpR[0]; /* DC gain of real coefficients */
for (Maxh=i=0; i<Nwing; i++)
Maxh = MAX(Maxh, fabs(ImpR[i]));
Scl = ((1<<(Nh-1))-1)/Maxh; /* Map largest coeff to 16-bit maximum */
temp = fabs((1<<(NLpScl+Nh))/(DCgain*Scl));
if (temp >= 1<<16)
return(4); /* Filter scale factor overflows UHWORD */
*LpScl = temp;
/* Scale filter coefficients for Nh bits and convert to integer */
if (ImpR[0] < 0) /* Need pos 1st value for LpScl storage */
Scl = -Scl;
for (i=0; i<Nwing; i++) /* Scale them */
ImpR[i] *= Scl;
for (i=0; i<Nwing; i++) /* Round them */
Imp[i] = ImpR[i] + 0.5;
/* ImpD makes linear interpolation of the filter coefficients faster */
for (i=0; i<Nwing-1; i++)
ImpD[i] = Imp[i+1] - Imp[i];
ImpD[Nwing-1] = - Imp[Nwing-1]; /* Last coeff. not interpolated */
return(0);
}
/* Read-in a filter
* Filter file format:
* file name: "F" Nmult "T" Nhc ".filter"
* 1st line: the string "ScaleFactor" followed by its value.
* 2nd line: the string "Length" followed by Nwing's value.
* 3rd line: the string "Coeffs:" on separate line.
* Nwing number of 16-bit impulse response values in the right
* wing of the impulse response. (Length=Npc*(Nmult+1)/2+1,
* where originally Npc=2^9, and Nmult=13.) Each on separate line.
* The string "Differences:" on separate line.
* Nwing number of 16-bit impulse-response successive differences:
* ImpDiff[i] = Imp[i+1] - Imp[i].
*
* ERROR return codes:
* 0 - no error
* 1 - file not found
* 2 - invalid ScaleFactor in file
* 3 - invalid Length in file
* 4 - invalid Nmult in file
*/
int readFilter(char *filterFile, HWORD **ImpP, HWORD **ImpDP, UHWORD *LpScl,
UHWORD *Nmult, UHWORD *Nwing)
{
char *fname;
FILE *fp;
int i, temp;
HWORD *Imp,*ImpD;
if (!filterFile || !(*filterFile)) {
fname = (char *) malloc(32);
if ((*Nmult)>0 && ((*Nmult)&1))
sprintf(fname, "F%dT%d.filter", *Nmult, Nhc);
else
sprintf(fname, "F65dT%d.filter", Nhc);
} else
fname = filterFile;
fp = fopen(fname, "r");
if (fp == NULL)
return(1);
fscanf(fp, "ScaleFactor ");
if (1 != fscanf(fp,"%d",&temp))
return(2);
*LpScl = temp;
fscanf(fp, "\nLength ");
if (1 != fscanf(fp,"%d",&temp))
return(3);
*Nwing = temp;
fscanf(fp, "\nNmult ");
if (1 != fscanf(fp,"%d",&temp))
return(4);
*Nmult = temp;
Imp = (HWORD *) malloc(*Nwing * sizeof(HWORD));
fscanf(fp, "\nCoeffs:\n");
for (i=0; i<*Nwing; i++) { /* Get array of 16-bit filter coefficients */
fscanf(fp, "%d\n", &temp);
Imp[i] = temp;
}
ImpD = (HWORD *) malloc(*Nwing * sizeof(HWORD));
fscanf(fp, "\nDifferences:\n");
for (i=0; i<*Nwing; i++) { /* Get array of 16bit filter coeff differences */
fscanf(fp, "%d\n", &temp);
ImpD[i] = temp;
}
fclose(fp);
if (!filterFile || !(*filterFile))
free(fname);
*ImpP = Imp;
*ImpDP = ImpD;
return(0);
}
WORD FilterUp(HWORD Imp[], HWORD ImpD[],
UHWORD Nwing, BOOL Interp,
HWORD *Xp, HWORD Ph, HWORD Inc)
{
HWORD *Hp, *Hdp = NULL, *End;
HWORD a = 0;
WORD v, t;
v=0;
Hp = &Imp[Ph>>Na];
End = &Imp[Nwing];
if (Interp) {
Hdp = &ImpD[Ph>>Na];
a = Ph & Amask;
}
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 += (((WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */
Hdp += Npc; /* Filter coeff differences step */
t *= *Xp; /* Mult coeff by input sample */
if (t & (1<<(Nhxn-1))) /* Round, if needed */
t += (1<<(Nhxn-1));
t >>= Nhxn; /* Leave some guard bits, but come back some */
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 */
if (t & (1<<(Nhxn-1))) /* Round, if needed */
t += (1<<(Nhxn-1));
t >>= Nhxn; /* Leave some guard bits, but come back some */
v += t; /* The filter output */
Hp += Npc; /* Filter coeff step */
Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */
}
return(v);
}
WORD FilterUD( HWORD Imp[], HWORD ImpD[],
UHWORD Nwing, BOOL Interp,
HWORD *Xp, HWORD Ph, HWORD Inc, UHWORD dhb)
{
HWORD a;
HWORD *Hp, *Hdp, *End;
WORD v, t;
UWORD Ho;
v=0;
Ho = (Ph*(UWORD)dhb)>>Np;
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[Ho>>Na]) < End) {
t = *Hp; /* Get IR sample */
Hdp = &ImpD[Ho>>Na]; /* get interp (lower Na) bits from diff table*/
a = Ho & Amask; /* a is logically between 0 and 1 */
t += (((WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */
t *= *Xp; /* Mult coeff by input sample */
if (t & 1<<(Nhxn-1)) /* Round, if needed */
t += 1<<(Nhxn-1);
t >>= Nhxn; /* Leave some guard bits, but come back some */
v += t; /* The filter output */
Ho += dhb; /* IR step */
Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */
}
else
while ((Hp = &Imp[Ho>>Na]) < End) {
t = *Hp; /* Get IR sample */
t *= *Xp; /* Mult coeff by input sample */
if (t & 1<<(Nhxn-1)) /* Round, if needed */
t += 1<<(Nhxn-1);
t >>= Nhxn; /* Leave some guard bits, but come back some */
v += t; /* The filter output */
Ho += dhb; /* IR step */
Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */
}
return(v);
}
/*
* double zerox(Data, Factor)
* HWORD *Data;
* double Factor;
* Given a pointer into a sound sample, this function uses a low-pass
* filter to estimate the x coordinate of the zero-crossing which must ocurr
* between Data[0] and Data[1]. This value is returned as the value of the
* function. A return value of -100 indicates there was no zero-crossing in
* the x interval [-1,2]. Factor is the resampling factor: Rate(out) /
* Rate(in). Nmult (which determines which filter is used) is passed the
* zerox's initialization routine: initZerox(Nmult)
* UHWORD Nmult;
*/
static UHWORD LpScl, Nmult, Nwing;
static HWORD Imp[MAXNWING];
static HWORD ImpD[MAXNWING];
/* ERROR return values:
* 0 - no error
* 1 - Nmult is even (should be odd)
* 2 - filter file not found
* 3 - invalid ScaleFactor in input file
* 4 - invalid Length in file
* 5 - invalid Nmult in file
*/
int initZerox(UHWORD tempNmult)
{
int err;
/* Check for illegal input values */
if (!(tempNmult % 2))
return(1);
if (err = readFilter(NULL, (HWORD **)&Imp, (HWORD **)&ImpD,
&LpScl, &tempNmult, &Nwing))
return(1+err);
Nmult = tempNmult;
return(0);
}
#define MAXITER 64
#define ZeroxEPSILON (1E-4)
#define ZeroxMAXERROR (5.0)
double zerox(HWORD *Data, double Factor)
{
double x, out;
double lo, hi;
double dh;
UWORD dhb;
WORD v;
int i;
if (!Data[0])
return (0.0);
if (!Data[1])
return (1.0);
if (Data[0] < Data[1])
{
lo = -1.0;
hi = 2.0;
}
else
{
lo = 2.0;
hi = -1.0;
}
dh = (Factor<1) ? (Factor*Npc) : (Npc);
dhb = dh * (1<<Na) + 0.5;
for (i=0; i<MAXITER; i++)
{
x = (hi+lo)/2.0;
v = FilterUD(Imp,ImpD,Nwing,TRUE,Data, (HWORD)(x*Pmask), -1,dhb);
v += FilterUD(Imp,ImpD,Nwing,TRUE,Data+1,(HWORD)((1-x)*Pmask), 1,dhb);
v >>= Nhg;
v *= LpScl;
out = (double)v / (double)(1<<NLpScl);
if (out < 0.0)
lo = x;
else
hi = x;
if (ABS(out) <= ZeroxEPSILON)
return(x);
}
printf("|ZeroX Error| x:%g, \t Data[x]:%d, \t Data[x+1]:%d\n",
x, *Data, *(Data+1));
printf("|\tABS(out):%g \t EPSILON:%g\n", ABS(out),ZeroxEPSILON);
if (ABS(out) <= ZeroxMAXERROR)
return(x);
return(-100.0);
}
BOOL Query(char *prompt, BOOL deflt, char *help)
{
char s[80];
while (TRUE)
{
sprintf(s,"\n%s%s", prompt, (*help) ? " (Type ? for help)" : "");
getstr(s,(deflt)?"yes":"no",s);
if (*s=='?' && *help)
printf(help);
if (*s=='Y' || *s=='y')
return(TRUE);
if (*s=='N' || *s=='n')
return(FALSE);
}
}
char *GetString(char *prompt, char *deflt, char *help)
{
char s[200];
while (TRUE)
{
sprintf(s,"\n%s%s",prompt, (*help) ? " (Type ? for Help)" : "");
getstr(s,deflt,s);
if (*s=='?' && *help)
printf(help);
else
return(s);
}
}
double GetDouble(char *title, double deflt, char *help)
{
char s[80],sdeflt[80];
double newval;
while (TRUE)
{
sprintf(s,"\n%s:%s",title, (*help) ? " (Type ? for Help)" : "");
sprintf(sdeflt,"%g",deflt);
getstr(s,sdeflt,s);
if (*s=='?' && *help)
printf(help);
else
{
if (!sscanf(s,"%lf",&newval))
return(deflt);
return(newval);
}
}
}
unsigned short GetUShort(char *title, unsigned short deflt, char *help)
{
char s[80],sdeflt[80];
int newval;
while (TRUE)
{
sprintf(s,"\n%s:%s",title, (*help) ? " (Type ? for Help)" : "");
sprintf(sdeflt,"%d",deflt);
getstr(s,sdeflt,s);
if (*s=='?' && *help)
printf(help);
else
{
if (!sscanf(s,"%d",&newval))
printf("unchanged (%d)\n",(newval=deflt));
if (newval < 0)
printf("Error: value must be >= zero\n");
else
return(newval);
}
}
}
These are the contents of the former NiCE NeXT User Group NeXTSTEP/OpenStep software archive, currently hosted by Netfuture.ch.