fixed-point: integerized lsp weight computation
[speexdsp.git] / libspeex / quant_lsp.c
index 9c994cb..a27a8ab 100644 (file)
@@ -2,42 +2,98 @@
    File: quant_lsp.c
    LSP vector quantization
 
-   This library is free software; you can redistribute it and/or
-   modify it under the terms of the GNU Lesser General Public
-   License as published by the Free Software Foundation; either
-   version 2.1 of the License, or (at your option) any later version.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions
+   are met:
    
-   This library is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-   Lesser General Public License for more details.
+   - Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
    
-   You should have received a copy of the GNU Lesser General Public
-   License along with this library; if not, write to the Free Software
-   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+   - Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+   
+   - Neither the name of the Xiph.org Foundation nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+   
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
+   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "quant_lsp.h"
 #include <math.h>
-#include <stdio.h>
 
-static float quant_weight[MAX_LSP_SIZE];
+#include "misc.h"
+
+/* FIXME: Get rid of this kludge quick before someone gets hurt */
+
+#ifdef FIXED_POINT
+#define LSP_LINEAR(i) (SHL(i+1,11))
+#define LSP_DIV_256(x) (SHL((spx_word16_t)x, 5))
+#define LSP_DIV_512(x) (SHL((spx_word16_t)x, 4))
+#define LSP_DIV_1024(x) (SHL((spx_word16_t)x, 3))
+#define LSP_PI 25736
+#else
+
+#define LSP_LINEAR(i) (.25*(i)+.25)
+#define LSP_SCALE 256.
+#define LSP_DIV_256(x) (0.0039062*(x))
+#define LSP_DIV_512(x) (0.0019531*(x))
+#define LSP_DIV_1024(x) (0.00097656*(x))
+#define LSP_PI M_PI
+#endif
+
+static void compute_quant_weights(spx_lsp_t *qlsp, spx_word16_t *quant_weight, int order)
+{
+   int i;
+   float tmp1, tmp2;
+   for (i=0;i<order;i++)
+   {
+      if (i==0)
+         tmp1 = qlsp[i];
+      else
+         tmp1 = qlsp[i]-qlsp[i-1];
+      if (i==order-1)
+         tmp2 = LSP_PI-qlsp[i];
+      else
+         tmp2 = qlsp[i+1]-qlsp[i];
+      if (tmp2<tmp1)
+         tmp1 = tmp2;
+#ifdef FIXED_POINT
+      quant_weight[i] = DIV32_16(81920,ADD16(300,tmp1));
+#else
+      quant_weight[i] = 10/(.04+tmp1);
+#endif
+   }
+
+}
 
 /* Note: x is modified*/
-static int lsp_quant(float *x, float *cdbk, int nbVec, int nbDim)
+static int lsp_quant(spx_word16_t *x, signed char *cdbk, int nbVec, int nbDim)
 {
    int i,j;
-   float dist, tmp;
-   float best_dist=0;
+   spx_word32_t dist;
+   spx_word16_t tmp;
+   spx_word32_t best_dist=0;
    int best_id=0;
-   float *ptr=cdbk;
+   signed char *ptr=cdbk;
    for (i=0;i<nbVec;i++)
    {
       dist=0;
       for (j=0;j<nbDim;j++)
       {
-         tmp=(x[j]-*ptr++);
-         dist+=tmp*tmp;
+         tmp=SUB16(x[j],SHL((spx_word16_t)*ptr++,5));
+         dist+=MULT16_16(tmp,tmp);
       }
       if (dist<best_dist || i==0)
       {
@@ -47,26 +103,27 @@ static int lsp_quant(float *x, float *cdbk, int nbVec, int nbDim)
    }
 
    for (j=0;j<nbDim;j++)
-      x[j] -= cdbk[best_id*nbDim+j];
+      x[j] -= SHL((spx_word16_t)cdbk[best_id*nbDim+j],5);
     
    return best_id;
 }
 
 /* Note: x is modified*/
-static int lsp_weight_quant(float *x, float *weight, float *cdbk, int nbVec, int nbDim)
+static int lsp_weight_quant(spx_word16_t *x, spx_word16_t *weight, signed char *cdbk, int nbVec, int nbDim)
 {
    int i,j;
-   float dist, tmp;
-   float best_dist=0;
+   spx_word32_t dist;
+   spx_word16_t tmp;
+   spx_word32_t best_dist=0;
    int best_id=0;
-   float *ptr=cdbk;
+   signed char *ptr=cdbk;
    for (i=0;i<nbVec;i++)
    {
       dist=0;
       for (j=0;j<nbDim;j++)
       {
-         tmp=(x[j]-*ptr++);
-         dist+=weight[j]*tmp*tmp;
+         tmp=SUB16(x[j],SHL((spx_word16_t)*ptr++,5));
+         dist+=MULT16_32_Q15(weight[j],MULT16_16(tmp,tmp));
       }
       if (dist<best_dist || i==0)
       {
@@ -76,210 +133,287 @@ static int lsp_weight_quant(float *x, float *weight, float *cdbk, int nbVec, int
    }
    
    for (j=0;j<nbDim;j++)
-      x[j] -= cdbk[best_id*nbDim+j];
+      x[j] -= SHL((spx_word16_t)cdbk[best_id*nbDim+j],5);
    return best_id;
 }
 
 
-void lsp_quant_nb(float *lsp, float *qlsp, int order, FrameBits *bits)
+void lsp_quant_nb(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 {
    int i;
-   float tmp1, tmp2;
    int id;
-
+   /* FIXME: get rid of that static allocation */
+   spx_word16_t quant_weight[10];
+   
    for (i=0;i<order;i++)
       qlsp[i]=lsp[i];
 
-   quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
-   quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
-   for (i=1;i<order-1;i++)
-   {
-      tmp1 = 1/(qlsp[i]-qlsp[i-1]);
-      tmp2 = 1/(qlsp[i+1]-qlsp[i]);
-      quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
-   }
+   compute_quant_weights(qlsp, quant_weight, order);
+
+   for (i=0;i<order;i++)
+      qlsp[i]-=LSP_LINEAR(i);
+
+#ifndef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i] = LSP_SCALE*qlsp[i];
+#endif
    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
-   frame_bits_pack(bits, id, 6);
+   speex_bits_pack(bits, id, 6);
 
+   for (i=0;i<order;i++)
+      qlsp[i]*=2;
    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
-   frame_bits_pack(bits, id, 6);
+   speex_bits_pack(bits, id, 6);
+
+   for (i=0;i<5;i++)
+      qlsp[i]*=2;
 
    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
-   frame_bits_pack(bits, id, 6);
+   speex_bits_pack(bits, id, 6);
 
    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
-   frame_bits_pack(bits, id, 6);
+   speex_bits_pack(bits, id, 6);
+
+   for (i=5;i<10;i++)
+      qlsp[i]*=2;
 
    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
-   frame_bits_pack(bits, id, 6);
+   speex_bits_pack(bits, id, 6);
+
+#ifdef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i]=PSHR(qlsp[i],2);
+#else
+   for (i=0;i<order;i++)
+      qlsp[i]=qlsp[i] * .00097656;
+#endif
 
    for (i=0;i<order;i++)
       qlsp[i]=lsp[i]-qlsp[i];
 }
 
-void lsp_unquant_nb(float *lsp, int order, FrameBits *bits)
+void lsp_unquant_nb(spx_lsp_t *lsp, int order, SpeexBits *bits)
 {
    int i, id;
    for (i=0;i<order;i++)
-      lsp[i]=0;
+      lsp[i]=LSP_LINEAR(i);
 
 
-   id=frame_bits_unpack_unsigned(bits, 6);
+   id=speex_bits_unpack_unsigned(bits, 6);
    for (i=0;i<10;i++)
-      lsp[i] += cdbk_nb[id*10+i];
+      lsp[i] += LSP_DIV_256(cdbk_nb[id*10+i]);
 
-   id=frame_bits_unpack_unsigned(bits, 6);
+   id=speex_bits_unpack_unsigned(bits, 6);
    for (i=0;i<5;i++)
-      lsp[i] += cdbk_nb_low1[id*5+i];
+      lsp[i] += LSP_DIV_512(cdbk_nb_low1[id*5+i]);
 
-   id=frame_bits_unpack_unsigned(bits, 6);
+   id=speex_bits_unpack_unsigned(bits, 6);
    for (i=0;i<5;i++)
-      lsp[i] += cdbk_nb_low2[id*5+i];
+      lsp[i] += LSP_DIV_1024(cdbk_nb_low2[id*5+i]);
 
-   id=frame_bits_unpack_unsigned(bits, 6);
+   id=speex_bits_unpack_unsigned(bits, 6);
    for (i=0;i<5;i++)
-      lsp[i+5] += cdbk_nb_high1[id*5+i];
+      lsp[i+5] += LSP_DIV_512(cdbk_nb_high1[id*5+i]);
    
-   id=frame_bits_unpack_unsigned(bits, 6);
+   id=speex_bits_unpack_unsigned(bits, 6);
    for (i=0;i<5;i++)
-      lsp[i+5] += cdbk_nb_high2[id*5+i];
+      lsp[i+5] += LSP_DIV_1024(cdbk_nb_high2[id*5+i]);
 }
 
 
-extern float lsp_cdbk_wb[];
-extern float lsp_cdbk_wb11[];
-extern float lsp_cdbk_wb12[];
-extern float lsp_cdbk_wb21[];
-extern float lsp_cdbk_wb22[];
-extern float lsp_cdbk_wb31[];
-extern float lsp_cdbk_wb32[];
-extern float lsp_cdbk_wb41[];
-extern float lsp_cdbk_wb42[];
-
-void lsp_quant_wb(float *lsp, float *qlsp, int order, FrameBits *bits)
+void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 {
    int i;
-   float tmp1, tmp2;
    int id;
+   spx_word16_t quant_weight[10];
+
    for (i=0;i<order;i++)
       qlsp[i]=lsp[i];
 
-   quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
-   quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
-   for (i=1;i<order-1;i++)
-   {
-      tmp1 = 1/(qlsp[i]-qlsp[i-1]);
-      tmp2 = 1/(qlsp[i+1]-qlsp[i]);
-      quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
-   }
-   id = lsp_quant(qlsp, lsp_cdbk_wb, 64, order);
-   frame_bits_pack(bits, id, 6);
+   compute_quant_weights(qlsp, quant_weight, order);
 
-   id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb11, 64, 4);
-   frame_bits_pack(bits, id, 6);
+   for (i=0;i<order;i++)
+      qlsp[i]-=LSP_LINEAR(i);
+#ifndef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i]=qlsp[i]*LSP_SCALE;
+#endif
+   id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
+   speex_bits_pack(bits, id, 6);
+   
+   for (i=0;i<order;i++)
+      qlsp[i]*=2;
+   
+   id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
+   speex_bits_pack(bits, id, 6);
 
-   id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb12, 64, 4);
-   frame_bits_pack(bits, id, 6);
+   id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
+   speex_bits_pack(bits, id, 6);
 
-   id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb21, 64, 4);
-   frame_bits_pack(bits, id, 6);
+#ifdef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i] = PSHR(qlsp[i],1);
+#else
+   for (i=0;i<order;i++)
+      qlsp[i] = qlsp[i]*0.0019531;
+#endif
 
-   id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb22, 64, 4);
-   frame_bits_pack(bits, id, 6);
+   for (i=0;i<order;i++)
+      qlsp[i]=lsp[i]-qlsp[i];
+}
 
-   id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb31, 64, 4);
-   frame_bits_pack(bits, id, 6);
+void lsp_unquant_lbr(spx_lsp_t *lsp, int order, SpeexBits *bits)
+{
+   int i, id;
+   for (i=0;i<order;i++)
+      lsp[i]=LSP_LINEAR(i);
 
-   id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb32, 16, 4);
-   frame_bits_pack(bits, id, 4);
 
-   id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb41, 64, 4);
-   frame_bits_pack(bits, id, 6);
+   id=speex_bits_unpack_unsigned(bits, 6);
+   for (i=0;i<10;i++)
+      lsp[i] += LSP_DIV_256(cdbk_nb[id*10+i]);
 
-   id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb42, 16, 4);
-   frame_bits_pack(bits, id, 4);
+   id=speex_bits_unpack_unsigned(bits, 6);
+   for (i=0;i<5;i++)
+      lsp[i] += LSP_DIV_512(cdbk_nb_low1[id*5+i]);
 
-   for (i=0;i<order;i++)
-      qlsp[i]=lsp[i]-qlsp[i];
+   id=speex_bits_unpack_unsigned(bits, 6);
+   for (i=0;i<5;i++)
+      lsp[i+5] += LSP_DIV_512(cdbk_nb_high1[id*5+i]);
+   
 }
 
 
-void lsp_unquant_wb(float *lsp, int order, FrameBits *bits)
-{
+extern signed char high_lsp_cdbk[];
+extern signed char high_lsp_cdbk2[];
 
-   int i, id;
-   for (i=0;i<order;i++)
-      lsp[i]=0;
 
+void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
+{
+   int i;
+   int id;
+   spx_word16_t quant_weight[10];
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<16;i++)
-      lsp[i] += lsp_cdbk_wb[id*16+i];
+   for (i=0;i<order;i++)
+      qlsp[i]=lsp[i];
 
+   compute_quant_weights(qlsp, quant_weight, order);
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<4;i++)
-      lsp[i] += lsp_cdbk_wb11[id*4+i];
+   /*   quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
+   quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
+   for (i=1;i<order-1;i++)
+   {
+      tmp1 = 10/(qlsp[i]-qlsp[i-1]);
+      tmp2 = 10/(qlsp[i+1]-qlsp[i]);
+      quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
+      }*/
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<4;i++)
-      lsp[i] += lsp_cdbk_wb12[id*4+i];
+   for (i=0;i<order;i++)
+      qlsp[i]-=LSP_SCALING*(.3125*i+.75);
+#ifndef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i] = qlsp[i]*LSP_SCALE;
+#endif
+   id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
+   speex_bits_pack(bits, id, 6);
 
+   for (i=0;i<order;i++)
+      qlsp[i]*=2;
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<4;i++)
-      lsp[i+4] += lsp_cdbk_wb21[id*4+i];
+   id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
+   speex_bits_pack(bits, id, 6);
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<4;i++)
-      lsp[i+4] += lsp_cdbk_wb22[id*4+i];
+#ifdef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i] = PSHR(qlsp[i],1);
+#else
+   for (i=0;i<order;i++)
+      qlsp[i] = qlsp[i]*0.0019531;
+#endif
 
+   for (i=0;i<order;i++)
+      qlsp[i]=lsp[i]-qlsp[i];
+}
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<4;i++)
-      lsp[i+8] += lsp_cdbk_wb31[id*4+i];
+void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
+{
 
-   id=frame_bits_unpack_unsigned(bits, 4);
-   for (i=0;i<4;i++)
-      lsp[i+8] += lsp_cdbk_wb32[id*4+i];
+   int i, id;
+   for (i=0;i<order;i++)
+      lsp[i]=LSP_SCALING*(.3125*i+.75);
 
 
-   id=frame_bits_unpack_unsigned(bits, 6);
-   for (i=0;i<4;i++)
-      lsp[i+12] += lsp_cdbk_wb41[id*4+i];
+   id=speex_bits_unpack_unsigned(bits, 6);
+   for (i=0;i<order;i++)
+      lsp[i] += LSP_SCALING*0.0039062*high_lsp_cdbk[id*order+i];
 
-   id=frame_bits_unpack_unsigned(bits, 4);
-   for (i=0;i<4;i++)
-      lsp[i+12] += lsp_cdbk_wb42[id*4+i];
 
+   id=speex_bits_unpack_unsigned(bits, 6);
+   for (i=0;i<order;i++)
+      lsp[i] += LSP_SCALING*0.0019531*high_lsp_cdbk2[id*order+i];
 }
 
-extern float high_lsp_cdbk[];
-extern float high_lsp_cdbk2[];
 
+#ifdef EPIC_48K
+
+extern signed char cdbk_lsp_vlbr[5120];
+extern signed char cdbk_lsp2_vlbr[160];
 
-void lsp_quant_high(float *lsp, float *qlsp, int order, FrameBits *bits)
+void lsp_quant_48k(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 {
    int i;
-   float tmp1, tmp2;
    int id;
+   spx_word16_t quant_weight[10];
+
    for (i=0;i<order;i++)
       qlsp[i]=lsp[i];
 
-   quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
-   quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
-   for (i=1;i<order-1;i++)
-   {
-      tmp1 = 1/(qlsp[i]-qlsp[i-1]);
-      tmp2 = 1/(qlsp[i+1]-qlsp[i]);
-      quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
-   }
-   id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
-   frame_bits_pack(bits, id, 6);
+   compute_quant_weights(qlsp, quant_weight, order);
 
-   id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
-   frame_bits_pack(bits, id, 6);
+   for (i=0;i<order;i++)
+      qlsp[i]-=LSP_SCALING*(.25*i+.3125);
+#ifndef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i] = qlsp[i]*LSP_SCALE;
+#endif
+   
+   id = lsp_quant(qlsp, cdbk_lsp_vlbr, 512, order);
+   speex_bits_pack(bits, id, 9);
+
+   for (i=0;i<order;i++)
+      qlsp[i]*=4;
+   
+   id = lsp_weight_quant(qlsp, quant_weight, cdbk_lsp2_vlbr, 16, 10);
+   speex_bits_pack(bits, id, 4);
+
+#ifdef FIXED_POINT
+   for (i=0;i<order;i++)
+      qlsp[i]=PSHR(qlsp[i],2);
+#else
+   for (i=0;i<order;i++)
+      qlsp[i]=qlsp[i]*0.00097655;
+#endif
 
    for (i=0;i<order;i++)
       qlsp[i]=lsp[i]-qlsp[i];
 }
+
+void lsp_unquant_48k(spx_lsp_t *lsp, int order, SpeexBits *bits)
+{
+   int i, id;
+   for (i=0;i<order;i++)
+      lsp[i]=LSP_SCALING*(.25*i+.3125);
+
+
+   id=speex_bits_unpack_unsigned(bits, 9);
+   for (i=0;i<10;i++)
+      lsp[i] += LSP_SCALING*0.0039062*cdbk_lsp_vlbr[id*10+i];
+
+   id=speex_bits_unpack_unsigned(bits, 4);
+   for (i=0;i<10;i++)
+      lsp[i] += LSP_SCALING*0.00097655*cdbk_lsp2_vlbr[id*10+i];
+   
+}
+
+#endif