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