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
|
/* For information on usage and redistribution, and for a DISCLAIMER OF ALL
* WARRANTIES, see the file, "LICENSE.txt," in this distribution.
iemlib1 written by Thomas Musil, Copyright (c) IEM KUG Graz Austria 2000 - 2011 */
#include "m_pd.h"
#include "iemlib.h"
#include <math.h>
/* ------------------------ biquad_freq_resp ------------------- */
/* -- calculates the frequency responce of a biquad structure -- */
typedef struct _biquad_freq_resp
{
t_object x_obj;
t_float a0;
t_float a1;
t_float a2;
t_float b1;
t_float b2;
t_outlet *x_out_re;
t_outlet *x_out_im;
t_outlet *x_out_abs;
t_outlet *x_out_arg;
} t_biquad_freq_resp;
static t_class *biquad_freq_resp_class;
static void biquad_freq_resp_float(t_biquad_freq_resp *x, t_floatarg freq_phase)
{
t_float re1, im1, re2, im2, re, im;
t_float c, s, ra;
t_float pi = 4.0 * atan(1.0);
if(freq_phase < 0.0)
freq_phase = 0.0;
else if(freq_phase > 180.0)
freq_phase = 180.0;
freq_phase *= pi;
freq_phase /= 180.0;
c = cos(freq_phase);
s = sin(freq_phase);
re1 = x->a0 + x->a1*c + x->a2*(c*c - s*s);
im1 = x->a1*s + x->a2*2.0*(s*c);
re2 = 1.0 - x->b1*c - x->b2*(c*c - s*s);
im2 = -x->b1*s - x->b2*2.0*(s*c);
ra = 1.0 / (re2*re2 + im2*im2);
im = (re1*im2 - re2*im1) * ra; /* because z^-1 = e^-jwt, negative sign */
re = (re1*re2 + im1*im2) * ra;
outlet_float(x->x_out_arg, 180.0*atan2(im, re)/pi);
outlet_float(x->x_out_abs, sqrt(re*re + im*im));
outlet_float(x->x_out_im, im);
outlet_float(x->x_out_re, re);
}
/* y/x = (a0 + a1*z-1 + a2*z-2)/(1 - b1*z-1 - b2*z-2);*/
static void biquad_freq_resp_list(t_biquad_freq_resp *x, t_symbol *s, int argc, t_atom *argv)
{
if((argc == 5)&&IS_A_FLOAT(argv,4)&&IS_A_FLOAT(argv,3)&&IS_A_FLOAT(argv,2)&&IS_A_FLOAT(argv,1)&&IS_A_FLOAT(argv,0))
{
x->b1 = (float)atom_getfloatarg(0, argc, argv);
x->b2 = (float)atom_getfloatarg(1, argc, argv);
x->a0 = (float)atom_getfloatarg(2, argc, argv);
x->a1 = (float)atom_getfloatarg(3, argc, argv);
x->a2 = (float)atom_getfloatarg(4, argc, argv);
}
}
static void *biquad_freq_resp_new(void)
{
t_biquad_freq_resp *x = (t_biquad_freq_resp *)pd_new(biquad_freq_resp_class);
x->x_out_re = outlet_new(&x->x_obj, &s_float);
x->x_out_im = outlet_new(&x->x_obj, &s_float);
x->x_out_abs = outlet_new(&x->x_obj, &s_float);
x->x_out_arg = outlet_new(&x->x_obj, &s_float);
x->b1 = 0.0;
x->b2 = 0.0;
x->a0 = 0.0;
x->a1 = 0.0;
x->a2 = 0.0;
return (x);
}
void biquad_freq_resp_setup(void)
{
biquad_freq_resp_class = class_new(gensym("biquad_freq_resp"), (t_newmethod)biquad_freq_resp_new, 0,
sizeof(t_biquad_freq_resp), 0, 0);
class_addfloat(biquad_freq_resp_class, biquad_freq_resp_float);
class_addlist(biquad_freq_resp_class, (t_method)biquad_freq_resp_list);
}
|