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