src/lib/doslib/ext/speex/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.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 #ifdef FIXED_POINT
 138
 139 #ifndef OVERRIDE_CHEB_POLY_EVA
 140 static inline spx_word32_t cheb_poly_eva(
 141   spx_word16_t *coef, /* P or Q coefs in Q13 format               */
 142   spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */
 143   int              m, /* LPC order/2                              */
 144   char         *stack
 145 )
 146 {
 147     int i;
 148     spx_word16_t b0, b1;
 149     spx_word32_t sum;
 150
 151     /*Prevents overflows*/
 152     if (x>16383)
 153        x = 16383;
 154     if (x<-16383)
 155        x = -16383;
 156
 157     /* Initialise values */
 158     b1=16384;
 159     b0=x;
 160
 161     /* Evaluate Chebyshev series formulation usin g iterative approach  */
 162     sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));
 163     for(i=2;i<=m;i++)
 164     {
 165        spx_word16_t tmp=b0;
 166        b0 = SUB16(MULT16_16_Q13(x,b0), b1);
 167        b1 = tmp;
 168        sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));
 169     }
 170
 171     return sum;
 172 }
 173 #endif
 174
 175 #else
 176
 177 static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack)
 178 {
 179    int k;
 180    float b0, b1, tmp;
 181
 182    /* Initial conditions */
 183    b0=0; /* b_(m+1) */
 184    b1=0; /* b_(m+2) */
 185
 186    x*=2;
 187
 188    /* Calculate the b_(k) */
 189    for(k=m;k>0;k--)
 190    {
 191       tmp=b0;                           /* tmp holds the previous value of b0 */
 192       b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */
 193       b1=tmp;                           /* b1 holds the previous value of b0 */
 194    }
 195
 196    return(-b1+.5*x*b0+coef[m]);
 197 }
 198 #endif
 199
 200 /*---------------------------------------------------------------------------*\
 201
 202     FUNCTION....: lpc_to_lsp()
 203
 204     AUTHOR......: David Rowe
 205     DATE CREATED: 24/2/93
 206
 207     This function converts LPC coefficients to LSP
 208     coefficients.
 209
 210 \*---------------------------------------------------------------------------*/
 211
 212 #ifdef FIXED_POINT
 213 #define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
 214 #else
 215 #define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
 216 #endif
 217
 218
 219 int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
 220 /*  float *a                    lpc coefficients                        */
 221 /*  int lpcrdr                  order of LPC coefficients (10)          */
 222 /*  float *freq                 LSP frequencies in the x domain         */
 223 /*  int nb                      number of sub-intervals (4)             */
 224 /*  float delta                 grid spacing interval (0.02)            */
 225
 226
 227 {
 228     spx_word16_t temp_xr,xl,xr,xm=0;
 229     spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
 230     int i,j,m,flag,k;
 231     VARDECL(spx_word32_t *Q);                   /* ptrs for memory allocation           */
 232     VARDECL(spx_word32_t *P);
 233     VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation           */
 234     VARDECL(spx_word16_t *P16);
 235     spx_word32_t *px;                   /* ptrs of respective P'(z) & Q'(z)     */
 236     spx_word32_t *qx;
 237     spx_word32_t *p;
 238     spx_word32_t *q;
 239     spx_word16_t *pt;                   /* ptr used for cheb_poly_eval()
 240                                 whether P' or Q'                        */
 241     int roots=0;                /* DR 8/2/94: number of roots found     */
 242     flag = 1;                   /*  program is searching for a root when,
 243                                 1 else has found one                    */
 244     m = lpcrdr/2;               /* order of P'(z) & Q'(z) polynomials   */
 245
 246     /* Allocate memory space for polynomials */
 247     ALLOC(Q, (m+1), spx_word32_t);
 248     ALLOC(P, (m+1), spx_word32_t);
 249
 250     /* determine P'(z)'s and Q'(z)'s coefficients where
 251       P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
 252
 253     px = P;                      /* initialise ptrs                     */
 254     qx = Q;
 255     p = px;
 256     q = qx;
 257
 258 #ifdef FIXED_POINT
 259     *px++ = LPC_SCALING;
 260     *qx++ = LPC_SCALING;
 261     for(i=0;i<m;i++){
 262        *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
 263        *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
 264     }
 265     px = P;
 266     qx = Q;
 267     for(i=0;i<m;i++)
 268     {
 269        /*if (fabs(*px)>=32768)
 270           speex_warning_int("px", *px);
 271        if (fabs(*qx)>=32768)
 272        speex_warning_int("qx", *qx);*/
 273        *px = PSHR32(*px,2);
 274        *qx = PSHR32(*qx,2);
 275        px++;
 276        qx++;
 277     }
 278     /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
 279     P[m] = PSHR32(P[m],3);
 280     Q[m] = PSHR32(Q[m],3);
 281 #else
 282     *px++ = LPC_SCALING;
 283     *qx++ = LPC_SCALING;
 284     for(i=0;i<m;i++){
 285        *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
 286        *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
 287     }
 288     px = P;
 289     qx = Q;
 290     for(i=0;i<m;i++){
 291        *px = 2**px;
 292        *qx = 2**qx;
 293        px++;
 294        qx++;
 295     }
 296 #endif
 297
 298     px = P;                     /* re-initialise ptrs                   */
 299     qx = Q;
 300
 301     /* now that we have computed P and Q convert to 16 bits to
 302        speed up cheb_poly_eval */
 303
 304     ALLOC(P16, m+1, spx_word16_t);
 305     ALLOC(Q16, m+1, spx_word16_t);
 306
 307     for (i=0;i<m+1;i++)
 308     {
 309        P16[i] = P[i];
 310        Q16[i] = Q[i];
 311     }
 312
 313     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
 314     Keep alternating between the two polynomials as each zero is found  */
 315
 316     xr = 0;                     /* initialise xr to zero                */
 317     xl = FREQ_SCALE;                    /* start at point xl = 1                */
 318
 319     for(j=0;j<lpcrdr;j++){
 320         if(j&1)                 /* determines whether P' or Q' is eval. */
 321             pt = Q16;
 322         else
 323             pt = P16;
 324
 325         psuml = cheb_poly_eva(pt,xl,m,stack);   /* evals poly. at xl    */
 326         flag = 1;
 327         while(flag && (xr >= -FREQ_SCALE)){
 328            spx_word16_t dd;
 329            /* Modified by JMV to provide smaller steps around x=+-1 */
 330 #ifdef FIXED_POINT
 331            dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
 332            if (psuml<512 && psuml>-512)
 333               dd = PSHR16(dd,1);
 334 #else
 335            dd=delta*(1-.9*xl*xl);
 336            if (fabs(psuml)<.2)
 337               dd *= .5;
 338 #endif
 339            xr = SUB16(xl, dd);                          /* interval spacing     */
 340             psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x)    */
 341             temp_psumr = psumr;
 342             temp_xr = xr;
 343
 344     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
 345     sign change.
 346     if a sign change has occurred the interval is bisected and then
 347     checked again for a sign change which determines in which
 348     interval the zero lies in.
 349     If there is no sign change between poly(xm) and poly(xl) set interval
 350     between xm and xr else set interval between xl and xr and repeat till
 351     root is located within the specified limits                         */
 352
 353             if(SIGN_CHANGE(psumr,psuml))
 354             {
 355                 roots++;
 356
 357                 psumm=psuml;
 358                 for(k=0;k<=nb;k++){
 359 #ifdef FIXED_POINT
 360                     xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));              /* bisect the interval  */
 361 #else
 362                     xm = .5*(xl+xr);            /* bisect the interval  */
 363 #endif
 364                     psumm=cheb_poly_eva(pt,xm,m,stack);
 365                     /*if(psumm*psuml>0.)*/
 366                     if(!SIGN_CHANGE(psumm,psuml))
 367                     {
 368                         psuml=psumm;
 369                         xl=xm;
 370                     } else {
 371                         psumr=psumm;
 372                         xr=xm;
 373                     }
 374                 }
 375
 376                /* once zero is found, reset initial interval to xr      */
 377                freq[j] = X2ANGLE(xm);
 378                xl = xm;
 379                flag = 0;                /* reset flag for next search   */
 380             }
 381             else{
 382                 psuml=temp_psumr;
 383                 xl=temp_xr;
 384             }
 385         }
 386     }
 387     return(roots);
 388 }
 389
 390 /*---------------------------------------------------------------------------*\
 391
 392         FUNCTION....: lsp_to_lpc()
 393
 394         AUTHOR......: David Rowe
 395         DATE CREATED: 24/2/93
 396
 397         Converts LSP coefficients to LPC coefficients.
 398
 399 \*---------------------------------------------------------------------------*/
 400
 401 #ifdef FIXED_POINT
 402
 403 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
 404 /*  float *freq         array of LSP frequencies in the x domain        */
 405 /*  float *ak           array of LPC coefficients                       */
 406 /*  int lpcrdr          order of LPC coefficients                       */
 407 {
 408     int i,j;
 409     spx_word32_t xout1,xout2,xin;
 410     spx_word32_t mult, a;
 411     VARDECL(spx_word16_t *freqn);
 412     VARDECL(spx_word32_t **xp);
 413     VARDECL(spx_word32_t *xpmem);
 414     VARDECL(spx_word32_t **xq);
 415     VARDECL(spx_word32_t *xqmem);
 416     int m = lpcrdr>>1;
 417
 418     /*
 419
 420        Reconstruct P(z) and Q(z) by cascading second order polynomials
 421        in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
 422        In the time domain this is:
 423
 424        y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
 425
 426        This is what the ALLOCS below are trying to do:
 427
 428          int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
 429          int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP
 430
 431        These matrices store the output of each stage on each row.  The
 432        final (m-th) row has the output of the final (m-th) cascaded
 433        2nd order filter.  The first row is the impulse input to the
 434        system (not written as it is known).
 435
 436        The version below takes advantage of the fact that a lot of the
 437        outputs are zero or known, for example if we put an inpulse
 438        into the first section the "clock" it 10 times only the first 3
 439        outputs samples are non-zero (it's an FIR filter).
 440     */
 441
 442     ALLOC(xp, (m+1), spx_word32_t*);
 443     ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
 444
 445     ALLOC(xq, (m+1), spx_word32_t*);
 446     ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
 447
 448     for(i=0; i<=m; i++) {
 449       xp[i] = xpmem + i*(lpcrdr+1+2);
 450       xq[i] = xqmem + i*(lpcrdr+1+2);
 451     }
 452
 453     /* work out 2cos terms in Q14 */
 454
 455     ALLOC(freqn, lpcrdr, spx_word16_t);
 456     for (i=0;i<lpcrdr;i++)
 457        freqn[i] = ANGLE2X(freq[i]);
 458
 459     #define QIMP  21   /* scaling for impulse */
 460
 461     xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
 462
 463     /* first col and last non-zero values of each row are trivial */
 464
 465     for(i=0;i<=m;i++) {
 466      xp[i][1] = 0;
 467      xp[i][2] = xin;
 468      xp[i][2+2*i] = xin;
 469      xq[i][1] = 0;
 470      xq[i][2] = xin;
 471      xq[i][2+2*i] = xin;
 472     }
 473
 474     /* 2nd row (first output row) is trivial */
 475
 476     xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
 477     xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);
 478
 479     xout1 = xout2 = 0;
 480
 481     /* now generate remaining rows */
 482
 483     for(i=1;i<m;i++) {
 484
 485       for(j=1;j<2*(i+1)-1;j++) {
 486         mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
 487         xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
 488         mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
 489         xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
 490       }
 491
 492       /* for last col xp[i][j+2] = xq[i][j+2] = 0 */
 493
 494       mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
 495       xp[i+1][j+2] = SUB32(xp[i][j], mult);
 496       mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
 497       xq[i+1][j+2] = SUB32(xq[i][j], mult);
 498     }
 499
 500     /* process last row to extra a{k} */
 501
 502     for(j=1;j<=lpcrdr;j++) {
 503       int shift = QIMP-13;
 504
 505       /* final filter sections */
 506       a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);
 507       xout1 = xp[m][j+2];
 508       xout2 = xq[m][j+2];
 509
 510       /* hard limit ak's to +/- 32767 */
 511
 512       if (a < -32767) a = -32767;
 513       if (a > 32767) a = 32767;
 514       ak[j-1] = (short)a;
 515
 516     }
 517
 518 }
 519
 520 #else
 521
 522 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
 523 /*  float *freq         array of LSP frequencies in the x domain        */
 524 /*  float *ak           array of LPC coefficients                       */
 525 /*  int lpcrdr          order of LPC coefficients                       */
 526
 527
 528 {
 529     int i,j;
 530     float xout1,xout2,xin1,xin2;
 531     VARDECL(float *Wp);
 532     float *pw,*n1,*n2,*n3,*n4=NULL;
 533     VARDECL(float *x_freq);
 534     int m = lpcrdr>>1;
 535
 536     ALLOC(Wp, 4*m+2, float);
 537     pw = Wp;
 538
 539     /* initialise contents of array */
 540
 541     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
 542         *pw++ = 0.0;
 543     }
 544
 545     /* Set pointers up */
 546
 547     pw = Wp;
 548     xin1 = 1.0;
 549     xin2 = 1.0;
 550
 551     ALLOC(x_freq, lpcrdr, float);
 552     for (i=0;i<lpcrdr;i++)
 553        x_freq[i] = ANGLE2X(freq[i]);
 554
 555     /* reconstruct P(z) and Q(z) by  cascading second order
 556       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
 557       LSP coefficient */
 558
 559     for(j=0;j<=lpcrdr;j++){
 560        int i2=0;
 561         for(i=0;i<m;i++,i2+=2){
 562             n1 = pw+(i*4);
 563             n2 = n1 + 1;
 564             n3 = n2 + 1;
 565             n4 = n3 + 1;
 566             xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
 567             xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
 568             *n2 = *n1;
 569             *n4 = *n3;
 570             *n1 = xin1;
 571             *n3 = xin2;
 572             xin1 = xout1;
 573             xin2 = xout2;
 574         }
 575         xout1 = xin1 + *(n4+1);
 576         xout2 = xin2 - *(n4+2);
 577         if (j>0)
 578            ak[j-1] = (xout1 + xout2)*0.5f;
 579         *(n4+1) = xin1;
 580         *(n4+2) = xin2;
 581
 582         xin1 = 0.0;
 583         xin2 = 0.0;
 584     }
 585
 586 }
 587 #endif
 588
 589
 590 #ifdef FIXED_POINT
 591
 592 /*Makes sure the LSPs are stable*/
 593 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
 594 {
 595    int i;
 596    spx_word16_t m = margin;
 597    spx_word16_t m2 = 25736-margin;
 598
 599    if (lsp[0]<m)
 600       lsp[0]=m;
 601    if (lsp[len-1]>m2)
 602       lsp[len-1]=m2;
 603    for (i=1;i<len-1;i++)
 604    {
 605       if (lsp[i]<lsp[i-1]+m)
 606          lsp[i]=lsp[i-1]+m;
 607
 608       if (lsp[i]>lsp[i+1]-m)
 609          lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
 610    }
 611 }
 612
 613
 614 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)
 615 {
 616    int i;
 617    spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
 618    spx_word16_t tmp2 = 16384-tmp;
 619    for (i=0;i<len;i++)
 620    {
 621       interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
 622    }
 623 }
 624
 625 #else
 626
 627 /*Makes sure the LSPs are stable*/
 628 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
 629 {
 630    int i;
 631    if (lsp[0]<LSP_SCALING*margin)
 632       lsp[0]=LSP_SCALING*margin;
 633    if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
 634       lsp[len-1]=LSP_SCALING*(M_PI-margin);
 635    for (i=1;i<len-1;i++)
 636    {
 637       if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
 638          lsp[i]=lsp[i-1]+LSP_SCALING*margin;
 639
 640       if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
 641          lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
 642    }
 643 }
 644
 645
 646 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)
 647 {
 648    int i;
 649    float tmp = (1.0f + subframe)/nb_subframes;
 650    for (i=0;i<len;i++)
 651    {
 652       interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
 653    }
 654 }
 655
 656 #endif