/********************************************************************
* *
* THIS FILE IS PART OF THE Ogg Vorbis SOFTWARE CODEC SOURCE CODE. *
* USE, DISTRIBUTION AND REPRODUCTION OF THIS SOURCE IS GOVERNED BY *
* THE GNU PUBLIC LICENSE 2, WHICH IS INCLUDED WITH THIS SOURCE. *
* PLEASE READ THESE TERMS DISTRIBUTING. *
* *
* THE OggSQUISH SOURCE CODE IS (C) COPYRIGHT 1994-2000 *
* by Monty <monty@xiph.org> and The XIPHOPHORUS Company *
* http://www.xiph.org/ *
* *
********************************************************************
function: Direct Form I, II IIR filters, plus some specializations
last mod: $Id$
********************************************************************/
/* LPC is actually a degenerate case of form I/II filters, but we need
both */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "iir.h"
void IIR_init(IIR_state *s,int stages,double gain, double *A, double *B){
memset(s,0,sizeof(IIR_state));
s->stages=stages;
s->gain=gain;
s->coeff_A=malloc(stages*sizeof(double));
s->coeff_B=malloc((stages+1)*sizeof(double));
s->z_A=calloc(stages*2,sizeof(double));
s->z_B=calloc(stages*2,sizeof(double));
memcpy(s->coeff_A,A,stages*sizeof(double));
memcpy(s->coeff_B,B,(stages+1)*sizeof(double));
}
void IIR_clear(IIR_state *s){
if(s){
free(s->coeff_A);
free(s->coeff_B);
free(s->z_A);
free(s->z_B);
memset(s,0,sizeof(IIR_state));
}
}
double IIR_filter(IIR_state *s,double in){
int stages=s->stages,i;
double newA;
double newB=0;
double *zA=s->z_A+s->ring;
newA=in/=s->gain;
for(i=0;i<stages;i++){
newA+= s->coeff_A[i] * zA[i];
newB+= s->coeff_B[i] * zA[i];
}
newB+=newA*s->coeff_B[stages];
zA[0]=zA[stages]=newA;
if(++s->ring>=stages)s->ring=0;
return(newB);
}
/* this assumes the symmetrical structure of the feed-forward stage of
a Chebyshev bandpass to save multiplies */
double IIR_filter_ChebBand(IIR_state *s,double in){
int stages=s->stages,i;
double newA;
double newB=0;
double *zA=s->z_A+s->ring;
newA=in/=s->gain;
newA+= s->coeff_A[0] * zA[0];
for(i=1;i<(stages>>1);i++){
newA+= s->coeff_A[i] * zA[i];
newB+= s->coeff_B[i] * (zA[i]-zA[stages-i]);
}
newB+= s->coeff_B[i] * zA[i];
for(;i<stages;i++)
newA+= s->coeff_A[i] * zA[i];
newB+= newA-zA[0];
zA[0]=zA[stages]=newA;
if(++s->ring>=stages)s->ring=0;
return(newB);
}
#ifdef _V_SELFTEST
/* z^-stage, z^-stage+1... */
static double cheb_bandpass_B[]={-1.,0.,5.,0.,-10.,0.,10.,0.,-5.,0.,1};
static double cheb_bandpass_A[]={-0.6665900311,
1.0070146601,
-3.1262875409,
3.5017171569,
-6.2779211945,
5.2966481740,
-6.7570216587,
4.0760335768,
-3.9134284363,
1.3997338886};
static double data[128]={
0.0426331,
0.0384521,
0.0345764,
0.0346069,
0.0314636,
0.0310059,
0.0318604,
0.0336304,
0.036438,
0.0348511,
0.0354919,
0.0343628,
0.0325623,
0.0318909,
0.0263367,
0.0225525,
0.0195618,
0.0160828,
0.0168762,
0.0145569,
0.0126343,
0.0127258,
0.00820923,
0.00787354,
0.00558472,
0.00204468,
3.05176e-05,
-0.00357056,
-0.00570679,
-0.00991821,
-0.0101013,
-0.00881958,
-0.0108948,
-0.0110168,
-0.0119324,
-0.0161438,
-0.0194702,
-0.0229187,
-0.0260315,
-0.0282288,
-0.0306091,
-0.0330505,
-0.0364685,
-0.0385742,
-0.0428772,
-0.043457,
-0.0425415,
-0.0462341,
-0.0467529,
-0.0489807,
-0.0520325,
-0.0558167,
-0.0596924,
-0.0591431,
-0.0612793,
-0.0618591,
-0.0615845,
-0.0634155,
-0.0639648,
-0.0683594,
-0.0718079,
-0.0729675,
-0.0791931,
-0.0860901,
-0.0885315,
-0.088623,
-0.089386,
-0.0899353,
-0.0886841,
-0.0910645,
-0.0948181,
-0.0919495,
-0.0891418,
-0.0916443,
-0.096344,
-0.100464,
-0.105499,
-0.108612,
-0.112213,
-0.117676,
-0.120911,
-0.124329,
-0.122162,
-0.120605,
-0.12326,
-0.12619,
-0.128998,
-0.13205,
-0.134247,
-0.137939,
-0.143555,
-0.14389,
-0.14859,
-0.153717,
-0.159851,
-0.164551,
-0.162811,
-0.164276,
-0.156952,
-0.140564,
-0.123291,
-0.10321,
-0.0827637,
-0.0652466,
-0.053772,
-0.0509949,
-0.0577698,
-0.0818176,
-0.114929,
-0.148895,
-0.181122,
-0.200714,
-0.21048,
-0.203644,
-0.179413,
-0.145325,
-0.104492,
-0.0658264,
-0.0332031,
-0.0106201,
-0.00363159,
-0.00909424,
-0.0244141,
-0.0422058,
-0.0537415,
-0.0610046,
-0.0609741,
-0.0547791};
/* comparison test code from http://www-users.cs.york.ac.uk/~fisher/mkfilter/
(the above page kicks ass, BTW)*/
#define NZEROS 10
#define NPOLES 10
#define GAIN 4.599477515e+02
static float xv[NZEROS+1], yv[NPOLES+1];
static double filterloop(double next){
xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4]; xv[4] = xv[5];
xv[5] = xv[6]; xv[6] = xv[7]; xv[7] = xv[8]; xv[8] = xv[9]; xv[9] = xv[10];
xv[10] = next / GAIN;
yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4]; yv[4] = yv[5];
yv[5] = yv[6]; yv[6] = yv[7]; yv[7] = yv[8]; yv[8] = yv[9]; yv[9] = yv[10];
yv[10] = (xv[10] - xv[0]) + 5 * (xv[2] - xv[8]) + 10 * (xv[6] - xv[4])
+ ( -0.6665900311 * yv[0]) + ( 1.0070146601 * yv[1])
+ ( -3.1262875409 * yv[2]) + ( 3.5017171569 * yv[3])
+ ( -6.2779211945 * yv[4]) + ( 5.2966481740 * yv[5])
+ ( -6.7570216587 * yv[6]) + ( 4.0760335768 * yv[7])
+ ( -3.9134284363 * yv[8]) + ( 1.3997338886 * yv[9]);
return(yv[10]);
}
#include <stdio.h>
int main(){
/* run the pregenerated Chebyshev filter, then our own distillation
through the generic and specialized code */
double *work=malloc(128*sizeof(double));
IIR_state iir;
int i;
for(i=0;i<128;i++)work[i]=filterloop(data[i]);
{
FILE *out=fopen("IIR_ref.m","w");
for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
fclose(out);
}
IIR_init(&iir,NPOLES,GAIN,cheb_bandpass_A,cheb_bandpass_B);
for(i=0;i<128;i++)work[i]=IIR_filter(&iir,data[i]);
{
FILE *out=fopen("IIR_gen.m","w");
for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
fclose(out);
}
IIR_clear(&iir);
IIR_init(&iir,NPOLES,GAIN,cheb_bandpass_A,cheb_bandpass_B);
for(i=0;i<128;i++)work[i]=IIR_filter_ChebBand(&iir,data[i]);
{
FILE *out=fopen("IIR_cheb.m","w");
for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
fclose(out);
}
IIR_clear(&iir);
return(0);
}
#endif