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