VAD should now work on wideband too.
[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, void *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, void *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, void *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, void *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 comb_filter(
215 float *exc,          /*decoded excitation*/
216 float *new_exc,      /*enhanced excitation*/
217 float *ak,           /*LPC filter coefs*/
218 int p,               /*LPC order*/
219 int nsf,             /*sub-frame size*/
220 int pitch,           /*pitch period*/
221 float *pitch_gain,   /*pitch gain (3-tap)*/
222 float  comb_gain     /*gain of comb filter*/
223 )
224 {
225    int i;
226    float exc_energy=0, new_exc_energy=0;
227    float gain;
228
229    /*Compute excitation energy prior to enhancement*/
230    for (i=0;i<nsf;i++)
231       exc_energy+=exc[i]*exc[i];
232
233    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
234    for (i=0;i<nsf;i++)
235    {
236       new_exc[i] = exc[i] + comb_gain * (
237                                          pitch_gain[0]*exc[i-pitch+1] +
238                                          pitch_gain[1]*exc[i-pitch] +
239                                          pitch_gain[2]*exc[i-pitch-1]
240                                          );
241    }
242    
243    /*Gain after enhancement*/
244    for (i=0;i<nsf;i++)
245       new_exc_energy+=new_exc[i]*new_exc[i];
246
247    /*Compute scaling factor and normalize energy*/
248    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
249    for (i=0;i<nsf;i++)
250       new_exc[i] *= gain;
251 }