Add speex to trunk/deps/third_party/....

speex/libspeex/lsp.c

+/*---------------------------------------------------------------------------*\
+Original copyright
+        FILE........: lsp.c
+        AUTHOR......: David Rowe
+        DATE CREATED: 24/2/93
+
+Heavily modified by Jean-Marc Valin (c) 2002-2006 (fixed-point, 
+                       optimizations, additional functions, ...)
+
+   This file contains functions for converting Linear Prediction
+   Coefficients (LPC) to Line Spectral Pair (LSP) and back. Note that the
+   LSP coefficients are not in radians format but in the x domain of the
+   unit circle.
+
+   Speex License:
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions
+   are met:
+   
+   - Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+   
+   - 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.
+*/
+
+/*---------------------------------------------------------------------------*\
+
+  Introduction to Line Spectrum Pairs (LSPs)
+  ------------------------------------------
+
+  LSPs are used to encode the LPC filter coefficients {ak} for
+  transmission over the channel.  LSPs have several properties (like
+  less sensitivity to quantisation noise) that make them superior to
+  direct quantisation of {ak}.
+
+  A(z) is a polynomial of order lpcrdr with {ak} as the coefficients.
+
+  A(z) is transformed to P(z) and Q(z) (using a substitution and some
+  algebra), to obtain something like:
+
+    A(z) = 0.5[P(z)(z+z^-1) + Q(z)(z-z^-1)]  (1)
+
+  As you can imagine A(z) has complex zeros all over the z-plane. P(z)
+  and Q(z) have the very neat property of only having zeros _on_ the
+  unit circle.  So to find them we take a test point z=exp(jw) and
+  evaluate P (exp(jw)) and Q(exp(jw)) using a grid of points between 0
+  and pi.
+
+  The zeros (roots) of P(z) also happen to alternate, which is why we
+  swap coefficients as we find roots.  So the process of finding the
+  LSP frequencies is basically finding the roots of 5th order
+  polynomials.
+
+  The root so P(z) and Q(z) occur in symmetrical pairs at +/-w, hence
+  the name Line Spectrum Pairs (LSPs).
+
+  To convert back to ak we just evaluate (1), "clocking" an impulse
+  thru it lpcrdr times gives us the impulse response of A(z) which is
+  {ak}.
+
+\*---------------------------------------------------------------------------*/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <math.h>
+#include "lsp.h"
+#include "stack_alloc.h"
+#include "math_approx.h"
+
+#ifndef M_PI
+#define M_PI           3.14159265358979323846  /* pi */
+#endif
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#ifdef FIXED_POINT
+
+#define FREQ_SCALE 16384
+
+/*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/
+#define ANGLE2X(a) (SHL16(spx_cos(a),2))
+
+/*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/
+#define X2ANGLE(x) (spx_acos(x))
+
+#ifdef BFIN_ASM
+#include "lsp_bfin.h"
+#endif
+
+#else
+
+/*#define C1 0.99940307
+#define C2 -0.49558072
+#define C3 0.03679168*/
+
+#define FREQ_SCALE 1.
+#define ANGLE2X(a) (spx_cos(a))
+#define X2ANGLE(x) (acos(x))
+
+#endif
+
+
+/*---------------------------------------------------------------------------*\
+
+   FUNCTION....: cheb_poly_eva()
+
+   AUTHOR......: David Rowe
+   DATE CREATED: 24/2/93
+
+   This function evaluates a series of Chebyshev polynomials
+
+\*---------------------------------------------------------------------------*/
+
+#ifdef FIXED_POINT
+
+#ifndef OVERRIDE_CHEB_POLY_EVA
+static inline spx_word32_t cheb_poly_eva(
+  spx_word16_t *coef, /* P or Q coefs in Q13 format               */
+  spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */
+  int              m, /* LPC order/2                              */
+  char         *stack
+)
+{
+    int i;
+    spx_word16_t b0, b1;
+    spx_word32_t sum;
+
+    /*Prevents overflows*/
+    if (x>16383)
+       x = 16383;
+    if (x<-16383)
+       x = -16383;
+
+    /* Initialise values */
+    b1=16384;
+    b0=x;
+
+    /* Evaluate Chebyshev series formulation usin g iterative approach  */
+    sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));
+    for(i=2;i<=m;i++)
+    {
+       spx_word16_t tmp=b0;
+       b0 = SUB16(MULT16_16_Q13(x,b0), b1);
+       b1 = tmp;
+       sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));
+    }
+    
+    return sum;
+}
+#endif
+
+#else
+
+static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack)
+{
+   int k;
+   float b0, b1, tmp;
+
+   /* Initial conditions */
+   b0=0; /* b_(m+1) */
+   b1=0; /* b_(m+2) */
+
+   x*=2;
+
+   /* Calculate the b_(k) */
+   for(k=m;k>0;k--)
+   {
+      tmp=b0;                           /* tmp holds the previous value of b0 */
+      b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */
+      b1=tmp;                           /* b1 holds the previous value of b0 */
+   }
+
+   return(-b1+.5*x*b0+coef[m]);
+}
+#endif
+
+/*---------------------------------------------------------------------------*\
+
+    FUNCTION....: lpc_to_lsp()
+
+    AUTHOR......: David Rowe
+    DATE CREATED: 24/2/93
+
+    This function converts LPC coefficients to LSP
+    coefficients.
+
+\*---------------------------------------------------------------------------*/
+
+#ifdef FIXED_POINT
+#define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
+#else
+#define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
+#endif
+
+
+int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
+/*  float *a                    lpc coefficients                        */
+/*  int lpcrdr                  order of LPC coefficients (10)          */
+/*  float *freq                 LSP frequencies in the x domain         */
+/*  int nb                      number of sub-intervals (4)             */
+/*  float delta                 grid spacing interval (0.02)            */
+
+
+{
+    spx_word16_t temp_xr,xl,xr,xm=0;
+    spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
+    int i,j,m,flag,k;
+    VARDECL(spx_word32_t *Q);                   /* ptrs for memory allocation           */
+    VARDECL(spx_word32_t *P);
+    VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation           */
+    VARDECL(spx_word16_t *P16);
+    spx_word32_t *px;                   /* ptrs of respective P'(z) & Q'(z)     */
+    spx_word32_t *qx;
+    spx_word32_t *p;
+    spx_word32_t *q;
+    spx_word16_t *pt;                   /* ptr used for cheb_poly_eval()
+                                whether P' or Q'                        */
+    int roots=0;                /* DR 8/2/94: number of roots found     */
+    flag = 1;                   /*  program is searching for a root when,
+                                1 else has found one                    */
+    m = lpcrdr/2;               /* order of P'(z) & Q'(z) polynomials   */
+
+    /* Allocate memory space for polynomials */
+    ALLOC(Q, (m+1), spx_word32_t);
+    ALLOC(P, (m+1), spx_word32_t);
+
+    /* determine P'(z)'s and Q'(z)'s coefficients where
+      P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
+
+    px = P;                      /* initialise ptrs                     */
+    qx = Q;
+    p = px;
+    q = qx;
+
+#ifdef FIXED_POINT
+    *px++ = LPC_SCALING;
+    *qx++ = LPC_SCALING;
+    for(i=0;i<m;i++){
+       *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
+       *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
+    }
+    px = P;
+    qx = Q;
+    for(i=0;i<m;i++)
+    {
+       /*if (fabs(*px)>=32768)
+          speex_warning_int("px", *px);
+       if (fabs(*qx)>=32768)
+       speex_warning_int("qx", *qx);*/
+       *px = PSHR32(*px,2);
+       *qx = PSHR32(*qx,2);
+       px++;
+       qx++;
+    }
+    /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
+    P[m] = PSHR32(P[m],3);
+    Q[m] = PSHR32(Q[m],3);
+#else
+    *px++ = LPC_SCALING;
+    *qx++ = LPC_SCALING;
+    for(i=0;i<m;i++){
+       *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
+       *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
+    }
+    px = P;
+    qx = Q;
+    for(i=0;i<m;i++){
+       *px = 2**px;
+       *qx = 2**qx;
+       px++;
+       qx++;
+    }
+#endif
+
+    px = P;                     /* re-initialise ptrs                   */
+    qx = Q;
+
+    /* now that we have computed P and Q convert to 16 bits to
+       speed up cheb_poly_eval */
+
+    ALLOC(P16, m+1, spx_word16_t);
+    ALLOC(Q16, m+1, spx_word16_t);
+
+    for (i=0;i<m+1;i++)
+    {
+       P16[i] = P[i];
+       Q16[i] = Q[i];
+    }
+
+    /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
+    Keep alternating between the two polynomials as each zero is found  */
+
+    xr = 0;                     /* initialise xr to zero                */
+    xl = FREQ_SCALE;                    /* start at point xl = 1                */
+
+    for(j=0;j<lpcrdr;j++){
+        if(j&1)                 /* determines whether P' or Q' is eval. */
+            pt = Q16;
+        else
+            pt = P16;
+
+        psuml = cheb_poly_eva(pt,xl,m,stack);   /* evals poly. at xl    */
+        flag = 1;
+        while(flag && (xr >= -FREQ_SCALE)){
+           spx_word16_t dd;
+           /* Modified by JMV to provide smaller steps around x=+-1 */
+#ifdef FIXED_POINT
+           dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
+           if (psuml<512 && psuml>-512)
+              dd = PSHR16(dd,1);
+#else
+           dd=delta*(1-.9*xl*xl);
+           if (fabs(psuml)<.2)
+              dd *= .5;
+#endif
+           xr = SUB16(xl, dd);                          /* interval spacing     */
+            psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x)    */
+            temp_psumr = psumr;
+            temp_xr = xr;
+
+    /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
+    sign change.
+    if a sign change has occurred the interval is bisected and then
+    checked again for a sign change which determines in which
+    interval the zero lies in.
+    If there is no sign change between poly(xm) and poly(xl) set interval
+    between xm and xr else set interval between xl and xr and repeat till
+    root is located within the specified limits                         */
+
+            if(SIGN_CHANGE(psumr,psuml))
+            {
+                roots++;
+
+                psumm=psuml;
+                for(k=0;k<=nb;k++){
+#ifdef FIXED_POINT
+                    xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));              /* bisect the interval  */
+#else
+                    xm = .5*(xl+xr);            /* bisect the interval  */
+#endif
+                    psumm=cheb_poly_eva(pt,xm,m,stack);
+                    /*if(psumm*psuml>0.)*/
+                    if(!SIGN_CHANGE(psumm,psuml))
+                    {
+                        psuml=psumm;
+                        xl=xm;
+                    } else {
+                        psumr=psumm;
+                        xr=xm;
+                    }
+                }
+
+               /* once zero is found, reset initial interval to xr      */
+               freq[j] = X2ANGLE(xm);
+               xl = xm;
+               flag = 0;                /* reset flag for next search   */
+            }
+            else{
+                psuml=temp_psumr;
+                xl=temp_xr;
+            }
+        }
+    }
+    return(roots);
+}
+
+/*---------------------------------------------------------------------------*\
+
+        FUNCTION....: lsp_to_lpc()
+
+        AUTHOR......: David Rowe
+        DATE CREATED: 24/2/93
+
+        Converts LSP coefficients to LPC coefficients.
+
+\*---------------------------------------------------------------------------*/
+
+#ifdef FIXED_POINT
+
+void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
+/*  float *freq         array of LSP frequencies in the x domain        */
+/*  float *ak           array of LPC coefficients                       */
+/*  int lpcrdr          order of LPC coefficients                       */
+{
+    int i,j;
+    spx_word32_t xout1,xout2,xin;
+    spx_word32_t mult, a;
+    VARDECL(spx_word16_t *freqn);
+    VARDECL(spx_word32_t **xp);
+    VARDECL(spx_word32_t *xpmem);
+    VARDECL(spx_word32_t **xq);
+    VARDECL(spx_word32_t *xqmem);
+    int m = lpcrdr>>1;
+
+    /* 
+    
+       Reconstruct P(z) and Q(z) by cascading second order polynomials
+       in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
+       In the time domain this is:
+
+       y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
+    
+       This is what the ALLOCS below are trying to do:
+
+         int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
+         int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP
+
+       These matrices store the output of each stage on each row.  The
+       final (m-th) row has the output of the final (m-th) cascaded
+       2nd order filter.  The first row is the impulse input to the
+       system (not written as it is known).
+
+       The version below takes advantage of the fact that a lot of the
+       outputs are zero or known, for example if we put an inpulse
+       into the first section the "clock" it 10 times only the first 3
+       outputs samples are non-zero (it's an FIR filter).
+    */
+
+    ALLOC(xp, (m+1), spx_word32_t*);
+    ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
+
+    ALLOC(xq, (m+1), spx_word32_t*);
+    ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
+    
+    for(i=0; i<=m; i++) {
+      xp[i] = xpmem + i*(lpcrdr+1+2);
+      xq[i] = xqmem + i*(lpcrdr+1+2);
+    }
+
+    /* work out 2cos terms in Q14 */
+
+    ALLOC(freqn, lpcrdr, spx_word16_t);
+    for (i=0;i<lpcrdr;i++) 
+       freqn[i] = ANGLE2X(freq[i]);
+
+    #define QIMP  21   /* scaling for impulse */
+
+    xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
+   
+    /* first col and last non-zero values of each row are trivial */
+    
+    for(i=0;i<=m;i++) {
+     xp[i][1] = 0;
+     xp[i][2] = xin;
+     xp[i][2+2*i] = xin;
+     xq[i][1] = 0;
+     xq[i][2] = xin;
+     xq[i][2+2*i] = xin;
+    }
+
+    /* 2nd row (first output row) is trivial */
+
+    xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
+    xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);
+
+    xout1 = xout2 = 0;
+
+    /* now generate remaining rows */
+
+    for(i=1;i<m;i++) {
+
+      for(j=1;j<2*(i+1)-1;j++) {
+        mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
+        xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
+        mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
+        xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
+      }
+
+      /* for last col xp[i][j+2] = xq[i][j+2] = 0 */
+
+      mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
+      xp[i+1][j+2] = SUB32(xp[i][j], mult);
+      mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
+      xq[i+1][j+2] = SUB32(xq[i][j], mult);
+    }
+
+    /* process last row to extra a{k} */
+
+    for(j=1;j<=lpcrdr;j++) {
+      int shift = QIMP-13;
+
+      /* final filter sections */
+      a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift); 
+      xout1 = xp[m][j+2];
+      xout2 = xq[m][j+2];
+      
+      /* hard limit ak's to +/- 32767 */
+
+      if (a < -32767) a = -32767;
+      if (a > 32767) a = 32767;
+      ak[j-1] = (short)a;
+     
+    }
+
+}
+
+#else
+
+void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
+/*  float *freq         array of LSP frequencies in the x domain        */
+/*  float *ak           array of LPC coefficients                       */
+/*  int lpcrdr          order of LPC coefficients                       */
+
+
+{
+    int i,j;
+    float xout1,xout2,xin1,xin2;
+    VARDECL(float *Wp);
+    float *pw,*n1,*n2,*n3,*n4=NULL;
+    VARDECL(float *x_freq);
+    int m = lpcrdr>>1;
+
+    ALLOC(Wp, 4*m+2, float);
+    pw = Wp;
+
+    /* initialise contents of array */
+
+    for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
+        *pw++ = 0.0;
+    }
+
+    /* Set pointers up */
+
+    pw = Wp;
+    xin1 = 1.0;
+    xin2 = 1.0;
+
+    ALLOC(x_freq, lpcrdr, float);
+    for (i=0;i<lpcrdr;i++)
+       x_freq[i] = ANGLE2X(freq[i]);
+
+    /* reconstruct P(z) and Q(z) by  cascading second order
+      polynomials in form 1 - 2xz(-1) +z(-2), where x is the
+      LSP coefficient */
+
+    for(j=0;j<=lpcrdr;j++){
+       int i2=0;
+        for(i=0;i<m;i++,i2+=2){
+            n1 = pw+(i*4);
+            n2 = n1 + 1;
+            n3 = n2 + 1;
+            n4 = n3 + 1;
+            xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
+            xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
+            *n2 = *n1;
+            *n4 = *n3;
+            *n1 = xin1;
+            *n3 = xin2;
+            xin1 = xout1;
+            xin2 = xout2;
+        }
+        xout1 = xin1 + *(n4+1);
+        xout2 = xin2 - *(n4+2);
+        if (j>0)
+           ak[j-1] = (xout1 + xout2)*0.5f;
+        *(n4+1) = xin1;
+        *(n4+2) = xin2;
+
+        xin1 = 0.0;
+        xin2 = 0.0;
+    }
+
+}
+#endif
+
+
+#ifdef FIXED_POINT
+
+/*Makes sure the LSPs are stable*/
+void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
+{
+   int i;
+   spx_word16_t m = margin;
+   spx_word16_t m2 = 25736-margin;
+  
+   if (lsp[0]<m)
+      lsp[0]=m;
+   if (lsp[len-1]>m2)
+      lsp[len-1]=m2;
+   for (i=1;i<len-1;i++)
+   {
+      if (lsp[i]<lsp[i-1]+m)
+         lsp[i]=lsp[i-1]+m;
+
+      if (lsp[i]>lsp[i+1]-m)
+         lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
+   }
+}
+
+
+void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
+{
+   int i;
+   spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
+   spx_word16_t tmp2 = 16384-tmp;
+   for (i=0;i<len;i++)
+   {
+      interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
+   }
+}
+
+#else
+
+/*Makes sure the LSPs are stable*/
+void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
+{
+   int i;
+   if (lsp[0]<LSP_SCALING*margin)
+      lsp[0]=LSP_SCALING*margin;
+   if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
+      lsp[len-1]=LSP_SCALING*(M_PI-margin);
+   for (i=1;i<len-1;i++)
+   {
+      if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
+         lsp[i]=lsp[i-1]+LSP_SCALING*margin;
+
+      if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
+         lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
+   }
+}
+
+
+void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
+{
+   int i;
+   float tmp = (1.0f + subframe)/nb_subframes;
+   for (i=0;i<len;i++)
+   {
+      interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
+   }
+}
+
+#endif