5e681159c5bc44d20d4225c22fe229fb2e284719
[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 COPYRIGHT OWNER
25    OR 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 #ifndef FIXED_POINT
78          /* Considering the range of xcorr16, this should avoid both underflows
79             and overflows (inf) when squaring xcorr16 */
80          xcorr16 *= 1e-12f;
81 #endif
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 void pitch_downsample(celt_sig * OPUS_RESTRICT x[], opus_val16 * OPUS_RESTRICT x_lp,
106       int len, int C)
107 {
108    int i;
109    opus_val32 ac[5];
110    opus_val16 tmp=Q15ONE;
111    opus_val16 lpc[4], mem[4]={0,0,0,0};
112 #ifdef FIXED_POINT
113    int shift;
114    opus_val32 maxabs = celt_maxabs32(x[0], len);
115    if (C==2)
116    {
117       opus_val32 maxabs_1 = celt_maxabs32(x[1], len);
118       maxabs = MAX32(maxabs, maxabs_1);
119    }
120    if (maxabs<1)
121       maxabs=1;
122    shift = celt_ilog2(maxabs)-10;
123    if (shift<0)
124       shift=0;
125    if (C==2)
126       shift++;
127 #endif
128    for (i=1;i<len>>1;i++)
129       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
130    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
131    if (C==2)
132    {
133       for (i=1;i<len>>1;i++)
134          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
135       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
136    }
137
138    _celt_autocorr(x_lp, ac, NULL, 0,
139                   4, len>>1);
140
141    /* Noise floor -40 dB */
142 #ifdef FIXED_POINT
143    ac[0] += SHR32(ac[0],13);
144 #else
145    ac[0] *= 1.0001f;
146 #endif
147    /* Lag windowing */
148    for (i=1;i<=4;i++)
149    {
150       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
151 #ifdef FIXED_POINT
152       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
153 #else
154       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
155 #endif
156    }
157
158    _celt_lpc(lpc, ac, 4);
159    for (i=0;i<4;i++)
160    {
161       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
162       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
163    }
164    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
165
166    mem[0]=0;
167    lpc[0]=QCONST16(.8f,12);
168    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
169
170 }
171
172 #if 0 /* This is a simple version of the pitch correlation that should work
173          well on DSPs like Blackfin and TI C5x/C6x */
174
175 static void pitch_xcorr(opus_val16 *x, opus_val16 *y, opus_val32 *xcorr, int len, int max_pitch
176 #ifdef FIXED_POINT
177       ,opus_val32 *maxval
178 #endif
179       )
180 {
181    int i, j;
182 #ifdef FIXED_POINT
183    opus_val32 maxcorr=1;
184 #endif
185    for (i=0;i<max_pitch;i++)
186    {
187       opus_val32 sum = 0;
188       for (j=0;j<len;j++)
189          sum = MAC16_16(sum, x[j],y[i+j]);
190       xcorr[i] = MAX32(-1, sum);
191 #ifdef FIXED_POINT
192       maxcorr = MAX32(maxcorr, sum);
193 #endif
194    }
195 #ifdef FIXED_POINT
196    *maxval = maxcorr;
197 #endif
198 }
199
200 #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */
201
202 static void pitch_xcorr(opus_val16 *_x, opus_val16 *_y, opus_val32 *xcorr, int len, int max_pitch
203 #ifdef FIXED_POINT
204       ,opus_val32 *maxval
205 #endif
206       )
207 {
208    int i,j;
209 #ifdef FIXED_POINT
210    opus_val32 maxcorr=1;
211 #endif
212    for (i=0;i<max_pitch;i+=4)
213    {
214       /* Compute correlation*/
215       /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/
216       opus_val32 sum1=0;
217       opus_val32 sum2=0;
218       opus_val32 sum3=0;
219       opus_val32 sum4=0;
220       const opus_val16 *y = _y+i;
221       const opus_val16 *x = _x;
222       opus_val16 y0, y1, y2, y3;
223       /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/
224       y0=*y++;
225       y1=*y++;
226       y2=*y++;
227       for (j=0;j<len;j+=4)
228       {
229          opus_val16 tmp;
230          tmp = *x++;
231          y3=*y++;
232          sum1 = MAC16_16(sum1,tmp,y0);
233          sum2 = MAC16_16(sum2,tmp,y1);
234          sum3 = MAC16_16(sum3,tmp,y2);
235          sum4 = MAC16_16(sum4,tmp,y3);
236          tmp=*x++;
237          y0=*y++;
238          sum1 = MAC16_16(sum1,tmp,y1);
239          sum2 = MAC16_16(sum2,tmp,y2);
240          sum3 = MAC16_16(sum3,tmp,y3);
241          sum4 = MAC16_16(sum4,tmp,y0);
242          tmp=*x++;
243          y1=*y++;
244          sum1 = MAC16_16(sum1,tmp,y2);
245          sum2 = MAC16_16(sum2,tmp,y3);
246          sum3 = MAC16_16(sum3,tmp,y0);
247          sum4 = MAC16_16(sum4,tmp,y1);
248          tmp=*x++;
249          y2=*y++;
250          sum1 = MAC16_16(sum1,tmp,y3);
251          sum2 = MAC16_16(sum2,tmp,y0);
252          sum3 = MAC16_16(sum3,tmp,y1);
253          sum4 = MAC16_16(sum4,tmp,y2);
254       }
255       xcorr[i]=MAX32(-1, sum1);
256       xcorr[i+1]=MAX32(-1, sum2);
257       xcorr[i+2]=MAX32(-1, sum3);
258       xcorr[i+3]=MAX32(-1, sum4);
259 #ifdef FIXED_POINT
260       sum1 = MAX32(sum1, sum2);
261       sum3 = MAX32(sum3, sum4);
262       sum1 = MAX32(sum1, sum3);
263       maxcorr = MAX32(maxcorr, sum1);
264 #endif
265    }
266 #ifdef FIXED_POINT
267    *maxval = maxcorr;
268 #endif
269 }
270
271 #endif
272 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y,
273                   int len, int max_pitch, int *pitch)
274 {
275    int i, j;
276    int lag;
277    int best_pitch[2]={0,0};
278    VARDECL(opus_val16, x_lp4);
279    VARDECL(opus_val16, y_lp4);
280    VARDECL(opus_val32, xcorr);
281 #ifdef FIXED_POINT
282    opus_val32 maxcorr;
283    opus_val32 xmax, ymax;
284    int shift=0;
285 #endif
286    int offset;
287
288    SAVE_STACK;
289
290    celt_assert(len>0);
291    celt_assert(max_pitch>0);
292    lag = len+max_pitch;
293
294    ALLOC(x_lp4, len>>2, opus_val16);
295    ALLOC(y_lp4, lag>>2, opus_val16);
296    ALLOC(xcorr, max_pitch>>1, opus_val32);
297
298    /* Downsample by 2 again */
299    for (j=0;j<len>>2;j++)
300       x_lp4[j] = x_lp[2*j];
301    for (j=0;j<lag>>2;j++)
302       y_lp4[j] = y[2*j];
303
304 #ifdef FIXED_POINT
305    xmax = celt_maxabs16(x_lp4, len>>2);
306    ymax = celt_maxabs16(y_lp4, lag>>2);
307    shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11;
308    if (shift>0)
309    {
310       for (j=0;j<len>>2;j++)
311          x_lp4[j] = SHR16(x_lp4[j], shift);
312       for (j=0;j<lag>>2;j++)
313          y_lp4[j] = SHR16(y_lp4[j], shift);
314       /* Use double the shift for a MAC */
315       shift *= 2;
316    } else {
317       shift = 0;
318    }
319 #endif
320
321    /* Coarse search with 4x decimation */
322
323    pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2
324 #ifdef FIXED_POINT
325          ,&maxcorr
326 #endif
327          );
328
329    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
330 #ifdef FIXED_POINT
331                    , 0, maxcorr
332 #endif
333                    );
334
335    /* Finer search with 2x decimation */
336 #ifdef FIXED_POINT
337    maxcorr=1;
338 #endif
339    for (i=0;i<max_pitch>>1;i++)
340    {
341       opus_val32 sum=0;
342       xcorr[i] = 0;
343       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
344          continue;
345       for (j=0;j<len>>1;j++)
346          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
347       xcorr[i] = MAX32(-1, sum);
348 #ifdef FIXED_POINT
349       maxcorr = MAX32(maxcorr, sum);
350 #endif
351    }
352    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
353 #ifdef FIXED_POINT
354                    , shift+1, maxcorr
355 #endif
356                    );
357
358    /* Refine by pseudo-interpolation */
359    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
360    {
361       opus_val32 a, b, c;
362       a = xcorr[best_pitch[0]-1];
363       b = xcorr[best_pitch[0]];
364       c = xcorr[best_pitch[0]+1];
365       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
366          offset = 1;
367       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
368          offset = -1;
369       else
370          offset = 0;
371    } else {
372       offset = 0;
373    }
374    *pitch = 2*best_pitch[0]-offset;
375
376    RESTORE_STACK;
377 }
378
379 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
380 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
381       int N, int *T0_, int prev_period, opus_val16 prev_gain)
382 {
383    int k, i, T, T0;
384    opus_val16 g, g0;
385    opus_val16 pg;
386    opus_val32 xy,xx,yy;
387    opus_val32 xcorr[3];
388    opus_val32 best_xy, best_yy;
389    int offset;
390    int minperiod0;
391
392    minperiod0 = minperiod;
393    maxperiod /= 2;
394    minperiod /= 2;
395    *T0_ /= 2;
396    prev_period /= 2;
397    N /= 2;
398    x += maxperiod;
399    if (*T0_>=maxperiod)
400       *T0_=maxperiod-1;
401
402    T = T0 = *T0_;
403    xx=xy=yy=0;
404    for (i=0;i<N;i++)
405    {
406       xy = MAC16_16(xy, x[i], x[i-T0]);
407       xx = MAC16_16(xx, x[i], x[i]);
408       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
409    }
410    best_xy = xy;
411    best_yy = yy;
412 #ifdef FIXED_POINT
413       {
414          opus_val32 x2y2;
415          int sh, t;
416          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
417          sh = celt_ilog2(x2y2)>>1;
418          t = VSHR32(x2y2, 2*(sh-7));
419          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
420       }
421 #else
422       g = g0 = xy/celt_sqrt(1+xx*yy);
423 #endif
424    /* Look for any pitch at T/k */
425    for (k=2;k<=15;k++)
426    {
427       int T1, T1b;
428       opus_val16 g1;
429       opus_val16 cont=0;
430       opus_val16 thresh;
431       T1 = (2*T0+k)/(2*k);
432       if (T1 < minperiod)
433          break;
434       /* Look for another strong correlation at T1b */
435       if (k==2)
436       {
437          if (T1+T0>maxperiod)
438             T1b = T0;
439          else
440             T1b = T0+T1;
441       } else
442       {
443          T1b = (2*second_check[k]*T0+k)/(2*k);
444       }
445       xy=yy=0;
446       for (i=0;i<N;i++)
447       {
448          xy = MAC16_16(xy, x[i], x[i-T1]);
449          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
450
451          xy = MAC16_16(xy, x[i], x[i-T1b]);
452          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
453       }
454 #ifdef FIXED_POINT
455       {
456          opus_val32 x2y2;
457          int sh, t;
458          x2y2 = 1+MULT32_32_Q31(xx,yy);
459          sh = celt_ilog2(x2y2)>>1;
460          t = VSHR32(x2y2, 2*(sh-7));
461          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
462       }
463 #else
464       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
465 #endif
466       if (abs(T1-prev_period)<=1)
467          cont = prev_gain;
468       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
469          cont = HALF32(prev_gain);
470       else
471          cont = 0;
472       thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont);
473       /* Bias against very high pitch (very short period) to avoid false-positives
474          due to short-term correlation */
475       if (T1<3*minperiod)
476          thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
477       else if (T1<2*minperiod)
478          thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
479       if (g1 > thresh)
480       {
481          best_xy = xy;
482          best_yy = yy;
483          T = T1;
484          g = g1;
485       }
486    }
487    best_xy = MAX32(0, best_xy);
488    if (best_yy <= best_xy)
489       pg = Q15ONE;
490    else
491       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
492
493    for (k=0;k<3;k++)
494    {
495       int T1 = T+k-1;
496       xy = 0;
497       for (i=0;i<N;i++)
498          xy = MAC16_16(xy, x[i], x[i-T1]);
499       xcorr[k] = xy;
500    }
501    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
502       offset = 1;
503    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
504       offset = -1;
505    else
506       offset = 0;
507    if (pg > g)
508       pg = g;
509    *T0_ = 2*T+offset;
510
511    if (*T0_<minperiod0)
512       *T0_=minperiod0;
513    return pg;
514 }