lib/iir.c

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