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