optimisation: Removed a bunch of conditional branches from comb2pulse()
[opus.git] / libcelt / pitch.c
index 68d3a24..1790c1b 100644 (file)
 #include "kiss_fftr.h"*/
 #include "kfft_single.h"
 
-#include <stdio.h>
-#include <math.h>
 #include "pitch.h"
 #include "psy.h"
 #include "os_support.h"
 #include "mathops.h"
+#include "stack_alloc.h"
 
 kiss_fftr_cfg pitch_state_alloc(int max_lag)
 {
-   return kiss_fftr_alloc_celt_single(max_lag, 0, 0);
+   return real16_fft_alloc(max_lag);
 }
 
 void pitch_state_free(kiss_fftr_cfg st)
 {
-   kiss_fft_free(st);
+   real16_fft_free(st);
 }
 
 #ifdef FIXED_POINT
 static void normalise16(celt_word16_t *x, int len, celt_word16_t val)
 {
    int i;
-   celt_word16_t maxval = 0;
-   for (i=0;i<len;i++)
-      maxval = MAX16(maxval, ABS16(x[i]));
-   if (maxval > val)
+   celt_word16_t maxabs;
+   maxabs = celt_maxabs16(x,len);
+   if (maxabs > val)
    {
       int shift = 0;
-      while (maxval > val)
+      while (maxabs > val)
       {
-         maxval >>= 1;
+         maxabs >>= 1;
          shift++;
       }
       if (shift==0)
          return;
-      for (i=0;i<len;i++)
+      i=0;
+      do{
          x[i] = SHR16(x[i], shift);
+      } while (++i<len);
    } else {
       int shift=0;
-      if (maxval == 0)
+      if (maxabs == 0)
          return;
       val >>= 1;
-      while (maxval < val)
+      while (maxabs < val)
       {
          val >>= 1;
          shift++;
       }
       if (shift==0)
          return;
-      for (i=0;i<len;i++)
+      i=0;
+      do{
          x[i] = SHL16(x[i], shift);
+      } while (++i<len);
    }
 }
 #else
@@ -101,99 +103,87 @@ static void normalise16(celt_word16_t *x, int len, celt_word16_t val)
 
 #define INPUT_SHIFT 15
 
-void find_spectral_pitch(kiss_fftr_cfg fft, const struct PsyDecay *decay, const celt_sig_t *x, const celt_sig_t *y, const celt_word16_t *window, int overlap, int lag, int len, int C, int *pitch)
+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)
 {
    int c, i;
-   celt_word32_t max_corr;
-   VARDECL(celt_word16_t, X);
-   VARDECL(celt_word16_t, Y);
+   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;
-   const int *bitrev;
    SAVE_STACK;
-   n2 = lag/2;
-   L2 = len/2;
-   ALLOC(X, lag, celt_word16_t);
+   n2 = lag>>1;
+   L2 = len>>1;
+   ALLOC(_X, lag, celt_word16_t);
+   X = _X;
    ALLOC(curve, n2, celt_mask_t);
 
-   bitrev = fft->substate->bitrev;
-   for (i=0;i<lag;i++)
-      X[i] = 0;
+   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++)
    {
       for (i=0;i<L2;i++)
       {
-         X[2*bitrev[i]] += SHR32(x[C*(2*i)+c],INPUT_SHIFT);
-         X[2*bitrev[i]+1] += SHR32(x[C*(2*i+1)+c],INPUT_SHIFT);
+         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);
       }
    }
    /* Applying the window in the bit-reverse domain. It's a bit weird, but it
       can help save memory */
-   for (i=0;i<overlap/2;i++)
+   for (i=0;i<overlap>>1;i++)
    {
-      X[2*bitrev[i]] = MULT16_32_Q15(window[2*i], X[2*bitrev[i]]);
-      X[2*bitrev[i]+1] = MULT16_32_Q15(window[2*i+1], X[2*bitrev[i]+1]);
-      X[2*bitrev[L2-i-1]] = MULT16_32_Q15(window[2*i+1], X[2*bitrev[L2-i-1]]);
-      X[2*bitrev[L2-i-1]+1] = MULT16_32_Q15(window[2*i], X[2*bitrev[L2-i-1]+1]);
+      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]);
    }
    normalise16(X, lag, 8192);
    /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
    /* Forward real FFT (in-place) */
-   kf_work((kiss_fft_cpx*)X, NULL, 1,1, fft->substate->factors,fft->substate, 1, 1, 1);
-   kiss_fftr_twiddles(fft,X);
+   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);
-   for (i=0;i<lag;i++)
-      Y[i] = 0;
+   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++)
    {
       for (i=0;i<n2;i++)
       {
-         Y[2*bitrev[i]] += SHR32(y[C*(2*i)+c],INPUT_SHIFT);
-         Y[2*bitrev[i]+1] += SHR32(y[C*(2*i+1)+c],INPUT_SHIFT);
+         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);
       }
    }
    normalise16(Y, lag, 8192);
    /* Forward real FFT (in-place) */
-   kf_work((kiss_fft_cpx*)Y, NULL, 1,1, fft->substate->factors,fft->substate, 1, 1, 1);
-   kiss_fftr_twiddles(fft,Y);
+   real16_fft_inplace(fft, Y, lag);
 
    /* Compute cross-spectrum using the inverse masking curve as weighting */
    for (i=1;i<n2;i++)
    {
-      celt_word16_t n;
-      celt_word32_t tmp;
-      /*printf ("%d %d ", X[2*i]*X[2*i]+X[2*i+1]*X[2*i+1], Y[2*i]*Y[2*i]+Y[2*i+1]*Y[2*i+1]);*/
-      n = DIV32_16(Q15ONE,celt_sqrt(EPSILON+curve[i]));
-      /*printf ("%f ", n);*/
-      tmp = X[2*i];
-      X[2*i] = MULT16_32_Q15(n, ADD32(MULT16_16(X[2*i  ],Y[2*i  ]), MULT16_16(X[2*i+1],Y[2*i+1])));
-      X[2*i+1] = MULT16_32_Q15(n, SUB32(MULT16_16(tmp,Y[2*i+1]), MULT16_16(X[2*i+1],Y[2*i  ])));
+      celt_word16_t Xr, Xi, n;
+      /* weight = 1/sqrt(curve) */
+      n = celt_rsqrt(EPSILON+curve[i]);
+      /*n = SHR32(32767,(celt_ilog2(EPSILON+curve[i])>>1));*/
+      /* 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  ]));
    }
    /*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 */
-   kiss_fftri(fft, X, Y);
+   real16_ifft(fft, X, Y, lag);
    
    /* The peak in the correlation gives us the pitch */
-   max_corr=-VERY_LARGE32;
-   *pitch = 0;
-   for (i=0;i<lag-len;i++)
-   {
-      /*printf ("%f ", xx[i]);*/
-      if (Y[i] > max_corr)
-      {
-         *pitch = i;
-         max_corr = Y[i];
-      }
-   }
+   *pitch = find_max16(Y, lag-len);
    RESTORE_STACK;
 }