Removes unused function parameters
[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    - Neither the name of the Xiph.org Foundation nor the names of its
22    contributors may be used to endorse or promote products derived from
23    this software without specific prior written permission.
24    
25    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
28    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
29    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
30    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
31    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
32    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
33    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
34    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
35    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 */
37
38
39 #ifdef HAVE_CONFIG_H
40 #include "config.h"
41 #endif
42
43 #include "pitch.h"
44 #include "os_support.h"
45 #include "modes.h"
46 #include "stack_alloc.h"
47 #include "mathops.h"
48
49 static void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y,
50                             int yshift, int len, int max_pitch, int best_pitch[2])
51 {
52    int i, j;
53    celt_word32 Syy=1;
54    celt_word16 best_num[2];
55    celt_word32 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 = MAC16_16(Syy, y[j],y[j]);
70    for (i=0;i<max_pitch;i++)
71    {
72       if (xcorr[i]>0)
73       {
74          celt_word16 num;
75          celt_word32 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 #include "plc.h"
101 void pitch_downsample(celt_sig * restrict x[], celt_word16 * restrict x_lp, int len, int end, int _C, celt_sig * restrict xmem, celt_word16 * restrict filt_mem)
102 {
103    int i;
104    celt_word32 ac[5];
105    celt_word16 tmp=Q15ONE;
106    celt_word16 lpc[4], mem[4]={0,0,0,0};
107    const int C = CHANNELS(_C);
108    for (i=1;i<len>>1;i++)
109       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+2);
110    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[0][1])+x[0][0]), SIG_SHIFT+2);
111    *xmem = x[0][end-1];
112    if (C==2)
113    {
114       for (i=1;i<len>>1;i++)
115          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+2);
116       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+2);
117       *xmem += x[1][end-1];
118    }
119
120    _celt_autocorr(x_lp, ac, NULL, 0,
121                   4, len>>1);
122
123    /* Noise floor -40 dB */
124 #ifdef FIXED_POINT
125    ac[0] += SHR32(ac[0],13);
126 #else
127    ac[0] *= 1.0001f;
128 #endif
129    /* Lag windowing */
130    for (i=1;i<=4;i++)
131    {
132       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
133 #ifdef FIXED_POINT
134       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
135 #else
136       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
137 #endif
138    }
139
140    _celt_lpc(lpc, ac, 4);
141    for (i=0;i<4;i++)
142    {
143       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
144       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
145    }
146    fir(x_lp, lpc, x_lp, len>>1, 4, mem);
147
148    mem[0]=0;
149    lpc[0]=QCONST16(.8,12);
150    fir(x_lp, lpc, x_lp, len>>1, 1, mem);
151
152 }
153
154 void pitch_search(const celt_word16 * restrict x_lp, celt_word16 * restrict y,
155                   int len, int max_pitch, int *pitch, celt_sig *xmem)
156 {
157    int i, j;
158    int lag;
159    int best_pitch[2]={0};
160    VARDECL(celt_word16, x_lp4);
161    VARDECL(celt_word16, y_lp4);
162    VARDECL(celt_word32, xcorr);
163    celt_word32 maxcorr=1;
164    int offset;
165    int shift=0;
166
167    SAVE_STACK;
168
169    lag = len+max_pitch;
170
171    ALLOC(x_lp4, len>>2, celt_word16);
172    ALLOC(y_lp4, lag>>2, celt_word16);
173    ALLOC(xcorr, max_pitch>>1, celt_word32);
174
175    /* Downsample by 2 again */
176    for (j=0;j<len>>2;j++)
177       x_lp4[j] = x_lp[2*j];
178    for (j=0;j<lag>>2;j++)
179       y_lp4[j] = y[2*j];
180
181 #ifdef FIXED_POINT
182    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
183    if (shift>0)
184    {
185       for (j=0;j<len>>2;j++)
186          x_lp4[j] = SHR16(x_lp4[j], shift);
187       for (j=0;j<lag>>2;j++)
188          y_lp4[j] = SHR16(y_lp4[j], shift);
189       /* Use double the shift for a MAC */
190       shift *= 2;
191    } else {
192       shift = 0;
193    }
194 #endif
195
196    /* Coarse search with 4x decimation */
197
198    for (i=0;i<max_pitch>>2;i++)
199    {
200       celt_word32 sum = 0;
201       for (j=0;j<len>>2;j++)
202          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
203       xcorr[i] = MAX32(-1, sum);
204       maxcorr = MAX32(maxcorr, sum);
205    }
206    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
207
208    /* Finer search with 2x decimation */
209    maxcorr=1;
210    for (i=0;i<max_pitch>>1;i++)
211    {
212       celt_word32 sum=0;
213       xcorr[i] = 0;
214       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
215          continue;
216       for (j=0;j<len>>1;j++)
217          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
218       xcorr[i] = MAX32(-1, sum);
219       maxcorr = MAX32(maxcorr, sum);
220    }
221    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
222
223    /* Refine by pseudo-interpolation */
224    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
225    {
226       celt_word32 a, b, c;
227       a = xcorr[best_pitch[0]-1];
228       b = xcorr[best_pitch[0]];
229       c = xcorr[best_pitch[0]+1];
230       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
231          offset = 1;
232       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
233          offset = -1;
234       else 
235          offset = 0;
236    } else {
237       offset = 0;
238    }
239    *pitch = 2*best_pitch[0]-offset;
240
241    RESTORE_STACK;
242 }
243
244 #ifdef ENABLE_POSTFILTER
245 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
246 celt_word16 remove_doubling(celt_word16 *x, int maxperiod, int minperiod,
247       int N, int *_T0, int prev_period, celt_word16 prev_gain)
248 {
249    int k, i, T, T0, k0;
250    celt_word16 g, g0;
251    celt_word16 pg;
252    celt_word32 xy,xx,yy;
253    celt_word32 xcorr[3];
254    celt_word32 best_xy, best_yy;
255    int offset;
256    int minperiod0;
257
258    minperiod0 = minperiod;
259    maxperiod /= 2;
260    minperiod /= 2;
261    *_T0 /= 2;
262    prev_period /= 2;
263    N /= 2;
264    x += maxperiod;
265    if (*_T0>=maxperiod)
266       *_T0=maxperiod-1;
267
268    T = T0 = *_T0;
269    xx=xy=yy=0;
270    for (i=0;i<N;i++)
271    {
272       xy = MAC16_16(xy, x[i], x[i-T0]);
273       xx = MAC16_16(xx, x[i], x[i]);
274       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
275    }
276    best_xy = xy;
277    best_yy = yy;
278 #ifdef FIXED_POINT
279       {
280          celt_word32 x2y2;
281          int sh, t;
282          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
283          sh = celt_ilog2(x2y2)>>1;
284          t = VSHR32(x2y2, 2*(sh-7));
285          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
286       }
287 #else
288       g = g0 = xy/sqrt(1+xx*yy);
289 #endif
290    k0 = 1;
291    /* Look for any pitch at T/k */
292    for (k=2;k<=15;k++)
293    {
294       int T1, T1b;
295       celt_word16 g1;
296       celt_word16 cont=0;
297       T1 = (2*T0+k)/(2*k);
298       if (T1 < minperiod)
299          break;
300       /* Look for another strong correlation at T1b */
301       if (k==2)
302       {
303          if (T1+T0>maxperiod)
304             T1b = T0;
305          else
306             T1b = T0+T1;
307       } else
308       {
309          T1b = (2*second_check[k]*T0+k)/(2*k);
310       }
311       xy=yy=0;
312       for (i=0;i<N;i++)
313       {
314          xy = MAC16_16(xy, x[i], x[i-T1]);
315          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
316
317          xy = MAC16_16(xy, x[i], x[i-T1b]);
318          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
319       }
320 #ifdef FIXED_POINT
321       {
322          celt_word32 x2y2;
323          int sh, t;
324          x2y2 = 1+MULT32_32_Q31(xx,yy);
325          sh = celt_ilog2(x2y2)>>1;
326          t = VSHR32(x2y2, 2*(sh-7));
327          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
328       }
329 #else
330       g1 = xy/sqrt(1+2.f*xx*1.f*yy);
331 #endif
332       if (abs(T1-prev_period)<=1)
333          cont = prev_gain;
334       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
335          cont = HALF32(prev_gain);
336       else
337          cont = 0;
338       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
339       {
340          best_xy = xy;
341          best_yy = yy;
342          T = T1;
343          g = g1;
344       }
345    }
346    if (best_yy <= best_xy)
347       pg = Q15ONE;
348    else
349       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
350
351    for (k=0;k<3;k++)
352    {
353       int T1 = T+k-1;
354       xy = 0;
355       for (i=0;i<N;i++)
356          xy = MAC16_16(xy, x[i], x[i-T1]);
357       xcorr[k] = xy;
358    }
359    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
360       offset = 1;
361    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
362       offset = -1;
363    else
364       offset = 0;
365    if (pg > g)
366       pg = g;
367    *_T0 = 2*T+offset;
368
369    if (*_T0<minperiod0)
370       *_T0=minperiod0;
371    return pg;
372 }
373
374 #endif /* ENABLE_POSTFILTER */