apps/codecs/libspeex/lsp.c

   1 /*---------------------------------------------------------------------------*\
   2 Original copyright
   3         FILE........: lsp.c
   4         AUTHOR......: David Rowe
   5         DATE CREATED: 24/2/93
   6
   7 Heavily modified by Jean-Marc Valin (c) 2002-2006 (fixed-point,
   8                        optimizations, additional functions, ...)
   9
  10    This file contains functions for converting Linear Prediction
  11    Coefficients (LPC) to Line Spectral Pair (LSP) and back. Note that the
  12    LSP coefficients are not in radians format but in the x domain of the
  13    unit circle.
  14
  15    Speex License:
  16
  17    Redistribution and use in source and binary forms, with or without
  18    modification, are permitted provided that the following conditions
  19    are met:
  20
  21    - Redistributions of source code must retain the above copyright
  22    notice, this list of conditions and the following disclaimer.
  23
  24    - Redistributions in binary form must reproduce the above copyright
  25    notice, this list of conditions and the following disclaimer in the
  26    documentation and/or other materials provided with the distribution.
  27
  28    - Neither the name of the Xiph.org Foundation nor the names of its
  29    contributors may be used to endorse or promote products derived from
  30    this software without specific prior written permission.
  31
  32    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  33    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  34    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  35    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  36    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  37    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  38    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  39    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  40    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  41    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  42    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  43 */
  44
  45 /*---------------------------------------------------------------------------*\
  46
  47   Introduction to Line Spectrum Pairs (LSPs)
  48   ------------------------------------------
  49
  50   LSPs are used to encode the LPC filter coefficients {ak} for
  51   transmission over the channel.  LSPs have several properties (like
  52   less sensitivity to quantisation noise) that make them superior to
  53   direct quantisation of {ak}.
  54
  55   A(z) is a polynomial of order lpcrdr with {ak} as the coefficients.
  56
  57   A(z) is transformed to P(z) and Q(z) (using a substitution and some
  58   algebra), to obtain something like:
  59
  60     A(z) = 0.5[P(z)(z+z^-1) + Q(z)(z-z^-1)]  (1)
  61
  62   As you can imagine A(z) has complex zeros all over the z-plane. P(z)
  63   and Q(z) have the very neat property of only having zeros _on_ the
  64   unit circle.  So to find them we take a test point z=exp(jw) and
  65   evaluate P (exp(jw)) and Q(exp(jw)) using a grid of points between 0
  66   and pi.
  67
  68   The zeros (roots) of P(z) also happen to alternate, which is why we
  69   swap coefficients as we find roots.  So the process of finding the
  70   LSP frequencies is basically finding the roots of 5th order
  71   polynomials.
  72
  73   The root so P(z) and Q(z) occur in symmetrical pairs at +/-w, hence
  74   the name Line Spectrum Pairs (LSPs).
  75
  76   To convert back to ak we just evaluate (1), "clocking" an impulse
  77   thru it lpcrdr times gives us the impulse response of A(z) which is
  78   {ak}.
  79
  80 \*---------------------------------------------------------------------------*/
  81
  82 #ifdef HAVE_CONFIG_H
  83 #include "config-speex.h"
  84 #endif
  85
  86 #include <math.h>
  87 #include "lsp.h"
  88 #include "stack_alloc.h"
  89 #include "math_approx.h"
  90
  91 #ifndef M_PI
  92 #define M_PI           3.14159265358979323846  /* pi */
  93 #endif
  94
  95 #ifndef NULL
  96 #define NULL 0
  97 #endif
  98
  99 #ifdef FIXED_POINT
 100
 101 #define FREQ_SCALE 16384
 102
 103 /*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/
 104 #define ANGLE2X(a) (SHL16(spx_cos(a),2))
 105
 106 /*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/
 107 #define X2ANGLE(x) (spx_acos(x))
 108
 109 #ifdef BFIN_ASM
 110 #include "lsp_bfin.h"
 111 #endif
 112
 113 #else
 114
 115 /*#define C1 0.99940307
 116 #define C2 -0.49558072
 117 #define C3 0.03679168*/
 118
 119 #define FREQ_SCALE 1.
 120 #define ANGLE2X(a) (spx_cos(a))
 121 #define X2ANGLE(x) (acos(x))
 122
 123 #endif
 124
 125
 126 /*---------------------------------------------------------------------------*\
 127
 128    FUNCTION....: cheb_poly_eva()
 129
 130    AUTHOR......: David Rowe
 131    DATE CREATED: 24/2/93
 132
 133    This function evaluates a series of Chebyshev polynomials
 134
 135 \*---------------------------------------------------------------------------*/
 136
 137 #ifndef SPEEX_DISABLE_ENCODER
 138
 139 #ifdef FIXED_POINT
 140
 141 #ifndef OVERRIDE_CHEB_POLY_EVA
 142 static inline spx_word32_t cheb_poly_eva(
 143   spx_word16_t *coef, /* P or Q coefs in Q13 format               */
 144   spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */
 145   int              m, /* LPC order/2                              */
 146   char         *stack
 147 )
 148 {
 149     int i;
 150     spx_word16_t b0, b1;
 151     spx_word32_t sum;
 152
 153     /*Prevents overflows*/
 154     if (x>16383)
 155        x = 16383;
 156     if (x<-16383)
 157        x = -16383;
 158
 159     /* Initialise values */
 160     b1=16384;
 161     b0=x;
 162
 163     /* Evaluate Chebyshev series formulation usin g iterative approach  */
 164     sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));
 165     for(i=2;i<=m;i++)
 166     {
 167        spx_word16_t tmp=b0;
 168        b0 = SUB16(MULT16_16_Q13(x,b0), b1);
 169        b1 = tmp;
 170        sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));
 171     }
 172
 173     return sum;
 174 }
 175 #endif
 176
 177 #else
 178
 179 static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack)
 180 {
 181    int k;
 182    float b0, b1, tmp;
 183
 184    /* Initial conditions */
 185    b0=0; /* b_(m+1) */
 186    b1=0; /* b_(m+2) */
 187
 188    x*=2;
 189
 190    /* Calculate the b_(k) */
 191    for(k=m;k>0;k--)
 192    {
 193       tmp=b0;                           /* tmp holds the previous value of b0 */
 194       b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */
 195       b1=tmp;                           /* b1 holds the previous value of b0 */
 196    }
 197
 198    return(-b1+.5*x*b0+coef[m]);
 199 }
 200 #endif
 201
 202 /*---------------------------------------------------------------------------*\
 203
 204     FUNCTION....: lpc_to_lsp()
 205
 206     AUTHOR......: David Rowe
 207     DATE CREATED: 24/2/93
 208
 209     This function converts LPC coefficients to LSP
 210     coefficients.
 211
 212 \*---------------------------------------------------------------------------*/
 213
 214 #ifdef FIXED_POINT
 215 #define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
 216 #else
 217 #define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
 218 #endif
 219
 220
 221 int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
 222 /*  float *a                    lpc coefficients                        */
 223 /*  int lpcrdr                  order of LPC coefficients (10)          */
 224 /*  float *freq                 LSP frequencies in the x domain         */
 225 /*  int nb                      number of sub-intervals (4)             */
 226 /*  float delta                 grid spacing interval (0.02)            */
 227
 228
 229 {
 230     spx_word16_t temp_xr,xl,xr,xm=0;
 231     spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
 232     int i,j,m,flag,k;
 233     VARDECL(spx_word32_t *Q);                   /* ptrs for memory allocation           */
 234     VARDECL(spx_word32_t *P);
 235     VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation           */
 236     VARDECL(spx_word16_t *P16);
 237     spx_word32_t *px;                   /* ptrs of respective P'(z) & Q'(z)     */
 238     spx_word32_t *qx;
 239     spx_word32_t *p;
 240     spx_word32_t *q;
 241     spx_word16_t *pt;                   /* ptr used for cheb_poly_eval()
 242                                 whether P' or Q'                        */
 243     int roots=0;                /* DR 8/2/94: number of roots found     */
 244     flag = 1;                   /*  program is searching for a root when,
 245                                 1 else has found one                    */
 246     m = lpcrdr/2;               /* order of P'(z) & Q'(z) polynomials   */
 247
 248     /* Allocate memory space for polynomials */
 249     ALLOC(Q, (m+1), spx_word32_t);
 250     ALLOC(P, (m+1), spx_word32_t);
 251
 252     /* determine P'(z)'s and Q'(z)'s coefficients where
 253       P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
 254
 255     px = P;                      /* initialise ptrs                     */
 256     qx = Q;
 257     p = px;
 258     q = qx;
 259
 260 #ifdef FIXED_POINT
 261     *px++ = LPC_SCALING;
 262     *qx++ = LPC_SCALING;
 263     for(i=0;i<m;i++){
 264        *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
 265        *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
 266     }
 267     px = P;
 268     qx = Q;
 269     for(i=0;i<m;i++)
 270     {
 271        /*if (fabs(*px)>=32768)
 272           speex_warning_int("px", *px);
 273        if (fabs(*qx)>=32768)
 274        speex_warning_int("qx", *qx);*/
 275        *px = PSHR32(*px,2);
 276        *qx = PSHR32(*qx,2);
 277        px++;
 278        qx++;
 279     }
 280     /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
 281     P[m] = PSHR32(P[m],3);
 282     Q[m] = PSHR32(Q[m],3);
 283 #else
 284     *px++ = LPC_SCALING;
 285     *qx++ = LPC_SCALING;
 286     for(i=0;i<m;i++){
 287        *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
 288        *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
 289     }
 290     px = P;
 291     qx = Q;
 292     for(i=0;i<m;i++){
 293        *px = 2**px;
 294        *qx = 2**qx;
 295        px++;
 296        qx++;
 297     }
 298 #endif
 299
 300     px = P;                     /* re-initialise ptrs                   */
 301     qx = Q;
 302
 303     /* now that we have computed P and Q convert to 16 bits to
 304        speed up cheb_poly_eval */
 305
 306     ALLOC(P16, m+1, spx_word16_t);
 307     ALLOC(Q16, m+1, spx_word16_t);
 308
 309     for (i=0;i<m+1;i++)
 310     {
 311        P16[i] = P[i];
 312        Q16[i] = Q[i];
 313     }
 314
 315     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
 316     Keep alternating between the two polynomials as each zero is found  */
 317
 318     xr = 0;                     /* initialise xr to zero                */
 319     xl = FREQ_SCALE;                    /* start at point xl = 1                */
 320
 321     for(j=0;j<lpcrdr;j++){
 322         if(j&1)                 /* determines whether P' or Q' is eval. */
 323             pt = Q16;
 324         else
 325             pt = P16;
 326
 327         psuml = cheb_poly_eva(pt,xl,m,stack);   /* evals poly. at xl    */
 328         flag = 1;
 329         while(flag && (xr >= -FREQ_SCALE)){
 330            spx_word16_t dd;
 331            /* Modified by JMV to provide smaller steps around x=+-1 */
 332 #ifdef FIXED_POINT
 333            dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
 334            if (psuml<512 && psuml>-512)
 335               dd = PSHR16(dd,1);
 336 #else
 337            dd=delta*(1-.9*xl*xl);
 338            if (fabs(psuml)<.2)
 339               dd *= .5;
 340 #endif
 341            xr = SUB16(xl, dd);                          /* interval spacing     */
 342             psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x)    */
 343             temp_psumr = psumr;
 344             temp_xr = xr;
 345
 346     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
 347     sign change.
 348     if a sign change has occurred the interval is bisected and then
 349     checked again for a sign change which determines in which
 350     interval the zero lies in.
 351     If there is no sign change between poly(xm) and poly(xl) set interval
 352     between xm and xr else set interval between xl and xr and repeat till
 353     root is located within the specified limits                         */
 354
 355             if(SIGN_CHANGE(psumr,psuml))
 356             {
 357                 roots++;
 358
 359                 psumm=psuml;
 360                 for(k=0;k<=nb;k++){
 361 #ifdef FIXED_POINT
 362                     xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));              /* bisect the interval  */
 363 #else
 364                     xm = .5*(xl+xr);            /* bisect the interval  */
 365 #endif
 366                     psumm=cheb_poly_eva(pt,xm,m,stack);
 367                     /*if(psumm*psuml>0.)*/
 368                     if(!SIGN_CHANGE(psumm,psuml))
 369                     {
 370                         psuml=psumm;
 371                         xl=xm;
 372                     } else {
 373                         psumr=psumm;
 374                         xr=xm;
 375                     }
 376                 }
 377
 378                /* once zero is found, reset initial interval to xr      */
 379                freq[j] = X2ANGLE(xm);
 380                xl = xm;
 381                flag = 0;                /* reset flag for next search   */
 382             }
 383             else{
 384                 psuml=temp_psumr;
 385                 xl=temp_xr;
 386             }
 387         }
 388     }
 389     return(roots);
 390 }
 391
 392 #endif /* SPEEX_DISABLE_ENCODER */
 393
 394 /*---------------------------------------------------------------------------*\
 395
 396         FUNCTION....: lsp_to_lpc()
 397
 398         AUTHOR......: David Rowe
 399         DATE CREATED: 24/2/93
 400
 401         Converts LSP coefficients to LPC coefficients.
 402
 403 \*---------------------------------------------------------------------------*/
 404
 405 #ifdef FIXED_POINT
 406
 407 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
 408 /*  float *freq         array of LSP frequencies in the x domain        */
 409 /*  float *ak           array of LPC coefficients                       */
 410 /*  int lpcrdr          order of LPC coefficients                       */
 411 {
 412     (void)stack;
 413     int i,j;
 414     spx_word32_t xout1,xout2,xin;
 415     spx_word32_t mult, a;
 416     VARDECL(spx_word16_t *freqn);
 417     VARDECL(spx_word32_t **xp);
 418     VARDECL(spx_word32_t *xpmem);
 419     VARDECL(spx_word32_t **xq);
 420     VARDECL(spx_word32_t *xqmem);
 421     int m = lpcrdr>>1;
 422
 423     /*
 424
 425        Reconstruct P(z) and Q(z) by cascading second order polynomials
 426        in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
 427        In the time domain this is:
 428
 429        y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
 430
 431        This is what the ALLOCS below are trying to do:
 432
 433          int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
 434          int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP
 435
 436        These matrices store the output of each stage on each row.  The
 437        final (m-th) row has the output of the final (m-th) cascaded
 438        2nd order filter.  The first row is the impulse input to the
 439        system (not written as it is known).
 440
 441        The version below takes advantage of the fact that a lot of the
 442        outputs are zero or known, for example if we put an inpulse
 443        into the first section the "clock" it 10 times only the first 3
 444        outputs samples are non-zero (it's an FIR filter).
 445     */
 446
 447     ALLOC(xp, (m+1), spx_word32_t*);
 448     ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
 449
 450     ALLOC(xq, (m+1), spx_word32_t*);
 451     ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
 452
 453     for(i=0; i<=m; i++) {
 454       xp[i] = xpmem + i*(lpcrdr+1+2);
 455       xq[i] = xqmem + i*(lpcrdr+1+2);
 456     }
 457
 458     /* work out 2cos terms in Q14 */
 459
 460     ALLOC(freqn, lpcrdr, spx_word16_t);
 461     for (i=0;i<lpcrdr;i++)
 462        freqn[i] = ANGLE2X(freq[i]);
 463
 464     #define QIMP  21   /* scaling for impulse */
 465
 466     xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
 467
 468     /* first col and last non-zero values of each row are trivial */
 469
 470     for(i=0;i<=m;i++) {
 471      xp[i][1] = 0;
 472      xp[i][2] = xin;
 473      xp[i][2+2*i] = xin;
 474      xq[i][1] = 0;
 475      xq[i][2] = xin;
 476      xq[i][2+2*i] = xin;
 477     }
 478
 479     /* 2nd row (first output row) is trivial */
 480
 481     xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
 482     xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);
 483
 484     xout1 = xout2 = 0;
 485
 486     /* now generate remaining rows */
 487
 488     for(i=1;i<m;i++) {
 489
 490       for(j=1;j<2*(i+1)-1;j++) {
 491         mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
 492         xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
 493         mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
 494         xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
 495       }
 496
 497       /* for last col xp[i][j+2] = xq[i][j+2] = 0 */
 498
 499       mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
 500       xp[i+1][j+2] = SUB32(xp[i][j], mult);
 501       mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
 502       xq[i+1][j+2] = SUB32(xq[i][j], mult);
 503     }
 504
 505     /* process last row to extra a{k} */
 506
 507     for(j=1;j<=lpcrdr;j++) {
 508       int shift = QIMP-13;
 509
 510       /* final filter sections */
 511       a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);
 512       xout1 = xp[m][j+2];
 513       xout2 = xq[m][j+2];
 514
 515       /* hard limit ak's to +/- 32767 */
 516
 517       if (a < -32767) a = -32767;
 518       if (a > 32767) a = 32767;
 519       ak[j-1] = (short)a;
 520
 521     }
 522
 523 }
 524
 525 #else
 526
 527 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
 528 /*  float *freq         array of LSP frequencies in the x domain        */
 529 /*  float *ak           array of LPC coefficients                       */
 530 /*  int lpcrdr          order of LPC coefficients                       */
 531
 532
 533 {
 534     int i,j;
 535     float xout1,xout2,xin1,xin2;
 536     VARDECL(float *Wp);
 537     float *pw,*n1,*n2,*n3,*n4=NULL;
 538     VARDECL(float *x_freq);
 539     int m = lpcrdr>>1;
 540
 541     ALLOC(Wp, 4*m+2, float);
 542     pw = Wp;
 543
 544     /* initialise contents of array */
 545
 546     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
 547         *pw++ = 0.0;
 548     }
 549
 550     /* Set pointers up */
 551
 552     pw = Wp;
 553     xin1 = 1.0;
 554     xin2 = 1.0;
 555
 556     ALLOC(x_freq, lpcrdr, float);
 557     for (i=0;i<lpcrdr;i++)
 558        x_freq[i] = ANGLE2X(freq[i]);
 559
 560     /* reconstruct P(z) and Q(z) by  cascading second order
 561       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
 562       LSP coefficient */
 563
 564     for(j=0;j<=lpcrdr;j++){
 565        int i2=0;
 566         for(i=0;i<m;i++,i2+=2){
 567             n1 = pw+(i*4);
 568             n2 = n1 + 1;
 569             n3 = n2 + 1;
 570             n4 = n3 + 1;
 571             xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
 572             xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
 573             *n2 = *n1;
 574             *n4 = *n3;
 575             *n1 = xin1;
 576             *n3 = xin2;
 577             xin1 = xout1;
 578             xin2 = xout2;
 579         }
 580         xout1 = xin1 + *(n4+1);
 581         xout2 = xin2 - *(n4+2);
 582         if (j>0)
 583            ak[j-1] = (xout1 + xout2)*0.5f;
 584         *(n4+1) = xin1;
 585         *(n4+2) = xin2;
 586
 587         xin1 = 0.0;
 588         xin2 = 0.0;
 589     }
 590
 591 }
 592 #endif
 593
 594
 595 #ifdef FIXED_POINT
 596
 597 /*Makes sure the LSPs are stable*/
 598 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
 599 {
 600    int i;
 601    spx_word16_t m = margin;
 602    spx_word16_t m2 = 25736-margin;
 603
 604    if (lsp[0]<m)
 605       lsp[0]=m;
 606    if (lsp[len-1]>m2)
 607       lsp[len-1]=m2;
 608    for (i=1;i<len-1;i++)
 609    {
 610       if (lsp[i]<lsp[i-1]+m)
 611          lsp[i]=lsp[i-1]+m;
 612
 613       if (lsp[i]>lsp[i+1]-m)
 614          lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
 615    }
 616 }
 617
 618
 619 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)
 620 {
 621    int i;
 622    spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
 623    spx_word16_t tmp2 = 16384-tmp;
 624    for (i=0;i<len;i++)
 625    {
 626       interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
 627    }
 628 }
 629
 630 #else
 631
 632 /*Makes sure the LSPs are stable*/
 633 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
 634 {
 635    int i;
 636    if (lsp[0]<LSP_SCALING*margin)
 637       lsp[0]=LSP_SCALING*margin;
 638    if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
 639       lsp[len-1]=LSP_SCALING*(M_PI-margin);
 640    for (i=1;i<len-1;i++)
 641    {
 642       if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
 643          lsp[i]=lsp[i-1]+LSP_SCALING*margin;
 644
 645       if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
 646          lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
 647    }
 648 }
 649
 650
 651 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)
 652 {
 653    int i;
 654    float tmp = (1.0f + subframe)/nb_subframes;
 655    for (i=0;i<len;i++)
 656    {
 657       interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
 658    }
 659 }
 660
 661 #endif