Fixed a bunch of typos pointed to by: larry@doolittle.boa.org
[speexdsp.git] / libspeex / filters.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: filters.c
3    Various analysis/synthesis filters
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include "filters.h"
34 #include "stack_alloc.h"
35 #include <math.h>
36
37
38 void bw_lpc(float gamma, float *lpc_in, float *lpc_out, int order)
39 {
40    int i;
41    float tmp=1;
42    for (i=0;i<order+1;i++)
43    {
44       lpc_out[i] = tmp * lpc_in[i];
45       tmp *= gamma;
46    }
47 }
48
49 #ifdef _USE_SSE
50 #include "filters_sse.h"
51 #else
52 void filter_mem2(float *x, float *num, float *den, float *y, int N, int ord, float *mem)
53 {
54    int i,j;
55    float xi,yi;
56    for (i=0;i<N;i++)
57    {
58       xi=x[i];
59       y[i] = num[0]*xi + mem[0];
60       yi=y[i];
61       for (j=0;j<ord-1;j++)
62       {
63          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*yi;
64       }
65       mem[ord-1] = num[ord]*xi - den[ord]*yi;
66    }
67 }
68
69
70 void iir_mem2(float *x, float *den, float *y, int N, int ord, float *mem)
71 {
72    int i,j;
73    for (i=0;i<N;i++)
74    {
75       y[i] = x[i] + mem[0];
76       for (j=0;j<ord-1;j++)
77       {
78          mem[j] = mem[j+1] - den[j+1]*y[i];
79       }
80       mem[ord-1] = - den[ord]*y[i];
81    }
82 }
83 #endif
84
85 void fir_mem2(float *x, float *num, float *y, int N, int ord, float *mem)
86 {
87    int i,j;
88    float xi;
89    for (i=0;i<N;i++)
90    {
91       xi=x[i];
92       y[i] = num[0]*xi + mem[0];
93       for (j=0;j<ord-1;j++)
94       {
95          mem[j] = mem[j+1] + num[j+1]*xi;
96       }
97       mem[ord-1] = num[ord]*xi;
98    }
99 }
100
101 void syn_percep_zero(float *xx, float *ak, float *awk1, float *awk2, float *y, int N, int ord, char *stack)
102 {
103    int i;
104    float *mem = PUSH(stack,ord, float);
105    for (i=0;i<ord;i++)
106       mem[i]=0;
107    filter_mem2(xx, awk1, ak, y, N, ord, mem);
108    for (i=0;i<ord;i++)
109      mem[i]=0;
110    iir_mem2(y, awk2, y, N, ord, mem);
111 }
112
113 void residue_percep_zero(float *xx, float *ak, float *awk1, float *awk2, float *y, int N, int ord, char *stack)
114 {
115    int i;
116    float *mem = PUSH(stack,ord, float);
117    for (i=0;i<ord;i++)
118       mem[i]=0;
119    filter_mem2(xx, ak, awk1, y, N, ord, mem);
120    for (i=0;i<ord;i++)
121      mem[i]=0;
122    fir_mem2(y, awk2, y, N, ord, mem);
123 }
124
125
126 void qmf_decomp(float *xx, float *aa, float *y1, float *y2, int N, int M, float *mem, char *stack)
127 {
128    int i,j,k,M2;
129    float *a;
130    float *x;
131    float *x2;
132    
133    a = PUSH(stack, M, float);
134    x = PUSH(stack, N+M-1, float);
135    x2=x+M-1;
136    M2=M>>1;
137    for (i=0;i<M;i++)
138       a[M-i-1]=aa[i];
139    for (i=0;i<M-1;i++)
140       x[i]=mem[M-i-2];
141    for (i=0;i<N;i++)
142       x[i+M-1]=xx[i];
143    for (i=0,k=0;i<N;i+=2,k++)
144    {
145       y1[k]=0;
146       y2[k]=0;
147       for (j=0;j<M2;j++)
148       {
149          y1[k]+=a[j]*(x[i+j]+x2[i-j]);
150          y2[k]-=a[j]*(x[i+j]-x2[i-j]);
151          j++;
152          y1[k]+=a[j]*(x[i+j]+x2[i-j]);
153          y2[k]+=a[j]*(x[i+j]-x2[i-j]);
154       }
155    }
156    for (i=0;i<M-1;i++)
157      mem[i]=xx[N-i-1];
158 }
159
160 /* By segher */
161 void fir_mem_up(float *x, float *a, float *y, int N, int M, float *mem, char *stack)
162    /* assumptions:
163       all odd x[i] are zero -- well, actually they are left out of the array now
164       N and M are multiples of 4 */
165 {
166    int i, j;
167    float *xx=PUSH(stack, M+N-1, float);
168
169    for (i = 0; i < N/2; i++)
170       xx[2*i] = x[N/2-1-i];
171    for (i = 0; i < M - 1; i += 2)
172       xx[N+i] = mem[i+1];
173
174    for (i = 0; i < N; i += 4) {
175       float y0, y1, y2, y3;
176       float x0;
177
178       y0 = y1 = y2 = y3 = 0.f;
179       x0 = xx[N-4-i];
180
181       for (j = 0; j < M; j += 4) {
182          float x1;
183          float a0, a1;
184
185          a0 = a[j];
186          a1 = a[j+1];
187          x1 = xx[N-2+j-i];
188
189          y0 += a0 * x1;
190          y1 += a1 * x1;
191          y2 += a0 * x0;
192          y3 += a1 * x0;
193
194          a0 = a[j+2];
195          a1 = a[j+3];
196          x0 = xx[N+j-i];
197
198          y0 += a0 * x0;
199          y1 += a1 * x0;
200          y2 += a0 * x1;
201          y3 += a1 * x1;
202       }
203       y[i] = y0;
204       y[i+1] = y1;
205       y[i+2] = y2;
206       y[i+3] = y3;
207    }
208
209    for (i = 0; i < M - 1; i += 2)
210       mem[i+1] = xx[i];
211 }
212
213
214 void comp_filter_mem_init (CombFilterMem *mem)
215 {
216    mem->last_pitch=0;
217    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0;
218    mem->smooth_gain=1;
219 }
220
221 void comb_filter(
222 float *exc,          /*decoded excitation*/
223 float *new_exc,      /*enhanced excitation*/
224 float *ak,           /*LPC filter coefs*/
225 int p,               /*LPC order*/
226 int nsf,             /*sub-frame size*/
227 int pitch,           /*pitch period*/
228 float *pitch_gain,   /*pitch gain (3-tap)*/
229 float  comb_gain,    /*gain of comb filter*/
230 CombFilterMem *mem
231 )
232 {
233    int i;
234    float exc_energy=0, new_exc_energy=0;
235    float gain;
236    float step;
237    float fact;
238    /*Compute excitation energy prior to enhancement*/
239    for (i=0;i<nsf;i++)
240       exc_energy+=exc[i]*exc[i];
241
242    /*Some gain adjustment is pitch is too high or if unvoiced*/
243    {
244       float g=0;
245       g = .5*fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2] +
246       mem->last_pitch_gain[0] + mem->last_pitch_gain[1] + mem->last_pitch_gain[2]);
247       if (g>1.3)
248          comb_gain*=1.3/g;
249       if (g<.5)
250          comb_gain*=2*g;
251    }
252    step = 1.0/nsf;
253    fact=0;
254    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
255    for (i=0;i<nsf;i++)
256    {
257       fact += step;
258
259       new_exc[i] = exc[i] + comb_gain * fact * (
260                                          pitch_gain[0]*exc[i-pitch+1] +
261                                          pitch_gain[1]*exc[i-pitch] +
262                                          pitch_gain[2]*exc[i-pitch-1]
263                                          )
264       + comb_gain * (1-fact) * (
265                                          mem->last_pitch_gain[0]*exc[i-mem->last_pitch+1] +
266                                          mem->last_pitch_gain[1]*exc[i-mem->last_pitch] +
267                                          mem->last_pitch_gain[2]*exc[i-mem->last_pitch-1]
268                                          );
269    }
270
271    mem->last_pitch_gain[0] = pitch_gain[0];
272    mem->last_pitch_gain[1] = pitch_gain[1];
273    mem->last_pitch_gain[2] = pitch_gain[2];
274    mem->last_pitch = pitch;
275
276    /*Gain after enhancement*/
277    for (i=0;i<nsf;i++)
278       new_exc_energy+=new_exc[i]*new_exc[i];
279
280    /*Compute scaling factor and normalize energy*/
281    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
282    if (gain < .5)
283       gain=.5;
284    if (gain>1)
285       gain=1;
286
287    for (i=0;i<nsf;i++)
288    {
289       mem->smooth_gain = .96*mem->smooth_gain + .04*gain;
290       new_exc[i] *= mem->smooth_gain;
291    }
292 }