lib/iir.c

   1 /********************************************************************
   2  *                                                                  *
   3  * THIS FILE IS PART OF THE Ogg Vorbis SOFTWARE CODEC SOURCE CODE.  *
   4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS SOURCE IS GOVERNED BY *
   5  * THE GNU PUBLIC LICENSE 2, WHICH IS INCLUDED WITH THIS SOURCE.    *
   6  * PLEASE READ THESE TERMS DISTRIBUTING.                            *
   7  *                                                                  *
   8  * THE OggSQUISH SOURCE CODE IS (C) COPYRIGHT 1994-2000             *
   9  * by Monty <monty@xiph.org> and The XIPHOPHORUS Company            *
  10  * http://www.xiph.org/                                             *
  11  *                                                                  *
  12  ********************************************************************
  13
  14   function: Direct Form I, II IIR filters, plus some specializations
  15   last mod: $Id: iir.c,v 1.2 2000/10/12 03:12:52 xiphmont Exp $
  16
  17  ********************************************************************/
  18
  19 /* LPC is actually a degenerate case of form I/II filters, but we need
  20    both */
  21
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include "iir.h"
  26
  27 void IIR_init(IIR_state *s,int stages,float gain, float *A, float *B){
  28   memset(s,0,sizeof(IIR_state));
  29   s->stages=stages;
  30   s->gain=gain;
  31   s->coeff_A=malloc(stages*sizeof(float));
  32   s->coeff_B=malloc((stages+1)*sizeof(float));
  33   s->z_A=calloc(stages*2,sizeof(float));
  34   s->z_B=calloc(stages*2,sizeof(float));
  35
  36   memcpy(s->coeff_A,A,stages*sizeof(float));
  37   memcpy(s->coeff_B,B,(stages+1)*sizeof(float));
  38 }
  39
  40 void IIR_clear(IIR_state *s){
  41   if(s){
  42     free(s->coeff_A);
  43     free(s->coeff_B);
  44     free(s->z_A);
  45     free(s->z_B);
  46     memset(s,0,sizeof(IIR_state));
  47   }
  48 }
  49
  50 float IIR_filter(IIR_state *s,float in){
  51   int stages=s->stages,i;
  52   float newA;
  53   float newB=0;
  54   float *zA=s->z_A+s->ring;
  55
  56   newA=in/=s->gain;
  57   for(i=0;i<stages;i++){
  58     newA+= s->coeff_A[i] * zA[i];
  59     newB+= s->coeff_B[i] * zA[i];
  60   }
  61   newB+=newA*s->coeff_B[stages];
  62
  63   zA[0]=zA[stages]=newA;
  64   if(++s->ring>=stages)s->ring=0;
  65
  66   return(newB);
  67 }
  68
  69 /* this assumes the symmetrical structure of the feed-forward stage of
  70    a Chebyshev bandpass to save multiplies */
  71 float IIR_filter_ChebBand(IIR_state *s,float in){
  72   int stages=s->stages,i;
  73   float newA;
  74   float newB=0;
  75   float *zA=s->z_A+s->ring;
  76
  77   newA=in/=s->gain;
  78
  79   newA+= s->coeff_A[0] * zA[0];
  80   for(i=1;i<(stages>>1);i++){
  81     newA+= s->coeff_A[i] * zA[i];
  82     newB+= s->coeff_B[i] * (zA[i]-zA[stages-i]);
  83   }
  84   newB+= s->coeff_B[i] * zA[i];
  85   for(;i<stages;i++)
  86     newA+= s->coeff_A[i] * zA[i];
  87
  88   newB+= newA-zA[0];
  89
  90   zA[0]=zA[stages]=newA;
  91   if(++s->ring>=stages)s->ring=0;
  92
  93   return(newB);
  94 }
  95
  96 #ifdef _V_SELFTEST
  97
  98 /* z^-stage, z^-stage+1... */
  99 static float cheb_bandpass_B[]={-1.,0.,5.,0.,-10.,0.,10.,0.,-5.,0.,1};
 100 static float cheb_bandpass_A[]={-0.6665900311,
 101                                   1.0070146601,
 102                                  -3.1262875409,
 103                                   3.5017171569,
 104                                  -6.2779211945,
 105                                   5.2966481740,
 106                                  -6.7570216587,
 107                                   4.0760335768,
 108                                  -3.9134284363,
 109                                   1.3997338886};
 110
 111 static float data[128]={
 112   0.0426331,
 113   0.0384521,
 114   0.0345764,
 115   0.0346069,
 116   0.0314636,
 117   0.0310059,
 118   0.0318604,
 119   0.0336304,
 120   0.036438,
 121   0.0348511,
 122   0.0354919,
 123   0.0343628,
 124   0.0325623,
 125   0.0318909,
 126   0.0263367,
 127   0.0225525,
 128   0.0195618,
 129   0.0160828,
 130   0.0168762,
 131   0.0145569,
 132   0.0126343,
 133   0.0127258,
 134   0.00820923,
 135   0.00787354,
 136   0.00558472,
 137   0.00204468,
 138   3.05176e-05,
 139   -0.00357056,
 140   -0.00570679,
 141   -0.00991821,
 142   -0.0101013,
 143   -0.00881958,
 144   -0.0108948,
 145   -0.0110168,
 146   -0.0119324,
 147   -0.0161438,
 148   -0.0194702,
 149   -0.0229187,
 150   -0.0260315,
 151   -0.0282288,
 152   -0.0306091,
 153   -0.0330505,
 154   -0.0364685,
 155   -0.0385742,
 156   -0.0428772,
 157   -0.043457,
 158   -0.0425415,
 159   -0.0462341,
 160   -0.0467529,
 161   -0.0489807,
 162   -0.0520325,
 163   -0.0558167,
 164   -0.0596924,
 165   -0.0591431,
 166   -0.0612793,
 167   -0.0618591,
 168   -0.0615845,
 169   -0.0634155,
 170   -0.0639648,
 171   -0.0683594,
 172   -0.0718079,
 173   -0.0729675,
 174   -0.0791931,
 175   -0.0860901,
 176   -0.0885315,
 177   -0.088623,
 178   -0.089386,
 179   -0.0899353,
 180   -0.0886841,
 181   -0.0910645,
 182   -0.0948181,
 183   -0.0919495,
 184   -0.0891418,
 185   -0.0916443,
 186   -0.096344,
 187   -0.100464,
 188   -0.105499,
 189   -0.108612,
 190   -0.112213,
 191   -0.117676,
 192   -0.120911,
 193   -0.124329,
 194   -0.122162,
 195   -0.120605,
 196   -0.12326,
 197   -0.12619,
 198   -0.128998,
 199   -0.13205,
 200   -0.134247,
 201   -0.137939,
 202   -0.143555,
 203   -0.14389,
 204   -0.14859,
 205   -0.153717,
 206   -0.159851,
 207   -0.164551,
 208   -0.162811,
 209   -0.164276,
 210   -0.156952,
 211   -0.140564,
 212   -0.123291,
 213   -0.10321,
 214   -0.0827637,
 215   -0.0652466,
 216   -0.053772,
 217   -0.0509949,
 218   -0.0577698,
 219   -0.0818176,
 220   -0.114929,
 221   -0.148895,
 222   -0.181122,
 223   -0.200714,
 224   -0.21048,
 225   -0.203644,
 226   -0.179413,
 227   -0.145325,
 228   -0.104492,
 229   -0.0658264,
 230   -0.0332031,
 231   -0.0106201,
 232   -0.00363159,
 233   -0.00909424,
 234   -0.0244141,
 235   -0.0422058,
 236   -0.0537415,
 237   -0.0610046,
 238   -0.0609741,
 239   -0.0547791};
 240
 241 /* comparison test code from http://www-users.cs.york.ac.uk/~fisher/mkfilter/
 242    (the above page kicks ass, BTW)*/
 243
 244 #define NZEROS 10
 245 #define NPOLES 10
 246 #define GAIN   4.599477515e+02
 247
 248 static float xv[NZEROS+1], yv[NPOLES+1];
 249
 250 static float filterloop(float next){
 251   xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4]; xv[4] = xv[5];
 252   xv[5] = xv[6]; xv[6] = xv[7]; xv[7] = xv[8]; xv[8] = xv[9]; xv[9] = xv[10];
 253   xv[10] = next / GAIN;
 254   yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4]; yv[4] = yv[5];
 255   yv[5] = yv[6]; yv[6] = yv[7]; yv[7] = yv[8]; yv[8] = yv[9]; yv[9] = yv[10];
 256   yv[10] =   (xv[10] - xv[0]) + 5 * (xv[2] - xv[8]) + 10 * (xv[6] - xv[4])
 257     + ( -0.6665900311 * yv[0]) + (  1.0070146601 * yv[1])
 258     + ( -3.1262875409 * yv[2]) + (  3.5017171569 * yv[3])
 259     + ( -6.2779211945 * yv[4]) + (  5.2966481740 * yv[5])
 260     + ( -6.7570216587 * yv[6]) + (  4.0760335768 * yv[7])
 261     + ( -3.9134284363 * yv[8]) + (  1.3997338886 * yv[9]);
 262   return(yv[10]);
 263 }
 264
 265 #include <stdio.h>
 266 int main(){
 267
 268   /* run the pregenerated Chebyshev filter, then our own distillation
 269      through the generic and specialized code */
 270   float *work=malloc(128*sizeof(float));
 271   IIR_state iir;
 272   int i;
 273
 274   for(i=0;i<128;i++)work[i]=filterloop(data[i]);
 275   {
 276     FILE *out=fopen("IIR_ref.m","w");
 277     for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
 278     fclose(out);
 279   }
 280
 281   IIR_init(&iir,NPOLES,GAIN,cheb_bandpass_A,cheb_bandpass_B);
 282   for(i=0;i<128;i++)work[i]=IIR_filter(&iir,data[i]);
 283   {
 284     FILE *out=fopen("IIR_gen.m","w");
 285     for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
 286     fclose(out);
 287   }
 288   IIR_clear(&iir);
 289
 290   IIR_init(&iir,NPOLES,GAIN,cheb_bandpass_A,cheb_bandpass_B);
 291   for(i=0;i<128;i++)work[i]=IIR_filter_ChebBand(&iir,data[i]);
 292   {
 293     FILE *out=fopen("IIR_cheb.m","w");
 294     for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
 295     fclose(out);
 296   }
 297   IIR_clear(&iir);
 298
 299   return(0);
 300 }
 301
 302 #endif