Fixes a bug introduced by the previous commit
[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 #include "celt_lpc.h"
44
45 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
46                             int max_pitch, int *best_pitch
47 #ifdef FIXED_POINT
48                             , int yshift, opus_val32 maxcorr
49 #endif
50                             )
51 {
52    int i, j;
53    opus_val32 Syy=1;
54    opus_val16 best_num[2];
55    opus_val32 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 = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
70    for (i=0;i<max_pitch;i++)
71    {
72       if (xcorr[i]>0)
73       {
74          opus_val16 num;
75          opus_val32 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 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 #ifdef FIXED_POINT
108    int shift;
109    opus_val32 maxabs = MAX32(1, celt_maxabs32(x[0], len));
110    if (C==2)
111       maxabs = MAX32(maxabs, celt_maxabs32(x[1], len));
112    shift = IMAX(0,celt_ilog2(maxabs)-11);
113    if (C==2)
114       shift++;
115 #endif
116    for (i=1;i<len>>1;i++)
117       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
118    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
119    if (C==2)
120    {
121       for (i=1;i<len>>1;i++)
122          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
123       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
124    }
125
126    _celt_autocorr(x_lp, ac, NULL, 0,
127                   4, len>>1);
128
129    /* Noise floor -40 dB */
130 #ifdef FIXED_POINT
131    ac[0] += SHR32(ac[0],13);
132 #else
133    ac[0] *= 1.0001f;
134 #endif
135    /* Lag windowing */
136    for (i=1;i<=4;i++)
137    {
138       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
139 #ifdef FIXED_POINT
140       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
141 #else
142       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
143 #endif
144    }
145
146    _celt_lpc(lpc, ac, 4);
147    for (i=0;i<4;i++)
148    {
149       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
150       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
151    }
152    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
153
154    mem[0]=0;
155    lpc[0]=QCONST16(.8f,12);
156    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
157
158 }
159
160 void pitch_search(const opus_val16 * restrict x_lp, opus_val16 * restrict y,
161                   int len, int max_pitch, int *pitch)
162 {
163    int i, j;
164    int lag;
165    int best_pitch[2]={0,0};
166    VARDECL(opus_val16, x_lp4);
167    VARDECL(opus_val16, y_lp4);
168    VARDECL(opus_val32, xcorr);
169 #ifdef FIXED_POINT
170    opus_val32 maxcorr=1;
171    int shift=0;
172 #endif
173    int offset;
174
175    SAVE_STACK;
176
177    celt_assert(len>0);
178    celt_assert(max_pitch>0);
179    lag = len+max_pitch;
180
181    ALLOC(x_lp4, len>>2, opus_val16);
182    ALLOC(y_lp4, lag>>2, opus_val16);
183    ALLOC(xcorr, max_pitch>>1, opus_val32);
184
185    /* Downsample by 2 again */
186    for (j=0;j<len>>2;j++)
187       x_lp4[j] = x_lp[2*j];
188    for (j=0;j<lag>>2;j++)
189       y_lp4[j] = y[2*j];
190
191 #ifdef FIXED_POINT
192    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
193    if (shift>0)
194    {
195       for (j=0;j<len>>2;j++)
196          x_lp4[j] = SHR16(x_lp4[j], shift);
197       for (j=0;j<lag>>2;j++)
198          y_lp4[j] = SHR16(y_lp4[j], shift);
199       /* Use double the shift for a MAC */
200       shift *= 2;
201    } else {
202       shift = 0;
203    }
204 #endif
205
206    /* Coarse search with 4x decimation */
207
208    for (i=0;i<max_pitch>>2;i++)
209    {
210       opus_val32 sum = 0;
211       for (j=0;j<len>>2;j++)
212          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
213       xcorr[i] = MAX32(-1, sum);
214 #ifdef FIXED_POINT
215       maxcorr = MAX32(maxcorr, sum);
216 #endif
217    }
218    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
219 #ifdef FIXED_POINT
220                    , 0, maxcorr
221 #endif
222                    );
223
224    /* Finer search with 2x decimation */
225 #ifdef FIXED_POINT
226    maxcorr=1;
227 #endif
228    for (i=0;i<max_pitch>>1;i++)
229    {
230       opus_val32 sum=0;
231       xcorr[i] = 0;
232       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
233          continue;
234       for (j=0;j<len>>1;j++)
235          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
236       xcorr[i] = MAX32(-1, sum);
237 #ifdef FIXED_POINT
238       maxcorr = MAX32(maxcorr, sum);
239 #endif
240    }
241    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
242 #ifdef FIXED_POINT
243                    , shift+1, maxcorr
244 #endif
245                    );
246
247    /* Refine by pseudo-interpolation */
248    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
249    {
250       opus_val32 a, b, c;
251       a = xcorr[best_pitch[0]-1];
252       b = xcorr[best_pitch[0]];
253       c = xcorr[best_pitch[0]+1];
254       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
255          offset = 1;
256       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
257          offset = -1;
258       else
259          offset = 0;
260    } else {
261       offset = 0;
262    }
263    *pitch = 2*best_pitch[0]-offset;
264
265    RESTORE_STACK;
266 }
267
268 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
269 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
270       int N, int *T0_, int prev_period, opus_val16 prev_gain)
271 {
272    int k, i, T, T0;
273    opus_val16 g, g0;
274    opus_val16 pg;
275    opus_val32 xy,xx,yy;
276    opus_val32 xcorr[3];
277    opus_val32 best_xy, best_yy;
278    int offset;
279    int minperiod0;
280
281    minperiod0 = minperiod;
282    maxperiod /= 2;
283    minperiod /= 2;
284    *T0_ /= 2;
285    prev_period /= 2;
286    N /= 2;
287    x += maxperiod;
288    if (*T0_>=maxperiod)
289       *T0_=maxperiod-1;
290
291    T = T0 = *T0_;
292    xx=xy=yy=0;
293    for (i=0;i<N;i++)
294    {
295       xy = MAC16_16(xy, x[i], x[i-T0]);
296       xx = MAC16_16(xx, x[i], x[i]);
297       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
298    }
299    best_xy = xy;
300    best_yy = yy;
301 #ifdef FIXED_POINT
302       {
303          opus_val32 x2y2;
304          int sh, t;
305          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
306          sh = celt_ilog2(x2y2)>>1;
307          t = VSHR32(x2y2, 2*(sh-7));
308          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
309       }
310 #else
311       g = g0 = xy/celt_sqrt(1+xx*yy);
312 #endif
313    /* Look for any pitch at T/k */
314    for (k=2;k<=15;k++)
315    {
316       int T1, T1b;
317       opus_val16 g1;
318       opus_val16 cont=0;
319       T1 = (2*T0+k)/(2*k);
320       if (T1 < minperiod)
321          break;
322       /* Look for another strong correlation at T1b */
323       if (k==2)
324       {
325          if (T1+T0>maxperiod)
326             T1b = T0;
327          else
328             T1b = T0+T1;
329       } else
330       {
331          T1b = (2*second_check[k]*T0+k)/(2*k);
332       }
333       xy=yy=0;
334       for (i=0;i<N;i++)
335       {
336          xy = MAC16_16(xy, x[i], x[i-T1]);
337          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
338
339          xy = MAC16_16(xy, x[i], x[i-T1b]);
340          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
341       }
342 #ifdef FIXED_POINT
343       {
344          opus_val32 x2y2;
345          int sh, t;
346          x2y2 = 1+MULT32_32_Q31(xx,yy);
347          sh = celt_ilog2(x2y2)>>1;
348          t = VSHR32(x2y2, 2*(sh-7));
349          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
350       }
351 #else
352       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
353 #endif
354       if (abs(T1-prev_period)<=1)
355          cont = prev_gain;
356       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
357          cont = HALF32(prev_gain);
358       else
359          cont = 0;
360       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
361       {
362          best_xy = xy;
363          best_yy = yy;
364          T = T1;
365          g = g1;
366       }
367    }
368    if (best_yy <= best_xy)
369       pg = Q15ONE;
370    else
371       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
372
373    for (k=0;k<3;k++)
374    {
375       int T1 = T+k-1;
376       xy = 0;
377       for (i=0;i<N;i++)
378          xy = MAC16_16(xy, x[i], x[i-T1]);
379       xcorr[k] = xy;
380    }
381    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
382       offset = 1;
383    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
384       offset = -1;
385    else
386       offset = 0;
387    if (pg > g)
388       pg = g;
389    *T0_ = 2*T+offset;
390
391    if (*T0_<minperiod0)
392       *T0_=minperiod0;
393    return pg;
394 }