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