Speed improvements (got rid of a couple divide ops), cleanup...
[speexdsp.git] / libspeex / cb_search.c
index 05d28af..8584175 100644 (file)
@@ -1,3 +1,4 @@
+/* Original copyright */
 /*-----------------------------------------------------------------------*\
 
     FILE........: GAINSHAPE.C
@@ -10,6 +11,7 @@
 
 \*-----------------------------------------------------------------------*/
 
+
 /* Modified by Jean-Marc Valin 2002
 
    This library is free software; you can redistribute it and/or
@@ -49,6 +51,7 @@ static float scal_gains4[16] = {
    2.42801
 };
 
+
 /*---------------------------------------------------------------------------*\
                                                                              
  void overlap_cb_search()                                                            
@@ -152,9 +155,9 @@ FrameBits *bits,
 float *stack
 )
 {
-   int i,j;
-   float *resp, *E, *Ee;
-   float *t, *r, *e, *tresp;
+   int i,j, id;
+   float *resp, *E, q;
+   float *t, *r, *e;
    float *gains;
    int *ind;
    float *shape_cb;
@@ -168,92 +171,96 @@ float *stack
    shape_cb_size = 1<<params->shape_bits;
    shape_cb = params->shape_cb;
    resp = PUSH(stack, shape_cb_size*subvect_size);
-   tresp = PUSH(stack, shape_cb_size*nsf);
    E = PUSH(stack, shape_cb_size);
-   Ee = PUSH(stack, shape_cb_size);
    t = PUSH(stack, nsf);
    r = PUSH(stack, nsf);
    e = PUSH(stack, nsf);
    gains = PUSH(stack, nb_subvect);
    ind = (int*)PUSH(stack, nb_subvect);
 
+   /* Compute energy of the "real excitation" */
    syn_filt_zero(target, awk1, e, nsf, p);
    residue_zero(e, ak, e, nsf, p);
    residue_zero(e, awk2, e, nsf, p);
    for (i=0;i<nsf;i++)
       exc_energy += e[i]*e[i];
-   exc_energy=sqrt(.125*exc_energy);
+   exc_energy=sqrt(exc_energy/nb_subvect);
+
+   /* Quantize global ("average") gain */
+   q=log(exc_energy+.1);
+   q=floor(.5+2*(q-2));
+   if (q<0)
+      q=0;
+   if (q>15)
+      q=15;
+   id = (int)q;
+   frame_bits_pack(bits, id, 4);
+   exc_energy=exp(.5*q+2);
 
-   /* Quantize global (average) gain */
-   {
-      float q;
-      int id;
-      q=log(exc_energy+.1);
-      q=floor(.5+2*(q-2));
-      if (q<0)
-         q=0;
-      if (q>15)
-         q=15;
-      id = (int)q;
-      frame_bits_pack(bits, id, 4);
-      exc_energy=exp(.5*q+2);
-   }
 
    for (i=0;i<nsf;i++)
       t[i]=target[i];
+
+   e[0]=1;
+   for (i=1;i<nsf;i++)
+      e[i]=0;
+   residue_zero(e, awk1, r, nsf, p);
+   syn_filt_zero(r, ak, r, nsf, p);
+   syn_filt_zero(r, awk2, r, nsf,p);
+   
+   /* Pre-compute codewords response and energy */
    for (i=0;i<shape_cb_size;i++)
    {
       float *res = resp+i*subvect_size;
-      residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
-      syn_filt_zero(res, ak, res, subvect_size, p);
-      syn_filt_zero(res, awk2, res, subvect_size,p);
+
+      /* Compute codeword response */
+      int k;
+      for(j=0;j<subvect_size;j++)
+         res[j]=0;
+      for(j=0;j<subvect_size;j++)
+      {
+         for (k=j;k<subvect_size;k++)
+            res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
+      }
+      /* Compute energy of codeword response */
       E[i]=0;
       for(j=0;j<subvect_size;j++)
          E[i]+=res[j]*res[j];
-      Ee[i]=0;
-      for(j=0;j<subvect_size;j++)
-         Ee[i]+=shape_cb[i*subvect_size+j]*shape_cb[i*subvect_size+j];
-      
+      E[i]=1/(.001+E[i]);
    }
 
    for (i=0;i<nb_subvect;i++)
    {
-      int best_index=0;
+      int best_index=0, k, m;
       float g, corr, best_gain=0, score, best_score=-1;
+      /* Find best codeword for current sub-vector */
       for (j=0;j<shape_cb_size;j++)
       {
          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
-         score=corr*corr/(.001+E[j]);
-         g = corr/(.001+E[j]);
+         score=corr*corr*E[j];
+         g = corr*E[j];
          if (score>best_score)
          {
             best_index=j;
             best_score=score;
-            best_gain=corr/(.001+E[j]);
+            best_gain=g;
          }
       }
       frame_bits_pack(bits,best_index,params->shape_bits);
+      
+      /* Quantize gain */
       {
-         int s=0, best_id, j;
-         float best_dist;
+         int s=0, best_id;
          best_gain /= .01+exc_energy;
          if (best_gain<0)
          {
             best_gain=-best_gain;
             s=1;
          }
-         best_dist=(best_gain-scal_gains4[0])*(best_gain-scal_gains4[0]);
-         best_id=0;
-         for (j=1;j<8;j++)
-         {
-            float dist;
-            dist=(best_gain-scal_gains4[j])*(best_gain-scal_gains4[j]);
-            if (dist<best_dist)
-            {
-               best_id=j;
-               best_dist=dist;
-            }
-         }
+
+         /* Find gain index (it's a scalar but we use the VQ code anyway)*/
+         best_id = vq_index(&best_gain, scal_gains4, 1, 8);
+
          best_gain=scal_gains4[best_id];
          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
          if (s)
@@ -264,26 +271,25 @@ float *stack
       }
       ind[i]=best_index;
       gains[i]=best_gain;
-
-      for (j=0;j<nsf;j++)
-         e[j]=0;
+      /* Update target for next subvector */
       for (j=0;j<subvect_size;j++)
-         e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
-      residue_zero(e, awk1, r, nsf, p);
-      syn_filt_zero(r, ak, r, nsf, p);
-      syn_filt_zero(r, awk2, r, nsf,p);
-      for (j=0;j<nsf;j++)
-         tresp[i*nsf+j]=r[j];
-      for (j=0;j<nsf;j++)
-         t[j]-=r[j];
+      {
+         g=best_gain*shape_cb[best_index*subvect_size+j];
+         for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
+            t[k] -= g*r[m];
+      }
    }
    
+   /* Put everything back together */
    for (i=0;i<nb_subvect;i++)
       for (j=0;j<subvect_size;j++)
          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
 
+   /* Update excitation */
    for (j=0;j<nsf;j++)
       exc[j]+=e[j];
+   
+   /* Update target */
    residue_zero(e, awk1, r, nsf, p);
    syn_filt_zero(r, ak, r, nsf, p);
    syn_filt_zero(r, awk2, r, nsf,p);
@@ -299,8 +305,6 @@ float *stack
    POP(stack);
    POP(stack);
    POP(stack);
-   POP(stack);
-   POP(stack);
 }
 
 
@@ -339,6 +343,7 @@ float *stack
       exc_energy=exp(.5*id+2);
    }
 
+   /* Decode codewords and gains */
    for (i=0;i<nb_subvect;i++)
    {
       int gain_id;
@@ -355,13 +360,12 @@ float *stack
 
 
    }
+
+   /* Compute decoded excitation */
    for (i=0;i<nb_subvect;i++)
       for (j=0;j<subvect_size;j++)
          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
-   printf ("decode_exc:");
-   for (i=0;i<nsf;i++)
-      printf (" %f", exc[i]);
-   printf ("\n");
+
    POP(stack);
    POP(stack);
    POP(stack);