Replace C99 restrict keyword with OPUS_RESTRICT.
[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-12;
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 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y,
173                   int len, int max_pitch, int *pitch)
174 {
175    int i, j;
176    int lag;
177    int best_pitch[2]={0,0};
178    VARDECL(opus_val16, x_lp4);
179    VARDECL(opus_val16, y_lp4);
180    VARDECL(opus_val32, xcorr);
181 #ifdef FIXED_POINT
182    opus_val32 maxcorr=1;
183    opus_val16 xmax, ymax;
184    int shift=0;
185 #endif
186    int offset;
187
188    SAVE_STACK;
189
190    celt_assert(len>0);
191    celt_assert(max_pitch>0);
192    lag = len+max_pitch;
193
194    ALLOC(x_lp4, len>>2, opus_val16);
195    ALLOC(y_lp4, lag>>2, opus_val16);
196    ALLOC(xcorr, max_pitch>>1, opus_val32);
197
198    /* Downsample by 2 again */
199    for (j=0;j<len>>2;j++)
200       x_lp4[j] = x_lp[2*j];
201    for (j=0;j<lag>>2;j++)
202       y_lp4[j] = y[2*j];
203
204 #ifdef FIXED_POINT
205    xmax = celt_maxabs16(x_lp4, len>>2);
206    ymax = celt_maxabs16(y_lp4, lag>>2);
207    shift = celt_ilog2(MAX16(1, MAX16(xmax, ymax)))-11;
208    if (shift>0)
209    {
210       for (j=0;j<len>>2;j++)
211          x_lp4[j] = SHR16(x_lp4[j], shift);
212       for (j=0;j<lag>>2;j++)
213          y_lp4[j] = SHR16(y_lp4[j], shift);
214       /* Use double the shift for a MAC */
215       shift *= 2;
216    } else {
217       shift = 0;
218    }
219 #endif
220
221    /* Coarse search with 4x decimation */
222
223    for (i=0;i<max_pitch>>2;i++)
224    {
225       opus_val32 sum = 0;
226       for (j=0;j<len>>2;j++)
227          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
228       xcorr[i] = MAX32(-1, sum);
229 #ifdef FIXED_POINT
230       maxcorr = MAX32(maxcorr, sum);
231 #endif
232    }
233    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
234 #ifdef FIXED_POINT
235                    , 0, maxcorr
236 #endif
237                    );
238
239    /* Finer search with 2x decimation */
240 #ifdef FIXED_POINT
241    maxcorr=1;
242 #endif
243    for (i=0;i<max_pitch>>1;i++)
244    {
245       opus_val32 sum=0;
246       xcorr[i] = 0;
247       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
248          continue;
249       for (j=0;j<len>>1;j++)
250          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
251       xcorr[i] = MAX32(-1, sum);
252 #ifdef FIXED_POINT
253       maxcorr = MAX32(maxcorr, sum);
254 #endif
255    }
256    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
257 #ifdef FIXED_POINT
258                    , shift+1, maxcorr
259 #endif
260                    );
261
262    /* Refine by pseudo-interpolation */
263    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
264    {
265       opus_val32 a, b, c;
266       a = xcorr[best_pitch[0]-1];
267       b = xcorr[best_pitch[0]];
268       c = xcorr[best_pitch[0]+1];
269       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
270          offset = 1;
271       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
272          offset = -1;
273       else
274          offset = 0;
275    } else {
276       offset = 0;
277    }
278    *pitch = 2*best_pitch[0]-offset;
279
280    RESTORE_STACK;
281 }
282
283 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
284 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
285       int N, int *T0_, int prev_period, opus_val16 prev_gain)
286 {
287    int k, i, T, T0;
288    opus_val16 g, g0;
289    opus_val16 pg;
290    opus_val32 xy,xx,yy;
291    opus_val32 xcorr[3];
292    opus_val32 best_xy, best_yy;
293    int offset;
294    int minperiod0;
295
296    minperiod0 = minperiod;
297    maxperiod /= 2;
298    minperiod /= 2;
299    *T0_ /= 2;
300    prev_period /= 2;
301    N /= 2;
302    x += maxperiod;
303    if (*T0_>=maxperiod)
304       *T0_=maxperiod-1;
305
306    T = T0 = *T0_;
307    xx=xy=yy=0;
308    for (i=0;i<N;i++)
309    {
310       xy = MAC16_16(xy, x[i], x[i-T0]);
311       xx = MAC16_16(xx, x[i], x[i]);
312       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
313    }
314    best_xy = xy;
315    best_yy = yy;
316 #ifdef FIXED_POINT
317       {
318          opus_val32 x2y2;
319          int sh, t;
320          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
321          sh = celt_ilog2(x2y2)>>1;
322          t = VSHR32(x2y2, 2*(sh-7));
323          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
324       }
325 #else
326       g = g0 = xy/celt_sqrt(1+xx*yy);
327 #endif
328    /* Look for any pitch at T/k */
329    for (k=2;k<=15;k++)
330    {
331       int T1, T1b;
332       opus_val16 g1;
333       opus_val16 cont=0;
334       T1 = (2*T0+k)/(2*k);
335       if (T1 < minperiod)
336          break;
337       /* Look for another strong correlation at T1b */
338       if (k==2)
339       {
340          if (T1+T0>maxperiod)
341             T1b = T0;
342          else
343             T1b = T0+T1;
344       } else
345       {
346          T1b = (2*second_check[k]*T0+k)/(2*k);
347       }
348       xy=yy=0;
349       for (i=0;i<N;i++)
350       {
351          xy = MAC16_16(xy, x[i], x[i-T1]);
352          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
353
354          xy = MAC16_16(xy, x[i], x[i-T1b]);
355          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
356       }
357 #ifdef FIXED_POINT
358       {
359          opus_val32 x2y2;
360          int sh, t;
361          x2y2 = 1+MULT32_32_Q31(xx,yy);
362          sh = celt_ilog2(x2y2)>>1;
363          t = VSHR32(x2y2, 2*(sh-7));
364          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
365       }
366 #else
367       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
368 #endif
369       if (abs(T1-prev_period)<=1)
370          cont = prev_gain;
371       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
372          cont = HALF32(prev_gain);
373       else
374          cont = 0;
375       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
376       {
377          best_xy = xy;
378          best_yy = yy;
379          T = T1;
380          g = g1;
381       }
382    }
383    best_xy = MAX32(0, best_xy);
384    if (best_yy <= best_xy)
385       pg = Q15ONE;
386    else
387       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
388
389    for (k=0;k<3;k++)
390    {
391       int T1 = T+k-1;
392       xy = 0;
393       for (i=0;i<N;i++)
394          xy = MAC16_16(xy, x[i], x[i-T1]);
395       xcorr[k] = xy;
396    }
397    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
398       offset = 1;
399    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
400       offset = -1;
401    else
402       offset = 0;
403    if (pg > g)
404       pg = g;
405    *T0_ = 2*T+offset;
406
407    if (*T0_<minperiod0)
408       *T0_=minperiod0;
409    return pg;
410 }