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