37129/middleear.c at master · ModelDBRepository/37129 · GitHub

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
/* middleear.c */
/* a simple middle ear model */

/* July 04, 2002 */
/* the output of this program now is meout[] */

/* Sept 26, 2002 */
/* simplify the program */

int middleear()
{
 int error_number=1;

 int pole_order=4;           // two pairs of poles
 int half_pole_order=2;
 int zero_order=2;           // two zeros

 extern double *soundin;
 extern double tdres;
 extern double PI;
 extern double *meout;
 extern int sound_length;

 double gain_norm;

 double fs_bilinear; //bilinear transformation frequency
 double kkd;
 double bpinput[4][4];
 double bpoutput[4][4];
 double zerobp;

 double preal;
 double pimg;

 double a1;
 double a2;
 double b1[3];
 double b2[3];
 double dy;

 mycomplex p_11, p_12, p_21, p_22; // poles in s plain
 mycomplex p[5];

 int n, i, j;

   //%========== locations of poles ===================%

   p_11.realpart=-250*2*PI;
   p_11.imgpart=400*2*PI;

   p_21.realpart = -2000*2*PI;
   p_21.imgpart = 6000*2*PI;

   p_12 = myconj(p_11);
   p_22 = myconj(p_21);

   p[1]=p_11;
   p[2]=p_12;
   p[3]=p_21;
   p[4]=p_22;

   fs_bilinear = 2.0/tdres;

   kkd=1.0;
   for (i=1; i<=half_pole_order; i=i+1)
    {
     kkd=kkd/((fs_bilinear-p[i*2].realpart)*(fs_bilinear-p[i*2].realpart)
              +p[i*2].imgpart*p[i*2].imgpart);
    }

   /* ========= setup zeros =================*/
   zerobp=-200;
   for (i=1; i<=zero_order; i=i+1)
    {
     kkd=kkd*(fs_bilinear - zerobp);
    }

  /*========= initialize some variables======*/
  for (i=1; i<=3; i=i+1)
   {
    for (j=1; j<=3; j=j+1)
     {
      bpinput[i][j]=0.0;
      bpoutput[i][j]=0.0;
     }
   }

  /*======= normalize gain at 1000Hz========== */
  gain_norm =1.0;
  for (i=1; i<=pole_order; i=i+1)
   {
    gain_norm=gain_norm*sqrt(p[i].realpart*p[i].realpart
                         +(p[i].imgpart-1000*2*PI)*(p[i].imgpart-1000*2*PI));
   }
  for (i=1; i<=zero_order; i=i+1)
   {
    gain_norm=gain_norm/sqrt(2*PI*zerobp*2*PI*zerobp+1000.0*2*PI*1000.0*2*PI);
   }

  /* filter coefficients */

  a1=(1-(fs_bilinear+zerobp)/(fs_bilinear-zerobp));
  a2=- (fs_bilinear+zerobp)/(fs_bilinear-zerobp);
  for (i=1; i<=half_pole_order; i=i+1)
   {
    b1[i]=2*(fs_bilinear*fs_bilinear
              -p[i*2].realpart*p[i*2].realpart-p[i*2].imgpart*p[i*2].imgpart)
           /((fs_bilinear-p[i*2].realpart)*(fs_bilinear-p[i*2].realpart)
              +p[i*2].imgpart*p[i*2].imgpart);
    b2[i]=-((fs_bilinear+p[i*2].realpart)*(fs_bilinear+p[i*2].realpart)
              +p[i*2].imgpart*p[i*2].imgpart)
           /((fs_bilinear-p[i*2].realpart)*(fs_bilinear-p[i*2].realpart)
              +p[i*2].imgpart*p[i*2].imgpart);
   }


  for (n=1; n<=sound_length ; n=n+1)
   {
   bpinput[1][3]=bpinput[1][2];
   bpinput[1][2]=bpinput[1][1];
   bpinput[1][1]=soundin[n];

   for (i=1; i<=half_pole_order; i=i+1)
    {
     dy = bpinput[i][1]+a1*bpinput[i][2]+a2*bpinput[i][3];
     dy = dy + b1[i]*bpoutput[i][1] + b2[i]*bpoutput[i][2];

     bpinput[i+1][3]=bpoutput[i][2];
     bpinput[i+1][2]=bpoutput[i][1];
     bpinput[i+1][1]=dy;

     bpoutput[i][2]=bpoutput[i][1];
     bpoutput[i][1]=dy;
    }

   meout[n]=dy*kkd*gain_norm;

  }
   return (error_number);
}