Temporal pitch search
[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 "_kiss_fft_guts.h"
44 #include "kiss_fftr.h"*/
45 #include "kfft_single.h"
46
47 #include "pitch.h"
48 #include "psy.h"
49 #include "os_support.h"
50 #include "mathops.h"
51 #include "modes.h"
52 #include "stack_alloc.h"
53
54 kiss_fftr_cfg pitch_state_alloc(int max_lag)
55 {
56    return real16_fft_alloc(max_lag);
57 }
58
59 void pitch_state_free(kiss_fftr_cfg st)
60 {
61    real16_fft_free(st);
62 }
63
64 #ifdef FIXED_POINT
65 static void normalise16(celt_word16 *x, int len, celt_word16 val)
66 {
67    int i;
68    celt_word16 maxabs;
69    maxabs = celt_maxabs16(x,len);
70    if (maxabs > val)
71    {
72       int shift = 0;
73       while (maxabs > val)
74       {
75          maxabs >>= 1;
76          shift++;
77       }
78       if (shift==0)
79          return;
80       i=0;
81       do{
82          x[i] = SHR16(x[i], shift);
83       } while (++i<len);
84    } else {
85       int shift=0;
86       if (maxabs == 0)
87          return;
88       val >>= 1;
89       while (maxabs < val)
90       {
91          val >>= 1;
92          shift++;
93       }
94       if (shift==0)
95          return;
96       i=0;
97       do{
98          x[i] = SHL16(x[i], shift);
99       } while (++i<len);
100    }
101 }
102 #else
103 #define normalise16(x,len,val)
104 #endif
105
106 #define INPUT_SHIFT 15
107
108 void find_spectral_pitch(const CELTMode *m, kiss_fftr_cfg fft, const struct PsyDecay *decay, const celt_sig * restrict x, const celt_sig * restrict y, const celt_word16 * restrict window, celt_word16 * restrict spectrum, int len, int max_pitch, int *pitch, int _C)
109 {
110    int c, i;
111    VARDECL(celt_word16, _X);
112    VARDECL(celt_word16, _Y);
113    const celt_word16 * restrict wptr;
114 #ifndef SHORTCUTS
115    VARDECL(celt_mask, curve);
116 #endif
117    celt_word16 * restrict X, * restrict Y;
118    celt_word16 * restrict Xptr, * restrict Yptr;
119    const celt_sig * restrict yptr;
120    int n2;
121    int L2;
122    const int C = CHANNELS(_C);
123    const int overlap = OVERLAP(m);
124    const int lag = MAX_PERIOD;
125    SAVE_STACK;
126    n2 = lag>>1;
127    L2 = len>>1;
128    ALLOC(_X, lag, celt_word16);
129    X = _X;
130 #ifndef SHORTCUTS
131    ALLOC(curve, n2, celt_mask);
132 #endif
133    CELT_MEMSET(X,0,lag);
134    /* Sum all channels of the current frame and copy into X in bit-reverse order */
135    for (c=0;c<C;c++)
136    {
137       const celt_sig * restrict xptr = &x[c];
138       for (i=0;i<L2;i++)
139       {
140          X[2*BITREV(fft,i)] += SHR32(*xptr,INPUT_SHIFT);
141          xptr += C;
142          X[2*BITREV(fft,i)+1] += SHR32(*xptr,INPUT_SHIFT);
143          xptr += C;
144       }
145    }
146    /* Applying the window in the bit-reverse domain. It's a bit weird, but it
147       can help save memory */
148    wptr = window;
149    for (i=0;i<overlap>>1;i++)
150    {
151       X[2*BITREV(fft,i)]        = MULT16_16_Q15(wptr[0], X[2*BITREV(fft,i)]);
152       X[2*BITREV(fft,i)+1]      = MULT16_16_Q15(wptr[1], X[2*BITREV(fft,i)+1]);
153       X[2*BITREV(fft,L2-i-1)]   = MULT16_16_Q15(wptr[1], X[2*BITREV(fft,L2-i-1)]);
154       X[2*BITREV(fft,L2-i-1)+1] = MULT16_16_Q15(wptr[0], X[2*BITREV(fft,L2-i-1)+1]);
155       wptr += 2;
156    }
157    normalise16(X, lag, 8192);
158    /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
159    /* Forward real FFT (in-place) */
160    real16_fft_inplace(fft, X, lag);
161
162    if (spectrum)
163    {
164       for (i=0;i<lag/4;i++)
165       {
166          spectrum[2*i] = X[4*i];
167          spectrum[2*i+1] = X[4*i+1];
168       }
169    }
170 #ifndef SHORTCUTS
171    compute_masking(decay, X, curve, lag);
172 #endif
173    
174    /* Deferred allocation to reduce peak stack usage */
175    ALLOC(_Y, lag, celt_word16);
176    Y = _Y;
177    yptr = &y[0];
178    /* Copy first channel of the past audio into Y in bit-reverse order */
179    for (i=0;i<n2;i++)
180    {
181       Y[2*BITREV(fft,i)] = SHR32(*yptr,INPUT_SHIFT);
182       yptr += C;
183       Y[2*BITREV(fft,i)+1] = SHR32(*yptr,INPUT_SHIFT);
184       yptr += C;
185    }
186    /* Add remaining channels into Y in bit-reverse order */
187    for (c=1;c<C;c++)
188    {
189       yptr = &y[c];
190       for (i=0;i<n2;i++)
191       {
192          Y[2*BITREV(fft,i)] += SHR32(*yptr,INPUT_SHIFT);
193          yptr += C;
194          Y[2*BITREV(fft,i)+1] += SHR32(*yptr,INPUT_SHIFT);
195          yptr += C;
196       }
197    }
198    normalise16(Y, lag, 8192);
199    /* Forward real FFT (in-place) */
200    real16_fft_inplace(fft, Y, lag);
201
202    /* Compute cross-spectrum using the inverse masking curve as weighting */
203    Xptr = &X[2];
204    Yptr = &Y[2];
205    for (i=1;i<n2;i++)
206    {
207       celt_word16 Xr, Xi, n;
208       /* weight = 1/sqrt(curve) */
209       Xr = Xptr[0];
210       Xi = Xptr[1];
211 #ifdef SHORTCUTS
212       /*n = SHR32(32767,(celt_ilog2(EPSILON+curve[i])>>1));*/
213       n = 1+(8192>>(celt_ilog2(1+MULT16_16(Xr,Xr)+MULT16_16(Xi,Xi))>>1));
214       /* Pre-multiply X by n, so we can keep everything in 16 bits */
215       Xr = MULT16_16_16(n, Xr);
216       Xi = MULT16_16_16(n, Xi);
217 #else
218       {
219          celt_word32 t;
220 #ifdef FIXED_POINT
221          int k;
222 #endif
223          t = EPSILON+curve[i];
224 #ifdef FIXED_POINT
225          k = celt_ilog2(t)>>1;
226 #endif
227          t = VSHR32(t, (k-7)<<1);
228          n = celt_rsqrt_norm(t);
229          /* Pre-multiply X by n, so we can keep everything in 16 bits */
230          Xr = EXTRACT16(PSHR32(MULT16_16(n, Xr),3+k));
231          Xi = EXTRACT16(PSHR32(MULT16_16(n, Xi),3+k));
232       }
233 #endif
234       /* Cross-spectrum between X and conj(Y) */
235       *Xptr++ = ADD16(MULT16_16_Q15(Xr, Yptr[0]), MULT16_16_Q15(Xi,Yptr[1]));
236       *Xptr++ = SUB16(MULT16_16_Q15(Xr, Yptr[1]), MULT16_16_Q15(Xi,Yptr[0]));
237       Yptr += 2;
238    }
239    /*printf ("\n");*/
240    X[0] = X[1] = 0;
241    /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
242    normalise16(X, lag, 50);
243    /* Inverse half-complex to real FFT gives us the correlation */
244    real16_ifft(fft, X, Y, lag);
245    
246    /* The peak in the correlation gives us the pitch */
247    *pitch = find_max16(Y, max_pitch);
248    /*printf ("%d ", *pitch);*/
249    RESTORE_STACK;
250 }
251
252 void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y, int yshift, int len, int max_pitch, int best_pitch[2])
253 {
254    int i, j;
255    celt_word32 Syy=1;
256    celt_word16 best_num[2];
257    celt_word32 best_den[2];
258 #ifdef FIXED_POINT
259    int xshift;
260
261    xshift = celt_ilog2(maxcorr)-14;
262 #endif
263
264    best_num[0] = -1;
265    best_num[1] = -1;
266    best_den[0] = 0;
267    best_den[1] = 0;
268    best_pitch[0] = 0;
269    best_pitch[1] = 1;
270    for (j=0;j<len;j++)
271       Syy = MAC16_16(Syy, y[j],y[j]);
272    for (i=0;i<max_pitch;i++)
273    {
274       float score;
275       if (xcorr[i]>0)
276       {
277          celt_word16 num;
278          celt_word32 xcorr16;
279          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
280          num = MULT16_16_Q15(xcorr16,xcorr16);
281          score = num*1./Syy;
282          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
283          {
284             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
285             {
286                best_num[1] = best_num[0];
287                best_den[1] = best_den[0];
288                best_pitch[1] = best_pitch[0];
289                best_num[0] = num;
290                best_den[0] = Syy;
291                best_pitch[0] = i;
292             } else {
293                best_num[1] = num;
294                best_den[1] = Syy;
295                best_pitch[1] = i;
296             }
297          }
298       }
299       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
300       Syy = MAX32(1, Syy);
301    }
302 }
303
304 void find_temporal_pitch(const CELTMode *m, const celt_sig * restrict x, celt_word16 * restrict y, int len, int max_pitch, int *pitch, int _C, celt_sig *xmem)
305 {
306    int i, j;
307    const int C = CHANNELS(_C);
308    const int lag = MAX_PERIOD;
309    const int N = FRAMESIZE(m);
310    int best_pitch[2]={0};
311    celt_word16 x_lp[len>>1];
312    celt_word16 x_lp4[len>>2];
313    celt_word16 y_lp4[lag>>2];
314    celt_word32 xcorr[max_pitch>>1];
315    celt_word32 maxcorr=1;
316    int offset;
317    int shift=0;
318
319    /* Down-sample by two and downmix to mono */
320    for (i=1;i<len>>1;i++)
321       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
322    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
323    *xmem = x[N-C];
324    if (C==2)
325    {
326       for (i=1;i<len>>1;i++)
327       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C+1]+x[(2*i+1)*C+1])+x[2*i*C+1]), SIG_SHIFT);
328       x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
329       *xmem += x[N-C+1];
330    }
331
332    /* Downsample by 2 again */
333    for (j=0;j<len>>2;j++)
334       x_lp4[j] = x_lp[2*j];
335    for (j=0;j<lag>>2;j++)
336       y_lp4[j] = y[2*j];
337
338 #ifdef FIXED_POINT
339    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
340    if (shift>0)
341    {
342       for (j=0;j<len>>2;j++)
343          x_lp4[j] = SHR16(x_lp4[j], shift);
344       for (j=0;j<lag>>2;j++)
345          y_lp4[j] = SHR16(y_lp4[j], shift);
346       /* Use double the shift for a MAC */
347       shift *= 2;
348    } else {
349       shift = 0;
350    }
351 #endif
352
353    /* Coarse search with 4x decimation */
354
355    for (i=0;i<max_pitch>>2;i++)
356    {
357       celt_word32 sum = 0;
358       for (j=0;j<len>>2;j++)
359          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
360       xcorr[i] = MAX32(-1, sum);
361       maxcorr = MAX32(maxcorr, sum);
362    }
363    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
364
365    /* Finer search with 2x decimation */
366    maxcorr=1;
367    for (i=0;i<max_pitch>>1;i++)
368    {
369       celt_word32 sum=0;
370       xcorr[i] = 0;
371       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
372          continue;
373       for (j=0;j<len>>1;j++)
374          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
375       xcorr[i] = MAX32(-1, sum);
376       maxcorr = MAX32(maxcorr, sum);
377    }
378    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
379
380    /* Refine by pseudo-interpolation */
381    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
382    {
383       celt_word32 a, b, c;
384       a = xcorr[best_pitch[0]-1];
385       b = xcorr[best_pitch[0]];
386       c = xcorr[best_pitch[0]+1];
387       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
388          offset = 1;
389       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
390          offset = -1;
391       else 
392          offset = 0;
393    } else {
394       offset = 0;
395    }
396    *pitch = 2*best_pitch[0]-offset;
397
398    CELT_COPY(y, y+(N>>1), (lag-N)>>1);
399    CELT_COPY(y+((lag-N)>>1), x_lp, N>>1);
400
401    /*printf ("%d\n", *pitch);*/
402 }