s/FOUNDATION/COPYRIGHT OWNER/ in CELT code and "glue code"
[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 = MAC16_16(Syy, y[j],y[j]);
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          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 void pitch_downsample(celt_sig * restrict x[], opus_val16 * restrict x_lp,
101       int len, int C)
102 {
103    int i;
104    opus_val32 ac[5];
105    opus_val16 tmp=Q15ONE;
106    opus_val16 lpc[4], mem[4]={0,0,0,0};
107    for (i=1;i<len>>1;i++)
108       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+3);
109    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), SIG_SHIFT+3);
110    if (C==2)
111    {
112       for (i=1;i<len>>1;i++)
113          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+3);
114       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+3);
115    }
116
117    _celt_autocorr(x_lp, ac, NULL, 0,
118                   4, len>>1);
119
120    /* Noise floor -40 dB */
121 #ifdef FIXED_POINT
122    ac[0] += SHR32(ac[0],13);
123 #else
124    ac[0] *= 1.0001f;
125 #endif
126    /* Lag windowing */
127    for (i=1;i<=4;i++)
128    {
129       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
130 #ifdef FIXED_POINT
131       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
132 #else
133       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
134 #endif
135    }
136
137    _celt_lpc(lpc, ac, 4);
138    for (i=0;i<4;i++)
139    {
140       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
141       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
142    }
143    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
144
145    mem[0]=0;
146    lpc[0]=QCONST16(.8f,12);
147    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
148
149 }
150
151 void pitch_search(const opus_val16 * restrict x_lp, opus_val16 * restrict y,
152                   int len, int max_pitch, int *pitch)
153 {
154    int i, j;
155    int lag;
156    int best_pitch[2]={0,0};
157    VARDECL(opus_val16, x_lp4);
158    VARDECL(opus_val16, y_lp4);
159    VARDECL(opus_val32, xcorr);
160 #ifdef FIXED_POINT
161    opus_val32 maxcorr=1;
162    int shift=0;
163 #endif
164    int offset;
165
166    SAVE_STACK;
167
168    celt_assert(len>0);
169    celt_assert(max_pitch>0);
170    lag = len+max_pitch;
171
172    ALLOC(x_lp4, len>>2, opus_val16);
173    ALLOC(y_lp4, lag>>2, opus_val16);
174    ALLOC(xcorr, max_pitch>>1, opus_val32);
175
176    /* Downsample by 2 again */
177    for (j=0;j<len>>2;j++)
178       x_lp4[j] = x_lp[2*j];
179    for (j=0;j<lag>>2;j++)
180       y_lp4[j] = y[2*j];
181
182 #ifdef FIXED_POINT
183    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
184    if (shift>0)
185    {
186       for (j=0;j<len>>2;j++)
187          x_lp4[j] = SHR16(x_lp4[j], shift);
188       for (j=0;j<lag>>2;j++)
189          y_lp4[j] = SHR16(y_lp4[j], shift);
190       /* Use double the shift for a MAC */
191       shift *= 2;
192    } else {
193       shift = 0;
194    }
195 #endif
196
197    /* Coarse search with 4x decimation */
198
199    for (i=0;i<max_pitch>>2;i++)
200    {
201       opus_val32 sum = 0;
202       for (j=0;j<len>>2;j++)
203          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
204       xcorr[i] = MAX32(-1, sum);
205 #ifdef FIXED_POINT
206       maxcorr = MAX32(maxcorr, sum);
207 #endif
208    }
209    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
210 #ifdef FIXED_POINT
211                    , 0, maxcorr
212 #endif
213                    );
214
215    /* Finer search with 2x decimation */
216 #ifdef FIXED_POINT
217    maxcorr=1;
218 #endif
219    for (i=0;i<max_pitch>>1;i++)
220    {
221       opus_val32 sum=0;
222       xcorr[i] = 0;
223       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
224          continue;
225       for (j=0;j<len>>1;j++)
226          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
227       xcorr[i] = MAX32(-1, sum);
228 #ifdef FIXED_POINT
229       maxcorr = MAX32(maxcorr, sum);
230 #endif
231    }
232    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
233 #ifdef FIXED_POINT
234                    , shift, maxcorr
235 #endif
236                    );
237
238    /* Refine by pseudo-interpolation */
239    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
240    {
241       opus_val32 a, b, c;
242       a = xcorr[best_pitch[0]-1];
243       b = xcorr[best_pitch[0]];
244       c = xcorr[best_pitch[0]+1];
245       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
246          offset = 1;
247       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
248          offset = -1;
249       else
250          offset = 0;
251    } else {
252       offset = 0;
253    }
254    *pitch = 2*best_pitch[0]-offset;
255
256    RESTORE_STACK;
257 }
258
259 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
260 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
261       int N, int *T0_, int prev_period, opus_val16 prev_gain)
262 {
263    int k, i, T, T0;
264    opus_val16 g, g0;
265    opus_val16 pg;
266    opus_val32 xy,xx,yy;
267    opus_val32 xcorr[3];
268    opus_val32 best_xy, best_yy;
269    int offset;
270    int minperiod0;
271
272    minperiod0 = minperiod;
273    maxperiod /= 2;
274    minperiod /= 2;
275    *T0_ /= 2;
276    prev_period /= 2;
277    N /= 2;
278    x += maxperiod;
279    if (*T0_>=maxperiod)
280       *T0_=maxperiod-1;
281
282    T = T0 = *T0_;
283    xx=xy=yy=0;
284    for (i=0;i<N;i++)
285    {
286       xy = MAC16_16(xy, x[i], x[i-T0]);
287       xx = MAC16_16(xx, x[i], x[i]);
288       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
289    }
290    best_xy = xy;
291    best_yy = yy;
292 #ifdef FIXED_POINT
293       {
294          opus_val32 x2y2;
295          int sh, t;
296          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
297          sh = celt_ilog2(x2y2)>>1;
298          t = VSHR32(x2y2, 2*(sh-7));
299          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
300       }
301 #else
302       g = g0 = xy/celt_sqrt(1+xx*yy);
303 #endif
304    /* Look for any pitch at T/k */
305    for (k=2;k<=15;k++)
306    {
307       int T1, T1b;
308       opus_val16 g1;
309       opus_val16 cont=0;
310       T1 = (2*T0+k)/(2*k);
311       if (T1 < minperiod)
312          break;
313       /* Look for another strong correlation at T1b */
314       if (k==2)
315       {
316          if (T1+T0>maxperiod)
317             T1b = T0;
318          else
319             T1b = T0+T1;
320       } else
321       {
322          T1b = (2*second_check[k]*T0+k)/(2*k);
323       }
324       xy=yy=0;
325       for (i=0;i<N;i++)
326       {
327          xy = MAC16_16(xy, x[i], x[i-T1]);
328          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
329
330          xy = MAC16_16(xy, x[i], x[i-T1b]);
331          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
332       }
333 #ifdef FIXED_POINT
334       {
335          opus_val32 x2y2;
336          int sh, t;
337          x2y2 = 1+MULT32_32_Q31(xx,yy);
338          sh = celt_ilog2(x2y2)>>1;
339          t = VSHR32(x2y2, 2*(sh-7));
340          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
341       }
342 #else
343       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
344 #endif
345       if (abs(T1-prev_period)<=1)
346          cont = prev_gain;
347       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
348          cont = HALF32(prev_gain);
349       else
350          cont = 0;
351       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
352       {
353          best_xy = xy;
354          best_yy = yy;
355          T = T1;
356          g = g1;
357       }
358    }
359    if (best_yy <= best_xy)
360       pg = Q15ONE;
361    else
362       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
363
364    for (k=0;k<3;k++)
365    {
366       int T1 = T+k-1;
367       xy = 0;
368       for (i=0;i<N;i++)
369          xy = MAC16_16(xy, x[i], x[i-T1]);
370       xcorr[k] = xy;
371    }
372    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
373       offset = 1;
374    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
375       offset = -1;
376    else
377       offset = 0;
378    if (pg > g)
379       pg = g;
380    *T0_ = 2*T+offset;
381
382    if (*T0_<minperiod0)
383       *T0_=minperiod0;
384    return pg;
385 }