File: filter.c

package info (click to toggle)
freebirth 0.3.2-9.2
  • links: PTS, VCS
  • area: main
  • in suites: sid
  • size: 3,632 kB
  • ctags: 640
  • sloc: ansic: 4,724; makefile: 89; sh: 1
file content (549 lines) | stat: -rw-r--r-- 15,788 bytes parent folder | download | duplicates (4)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
/* Freebirth
 * Copyright (C) 1999 topher lafata <topher@topher.com>,
 *		      Jake Donham <jake@bitmechanic.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public
 * License along with this program (see COPYING); if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

#include "filter.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define FILTER_SECTIONS        2 /* 2 filter sections for 24 db/oct filter */
#define MIN_GAIN              .4
#define MAX_GAIN              .7

#define SAMPLE_RATE       44100


void filter_next_buffer(filter *this);

typedef struct {
        double a0, a1, a2;       /* numerator coefficients */
        double b0, b1, b2;       /* denominator coefficients */
} BIQUAD;

BIQUAD ProtoCoef[FILTER_SECTIONS] = {
  { 1.0, 0, 0, 1.0, 0.765367, 1.0 },
  { 1.0, 0, 0, 1.0, 1.847759, 1.0 }
};


static float **coef_lookup_table;
static int looked_up = 0;



/*coeficient calculation function */
void szxform(
    double *a0, double *a1, double *a2,     /* numerator coefficients */
    double *b0, double *b1, double *b2,   /* denominator coefficients */
    double fc,           /* Filter cutoff frequency */
    double fs,           /* sampling rate */
    double *k,           /* overall gain factor */
    float *coef);         /* pointer to 4 iir coefficients */

float *filter_get_coef(int sections,double cutoff,double q, double gain)
{
  
  int nInd;

  double k    = gain;
  float *Coef = (float *)calloc(4 * sections + 1, sizeof(float));
  float *coef = Coef + 1;
  double Q    = q;
  double fc   = cutoff;
  double fs   = SAMPLE_RATE;
  
  double   a0, a1, a2, b0, b1, b2;


  /* printf("cutoff is %f\n",fc); */
  /* printf("Q is %f\n",Q);  */

 for(nInd = 0; nInd < sections; nInd++)
    {
      a0 = ProtoCoef[nInd].a0;
      a1 = ProtoCoef[nInd].a1;
      a2 = ProtoCoef[nInd].a2;
      
      b0 = ProtoCoef[nInd].b0;
      b1 = ProtoCoef[nInd].b1 / Q;      /* Divide by resonance or Q */
      b2 = ProtoCoef[nInd].b2;
      szxform(&a0, &a1, &a2, &b0, &b1, &b2, fc, fs, &k, coef);
      coef += 4;                       /* Point to next filter section */      

    }
  
  Coef[0] = k;
  return Coef;
}


void filter_init_lookup_table()
{
  int index;
  double q,fc;
  int freq_range, q_range, table_length;
  float q_scale ;
  float gain_scale;

  q_scale = MAX_Q/MAX_FC;
  gain_scale   = - (MAX_GAIN - MIN_GAIN) / MAX_FC; 
  freq_range   = MAX_FC - MIN_FC;
  q_range      = (MAX_Q - MIN_Q) / (double)Q_INCREMENT;

  table_length = freq_range * q_range;

  printf("Calculating coefficient table.");

  index = 0;
  for(fc = MIN_FC; fc < MAX_FC;fc++)
    for(q = MIN_Q;q < MAX_Q; q += (double)Q_INCREMENT){
      /*PATCH BY: Harold Gutch <logix@foobar.franken.de>
       FreeBSD can't handle devide by zero and it was possible
       for scaled_q to go to zero*/
      double scaled_q =  (((q_scale * (fc))  / MAX_Q)* q) + MIN_Q;
      coef_lookup_table[index] = filter_get_coef(FILTER_SECTIONS,
						 fc,
						 scaled_q,
						 gain_scale * fc + MAX_GAIN);
      
      index++;
      if(index % 1000 == 0){
	printf(".");
	fflush(0);
	}
    }
  looked_up = 1;
  printf("\nDone\n");
}

void filter_set_coeffs(filter *this, int fc, float q)
{
  int index;
  index  = ((fc - MIN_FC) * (MAX_Q - MIN_Q) / Q_INCREMENT) 
    + (q / Q_INCREMENT);
 
  this->coef = coef_lookup_table[index];
  
}




sample filter_process_sample(filter *this, sample input)
{
  
  unsigned int i;
  float *hist1_ptr,*hist2_ptr,*coef_ptr;
  float output,new_hist,history1,history2;


/* allocate history array if different size than last call */
  if(!this->history) 
    this->history = (float *) calloc(2*this->length,sizeof(float));
  
  coef_ptr = this->coef;                /* coefficient pointer */
  
  hist1_ptr = this->history;            /* first history */
  hist2_ptr = hist1_ptr + 1;           /* next history */

  
  output = (float)input * (*coef_ptr++);
  
  for (i = 0 ; i < this->length; i++)
    {
      history1 = *hist1_ptr;           /* history values */
      history2 = *hist2_ptr;
      
      output = output - history1 * (*coef_ptr++);
      new_hist = output - history2 * (*coef_ptr++);    /* poles */
      
      output = new_hist + history1 * (*coef_ptr++);
      output = output + history2 * (*coef_ptr++);      /* zeros */
      
      *hist2_ptr++ = *hist1_ptr;
      *hist1_ptr++ = new_hist;
      hist1_ptr++;
      hist2_ptr++;
    } 
  
  return (sample)output;
}

void filter_next_buffer(filter *this) {
  this->next = 1;
}

sample *filter_get_buffer(filter *this)
{
  int i;
  sample *filt_e_buffer;
  sample *buffer;

  if (this->next) {
    this->next = 0;
    buffer = this->sp->get_buffer(this->sp);
    filt_e_buffer = this->filt_e->get_buffer(this->filt_e);
    for(i = 0; i < TBASS_BUFF_SIZE; i++)
      {
	float s;

	/*
	  we're scaling the 0-SAMPLE_MAX range of the envelope into
          a LIMIT-SAMPLE_MAX range 'cause the filter doesn't like the
	  cutoff to be 0
	*/
	int fe = filt_e_buffer[i] * (SAMPLE_MAX - LIMIT) / SAMPLE_MAX + LIMIT;
	int fc = this->fc * fe / SAMPLE_MAX;
	filter_set_coeffs(this,
				fc,
				this->Q);
	s = filter_process_sample(this,buffer[i]);
      
	/*
	  PATCH: Benno Senoner <benno@gardena.net>
	         clip on underflow as well as overflow
	*/
	if(s > SAMPLE_MAX)
	  s = SAMPLE_MAX;
	else if (s < SAMPLE_MIN)
	  s = SAMPLE_MIN;

	this->buffer[i] = s;

      }
  }
  return this->buffer;
}


void filter_set_cutoff(filter *this,double cutoff)
{
  this->fc = cutoff;
  filter_set_coeffs(this,this->fc,this->Q);

}

void filter_set_gain(filter *this,double gain)
{
  this->gain = gain;

}

void filter_set_q(filter *this,double q)
{
  this->Q = q;
  filter_set_coeffs(this,this->fc,this->Q);
}


void filter_delete(filter *this)
{
  free(this->history);
  free(this->coef);
  free(this);

}



/*
 * ----------------------------------------------------------
 *      bilinear.c
 *
 *      Perform bilinear transformation on s-domain coefficients
 *      of 2nd order biquad section.
 *      First design an analog filter and use s-domain coefficients
 *      as input to szxform() to convert them to z-domain.
 *
 * Here's the butterworth polinomials for 2nd, 4th and 6th order sections.
 *      When we construct a 24 db/oct filter, we take to 2nd order
 *      sections and compute the coefficients separately for each section.
 *
 *      n       Polinomials
 * --------------------------------------------------------------------
 *      2       s^2 + 1.4142s +1
 *      4       (s^2 + 0.765367s + 1) (s^2 + 1.847759s + 1)
 *      6       (s^2 + 0.5176387s + 1) (s^2 + 1.414214 + 1) (s^2 + 1.931852s + 1)
 *
 *      Where n is a filter order.
 *      For n=4, or two second order sections, we have following equasions for each
 *      2nd order stage:
 *
 *      (1 / (s^2 + (1/Q) * 0.765367s + 1)) * (1 / (s^2 + (1/Q) * 1.847759s + 1))
 *
 *      Where Q is filter quality factor in the range of
 *      1 to 1000. The overall filter Q is a product of all
 *      2nd order stages. For example, the 6th order filter
 *      (3 stages, or biquads) with individual Q of 2 will
 *      have filter Q = 2 * 2 * 2 = 8.
 *
 *      The nominator part is just 1.
 *      The denominator coefficients for stage 1 of filter are:
 *      b2 = 1; b1 = 0.765367; b0 = 1;
 *      numerator is
 *      a2 = 0; a1 = 0; a0 = 1;
 *
 *      The denominator coefficients for stage 1 of filter are:
 *      b2 = 1; b1 = 1.847759; b0 = 1;
 *      numerator is
 *      a2 = 0; a1 = 0; a0 = 1;
 *
 *      These coefficients are used directly by the szxform()
 *      and bilinear() functions. For all stages the numerator
 *      is the same and the only thing that is different between
 *      different stages is 1st order coefficient. The rest of
 *      coefficients are the same for any stage and equal to 1.
 *
 *      Any filter could be constructed using this approach.
 *
 *      References:
 *             Van Valkenburg, "Analog Filter Design"
 *             Oxford University Press 1982
 *             ISBN 0-19-510734-9
 *
 *             C Language Algorithms for Digital Signal Processing
 *             Paul Embree, Bruce Kimble
 *             Prentice Hall, 1991
 *             ISBN 0-13-133406-9
 *
 *             Digital Filter Designer's Handbook
 *             With C++ Algorithms
 *             Britton Rorabaugh
 *             McGraw Hill, 1997
 *             ISBN 0-07-053806-9
 * ----------------------------------------------------------
 */


void prewarp(double *a0, double *a1, double *a2, double fc, double fs);
void bilinear(
    double a0, double a1, double a2,    /* numerator coefficients */
    double b0, double b1, double b2,    /* denominator coefficients */
    double *k,                                   /* overall gain factor */
    double fs,                                   /* sampling rate */
    float *coef);                         /* pointer to 4 iir coefficients */


/*
 * ----------------------------------------------------------
 *      Pre-warp the coefficients of a numerator or denominator.
 *      Note that a0 is assumed to be 1, so there is no wrapping
 *      of it.
 * ----------------------------------------------------------
 */
void prewarp(
    double *a0, double *a1, double *a2,
    double fc, double fs)
{
    double wp, pi;

    pi = 4.0 * atan(1.0);
    wp = 2.0 * fs * tan(pi * fc / fs);

    if(wp == 0)   /*PATCH BY:  free bsd from Hans Huebner<hans@huebner.org> */
      wp = 0.0001;
    *a2 = (*a2) / (wp * wp);
    *a1 = (*a1) / wp;
}






/*
 * ----------------------------------------------------------
 * bilinear()
 *
 * Transform the numerator and denominator coefficients
 * of s-domain biquad section into corresponding
 * z-domain coefficients.
 *
 *      Store the 4 IIR coefficients in array pointed by coef
 *      in following order:
 *             beta1, beta2    (denominator)
 *             alpha1, alpha2  (numerator)
 *
 * Arguments:
 *             a0-a2   - s-domain numerator coefficients
 *             b0-b2   - s-domain denominator coefficients
 *             k               - filter gain factor. initially set to 1
 *                                and modified by each biquad section in such
 *                                a way, as to make it the coefficient by
 *                                which to multiply the overall filter gain
 *                                in order to achieve a desired overall filter gain,
 *                                specified in initial value of k.
 *             fs             - sampling rate (Hz)
 *             coef    - array of z-domain coefficients to be filled in.
 *
 * Return:
 *             On return, set coef z-domain coefficients
 * ----------------------------------------------------------
 */
void bilinear(
    double a0, double a1, double a2,    /* numerator coefficients */
    double b0, double b1, double b2,    /* denominator coefficients */
    double *k,           /* overall gain factor */
    double fs,           /* sampling rate */
    float *coef         /* pointer to 4 iir coefficients */
)
{
    double ad, bd;

                 /* alpha (Numerator in s-domain) */
    ad = 4. * a2 * fs * fs + 2. * a1 * fs + a0;
                 /* beta (Denominator in s-domain) */
    bd = 4. * b2 * fs * fs + 2. * b1* fs + b0;

                 /* update gain constant for this section */
    *k *= ad/bd;

                 /* Denominator */
    *coef++ = (2. * b0 - 8. * b2 * fs * fs)
                           / bd;         /* beta1 */
    *coef++ = (4. * b2 * fs * fs - 2. * b1 * fs + b0)
                           / bd; /* beta2 */

                 /* Nominator */
    *coef++ = (2. * a0 - 8. * a2 * fs * fs)
                           / ad;         /* alpha1 */
    *coef = (4. * a2 * fs * fs - 2. * a1 * fs + a0)
                           / ad;   /* alpha2 */
}


/*
 * ----------------------------------------------------------
 * Transform from s to z domain using bilinear transform
 * with prewarp.
 *
 * Arguments:
 *      For argument description look at bilinear()
 *
 *      coef - pointer to array of floating point coefficients,
 *                     corresponding to output of bilinear transofrm
 *                     (z domain).
 *
 * Note: frequencies are in Hz.
 * ----------------------------------------------------------
 */
void szxform(
    double *a0, double *a1, double *a2, /* numerator coefficients */
    double *b0, double *b1, double *b2, /* denominator coefficients */
    double fc,         /* Filter cutoff frequency */
    double fs,         /* sampling rate */
    double *k,         /* overall gain factor */
    float *coef)         /* pointer to 4 iir coefficients */
{
                 /* Calculate a1 and a2 and overwrite the original values */
        prewarp(a0, a1, a2, fc, fs);
        prewarp(b0, b1, b2, fc, fs);
        bilinear(*a0, *a1, *a2, *b0, *b1, *b2, k, fs, coef);
}



static sample_producer **get_children(filter *this)
{
  return &(this->sp);
}

static char **get_header(filter *this)
{
  static char *header[] = {
    "int $n_fc = ((filter *)$t)->fc;",
    "float $n_Q = ((filter *)$t)->Q;",
    "float $n_s;",
    NULL
  };
  return header;
}

static char **get_code(filter *this)
{
  static char *code[] = {
    "filter_set_coeffs((filter *)$t, $n_fc *",
    "                  ($i1 * (SAMPLE_MAX - LIMIT) / SAMPLE_MAX + LIMIT) /",
    "                  SAMPLE_MAX, $n_Q);",
    "$n_s = filter_process_sample((filter *)$t, $i0);",
    "if ($n_s > SAMPLE_MAX) $n_s = SAMPLE_MAX;",
    "else if ($n_s < SAMPLE_MIN) $n_s = SAMPLE_MIN;",
    "$o = $n_s;",
    NULL
  };
  return code;
}

static char **get_footer(filter *this)
{
  static char *footer[] = { NULL };
  return footer;
}

filter *filter_new(double cutoff, double q, sample_producer *filt_e,
		   sample_producer* sp)
{
  int i;
  filter *out = malloc(sizeof(filter)); 
  out->get_buffer = filter_get_buffer;
  out->next_buffer = filter_next_buffer;
  out->get_children	= get_children;
  out->get_header	= get_header;
  out->get_code		= get_code;
  out->get_footer	= get_footer;
  out->next = 0;
  out->schedule   = sp_schedule_event;
  out->trigger    = NULL; /*what does it mean to trigger the filter? */

  /*we aren't responding to events yet in the filter */
  out->events     = (event **)malloc(sizeof(event *) 
			 	 * TBASS_BUFF_SIZE);

  for(i = 0; i < TBASS_BUFF_SIZE; i++)
    out->events[i] = NULL; 


  out->length     = FILTER_SECTIONS;
  out->gain       = MAX_GAIN;
  out->coef       = (float *) calloc(4 * out->length + 1, sizeof(float));
  out->fc         = cutoff;
  out->Q          = q;
  out->sp         = sp;
  out->filt_e     = filt_e;
  out->unused     = NULL;
  out->buffer      = (sample *)malloc(sizeof(sample) * TBASS_BUFF_SIZE);

  /* we're doing this here because we don't call filter_init_lookup_table
     in fusebirth, and we need to create the filter but not have it
     blow up in filter_set_coeffs because the table doesn't exist. */
  coef_lookup_table = (float **)
    malloc(sizeof(float*) *
	   (MAX_FC - MIN_FC) * (MAX_Q - MIN_Q) / (double)Q_INCREMENT);

  filter_set_coeffs(out,out->fc,out->Q);
  return out;
  
}

/*
  Local Variables:
  mode: font-lock
  End:
*/