lib/iir.c

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