lib/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
  15   The LSP generation code is taken (with minimal modification and a
  16   few bugfixes) from "On the Computation of the LSP Frequencies" by
  17   Joseph Rothweiler (see http://www.rothweiler.us for contact info).
  18   The paper is available at:
  19
  20   http://www.myown1.com/joe/lsf
  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    separately 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 /* define either of these (preferably FLOAT_LOOKUP) to have faster
  54    but less 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     while(c--){
  84       q*=ftmp[0]-w;
  85       p*=ftmp[1]-w;
  86       ftmp+=2;
  87     }
  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 const 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 const 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 const 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(*ilsp));
 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     }else{
 199       /* even order filter; still symmetric */
 200
 201       /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
 202          worth tracking step by step */
 203
 204       pi>>=shift;
 205       qi>>=shift;
 206       qexp+=shift-7*m;
 207
 208       pi=((pi*pi)>>16);
 209       qi=((qi*qi)>>16);
 210       qexp=qexp*2+m;
 211
 212       pi*=(1<<14)-wi;
 213       qi*=(1<<14)+wi;
 214       qi=(qi+pi)>>14;
 215
 216     }
 217
 218
 219     /* we've let the normalization drift because it wasn't important;
 220        however, for the lookup, things must be normalized again.  We
 221        need at most one right shift or a number of left shifts */
 222
 223     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
 224       qi>>=1; qexp++;
 225     }else
 226       while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
 227         qi<<=1; qexp--;
 228       }
 229
 230     amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
 231                             vorbis_invsqlook_i(qi,qexp)-
 232                                                       /*  m.8, m+n<=8 */
 233                             ampoffseti);              /*  8.12[0]     */
 234
 235     curve[i]*=amp;
 236     while(map[++i]==k)curve[i]*=amp;
 237   }
 238 }
 239
 240 #else
 241
 242 /* old, nonoptimized but simple version for any poor sap who needs to
 243    figure out what the hell this code does, or wants the other
 244    fraction of a dB precision */
 245
 246 /* side effect: changes *lsp to cosines of lsp */
 247 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 248                             float amp,float ampoffset){
 249   int i;
 250   float wdel=M_PI/ln;
 251   for(i=0;i<m;i++)lsp[i]=2.f*cos(lsp[i]);
 252
 253   i=0;
 254   while(i<n){
 255     int j,k=map[i];
 256     float p=.5f;
 257     float q=.5f;
 258     float w=2.f*cos(wdel*k);
 259     for(j=1;j<m;j+=2){
 260       q *= w-lsp[j-1];
 261       p *= w-lsp[j];
 262     }
 263     if(j==m){
 264       /* odd order filter; slightly assymetric */
 265       /* the last coefficient */
 266       q*=w-lsp[j-1];
 267       p*=p*(4.f-w*w);
 268       q*=q;
 269     }else{
 270       /* even order filter; still symmetric */
 271       p*=p*(2.f-w);
 272       q*=q*(2.f+w);
 273     }
 274
 275     q=fromdB(amp/sqrt(p+q)-ampoffset);
 276
 277     curve[i]*=q;
 278     while(map[++i]==k)curve[i]*=q;
 279   }
 280 }
 281
 282 #endif
 283 #endif
 284
 285 static void cheby(float *g, int ord) {
 286   int i, j;
 287
 288   g[0] *= .5f;
 289   for(i=2; i<= ord; i++) {
 290     for(j=ord; j >= i; j--) {
 291       g[j-2] -= g[j];
 292       g[j] += g[j];
 293     }
 294   }
 295 }
 296
 297 static int comp(const void *a,const void *b){
 298   return (*(float *)a<*(float *)b)-(*(float *)a>*(float *)b);
 299 }
 300
 301 /* Newton-Raphson-Maehly actually functioned as a decent root finder,
 302    but there are root sets for which it gets into limit cycles
 303    (exacerbated by zero suppression) and fails.  We can't afford to
 304    fail, even if the failure is 1 in 100,000,000, so we now use
 305    Laguerre and later polish with Newton-Raphson (which can then
 306    afford to fail) */
 307
 308 #define EPSILON 10e-7
 309 static int Laguerre_With_Deflation(float *a,int ord,float *r){
 310   int i,m;
 311   double *defl=alloca(sizeof(*defl)*(ord+1));
 312   for(i=0;i<=ord;i++)defl[i]=a[i];
 313
 314   for(m=ord;m>0;m--){
 315     double new=0.f,delta;
 316
 317     /* iterate a root */
 318     while(1){
 319       double p=defl[m],pp=0.f,ppp=0.f,denom;
 320
 321       /* eval the polynomial and its first two derivatives */
 322       for(i=m;i>0;i--){
 323         ppp = new*ppp + pp;
 324         pp  = new*pp  + p;
 325         p   = new*p   + defl[i-1];
 326       }
 327
 328       /* Laguerre's method */
 329       denom=(m-1) * ((m-1)*pp*pp - m*p*ppp);
 330       if(denom<0)
 331         return(-1);  /* complex root!  The LPC generator handed us a bad filter */
 332
 333       if(pp>0){
 334         denom = pp + sqrt(denom);
 335         if(denom<EPSILON)denom=EPSILON;
 336       }else{
 337         denom = pp - sqrt(denom);
 338         if(denom>-(EPSILON))denom=-(EPSILON);
 339       }
 340
 341       delta  = m*p/denom;
 342       new   -= delta;
 343
 344       if(delta<0.f)delta*=-1;
 345
 346       if(fabs(delta/new)<10e-12)break;
 347     }
 348
 349     r[m-1]=new;
 350
 351     /* forward deflation */
 352
 353     for(i=m;i>0;i--)
 354       defl[i-1]+=new*defl[i];
 355     defl++;
 356
 357   }
 358   return(0);
 359 }
 360
 361
 362 /* for spit-and-polish only */
 363 static int Newton_Raphson(float *a,int ord,float *r){
 364   int i, k, count=0;
 365   double error=1.f;
 366   double *root=alloca(ord*sizeof(*root));
 367
 368   for(i=0; i<ord;i++) root[i] = r[i];
 369
 370   while(error>1e-20){
 371     error=0;
 372
 373     for(i=0; i<ord; i++) { /* Update each point. */
 374       double pp=0.,delta;
 375       double rooti=root[i];
 376       double p=a[ord];
 377       for(k=ord-1; k>= 0; k--) {
 378
 379         pp= pp* rooti + p;
 380         p = p * rooti + a[k];
 381       }
 382
 383       delta = p/pp;
 384       root[i] -= delta;
 385       error+= delta*delta;
 386     }
 387
 388     if(count>40)return(-1);
 389
 390     count++;
 391   }
 392
 393   /* Replaced the original bubble sort with a real sort.  With your
 394      help, we can eliminate the bubble sort in our lifetime. --Monty */
 395
 396   for(i=0; i<ord;i++) r[i] = root[i];
 397   return(0);
 398 }
 399
 400
 401 /* Convert lpc coefficients to lsp coefficients */
 402 int vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
 403   int order2=(m+1)>>1;
 404   int g1_order,g2_order;
 405   float *g1=alloca(sizeof(*g1)*(order2+1));
 406   float *g2=alloca(sizeof(*g2)*(order2+1));
 407   float *g1r=alloca(sizeof(*g1r)*(order2+1));
 408   float *g2r=alloca(sizeof(*g2r)*(order2+1));
 409   int i;
 410
 411   /* even and odd are slightly different base cases */
 412   g1_order=(m+1)>>1;
 413   g2_order=(m)  >>1;
 414
 415   /* Compute the lengths of the x polynomials. */
 416   /* Compute the first half of K & R F1 & F2 polynomials. */
 417   /* Compute half of the symmetric and antisymmetric polynomials. */
 418   /* Remove the roots at +1 and -1. */
 419
 420   g1[g1_order] = 1.f;
 421   for(i=1;i<=g1_order;i++) g1[g1_order-i] = lpc[i-1]+lpc[m-i];
 422   g2[g2_order] = 1.f;
 423   for(i=1;i<=g2_order;i++) g2[g2_order-i] = lpc[i-1]-lpc[m-i];
 424
 425   if(g1_order>g2_order){
 426     for(i=2; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+2];
 427   }else{
 428     for(i=1; i<=g1_order;i++) g1[g1_order-i] -= g1[g1_order-i+1];
 429     for(i=1; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+1];
 430   }
 431
 432   /* Convert into polynomials in cos(alpha) */
 433   cheby(g1,g1_order);
 434   cheby(g2,g2_order);
 435
 436   /* Find the roots of the 2 even polynomials.*/
 437   if(Laguerre_With_Deflation(g1,g1_order,g1r) ||
 438      Laguerre_With_Deflation(g2,g2_order,g2r))
 439     return(-1);
 440
 441   Newton_Raphson(g1,g1_order,g1r); /* if it fails, it leaves g1r alone */
 442   Newton_Raphson(g2,g2_order,g2r); /* if it fails, it leaves g2r alone */
 443
 444   qsort(g1r,g1_order,sizeof(*g1r),comp);
 445   qsort(g2r,g2_order,sizeof(*g2r),comp);
 446
 447   for(i=0;i<g1_order;i++)
 448     lsp[i*2] = acos(g1r[i]);
 449
 450   for(i=0;i<g2_order;i++)
 451     lsp[i*2+1] = acos(g2r[i]);
 452   return(0);
 453 }