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     int i,j;
 413     spx_word32_t xout1,xout2,xin;
 414     spx_word32_t mult, a;
 415     VARDECL(spx_word16_t *freqn);
 416     VARDECL(spx_word32_t **xp);
 417     VARDECL(spx_word32_t *xpmem);
 418     VARDECL(spx_word32_t **xq);
 419     VARDECL(spx_word32_t *xqmem);
 420     int m = lpcrdr>>1;
 421
 422     /*
 423
 424        Reconstruct P(z) and Q(z) by cascading second order polynomials
 425        in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
 426        In the time domain this is:
 427
 428        y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
 429
 430        This is what the ALLOCS below are trying to do:
 431
 432          int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
 433          int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP
 434
 435        These matrices store the output of each stage on each row.  The
 436        final (m-th) row has the output of the final (m-th) cascaded
 437        2nd order filter.  The first row is the impulse input to the
 438        system (not written as it is known).
 439
 440        The version below takes advantage of the fact that a lot of the
 441        outputs are zero or known, for example if we put an inpulse
 442        into the first section the "clock" it 10 times only the first 3
 443        outputs samples are non-zero (it's an FIR filter).
 444     */
 445
 446     ALLOC(xp, (m+1), spx_word32_t*);
 447     ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
 448
 449     ALLOC(xq, (m+1), spx_word32_t*);
 450     ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
 451
 452     for(i=0; i<=m; i++) {
 453       xp[i] = xpmem + i*(lpcrdr+1+2);
 454       xq[i] = xqmem + i*(lpcrdr+1+2);
 455     }
 456
 457     /* work out 2cos terms in Q14 */
 458
 459     ALLOC(freqn, lpcrdr, spx_word16_t);
 460     for (i=0;i<lpcrdr;i++)
 461        freqn[i] = ANGLE2X(freq[i]);
 462
 463     #define QIMP  21   /* scaling for impulse */
 464
 465     xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
 466
 467     /* first col and last non-zero values of each row are trivial */
 468
 469     for(i=0;i<=m;i++) {
 470      xp[i][1] = 0;
 471      xp[i][2] = xin;
 472      xp[i][2+2*i] = xin;
 473      xq[i][1] = 0;
 474      xq[i][2] = xin;
 475      xq[i][2+2*i] = xin;
 476     }
 477
 478     /* 2nd row (first output row) is trivial */
 479
 480     xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
 481     xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);
 482
 483     xout1 = xout2 = 0;
 484
 485     /* now generate remaining rows */
 486
 487     for(i=1;i<m;i++) {
 488
 489       for(j=1;j<2*(i+1)-1;j++) {
 490         mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
 491         xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
 492         mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
 493         xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
 494       }
 495
 496       /* for last col xp[i][j+2] = xq[i][j+2] = 0 */
 497
 498       mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
 499       xp[i+1][j+2] = SUB32(xp[i][j], mult);
 500       mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
 501       xq[i+1][j+2] = SUB32(xq[i][j], mult);
 502     }
 503
 504     /* process last row to extra a{k} */
 505
 506     for(j=1;j<=lpcrdr;j++) {
 507       int shift = QIMP-13;
 508
 509       /* final filter sections */
 510       a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);
 511       xout1 = xp[m][j+2];
 512       xout2 = xq[m][j+2];
 513
 514       /* hard limit ak's to +/- 32767 */
 515
 516       if (a < -32767) a = -32767;
 517       if (a > 32767) a = 32767;
 518       ak[j-1] = (short)a;
 519
 520     }
 521
 522 }
 523
 524 #else
 525
 526 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
 527 /*  float *freq         array of LSP frequencies in the x domain        */
 528 /*  float *ak           array of LPC coefficients                       */
 529 /*  int lpcrdr          order of LPC coefficients                       */
 530
 531
 532 {
 533     int i,j;
 534     float xout1,xout2,xin1,xin2;
 535     VARDECL(float *Wp);
 536     float *pw,*n1,*n2,*n3,*n4=NULL;
 537     VARDECL(float *x_freq);
 538     int m = lpcrdr>>1;
 539
 540     ALLOC(Wp, 4*m+2, float);
 541     pw = Wp;
 542
 543     /* initialise contents of array */
 544
 545     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
 546         *pw++ = 0.0;
 547     }
 548
 549     /* Set pointers up */
 550
 551     pw = Wp;
 552     xin1 = 1.0;
 553     xin2 = 1.0;
 554
 555     ALLOC(x_freq, lpcrdr, float);
 556     for (i=0;i<lpcrdr;i++)
 557        x_freq[i] = ANGLE2X(freq[i]);
 558
 559     /* reconstruct P(z) and Q(z) by  cascading second order
 560       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
 561       LSP coefficient */
 562
 563     for(j=0;j<=lpcrdr;j++){
 564        int i2=0;
 565         for(i=0;i<m;i++,i2+=2){
 566             n1 = pw+(i*4);
 567             n2 = n1 + 1;
 568             n3 = n2 + 1;
 569             n4 = n3 + 1;
 570             xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
 571             xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
 572             *n2 = *n1;
 573             *n4 = *n3;
 574             *n1 = xin1;
 575             *n3 = xin2;
 576             xin1 = xout1;
 577             xin2 = xout2;
 578         }
 579         xout1 = xin1 + *(n4+1);
 580         xout2 = xin2 - *(n4+2);
 581         if (j>0)
 582            ak[j-1] = (xout1 + xout2)*0.5f;
 583         *(n4+1) = xin1;
 584         *(n4+2) = xin2;
 585
 586         xin1 = 0.0;
 587         xin2 = 0.0;
 588     }
 589
 590 }
 591 #endif
 592
 593
 594 #ifdef FIXED_POINT
 595
 596 /*Makes sure the LSPs are stable*/
 597 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
 598 {
 599    int i;
 600    spx_word16_t m = margin;
 601    spx_word16_t m2 = 25736-margin;
 602
 603    if (lsp[0]<m)
 604       lsp[0]=m;
 605    if (lsp[len-1]>m2)
 606       lsp[len-1]=m2;
 607    for (i=1;i<len-1;i++)
 608    {
 609       if (lsp[i]<lsp[i-1]+m)
 610          lsp[i]=lsp[i-1]+m;
 611
 612       if (lsp[i]>lsp[i+1]-m)
 613          lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
 614    }
 615 }
 616
 617
 618 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)
 619 {
 620    int i;
 621    spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
 622    spx_word16_t tmp2 = 16384-tmp;
 623    for (i=0;i<len;i++)
 624    {
 625       interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
 626    }
 627 }
 628
 629 #else
 630
 631 /*Makes sure the LSPs are stable*/
 632 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
 633 {
 634    int i;
 635    if (lsp[0]<LSP_SCALING*margin)
 636       lsp[0]=LSP_SCALING*margin;
 637    if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
 638       lsp[len-1]=LSP_SCALING*(M_PI-margin);
 639    for (i=1;i<len-1;i++)
 640    {
 641       if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
 642          lsp[i]=lsp[i-1]+LSP_SCALING*margin;
 643
 644       if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
 645          lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
 646    }
 647 }
 648
 649
 650 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)
 651 {
 652    int i;
 653    float tmp = (1.0f + subframe)/nb_subframes;
 654    for (i=0;i<len;i++)
 655    {
 656       interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
 657    }
 658 }
 659
 660 #endif