lib/lsp.c

   1 /********************************************************************
   2  *                                                                  *
   3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
   4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS SOURCE IS GOVERNED BY *
   5  * THE GNU LESSER/LIBRARY PUBLIC LICENSE, WHICH IS INCLUDED WITH    *
   6  * THIS SOURCE. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.        *
   7  *                                                                  *
   8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2000             *
   9  * by Monty <monty@xiph.org> and the XIPHOPHORUS Company            *
  10  * http://www.xiph.org/                                             *
  11  *                                                                  *
  12  ********************************************************************
  13
  14   function: LSP (also called LSF) conversion routines
  15   last mod: $Id: lsp.c,v 1.14 2001/01/22 01:38:25 xiphmont Exp $
  16
  17   The LSP generation code is taken (with minimal modification and a
  18   few bugfixes) from "On the Computation of the LSP Frequencies" by
  19   Joseph Rothweiler <rothwlr@altavista.net>, available at:
  20
  21   http://www2.xtdl.com/~rothwlr/lsfpaper/lsfpage.html
  22
  23  ********************************************************************/
  24
  25 /* Note that the lpc-lsp conversion finds the roots of polynomial with
  26    an iterative root polisher (CACM algorithm 283).  It *is* possible
  27    to confuse this algorithm into not converging; that should only
  28    happen with absurdly closely spaced roots (very sharp peaks in the
  29    LPC f response) which in turn should be impossible in our use of
  30    the code.  If this *does* happen anyway, it's a bug in the floor
  31    finder; find the cause of the confusion (probably a single bin
  32    spike or accidental near-float-limit resolution problems) and
  33    correct it. */
  34
  35 #include <math.h>
  36 #include <string.h>
  37 #include <stdlib.h>
  38 #include "lsp.h"
  39 #include "os.h"
  40 #include "misc.h"
  41 #include "lookup.h"
  42 #include "scales.h"
  43
  44 /* three possible LSP to f curve functions; the exact computation
  45    (float), a lookup based float implementation, and an integer
  46    implementation.  The float lookup is likely the optimal choice on
  47    any machine with an FPU.  The integer implementation is *not* fixed
  48    point (due to the need for a large dynamic range and thus a
  49    seperately tracked exponent) and thus much more complex than the
  50    relatively simple float implementations. It's mostly for future
  51    work on a fully fixed point implementation for processors like the
  52    ARM family. */
  53
  54 /* undefine both for the 'old' but more precise implementation */
  55 #undef   FLOAT_LOOKUP
  56 #undef    INT_LOOKUP
  57
  58 #ifdef FLOAT_LOOKUP
  59 #include "lookup.c" /* catch this in the build system; we #include for
  60                        compilers (like gcc) that can't inline across
  61                        modules */
  62
  63 /* side effect: changes *lsp to cosines of lsp */
  64 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
  65                             float amp,float ampoffset){
  66   int i;
  67   float wdel=M_PI/ln;
  68   vorbis_fpu_control fpu;
  69
  70   vorbis_fpu_setround(&fpu);
  71   for(i=0;i<m;i++)lsp[i]=vorbis_coslook(lsp[i]);
  72
  73   i=0;
  74   while(i<n){
  75     int k=map[i];
  76     int qexp;
  77     float p=.7071067812f;
  78     float q=.7071067812f;
  79     float w=vorbis_coslook(wdel*k);
  80     float *ftmp=lsp;
  81     int c=m>>1;
  82
  83     do{
  84       q*=ftmp[0]-w;
  85       p*=ftmp[1]-w;
  86       ftmp+=2;
  87     }while(--c);
  88
  89     if(m&1){
  90       /* odd order filter; slightly assymetric */
  91       /* the last coefficient */
  92       q*=ftmp[0]-w;
  93       q*=q;
  94       p*=p*(1.f-w*w);
  95     }else{
  96       /* even order filter; still symmetric */
  97       q*=q*(1.f+w);
  98       p*=p*(1.f-w);
  99     }
 100
 101     q=frexp(p+q,&qexp);
 102     q=vorbis_fromdBlook(amp*
 103                         vorbis_invsqlook(q)*
 104                         vorbis_invsq2explook(qexp+m)-
 105                         ampoffset);
 106
 107     do{
 108       curve[i++]=q;
 109     }while(map[i]==k);
 110   }
 111   vorbis_fpu_restore(fpu);
 112 }
 113
 114 #else
 115
 116 #ifdef INT_LOOKUP
 117 #include "lookup.c" /* catch this in the build system; we #include for
 118                        compilers (like gcc) that can't inline across
 119                        modules */
 120
 121 static int MLOOP_1[64]={
 122    0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
 123   14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
 124   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
 125   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
 126 };
 127
 128 static int MLOOP_2[64]={
 129   0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
 130   8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
 131   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
 132   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
 133 };
 134
 135 static int MLOOP_3[8]={0,1,2,2,3,3,3,3};
 136
 137
 138 /* side effect: changes *lsp to cosines of lsp */
 139 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 140                             float amp,float ampoffset){
 141
 142   /* 0 <= m < 256 */
 143
 144   /* set up for using all int later */
 145   int i;
 146   int ampoffseti=rint(ampoffset*4096.f);
 147   int ampi=rint(amp*16.f);
 148   long *ilsp=alloca(m*sizeof(long));
 149   for(i=0;i<m;i++)ilsp[i]=vorbis_coslook_i(lsp[i]/M_PI*65536.f+.5f);
 150
 151   i=0;
 152   while(i<n){
 153     int j,k=map[i];
 154     unsigned long pi=46341; /* 2**-.5 in 0.16 */
 155     unsigned long qi=46341;
 156     int qexp=0,shift;
 157     long wi=vorbis_coslook_i(k*65536/ln);
 158
 159     qi*=labs(ilsp[0]-wi);
 160     pi*=labs(ilsp[1]-wi);
 161
 162     for(j=3;j<m;j+=2){
 163       if(!(shift=MLOOP_1[(pi|qi)>>25]))
 164         if(!(shift=MLOOP_2[(pi|qi)>>19]))
 165           shift=MLOOP_3[(pi|qi)>>16];
 166       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
 167       pi=(pi>>shift)*labs(ilsp[j]-wi);
 168       qexp+=shift;
 169     }
 170     if(!(shift=MLOOP_1[(pi|qi)>>25]))
 171       if(!(shift=MLOOP_2[(pi|qi)>>19]))
 172         shift=MLOOP_3[(pi|qi)>>16];
 173
 174     /* pi,qi normalized collectively, both tracked using qexp */
 175
 176     if(m&1){
 177       /* odd order filter; slightly assymetric */
 178       /* the last coefficient */
 179       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
 180       pi=(pi>>shift)<<14;
 181       qexp+=shift;
 182
 183       if(!(shift=MLOOP_1[(pi|qi)>>25]))
 184         if(!(shift=MLOOP_2[(pi|qi)>>19]))
 185           shift=MLOOP_3[(pi|qi)>>16];
 186
 187       pi>>=shift;
 188       qi>>=shift;
 189       qexp+=shift-14*((m+1)>>1);
 190
 191       pi=((pi*pi)>>16);
 192       qi=((qi*qi)>>16);
 193       qexp=qexp*2+m;
 194
 195       pi*=(1<<14)-((wi*wi)>>14);
 196       qi+=pi>>14;
 197
 198       //q*=ftmp[0]-w;
 199       //q*=q;
 200       //p*=p*(1.f-w*w);
 201     }else{
 202       /* even order filter; still symmetric */
 203
 204       /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
 205          worth tracking step by step */
 206
 207       pi>>=shift;
 208       qi>>=shift;
 209       qexp+=shift-7*m;
 210
 211       pi=((pi*pi)>>16);
 212       qi=((qi*qi)>>16);
 213       qexp=qexp*2+m;
 214
 215       pi*=(1<<14)-wi;
 216       qi*=(1<<14)+wi;
 217       qi=(qi+pi)>>14;
 218
 219     }
 220
 221
 222     /* we've let the normalization drift because it wasn't important;
 223        however, for the lookup, things must be normalized again.  We
 224        need at most one right shift or a number of left shifts */
 225
 226     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
 227       qi>>=1; qexp++;
 228     }else
 229       while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
 230         qi<<=1; qexp--;
 231       }
 232
 233     amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
 234                             vorbis_invsqlook_i(qi,qexp)-
 235                                                       /*  m.8, m+n<=8 */
 236                             ampoffseti);              /*  8.12[0]     */
 237
 238     curve[i]=amp;
 239     while(map[++i]==k)curve[i]=amp;
 240   }
 241 }
 242
 243 #else
 244
 245 /* old, nonoptimized but simple version for any poor sap who needs to
 246    figure out what the hell this code does, or wants the other
 247    fraction of a dB precision */
 248
 249 /* side effect: changes *lsp to cosines of lsp */
 250 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 251                             float amp,float ampoffset){
 252   int i;
 253   float wdel=M_PI/ln;
 254   for(i=0;i<m;i++)lsp[i]=2.f*cos(lsp[i]);
 255
 256   i=0;
 257   while(i<n){
 258     int j,k=map[i];
 259     float p=.5f;
 260     float q=.5f;
 261     float w=2.f*cos(wdel*k);
 262     for(j=1;j<m;j+=2){
 263       q *= w-lsp[j-1];
 264       p *= w-lsp[j];
 265     }
 266     if(j==m){
 267       /* odd order filter; slightly assymetric */
 268       /* the last coefficient */
 269       q*=w-lsp[j-1];
 270       p*=p*(4.f-w*w);
 271       q*=q;
 272     }else{
 273       /* even order filter; still symmetric */
 274       p*=p*(2.f-w);
 275       q*=q*(2.f+w);
 276     }
 277
 278     q=fromdB(amp/sqrt(p+q)-ampoffset);
 279
 280     curve[i]=q;
 281     while(map[++i]==k)curve[i]=q;
 282   }
 283 }
 284
 285 #endif
 286 #endif
 287
 288 static void cheby(float *g, int ord) {
 289   int i, j;
 290
 291   g[0] *= .5f;
 292   for(i=2; i<= ord; i++) {
 293     for(j=ord; j >= i; j--) {
 294       g[j-2] -= g[j];
 295       g[j] += g[j];
 296     }
 297   }
 298 }
 299
 300 static int comp(const void *a,const void *b){
 301   if(*(float *)a<*(float *)b)
 302     return(1);
 303   else
 304     return(-1);
 305 }
 306
 307 /* This is one of those 'mathemeticians should not write code' kind of
 308    cases.  Newton's method of polishing roots is straightforward
 309    enough... except in those cases where it just fails in the real
 310    world.  In our case below, we're worried about a local mini/maxima
 311    shooting a root estimation off to infinity, or the new estimation
 312    chaotically oscillating about convergence (shouldn't actually be a
 313    problem in our usage.
 314
 315    Maehly's modification (zero suppression, to prevent two tenative
 316    roots from collapsing to the same actual root) similarly can
 317    temporarily shoot a root off toward infinity.  It would come
 318    back... if it were not for the fact that machine representation has
 319    limited dynamic range and resolution.  This too is guarded by
 320    limiting delta.
 321
 322    Last problem is convergence criteria; we don't know what a 'double'
 323    is on our hardware/compiler, and the convergence limit is bounded
 324    by roundoff noise.  So, we hack convergence:
 325
 326    Require at most 1e-6 mean squared error for all zeroes.  When
 327    converging, start the clock ticking at 1e-6; limit our polishing to
 328    as many more iterations as took us to get this far, 100 max.
 329
 330    Past max iters, quit when MSE is no longer decreasing *or* we go
 331    below ~1e-20 MSE, whichever happens first. */
 332
 333 static void Newton_Raphson_Maehly(float *a,int ord,float *r){
 334   int i, k, count=0, maxiter=0;
 335   double error=1.,besterror=1.;
 336   double *root=alloca(ord*sizeof(double));
 337
 338   for(i=0; i<ord;i++) root[i] = 2.0 * (i+0.5) / ord - 1.0;
 339
 340   while(error>1e-20){
 341     error=0;
 342
 343     for(i=0; i<ord; i++) { /* Update each point. */
 344       double ac=0.,pp=0.,delta;
 345       double rooti=root[i];
 346       double p=a[ord];
 347       for(k=ord-1; k>= 0; k--) {
 348
 349         pp= pp* rooti + p;
 350         p = p * rooti+ a[k];
 351         if (k != i) ac += 1./(rooti - root[k]);
 352       }
 353       ac=p*ac;
 354
 355       delta = p/(pp-ac);
 356
 357       /* don't allow the correction to scream off into infinity if we
 358          happened to polish right at a local mini/maximum */
 359
 360       if(delta<-3.)delta=-3.;
 361       if(delta>3.)delta=3.; /* 3 is not a random choice; it's large
 362                                enough to make sure the first pass
 363                                can't accidentally limit two poles to
 364                                the same value in a fatal nonelastic
 365                                collision.  */
 366
 367       root[i] -= delta;
 368       error += delta*delta;
 369     }
 370
 371     if(maxiter && count>maxiter && error>=besterror)break;
 372
 373     /* anything to help out the polisher; converge using doubles */
 374     if(!count || error<besterror){
 375       for(i=0; i<ord; i++) r[i]=root[i];
 376       besterror=error;
 377       if(error<1e-6){ /* rough minimum criteria */
 378         maxiter=count*2+10;
 379         if(maxiter>100)maxiter=100;
 380       }
 381     }
 382
 383     count++;
 384   }
 385
 386   /* Replaced the original bubble sort with a real sort.  With your
 387      help, we can eliminate the bubble sort in our lifetime. --Monty */
 388
 389   qsort(r,ord,sizeof(float),comp);
 390
 391 }
 392
 393 /* Convert lpc coefficients to lsp coefficients */
 394 void vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
 395   int order2=(m+1)>>1;
 396   int g1_order,g2_order;
 397   float *g1=alloca(sizeof(float)*(order2+1));
 398   float *g2=alloca(sizeof(float)*(order2+1));
 399   float *g1r=alloca(sizeof(float)*(order2+1));
 400   float *g2r=alloca(sizeof(float)*(order2+1));
 401   int i;
 402
 403   /* even and odd are slightly different base cases */
 404   g1_order=(m+1)>>1;
 405   g2_order=(m)  >>1;
 406
 407   /* Compute the lengths of the x polynomials. */
 408   /* Compute the first half of K & R F1 & F2 polynomials. */
 409   /* Compute half of the symmetric and antisymmetric polynomials. */
 410   /* Remove the roots at +1 and -1. */
 411
 412   g1[g1_order] = 1.f;
 413   for(i=1;i<=g1_order;i++) g1[g1_order-i] = lpc[i-1]+lpc[m-i];
 414   g2[g2_order] = 1.f;
 415   for(i=1;i<=g2_order;i++) g2[g2_order-i] = lpc[i-1]-lpc[m-i];
 416
 417   if(g1_order>g2_order){
 418     for(i=2; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+2];
 419   }else{
 420     for(i=1; i<=g1_order;i++) g1[g1_order-i] -= g1[g1_order-i+1];
 421     for(i=1; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+1];
 422   }
 423
 424   /* Convert into polynomials in cos(alpha) */
 425   cheby(g1,g1_order);
 426   cheby(g2,g2_order);
 427
 428   /* Find the roots of the 2 even polynomials.*/
 429
 430   Newton_Raphson_Maehly(g1,g1_order,g1r);
 431   Newton_Raphson_Maehly(g2,g2_order,g2r);
 432
 433   for(i=0;i<g1_order;i++)
 434     lsp[i*2] = acos(g1r[i]);
 435
 436   for(i=0;i<g2_order;i++)
 437     lsp[i*2+1] = acos(g2r[i]);
 438
 439 }