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