git.mutantstargoat.com Git - laserbrain_demo/blob - libs/vorbis/lsp.c

   1 /********************************************************************
   2  *                                                                  *
   3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
   4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
   5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
   6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
   7  *                                                                  *
   8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2009             *
   9  * by the Xiph.Org Foundation http://www.xiph.org/                  *
  10  *                                                                  *
  11  ********************************************************************
  12
  13   function: LSP (also called LSF) conversion routines
  14   last mod: $Id: lsp.c 17538 2010-10-15 02:52:29Z tterribe $
  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 (see http://www.rothweiler.us for contact info).
  19   The paper is available at:
  20
  21   http://www.myown1.com/joe/lsf
  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    separately 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 /* define either of these (preferably FLOAT_LOOKUP) to have faster
  55    but less precise implementation. */
  56 #undef FLOAT_LOOKUP
  57 #undef INT_LOOKUP
  58
  59 #ifdef FLOAT_LOOKUP
  60 #include "lookup.c" /* catch this in the build system; we #include for
  61                        compilers (like gcc) that can't inline across
  62                        modules */
  63
  64 /* side effect: changes *lsp to cosines of lsp */
  65 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
  66                             float amp,float ampoffset){
  67   int i;
  68   float wdel=M_PI/ln;
  69   vorbis_fpu_control fpu;
  70
  71   vorbis_fpu_setround(&fpu);
  72   for(i=0;i<m;i++)lsp[i]=vorbis_coslook(lsp[i]);
  73
  74   i=0;
  75   while(i<n){
  76     int k=map[i];
  77     int qexp;
  78     float p=.7071067812f;
  79     float q=.7071067812f;
  80     float w=vorbis_coslook(wdel*k);
  81     float *ftmp=lsp;
  82     int c=m>>1;
  83
  84     while(c--){
  85       q*=ftmp[0]-w;
  86       p*=ftmp[1]-w;
  87       ftmp+=2;
  88     }
  89
  90     if(m&1){
  91       /* odd order filter; slightly assymetric */
  92       /* the last coefficient */
  93       q*=ftmp[0]-w;
  94       q*=q;
  95       p*=p*(1.f-w*w);
  96     }else{
  97       /* even order filter; still symmetric */
  98       q*=q*(1.f+w);
  99       p*=p*(1.f-w);
 100     }
 101
 102     q=frexp(p+q,&qexp);
 103     q=vorbis_fromdBlook(amp*
 104                         vorbis_invsqlook(q)*
 105                         vorbis_invsq2explook(qexp+m)-
 106                         ampoffset);
 107
 108     do{
 109       curve[i++]*=q;
 110     }while(map[i]==k);
 111   }
 112   vorbis_fpu_restore(fpu);
 113 }
 114
 115 #else
 116
 117 #ifdef INT_LOOKUP
 118 #include "lookup.c" /* catch this in the build system; we #include for
 119                        compilers (like gcc) that can't inline across
 120                        modules */
 121
 122 static const int MLOOP_1[64]={
 123    0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
 124   14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
 125   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
 126   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
 127 };
 128
 129 static const int MLOOP_2[64]={
 130   0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
 131   8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
 132   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
 133   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
 134 };
 135
 136 static const int MLOOP_3[8]={0,1,2,2,3,3,3,3};
 137
 138
 139 /* side effect: changes *lsp to cosines of lsp */
 140 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 141                             float amp,float ampoffset){
 142
 143   /* 0 <= m < 256 */
 144
 145   /* set up for using all int later */
 146   int i;
 147   int ampoffseti=rint(ampoffset*4096.f);
 148   int ampi=rint(amp*16.f);
 149   long *ilsp=alloca(m*sizeof(*ilsp));
 150   for(i=0;i<m;i++)ilsp[i]=vorbis_coslook_i(lsp[i]/M_PI*65536.f+.5f);
 151
 152   i=0;
 153   while(i<n){
 154     int j,k=map[i];
 155     unsigned long pi=46341; /* 2**-.5 in 0.16 */
 156     unsigned long qi=46341;
 157     int qexp=0,shift;
 158     long wi=vorbis_coslook_i(k*65536/ln);
 159
 160     qi*=labs(ilsp[0]-wi);
 161     pi*=labs(ilsp[1]-wi);
 162
 163     for(j=3;j<m;j+=2){
 164       if(!(shift=MLOOP_1[(pi|qi)>>25]))
 165         if(!(shift=MLOOP_2[(pi|qi)>>19]))
 166           shift=MLOOP_3[(pi|qi)>>16];
 167       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
 168       pi=(pi>>shift)*labs(ilsp[j]-wi);
 169       qexp+=shift;
 170     }
 171     if(!(shift=MLOOP_1[(pi|qi)>>25]))
 172       if(!(shift=MLOOP_2[(pi|qi)>>19]))
 173         shift=MLOOP_3[(pi|qi)>>16];
 174
 175     /* pi,qi normalized collectively, both tracked using qexp */
 176
 177     if(m&1){
 178       /* odd order filter; slightly assymetric */
 179       /* the last coefficient */
 180       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
 181       pi=(pi>>shift)<<14;
 182       qexp+=shift;
 183
 184       if(!(shift=MLOOP_1[(pi|qi)>>25]))
 185         if(!(shift=MLOOP_2[(pi|qi)>>19]))
 186           shift=MLOOP_3[(pi|qi)>>16];
 187
 188       pi>>=shift;
 189       qi>>=shift;
 190       qexp+=shift-14*((m+1)>>1);
 191
 192       pi=((pi*pi)>>16);
 193       qi=((qi*qi)>>16);
 194       qexp=qexp*2+m;
 195
 196       pi*=(1<<14)-((wi*wi)>>14);
 197       qi+=pi>>14;
 198
 199     }else{
 200       /* even order filter; still symmetric */
 201
 202       /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
 203          worth tracking step by step */
 204
 205       pi>>=shift;
 206       qi>>=shift;
 207       qexp+=shift-7*m;
 208
 209       pi=((pi*pi)>>16);
 210       qi=((qi*qi)>>16);
 211       qexp=qexp*2+m;
 212
 213       pi*=(1<<14)-wi;
 214       qi*=(1<<14)+wi;
 215       qi=(qi+pi)>>14;
 216
 217     }
 218
 219
 220     /* we've let the normalization drift because it wasn't important;
 221        however, for the lookup, things must be normalized again.  We
 222        need at most one right shift or a number of left shifts */
 223
 224     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
 225       qi>>=1; qexp++;
 226     }else
 227       while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
 228         qi<<=1; qexp--;
 229       }
 230
 231     amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
 232                             vorbis_invsqlook_i(qi,qexp)-
 233                                                       /*  m.8, m+n<=8 */
 234                             ampoffseti);              /*  8.12[0]     */
 235
 236     curve[i]*=amp;
 237     while(map[++i]==k)curve[i]*=amp;
 238   }
 239 }
 240
 241 #else
 242
 243 /* old, nonoptimized but simple version for any poor sap who needs to
 244    figure out what the hell this code does, or wants the other
 245    fraction of a dB precision */
 246
 247 /* side effect: changes *lsp to cosines of lsp */
 248 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 249                             float amp,float ampoffset){
 250   int i;
 251   float wdel=M_PI/ln;
 252   for(i=0;i<m;i++)lsp[i]=2.f*cos(lsp[i]);
 253
 254   i=0;
 255   while(i<n){
 256     int j,k=map[i];
 257     float p=.5f;
 258     float q=.5f;
 259     float w=2.f*cos(wdel*k);
 260     for(j=1;j<m;j+=2){
 261       q *= w-lsp[j-1];
 262       p *= w-lsp[j];
 263     }
 264     if(j==m){
 265       /* odd order filter; slightly assymetric */
 266       /* the last coefficient */
 267       q*=w-lsp[j-1];
 268       p*=p*(4.f-w*w);
 269       q*=q;
 270     }else{
 271       /* even order filter; still symmetric */
 272       p*=p*(2.f-w);
 273       q*=q*(2.f+w);
 274     }
 275
 276     q=fromdB(amp/sqrt(p+q)-ampoffset);
 277
 278     curve[i]*=q;
 279     while(map[++i]==k)curve[i]*=q;
 280   }
 281 }
 282
 283 #endif
 284 #endif
 285
 286 static void cheby(float *g, int ord) {
 287   int i, j;
 288
 289   g[0] *= .5f;
 290   for(i=2; i<= ord; i++) {
 291     for(j=ord; j >= i; j--) {
 292       g[j-2] -= g[j];
 293       g[j] += g[j];
 294     }
 295   }
 296 }
 297
 298 static int comp(const void *a,const void *b){
 299   return (*(float *)a<*(float *)b)-(*(float *)a>*(float *)b);
 300 }
 301
 302 /* Newton-Raphson-Maehly actually functioned as a decent root finder,
 303    but there are root sets for which it gets into limit cycles
 304    (exacerbated by zero suppression) and fails.  We can't afford to
 305    fail, even if the failure is 1 in 100,000,000, so we now use
 306    Laguerre and later polish with Newton-Raphson (which can then
 307    afford to fail) */
 308
 309 #define EPSILON 10e-7
 310 static int Laguerre_With_Deflation(float *a,int ord,float *r){
 311   int i,m;
 312   double *defl=alloca(sizeof(*defl)*(ord+1));
 313   for(i=0;i<=ord;i++)defl[i]=a[i];
 314
 315   for(m=ord;m>0;m--){
 316     double new=0.f,delta;
 317
 318     /* iterate a root */
 319     while(1){
 320       double p=defl[m],pp=0.f,ppp=0.f,denom;
 321
 322       /* eval the polynomial and its first two derivatives */
 323       for(i=m;i>0;i--){
 324         ppp = new*ppp + pp;
 325         pp  = new*pp  + p;
 326         p   = new*p   + defl[i-1];
 327       }
 328
 329       /* Laguerre's method */
 330       denom=(m-1) * ((m-1)*pp*pp - m*p*ppp);
 331       if(denom<0)
 332         return(-1);  /* complex root!  The LPC generator handed us a bad filter */
 333
 334       if(pp>0){
 335         denom = pp + sqrt(denom);
 336         if(denom<EPSILON)denom=EPSILON;
 337       }else{
 338         denom = pp - sqrt(denom);
 339         if(denom>-(EPSILON))denom=-(EPSILON);
 340       }
 341
 342       delta  = m*p/denom;
 343       new   -= delta;
 344
 345       if(delta<0.f)delta*=-1;
 346
 347       if(fabs(delta/new)<10e-12)break;
 348     }
 349
 350     r[m-1]=new;
 351
 352     /* forward deflation */
 353
 354     for(i=m;i>0;i--)
 355       defl[i-1]+=new*defl[i];
 356     defl++;
 357
 358   }
 359   return(0);
 360 }
 361
 362
 363 /* for spit-and-polish only */
 364 static int Newton_Raphson(float *a,int ord,float *r){
 365   int i, k, count=0;
 366   double error=1.f;
 367   double *root=alloca(ord*sizeof(*root));
 368
 369   for(i=0; i<ord;i++) root[i] = r[i];
 370
 371   while(error>1e-20){
 372     error=0;
 373
 374     for(i=0; i<ord; i++) { /* Update each point. */
 375       double pp=0.,delta;
 376       double rooti=root[i];
 377       double p=a[ord];
 378       for(k=ord-1; k>= 0; k--) {
 379
 380         pp= pp* rooti + p;
 381         p = p * rooti + a[k];
 382       }
 383
 384       delta = p/pp;
 385       root[i] -= delta;
 386       error+= delta*delta;
 387     }
 388
 389     if(count>40)return(-1);
 390
 391     count++;
 392   }
 393
 394   /* Replaced the original bubble sort with a real sort.  With your
 395      help, we can eliminate the bubble sort in our lifetime. --Monty */
 396
 397   for(i=0; i<ord;i++) r[i] = root[i];
 398   return(0);
 399 }
 400
 401
 402 /* Convert lpc coefficients to lsp coefficients */
 403 int vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
 404   int order2=(m+1)>>1;
 405   int g1_order,g2_order;
 406   float *g1=alloca(sizeof(*g1)*(order2+1));
 407   float *g2=alloca(sizeof(*g2)*(order2+1));
 408   float *g1r=alloca(sizeof(*g1r)*(order2+1));
 409   float *g2r=alloca(sizeof(*g2r)*(order2+1));
 410   int i;
 411
 412   /* even and odd are slightly different base cases */
 413   g1_order=(m+1)>>1;
 414   g2_order=(m)  >>1;
 415
 416   /* Compute the lengths of the x polynomials. */
 417   /* Compute the first half of K & R F1 & F2 polynomials. */
 418   /* Compute half of the symmetric and antisymmetric polynomials. */
 419   /* Remove the roots at +1 and -1. */
 420
 421   g1[g1_order] = 1.f;
 422   for(i=1;i<=g1_order;i++) g1[g1_order-i] = lpc[i-1]+lpc[m-i];
 423   g2[g2_order] = 1.f;
 424   for(i=1;i<=g2_order;i++) g2[g2_order-i] = lpc[i-1]-lpc[m-i];
 425
 426   if(g1_order>g2_order){
 427     for(i=2; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+2];
 428   }else{
 429     for(i=1; i<=g1_order;i++) g1[g1_order-i] -= g1[g1_order-i+1];
 430     for(i=1; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+1];
 431   }
 432
 433   /* Convert into polynomials in cos(alpha) */
 434   cheby(g1,g1_order);
 435   cheby(g2,g2_order);
 436
 437   /* Find the roots of the 2 even polynomials.*/
 438   if(Laguerre_With_Deflation(g1,g1_order,g1r) ||
 439      Laguerre_With_Deflation(g2,g2_order,g2r))
 440     return(-1);
 441
 442   Newton_Raphson(g1,g1_order,g1r); /* if it fails, it leaves g1r alone */
 443   Newton_Raphson(g2,g2_order,g2r); /* if it fails, it leaves g2r alone */
 444
 445   qsort(g1r,g1_order,sizeof(*g1r),comp);
 446   qsort(g2r,g2_order,sizeof(*g2r),comp);
 447
 448   for(i=0;i<g1_order;i++)
 449     lsp[i*2] = acos(g1r[i]);
 450
 451   for(i=0;i<g2_order;i++)
 452     lsp[i*2+1] = acos(g2r[i]);
 453   return(0);
 454 }