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