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