← Back to branch summary

~ubuntu-branches/ubuntu/wily/sflphone/wily

« back to all changes in this revision

Viewing changes to daemon/libs/pjproject-2.1.0/third_party/speex/libspeex/lsp.c

  • Committer: Package Import Robot
  • Author(s): Jonathan Riddell
  • Date: 2015-01-07 14:51:16 UTC
  • mfrom: (4.3.5 sid)
  • Revision ID: package-import@ubuntu.com-20150107145116-yxnafinf4lrdvrmx
Tags: 1.4.1-0.1ubuntu1
* Merge with Debian, remaining changes:
 - Drop soprano, nepomuk build-dep
* Drop ubuntu patches, now upstream

Show diffs side-by-side

added added

removed removed

Lines of Context:
daemon/libs/pjproject-2.1.0/third_party/speex/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.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