Apply band caps to the band allocation table.
[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 #include "pitch.h"
44 #include "os_support.h"
45 #include "modes.h"
46 #include "stack_alloc.h"
47 #include "mathops.h"
48
49 static void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y,
50                             int yshift, int len, int max_pitch, int best_pitch[2])
51 {
52    int i, j;
53    celt_word32 Syy=1;
54    celt_word16 best_num[2];
55    celt_word32 best_den[2];
56 #ifdef FIXED_POINT
57    int xshift;
58
59    xshift = celt_ilog2(maxcorr)-14;
60 #endif
61
62    best_num[0] = -1;
63    best_num[1] = -1;
64    best_den[0] = 0;
65    best_den[1] = 0;
66    best_pitch[0] = 0;
67    best_pitch[1] = 1;
68    for (j=0;j<len;j++)
69       Syy = MAC16_16(Syy, y[j],y[j]);
70    for (i=0;i<max_pitch;i++)
71    {
72       if (xcorr[i]>0)
73       {
74          celt_word16 num;
75          celt_word32 xcorr16;
76          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
77          num = MULT16_16_Q15(xcorr16,xcorr16);
78          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
79          {
80             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
81             {
82                best_num[1] = best_num[0];
83                best_den[1] = best_den[0];
84                best_pitch[1] = best_pitch[0];
85                best_num[0] = num;
86                best_den[0] = Syy;
87                best_pitch[0] = i;
88             } else {
89                best_num[1] = num;
90                best_den[1] = Syy;
91                best_pitch[1] = i;
92             }
93          }
94       }
95       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
96       Syy = MAX32(1, Syy);
97    }
98 }
99
100 #include "plc.h"
101 void pitch_downsample(celt_sig * restrict x[], celt_word16 * restrict x_lp,
102       int len, int _C)
103 {
104    int i;
105    celt_word32 ac[5];
106    celt_word16 tmp=Q15ONE;
107    celt_word16 lpc[4], mem[4]={0,0,0,0};
108    const int C = CHANNELS(_C);
109    for (i=1;i<len>>1;i++)
110       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+3);
111    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), SIG_SHIFT+3);
112    if (C==2)
113    {
114       for (i=1;i<len>>1;i++)
115          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+3);
116       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+3);
117    }
118
119    _celt_autocorr(x_lp, ac, NULL, 0,
120                   4, len>>1);
121
122    /* Noise floor -40 dB */
123 #ifdef FIXED_POINT
124    ac[0] += SHR32(ac[0],13);
125 #else
126    ac[0] *= 1.0001f;
127 #endif
128    /* Lag windowing */
129    for (i=1;i<=4;i++)
130    {
131       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
132 #ifdef FIXED_POINT
133       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
134 #else
135       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
136 #endif
137    }
138
139    _celt_lpc(lpc, ac, 4);
140    for (i=0;i<4;i++)
141    {
142       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
143       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
144    }
145    fir(x_lp, lpc, x_lp, len>>1, 4, mem);
146
147    mem[0]=0;
148    lpc[0]=QCONST16(.8f,12);
149    fir(x_lp, lpc, x_lp, len>>1, 1, mem);
150
151 }
152
153 void pitch_search(const celt_word16 * restrict x_lp, celt_word16 * restrict y,
154                   int len, int max_pitch, int *pitch)
155 {
156    int i, j;
157    int lag;
158    int best_pitch[2]={0};
159    VARDECL(celt_word16, x_lp4);
160    VARDECL(celt_word16, y_lp4);
161    VARDECL(celt_word32, xcorr);
162    celt_word32 maxcorr=1;
163    int offset;
164    int shift=0;
165
166    SAVE_STACK;
167
168    lag = len+max_pitch;
169
170    ALLOC(x_lp4, len>>2, celt_word16);
171    ALLOC(y_lp4, lag>>2, celt_word16);
172    ALLOC(xcorr, max_pitch>>1, celt_word32);
173
174    /* Downsample by 2 again */
175    for (j=0;j<len>>2;j++)
176       x_lp4[j] = x_lp[2*j];
177    for (j=0;j<lag>>2;j++)
178       y_lp4[j] = y[2*j];
179
180 #ifdef FIXED_POINT
181    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
182    if (shift>0)
183    {
184       for (j=0;j<len>>2;j++)
185          x_lp4[j] = SHR16(x_lp4[j], shift);
186       for (j=0;j<lag>>2;j++)
187          y_lp4[j] = SHR16(y_lp4[j], shift);
188       /* Use double the shift for a MAC */
189       shift *= 2;
190    } else {
191       shift = 0;
192    }
193 #endif
194
195    /* Coarse search with 4x decimation */
196
197    for (i=0;i<max_pitch>>2;i++)
198    {
199       celt_word32 sum = 0;
200       for (j=0;j<len>>2;j++)
201          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
202       xcorr[i] = MAX32(-1, sum);
203       maxcorr = MAX32(maxcorr, sum);
204    }
205    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
206
207    /* Finer search with 2x decimation */
208    maxcorr=1;
209    for (i=0;i<max_pitch>>1;i++)
210    {
211       celt_word32 sum=0;
212       xcorr[i] = 0;
213       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
214          continue;
215       for (j=0;j<len>>1;j++)
216          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
217       xcorr[i] = MAX32(-1, sum);
218       maxcorr = MAX32(maxcorr, sum);
219    }
220    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
221
222    /* Refine by pseudo-interpolation */
223    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
224    {
225       celt_word32 a, b, c;
226       a = xcorr[best_pitch[0]-1];
227       b = xcorr[best_pitch[0]];
228       c = xcorr[best_pitch[0]+1];
229       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
230          offset = 1;
231       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
232          offset = -1;
233       else 
234          offset = 0;
235    } else {
236       offset = 0;
237    }
238    *pitch = 2*best_pitch[0]-offset;
239
240    RESTORE_STACK;
241 }
242
243 #ifdef ENABLE_POSTFILTER
244 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
245 celt_word16 remove_doubling(celt_word16 *x, int maxperiod, int minperiod,
246       int N, int *_T0, int prev_period, celt_word16 prev_gain)
247 {
248    int k, i, T, T0, k0;
249    celt_word16 g, g0;
250    celt_word16 pg;
251    celt_word32 xy,xx,yy;
252    celt_word32 xcorr[3];
253    celt_word32 best_xy, best_yy;
254    int offset;
255    int minperiod0;
256
257    minperiod0 = minperiod;
258    maxperiod /= 2;
259    minperiod /= 2;
260    *_T0 /= 2;
261    prev_period /= 2;
262    N /= 2;
263    x += maxperiod;
264    if (*_T0>=maxperiod)
265       *_T0=maxperiod-1;
266
267    T = T0 = *_T0;
268    xx=xy=yy=0;
269    for (i=0;i<N;i++)
270    {
271       xy = MAC16_16(xy, x[i], x[i-T0]);
272       xx = MAC16_16(xx, x[i], x[i]);
273       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
274    }
275    best_xy = xy;
276    best_yy = yy;
277 #ifdef FIXED_POINT
278       {
279          celt_word32 x2y2;
280          int sh, t;
281          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
282          sh = celt_ilog2(x2y2)>>1;
283          t = VSHR32(x2y2, 2*(sh-7));
284          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
285       }
286 #else
287       g = g0 = xy/sqrt(1+xx*yy);
288 #endif
289    k0 = 1;
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 */