renames the libcelt/ directory to celt/
[opus.git] / celt / 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 #ifdef HAVE_CONFIG_H
35 #include "config.h"
36 #endif
37
38 #include "pitch.h"
39 #include "os_support.h"
40 #include "modes.h"
41 #include "stack_alloc.h"
42 #include "mathops.h"
43
44 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
45                             int max_pitch, int *best_pitch
46 #ifdef FIXED_POINT
47                             , int yshift, opus_val32 maxcorr
48 #endif
49                             )
50 {
51    int i, j;
52    opus_val32 Syy=1;
53    opus_val16 best_num[2];
54    opus_val32 best_den[2];
55 #ifdef FIXED_POINT
56    int xshift;
57
58    xshift = celt_ilog2(maxcorr)-14;
59 #endif
60
61    best_num[0] = -1;
62    best_num[1] = -1;
63    best_den[0] = 0;
64    best_den[1] = 0;
65    best_pitch[0] = 0;
66    best_pitch[1] = 1;
67    for (j=0;j<len;j++)
68       Syy = MAC16_16(Syy, y[j],y[j]);
69    for (i=0;i<max_pitch;i++)
70    {
71       if (xcorr[i]>0)
72       {
73          opus_val16 num;
74          opus_val32 xcorr16;
75          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
76          num = MULT16_16_Q15(xcorr16,xcorr16);
77          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
78          {
79             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
80             {
81                best_num[1] = best_num[0];
82                best_den[1] = best_den[0];
83                best_pitch[1] = best_pitch[0];
84                best_num[0] = num;
85                best_den[0] = Syy;
86                best_pitch[0] = i;
87             } else {
88                best_num[1] = num;
89                best_den[1] = Syy;
90                best_pitch[1] = i;
91             }
92          }
93       }
94       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
95       Syy = MAX32(1, Syy);
96    }
97 }
98
99 #include "plc.h"
100 void pitch_downsample(celt_sig * restrict x[], opus_val16 * restrict x_lp,
101       int len, int _C)
102 {
103    int i;
104    opus_val32 ac[5];
105    opus_val16 tmp=Q15ONE;
106    opus_val16 lpc[4], mem[4]={0,0,0,0};
107    const int C = CHANNELS(_C);
108    for (i=1;i<len>>1;i++)
109       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+3);
110    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), SIG_SHIFT+3);
111    if (C==2)
112    {
113       for (i=1;i<len>>1;i++)
114          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+3);
115       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+3);
116    }
117
118    _celt_autocorr(x_lp, ac, NULL, 0,
119                   4, len>>1);
120
121    /* Noise floor -40 dB */
122 #ifdef FIXED_POINT
123    ac[0] += SHR32(ac[0],13);
124 #else
125    ac[0] *= 1.0001f;
126 #endif
127    /* Lag windowing */
128    for (i=1;i<=4;i++)
129    {
130       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
131 #ifdef FIXED_POINT
132       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
133 #else
134       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
135 #endif
136    }
137
138    _celt_lpc(lpc, ac, 4);
139    for (i=0;i<4;i++)
140    {
141       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
142       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
143    }
144    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
145
146    mem[0]=0;
147    lpc[0]=QCONST16(.8f,12);
148    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
149
150 }
151
152 void pitch_search(const opus_val16 * restrict x_lp, opus_val16 * restrict y,
153                   int len, int max_pitch, int *pitch)
154 {
155    int i, j;
156    int lag;
157    int best_pitch[2]={0,0};
158    VARDECL(opus_val16, x_lp4);
159    VARDECL(opus_val16, y_lp4);
160    VARDECL(opus_val32, xcorr);
161 #ifdef FIXED_POINT
162    opus_val32 maxcorr=1;
163    int shift=0;
164 #endif
165    int offset;
166
167    SAVE_STACK;
168
169    lag = len+max_pitch;
170
171    ALLOC(x_lp4, len>>2, opus_val16);
172    ALLOC(y_lp4, lag>>2, opus_val16);
173    ALLOC(xcorr, max_pitch>>1, opus_val32);
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       opus_val32 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 #ifdef FIXED_POINT
205       maxcorr = MAX32(maxcorr, sum);
206 #endif
207    }
208    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
209 #ifdef FIXED_POINT
210                    , 0, maxcorr
211 #endif
212                    );
213
214    /* Finer search with 2x decimation */
215 #ifdef FIXED_POINT
216    maxcorr=1;
217 #endif
218    for (i=0;i<max_pitch>>1;i++)
219    {
220       opus_val32 sum=0;
221       xcorr[i] = 0;
222       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
223          continue;
224       for (j=0;j<len>>1;j++)
225          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
226       xcorr[i] = MAX32(-1, sum);
227 #ifdef FIXED_POINT
228       maxcorr = MAX32(maxcorr, sum);
229 #endif
230    }
231    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
232 #ifdef FIXED_POINT
233                    , shift, maxcorr
234 #endif
235                    );
236
237    /* Refine by pseudo-interpolation */
238    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
239    {
240       opus_val32 a, b, c;
241       a = xcorr[best_pitch[0]-1];
242       b = xcorr[best_pitch[0]];
243       c = xcorr[best_pitch[0]+1];
244       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
245          offset = 1;
246       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
247          offset = -1;
248       else
249          offset = 0;
250    } else {
251       offset = 0;
252    }
253    *pitch = 2*best_pitch[0]-offset;
254
255    RESTORE_STACK;
256 }
257
258 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
259 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
260       int N, int *_T0, int prev_period, opus_val16 prev_gain)
261 {
262    int k, i, T, T0;
263    opus_val16 g, g0;
264    opus_val16 pg;
265    opus_val32 xy,xx,yy;
266    opus_val32 xcorr[3];
267    opus_val32 best_xy, best_yy;
268    int offset;
269    int minperiod0;
270
271    minperiod0 = minperiod;
272    maxperiod /= 2;
273    minperiod /= 2;
274    *_T0 /= 2;
275    prev_period /= 2;
276    N /= 2;
277    x += maxperiod;
278    if (*_T0>=maxperiod)
279       *_T0=maxperiod-1;
280
281    T = T0 = *_T0;
282    xx=xy=yy=0;
283    for (i=0;i<N;i++)
284    {
285       xy = MAC16_16(xy, x[i], x[i-T0]);
286       xx = MAC16_16(xx, x[i], x[i]);
287       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
288    }
289    best_xy = xy;
290    best_yy = yy;
291 #ifdef FIXED_POINT
292       {
293          opus_val32 x2y2;
294          int sh, t;
295          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
296          sh = celt_ilog2(x2y2)>>1;
297          t = VSHR32(x2y2, 2*(sh-7));
298          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
299       }
300 #else
301       g = g0 = xy/celt_sqrt(1+xx*yy);
302 #endif
303    /* Look for any pitch at T/k */
304    for (k=2;k<=15;k++)
305    {
306       int T1, T1b;
307       opus_val16 g1;
308       opus_val16 cont=0;
309       T1 = (2*T0+k)/(2*k);
310       if (T1 < minperiod)
311          break;
312       /* Look for another strong correlation at T1b */
313       if (k==2)
314       {
315          if (T1+T0>maxperiod)
316             T1b = T0;
317          else
318             T1b = T0+T1;
319       } else
320       {
321          T1b = (2*second_check[k]*T0+k)/(2*k);
322       }
323       xy=yy=0;
324       for (i=0;i<N;i++)
325       {
326          xy = MAC16_16(xy, x[i], x[i-T1]);
327          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
328
329          xy = MAC16_16(xy, x[i], x[i-T1b]);
330          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
331       }
332 #ifdef FIXED_POINT
333       {
334          opus_val32 x2y2;
335          int sh, t;
336          x2y2 = 1+MULT32_32_Q31(xx,yy);
337          sh = celt_ilog2(x2y2)>>1;
338          t = VSHR32(x2y2, 2*(sh-7));
339          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
340       }
341 #else
342       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
343 #endif
344       if (abs(T1-prev_period)<=1)
345          cont = prev_gain;
346       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
347          cont = HALF32(prev_gain);
348       else
349          cont = 0;
350       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
351       {
352          best_xy = xy;
353          best_yy = yy;
354          T = T1;
355          g = g1;
356       }
357    }
358    if (best_yy <= best_xy)
359       pg = Q15ONE;
360    else
361       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
362
363    for (k=0;k<3;k++)
364    {
365       int T1 = T+k-1;
366       xy = 0;
367       for (i=0;i<N;i++)
368          xy = MAC16_16(xy, x[i], x[i-T1]);
369       xcorr[k] = xy;
370    }
371    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
372       offset = 1;
373    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
374       offset = -1;
375    else
376       offset = 0;
377    if (pg > g)
378       pg = g;
379    *_T0 = 2*T+offset;
380
381    if (*_T0<minperiod0)
382       *_T0=minperiod0;
383    return pg;
384 }