cwrs.c links to derf's article on pulse vector encoding.
[opus.git] / libcelt / vq.c
index 5ae7f54..7403077 100644 (file)
 #include "cwrs.h"
 #include "vq.h"
 
+/* Enable this or define your own implementation if you want to speed up the
+   VQ search (used in inner loop only) */
+#if 0
+#include <xmmintrin.h>
+static inline float approx_sqrt(float x)
+{
+   _mm_store_ss(&x, _mm_sqrt_ss(_mm_set_ss(x)));
+   return x;
+}
+static inline float approx_inv(float x)
+{
+   _mm_store_ss(&x, _mm_rcp_ss(_mm_set_ss(x)));
+   return x;
+}
+#else
+#define approx_sqrt(x) (sqrt(x))
+#define approx_inv(x) (1.f/(x))
+#endif
+
 struct NBest {
    float score;
    float gain;
@@ -59,7 +78,7 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
    int _iny[L][N];
    float *(ny[L]), *(y[L]);
    int *(iny[L]), *(iy[L]);
-   int i, j, m;
+   int i, j, k, m;
    int pulsesLeft;
    float xy[L];
    float yy[L];
@@ -81,32 +100,26 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
    }
    
    for (j=0;j<N;j++)
+   {
       Rpp += p[j]*p[j];
-   //if (Rpp>.01)
-   //   alpha = (1-sqrt(1-Rpp))/Rpp;
-   for (j=0;j<N;j++)
       Rxp += x[j]*p[j];
-   for (m=0;m<L;m++)
-      for (i=0;i<N;i++)
-         y[m][i] = 0;
-      
-   for (m=0;m<L;m++)
-      for (i=0;i<N;i++)
-         ny[m][i] = 0;
-
-   for (m=0;m<L;m++)
-      for (i=0;i<N;i++)
-         iy[m][i] = iny[m][i] = 0;
-
-   for (m=0;m<L;m++)
-      xy[m] = yy[m] = yp[m] = 0;
+   }
    
+   /* We only need to initialise the zero because the first iteration only uses that */
+   for (i=0;i<N;i++)
+      y[0][i] = 0;
+   for (i=0;i<N;i++)
+      iy[0][i] = 0;
+   xy[0] = yy[0] = yp[0] = 0;
+
    pulsesLeft = K;
    while (pulsesLeft > 0)
    {
       int pulsesAtOnce=1;
       int Lupdate = L;
       int L2 = L;
+      
+      /* Decide on complexity strategy */
       pulsesAtOnce = pulsesLeft/N;
       if (pulsesAtOnce<1)
          pulsesAtOnce = 1;
@@ -120,21 +133,19 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
          maxL *= N;
       }
 
-      for (m=0;m<L;m++)
-         nbest[m]->score = -1e10;
+      for (m=0;m<Lupdate;m++)
+         nbest[m]->score = -1e10f;
 
       for (m=0;m<L2;m++)
       {
          for (j=0;j<N;j++)
          {
             int sign;
-            //if (x[j]>0) sign=1; else sign=-1;
+            /*if (x[j]>0) sign=1; else sign=-1;*/
             for (sign=-1;sign<=1;sign+=2)
             {
-               /* All pulses at one location must have the same sign. Also,
-                  only consider sign in the same direction as x[j], except for the
-                  last pulses */
-               if (iy[m][j]*sign < 0 || (x[j]*sign<0 && pulsesLeft>((K+1)>>1)))
+               /* All pulses at one location must have the same sign. */
+               if (iy[m][j]*sign < 0)
                   continue;
                //fprintf (stderr, "%d/%d %d/%d %d/%d\n", i, K, m, L2, j, N);
                float tmp_xy, tmp_yy, tmp_yp;
@@ -142,12 +153,16 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
                float g;
                float s = sign*pulsesAtOnce;
                
-               /* Updating the sums the the new pulse(s) */
+               /* Updating the sums of the new pulse(s) */
                tmp_xy = xy[m] + s*x[j]               - alpha*s*p[j]*Rxp;
-               tmp_yy = yy[m] + 2*s*y[m][j] + s*s      +s*s*alpha*alpha*p[j]*p[j]*Rpp - 2*alpha*s*p[j]*yp[m] - 2*s*s*alpha*p[j]*p[j];
-               tmp_yp = yp[m] + s*p[j]               *(1-alpha*Rpp);
-               g = (sqrt(tmp_yp*tmp_yp + tmp_yy - tmp_yy*Rpp) - tmp_yp)/tmp_yy;
-               score = 2*g*tmp_xy - g*g*tmp_yy;
+               tmp_yy = yy[m] + 2.f*s*y[m][j] + s*s      +s*s*alpha*alpha*p[j]*p[j]*Rpp - 2.f*alpha*s*p[j]*yp[m] - 2.f*s*s*alpha*p[j]*p[j];
+               tmp_yp = yp[m] + s*p[j]               *(1.f-alpha*Rpp);
+               
+               /* Compute the gain such that ||p + g*y|| = 1 */
+               g = (approx_sqrt(tmp_yp*tmp_yp + tmp_yy - tmp_yy*Rpp) - tmp_yp)*approx_inv(tmp_yy);
+               /* Knowing that gain, what the error: (x-g*y)^2 
+                  (result is negated and we discard x^2 because it's constant) */
+               score = 2.f*g*tmp_xy - g*g*tmp_yy;
 
                if (score>nbest[Lupdate-1]->score)
                {
@@ -177,7 +192,7 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
          }
 
       }
-      int k;
+      /* Only now that we've made the final choice, update ny/iny and others */
       for (k=0;k<Lupdate;k++)
       {
          int n;
@@ -188,16 +203,16 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
          for (n=0;n<N;n++)
             ny[k][n] = y[nbest[k]->orig][n] - alpha*s*p[nbest[k]->pos]*p[n];
          ny[k][nbest[k]->pos] += s;
-      
+
          for (n=0;n<N;n++)
             iny[k][n] = iy[nbest[k]->orig][n];
          iny[k][nbest[k]->pos] += is;
-         
+
          xy[k] = nbest[k]->xy;
          yy[k] = nbest[k]->yy;
          yp[k] = nbest[k]->yp;
       }
-         
+      /* Swap ny/iny with y/iy */
       for (k=0;k<Lupdate;k++)
       {
          float *tmp_ny;
@@ -222,6 +237,7 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
    }
    for (i=0;i<N;i++)
       x[i] = p[i]+nbest[0]->gain*y[0][i];
+   /* Sanity checks, don't bother */
    if (0) {
       float E=1e-15;
       int ABS = 0;
@@ -239,17 +255,13 @@ void alg_quant(float *x, float *W, int N, int K, float *p, float alpha, ec_enc *
    
    encode_pulses(iy[0], N, K, enc);
    
-   //printf ("%llu ", icwrs64(N, K, comb, signs));
    /* Recompute the gain in one pass to reduce the encoder-decoder mismatch
       due to the recursive computation used in quantisation.
       Not quite sure whether we need that or not */
    if (1) {
       float Ryp=0;
-      float Rpp=0;
       float Ryy=0;
       float g=0;
-      for (i=0;i<N;i++)
-         Rpp += p[i]*p[i];
       
       for (i=0;i<N;i++)
          Ryp += iy[0][i]*p[i];