Some work towards being able to encode a 48 kHz stream from 32 kHz audio (incomplete)
[opus.git] / libcelt / pitch.c
index 7907e6f..278a4e8 100644 (file)
@@ -1,5 +1,6 @@
-/* (C) 2007-2008 Jean-Marc Valin, CSIRO
-*/
+/* Copyright (c) 2007-2008 CSIRO
+   Copyright (c) 2007-2009 Xiph.Org Foundation
+   Written by Jean-Marc Valin */
 /**
    @file pitch.c
    @brief Pitch analysis
 #include "config.h"
 #endif
 
-/*#include "_kiss_fft_guts.h"
-#include "kiss_fftr.h"*/
-#include "kfft_single.h"
-
 #include "pitch.h"
-#include "psy.h"
 #include "os_support.h"
-#include "mathops.h"
+#include "modes.h"
 #include "stack_alloc.h"
+#include "mathops.h"
 
-kiss_fftr_cfg pitch_state_alloc(int max_lag)
+void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y, int yshift, int len, int max_pitch, int best_pitch[2])
 {
-   return real16_fft_alloc(max_lag);
-}
+   int i, j;
+   celt_word32 Syy=1;
+   celt_word16 best_num[2];
+   celt_word32 best_den[2];
+#ifdef FIXED_POINT
+   int xshift;
 
-void pitch_state_free(kiss_fftr_cfg st)
-{
-   real16_fft_free(st);
+   xshift = celt_ilog2(maxcorr)-14;
+#endif
+
+   best_num[0] = -1;
+   best_num[1] = -1;
+   best_den[0] = 0;
+   best_den[1] = 0;
+   best_pitch[0] = 0;
+   best_pitch[1] = 1;
+   for (j=0;j<len;j++)
+      Syy = MAC16_16(Syy, y[j],y[j]);
+   for (i=0;i<max_pitch;i++)
+   {
+      float score;
+      if (xcorr[i]>0)
+      {
+         celt_word16 num;
+         celt_word32 xcorr16;
+         xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
+         num = MULT16_16_Q15(xcorr16,xcorr16);
+         score = num*1./Syy;
+         if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
+         {
+            if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
+            {
+               best_num[1] = best_num[0];
+               best_den[1] = best_den[0];
+               best_pitch[1] = best_pitch[0];
+               best_num[0] = num;
+               best_den[0] = Syy;
+               best_pitch[0] = i;
+            } else {
+               best_num[1] = num;
+               best_den[1] = Syy;
+               best_pitch[1] = i;
+            }
+         }
+      }
+      Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
+      Syy = MAX32(1, Syy);
+   }
 }
 
-#ifdef FIXED_POINT
-static void normalise16(celt_word16_t *x, int len, celt_word16_t val)
+void pitch_downsample(const celt_sig * restrict x, celt_word16 * restrict x_lp, int len, int end, int _C, celt_sig * restrict xmem, celt_word16 * restrict filt_mem)
 {
    int i;
-   celt_word16_t maxabs;
-   maxabs = celt_maxabs16(x,len);
-   if (maxabs > val)
+   const int C = CHANNELS(_C);
+   for (i=1;i<len>>1;i++)
+      x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
+   x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
+   *xmem = x[end-C];
+   if (C==2)
+   {
+      for (i=1;i<len>>1;i++)
+      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);
+      x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
+      *xmem += x[end-C+1];
+   }
+
+#if 0
    {
-      int shift = 0;
-      while (maxabs > val)
+      int j;
+      float ac[3]={0,0,0};
+      float ak[2];
+      float det;
+      celt_word16 mem[2];
+      for (i=0;i<3;i++)
       {
-         maxabs >>= 1;
-         shift++;
+         for (j=0;j<(len>>1)-i;j++)
+         {
+            ac[i] += x_lp[j]*x_lp[j+i];
+         }
       }
-      if (shift==0)
-         return;
-      i=0;
-      do{
-         x[i] = SHR16(x[i], shift);
-      } while (++i<len);
-   } else {
-      int shift=0;
-      if (maxabs == 0)
-         return;
-      val >>= 1;
-      while (maxabs < val)
+      det = 1./(.1+ac[0]*ac[0]-ac[1]*ac[1]);
+      ak[0] = .9*det*(ac[0]*ac[1] - ac[1]*ac[2]);
+      ak[1] = .81*det*(-ac[1]*ac[1] + ac[0]*ac[2]);
+      /*printf ("%f %f %f\n", 1., -ak[0], -ak[1]);*/
+      mem[0]=filt_mem[0];
+      mem[1]=filt_mem[1];
+      filt_mem[0]=x_lp[(end>>1)-1];
+      filt_mem[1]=x_lp[(end>>1)-2];
+      for (j=0;j<len>>1;j++)
       {
-         val >>= 1;
-         shift++;
+         float tmp = x_lp[j];
+         x_lp[j] = x_lp[j] - ak[0]*mem[0] - ak[1]*mem[1];
+         mem[1]=mem[0];
+         mem[0]=tmp;
       }
-      if (shift==0)
-         return;
-      i=0;
-      do{
-         x[i] = SHL16(x[i], shift);
-      } while (++i<len);
    }
-}
-#else
-#define normalise16(x,len,val)
 #endif
 
-#define INPUT_SHIFT 15
+}
 
-void find_spectral_pitch(kiss_fftr_cfg fft, const struct PsyDecay *decay, const celt_sig_t * restrict x, const celt_sig_t * restrict y, const celt_word16_t * restrict window, int overlap, int lag, int len, int C, int *pitch)
+void pitch_search(const CELTMode *m, const celt_word16 * restrict x_lp, celt_word16 * restrict y, int len, int max_pitch, int *pitch, celt_sig *xmem, int M)
 {
-   int c, i;
-   VARDECL(celt_word16_t, _X);
-   VARDECL(celt_word16_t, _Y);
-   VARDECL(celt_mask_t, curve);
-   celt_word16_t * restrict X, * restrict Y;
-   int n2;
-   int L2;
+   int i, j;
+   const int lag = MAX_PERIOD;
+   const int N = M*m->shortMdctSize;
+   int best_pitch[2]={0};
+   VARDECL(celt_word16, x_lp4);
+   VARDECL(celt_word16, y_lp4);
+   VARDECL(celt_word32, xcorr);
+   celt_word32 maxcorr=1;
+   int offset;
+   int shift=0;
+
    SAVE_STACK;
-   n2 = lag>>1;
-   L2 = len>>1;
-   ALLOC(_X, lag, celt_word16_t);
-   X = _X;
-   ALLOC(curve, n2, celt_mask_t);
-
-   CELT_MEMSET(X,0,lag);
-   /* Sum all channels of the current frame and copy into X in bit-reverse order */
-   for (c=0;c<C;c++)
+
+   ALLOC(x_lp4, len>>2, celt_word16);
+   ALLOC(y_lp4, lag>>2, celt_word16);
+   ALLOC(xcorr, max_pitch>>1, celt_word32);
+
+   /* Downsample by 2 again */
+   for (j=0;j<len>>2;j++)
+      x_lp4[j] = x_lp[2*j];
+   for (j=0;j<lag>>2;j++)
+      y_lp4[j] = y[2*j];
+
+#ifdef FIXED_POINT
+   shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
+   if (shift>0)
    {
-      for (i=0;i<L2;i++)
-      {
-         X[2*BITREV(fft,i)] += SHR32(x[C*(2*i)+c],INPUT_SHIFT);
-         X[2*BITREV(fft,i)+1] += SHR32(x[C*(2*i+1)+c],INPUT_SHIFT);
-      }
+      for (j=0;j<len>>2;j++)
+         x_lp4[j] = SHR16(x_lp4[j], shift);
+      for (j=0;j<lag>>2;j++)
+         y_lp4[j] = SHR16(y_lp4[j], shift);
+      /* Use double the shift for a MAC */
+      shift *= 2;
+   } else {
+      shift = 0;
    }
-   /* Applying the window in the bit-reverse domain. It's a bit weird, but it
-      can help save memory */
-   for (i=0;i<overlap>>1;i++)
+#endif
+
+   /* Coarse search with 4x decimation */
+
+   for (i=0;i<max_pitch>>2;i++)
    {
-      X[2*BITREV(fft,i)] = MULT16_16_Q15(window[2*i], X[2*BITREV(fft,i)]);
-      X[2*BITREV(fft,i)+1] = MULT16_16_Q15(window[2*i+1], X[2*BITREV(fft,i)+1]);
-      X[2*BITREV(fft,L2-i-1)] = MULT16_16_Q15(window[2*i+1], X[2*BITREV(fft,L2-i-1)]);
-      X[2*BITREV(fft,L2-i-1)+1] = MULT16_16_Q15(window[2*i], X[2*BITREV(fft,L2-i-1)+1]);
+      celt_word32 sum = 0;
+      for (j=0;j<len>>2;j++)
+         sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
+      xcorr[i] = MAX32(-1, sum);
+      maxcorr = MAX32(maxcorr, sum);
    }
-   normalise16(X, lag, 8192);
-   /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
-   /* Forward real FFT (in-place) */
-   real16_fft_inplace(fft, X, lag);
-
-   compute_masking(decay, X, curve, lag);
-
-   /* Deferred allocation to reduce peak stack usage */
-   ALLOC(_Y, lag, celt_word16_t);
-   Y = _Y;
-   CELT_MEMSET(Y,0,lag);
-   /* Sum all channels of the past audio and copy into Y in bit-reverse order */
-   for (c=0;c<C;c++)
+   find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
+
+   /* Finer search with 2x decimation */
+   maxcorr=1;
+   for (i=0;i<max_pitch>>1;i++)
    {
-      for (i=0;i<n2;i++)
-      {
-         Y[2*BITREV(fft,i)] += SHR32(y[C*(2*i)+c],INPUT_SHIFT);
-         Y[2*BITREV(fft,i)+1] += SHR32(y[C*(2*i+1)+c],INPUT_SHIFT);
-      }
+      celt_word32 sum=0;
+      xcorr[i] = 0;
+      if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
+         continue;
+      for (j=0;j<len>>1;j++)
+         sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
+      xcorr[i] = MAX32(-1, sum);
+      maxcorr = MAX32(maxcorr, sum);
    }
-   normalise16(Y, lag, 8192);
-   /* Forward real FFT (in-place) */
-   real16_fft_inplace(fft, Y, lag);
+   find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
 
-   /* Compute cross-spectrum using the inverse masking curve as weighting */
-   for (i=1;i<n2;i++)
+   /* Refine by pseudo-interpolation */
+   if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
    {
-      celt_word16_t Xr, Xi, n;
-      /* weight = 1/sqrt(curve) */
-      n = celt_rsqrt(EPSILON+curve[i]);
-      /* Pre-multiply X by n, so we can keep everything in 16 bits */
-      Xr = EXTRACT16(SHR32(MULT16_16(n, X[2*i  ]),3));
-      Xi = EXTRACT16(SHR32(MULT16_16(n, X[2*i+1]),3));
-      /* Cross-spectrum between X and conj(Y) */
-      X[2*i]   = ADD16(MULT16_16_Q15(Xr, Y[2*i  ]), MULT16_16_Q15(Xi,Y[2*i+1]));
-      X[2*i+1] = SUB16(MULT16_16_Q15(Xr, Y[2*i+1]), MULT16_16_Q15(Xi,Y[2*i  ]));
+      celt_word32 a, b, c;
+      a = xcorr[best_pitch[0]-1];
+      b = xcorr[best_pitch[0]];
+      c = xcorr[best_pitch[0]+1];
+      if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
+         offset = 1;
+      else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
+         offset = -1;
+      else 
+         offset = 0;
+   } else {
+      offset = 0;
    }
-   /*printf ("\n");*/
-   X[0] = X[1] = 0;
-   /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
-   normalise16(X, lag, 50);
-   /* Inverse half-complex to real FFT gives us the correlation */
-   real16_ifft(fft, X, Y, lag);
-   
-   /* The peak in the correlation gives us the pitch */
-   *pitch = find_max16(Y, lag-len);
+   *pitch = 2*best_pitch[0]-offset;
+
+   CELT_MOVE(y, y+(N>>1), (lag-N)>>1);
+   CELT_MOVE(y+((lag-N)>>1), x_lp, N>>1);
+
    RESTORE_STACK;
+
+   /*printf ("%d\n", *pitch);*/
 }