Enabling the post-filter and exporting the ec functions for Opus
[opus.git] / libcelt / pitch.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2009 Xiph.Org Foundation
3    Written by Jean-Marc Valin */
4 /**
5    @file pitch.c
6    @brief Pitch analysis
7  */
8
9 /*
10    Redistribution and use in source and binary forms, with or without
11    modification, are permitted provided that the following conditions
12    are met:
13    
14    - Redistributions of source code must retain the above copyright
15    notice, this list of conditions and the following disclaimer.
16    
17    - Redistributions in binary form must reproduce the above copyright
18    notice, this list of conditions and the following disclaimer in the
19    documentation and/or other materials provided with the distribution.
20    
21    - Neither the name of the Xiph.org Foundation nor the names of its
22    contributors may be used to endorse or promote products derived from
23    this software without specific prior written permission.
24    
25    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
28    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
29    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
30    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
31    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
32    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
33    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
34    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
35    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 */
37
38
39 #ifdef HAVE_CONFIG_H
40 #include "config.h"
41 #endif
42
43 /* Always enable postfilter for Opus */
44 #if defined(OPUS_BUILD) && !defined(ENABLE_POSTFILTER)
45 #define ENABLE_POSTFILTER
46 #endif
47
48 #include "pitch.h"
49 #include "os_support.h"
50 #include "modes.h"
51 #include "stack_alloc.h"
52 #include "mathops.h"
53
54 static void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y,
55                             int yshift, int len, int max_pitch, int best_pitch[2])
56 {
57    int i, j;
58    celt_word32 Syy=1;
59    celt_word16 best_num[2];
60    celt_word32 best_den[2];
61 #ifdef FIXED_POINT
62    int xshift;
63
64    xshift = celt_ilog2(maxcorr)-14;
65 #endif
66
67    best_num[0] = -1;
68    best_num[1] = -1;
69    best_den[0] = 0;
70    best_den[1] = 0;
71    best_pitch[0] = 0;
72    best_pitch[1] = 1;
73    for (j=0;j<len;j++)
74       Syy = MAC16_16(Syy, y[j],y[j]);
75    for (i=0;i<max_pitch;i++)
76    {
77       if (xcorr[i]>0)
78       {
79          celt_word16 num;
80          celt_word32 xcorr16;
81          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
82          num = MULT16_16_Q15(xcorr16,xcorr16);
83          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
84          {
85             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
86             {
87                best_num[1] = best_num[0];
88                best_den[1] = best_den[0];
89                best_pitch[1] = best_pitch[0];
90                best_num[0] = num;
91                best_den[0] = Syy;
92                best_pitch[0] = i;
93             } else {
94                best_num[1] = num;
95                best_den[1] = Syy;
96                best_pitch[1] = i;
97             }
98          }
99       }
100       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
101       Syy = MAX32(1, Syy);
102    }
103 }
104
105 #include "plc.h"
106 void pitch_downsample(celt_sig * restrict x[], celt_word16 * restrict x_lp,
107       int len, int _C)
108 {
109    int i;
110    celt_word32 ac[5];
111    celt_word16 tmp=Q15ONE;
112    celt_word16 lpc[4], mem[4]={0,0,0,0};
113    const int C = CHANNELS(_C);
114    for (i=1;i<len>>1;i++)
115       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+3);
116    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), SIG_SHIFT+3);
117    if (C==2)
118    {
119       for (i=1;i<len>>1;i++)
120          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+3);
121       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+3);
122    }
123
124    _celt_autocorr(x_lp, ac, NULL, 0,
125                   4, len>>1);
126
127    /* Noise floor -40 dB */
128 #ifdef FIXED_POINT
129    ac[0] += SHR32(ac[0],13);
130 #else
131    ac[0] *= 1.0001f;
132 #endif
133    /* Lag windowing */
134    for (i=1;i<=4;i++)
135    {
136       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
137 #ifdef FIXED_POINT
138       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
139 #else
140       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
141 #endif
142    }
143
144    _celt_lpc(lpc, ac, 4);
145    for (i=0;i<4;i++)
146    {
147       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
148       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
149    }
150    fir(x_lp, lpc, x_lp, len>>1, 4, mem);
151
152    mem[0]=0;
153    lpc[0]=QCONST16(.8f,12);
154    fir(x_lp, lpc, x_lp, len>>1, 1, mem);
155
156 }
157
158 void pitch_search(const celt_word16 * restrict x_lp, celt_word16 * restrict y,
159                   int len, int max_pitch, int *pitch)
160 {
161    int i, j;
162    int lag;
163    int best_pitch[2]={0};
164    VARDECL(celt_word16, x_lp4);
165    VARDECL(celt_word16, y_lp4);
166    VARDECL(celt_word32, xcorr);
167    celt_word32 maxcorr=1;
168    int offset;
169    int shift=0;
170
171    SAVE_STACK;
172
173    lag = len+max_pitch;
174
175    ALLOC(x_lp4, len>>2, celt_word16);
176    ALLOC(y_lp4, lag>>2, celt_word16);
177    ALLOC(xcorr, max_pitch>>1, celt_word32);
178
179    /* Downsample by 2 again */
180    for (j=0;j<len>>2;j++)
181       x_lp4[j] = x_lp[2*j];
182    for (j=0;j<lag>>2;j++)
183       y_lp4[j] = y[2*j];
184
185 #ifdef FIXED_POINT
186    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
187    if (shift>0)
188    {
189       for (j=0;j<len>>2;j++)
190          x_lp4[j] = SHR16(x_lp4[j], shift);
191       for (j=0;j<lag>>2;j++)
192          y_lp4[j] = SHR16(y_lp4[j], shift);
193       /* Use double the shift for a MAC */
194       shift *= 2;
195    } else {
196       shift = 0;
197    }
198 #endif
199
200    /* Coarse search with 4x decimation */
201
202    for (i=0;i<max_pitch>>2;i++)
203    {
204       celt_word32 sum = 0;
205       for (j=0;j<len>>2;j++)
206          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
207       xcorr[i] = MAX32(-1, sum);
208       maxcorr = MAX32(maxcorr, sum);
209    }
210    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
211
212    /* Finer search with 2x decimation */
213    maxcorr=1;
214    for (i=0;i<max_pitch>>1;i++)
215    {
216       celt_word32 sum=0;
217       xcorr[i] = 0;
218       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
219          continue;
220       for (j=0;j<len>>1;j++)
221          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
222       xcorr[i] = MAX32(-1, sum);
223       maxcorr = MAX32(maxcorr, sum);
224    }
225    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
226
227    /* Refine by pseudo-interpolation */
228    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
229    {
230       celt_word32 a, b, c;
231       a = xcorr[best_pitch[0]-1];
232       b = xcorr[best_pitch[0]];
233       c = xcorr[best_pitch[0]+1];
234       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
235          offset = 1;
236       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
237          offset = -1;
238       else 
239          offset = 0;
240    } else {
241       offset = 0;
242    }
243    *pitch = 2*best_pitch[0]-offset;
244
245    RESTORE_STACK;
246 }
247
248 #ifdef ENABLE_POSTFILTER
249 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
250 celt_word16 remove_doubling(celt_word16 *x, int maxperiod, int minperiod,
251       int N, int *_T0, int prev_period, celt_word16 prev_gain)
252 {
253    int k, i, T, T0, k0;
254    celt_word16 g, g0;
255    celt_word16 pg;
256    celt_word32 xy,xx,yy;
257    celt_word32 xcorr[3];
258    celt_word32 best_xy, best_yy;
259    int offset;
260    int minperiod0;
261
262    minperiod0 = minperiod;
263    maxperiod /= 2;
264    minperiod /= 2;
265    *_T0 /= 2;
266    prev_period /= 2;
267    N /= 2;
268    x += maxperiod;
269    if (*_T0>=maxperiod)
270       *_T0=maxperiod-1;
271
272    T = T0 = *_T0;
273    xx=xy=yy=0;
274    for (i=0;i<N;i++)
275    {
276       xy = MAC16_16(xy, x[i], x[i-T0]);
277       xx = MAC16_16(xx, x[i], x[i]);
278       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
279    }
280    best_xy = xy;
281    best_yy = yy;
282 #ifdef FIXED_POINT
283       {
284          celt_word32 x2y2;
285          int sh, t;
286          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
287          sh = celt_ilog2(x2y2)>>1;
288          t = VSHR32(x2y2, 2*(sh-7));
289          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
290       }
291 #else
292       g = g0 = xy/sqrt(1+xx*yy);
293 #endif
294    k0 = 1;
295    /* Look for any pitch at T/k */
296    for (k=2;k<=15;k++)
297    {
298       int T1, T1b;
299       celt_word16 g1;
300       celt_word16 cont=0;
301       T1 = (2*T0+k)/(2*k);
302       if (T1 < minperiod)
303          break;
304       /* Look for another strong correlation at T1b */
305       if (k==2)
306       {
307          if (T1+T0>maxperiod)
308             T1b = T0;
309          else
310             T1b = T0+T1;
311       } else
312       {
313          T1b = (2*second_check[k]*T0+k)/(2*k);
314       }
315       xy=yy=0;
316       for (i=0;i<N;i++)
317       {
318          xy = MAC16_16(xy, x[i], x[i-T1]);
319          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
320
321          xy = MAC16_16(xy, x[i], x[i-T1b]);
322          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
323       }
324 #ifdef FIXED_POINT
325       {
326          celt_word32 x2y2;
327          int sh, t;
328          x2y2 = 1+MULT32_32_Q31(xx,yy);
329          sh = celt_ilog2(x2y2)>>1;
330          t = VSHR32(x2y2, 2*(sh-7));
331          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
332       }
333 #else
334       g1 = xy/sqrt(1+2.f*xx*1.f*yy);
335 #endif
336       if (abs(T1-prev_period)<=1)
337          cont = prev_gain;
338       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
339          cont = HALF32(prev_gain);
340       else
341          cont = 0;
342       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
343       {
344          best_xy = xy;
345          best_yy = yy;
346          T = T1;
347          g = g1;
348       }
349    }
350    if (best_yy <= best_xy)
351       pg = Q15ONE;
352    else
353       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
354
355    for (k=0;k<3;k++)
356    {
357       int T1 = T+k-1;
358       xy = 0;
359       for (i=0;i<N;i++)
360          xy = MAC16_16(xy, x[i], x[i-T1]);
361       xcorr[k] = xy;
362    }
363    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
364       offset = 1;
365    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
366       offset = -1;
367    else
368       offset = 0;
369    if (pg > g)
370       pg = g;
371    *_T0 = 2*T+offset;
372
373    if (*_T0<minperiod0)
374       *_T0=minperiod0;
375    return pg;
376 }
377
378 #endif /* ENABLE_POSTFILTER */