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.12 2000/11/06 00:07:01 xiphmont Exp $
  16
  17   The LSP generation code is taken (with minimal modification) from
  18   "On the Computation of the LSP Frequencies" by Joseph Rothweiler
  19   <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 #define  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=.7071067812;
  78     float q=.7071067812;
  79     float w=vorbis_coslook(wdel*k);
  80     float *ftmp=lsp;
  81     int c=m>>1;
  82
  83     do{
  84       p*=ftmp[0]-w;
  85       q*=ftmp[1]-w;
  86       ftmp+=2;
  87     }while(--c);
  88
  89     q=frexp(p*p*(1.+w)+q*q*(1.-w),&qexp);
  90     q=vorbis_fromdBlook(amp*
  91                         vorbis_invsqlook(q)*
  92                         vorbis_invsq2explook(qexp+m)-
  93                         ampoffset);
  94
  95     do{
  96       curve[i++]=q;
  97     }while(map[i]==k);
  98   }
  99   vorbis_fpu_restore(fpu);
 100 }
 101
 102 #else
 103
 104 #ifdef INT_LOOKUP
 105 #include "lookup.c" /* catch this in the build system; we #include for
 106                        compilers (like gcc) that can't inline across
 107                        modules */
 108
 109 static int MLOOP_1[64]={
 110    0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
 111   14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
 112   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
 113   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
 114 };
 115
 116 static int MLOOP_2[64]={
 117   0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
 118   8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
 119   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
 120   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
 121 };
 122
 123 static int MLOOP_3[8]={0,1,2,2,3,3,3,3};
 124
 125
 126 /* side effect: changes *lsp to cosines of lsp */
 127 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 128                             float amp,float ampoffset){
 129
 130   /* 0 <= m < 256 */
 131
 132   /* set up for using all int later */
 133   int i;
 134   int ampoffseti=rint(ampoffset*4096.);
 135   int ampi=rint(amp*16.);
 136   long *ilsp=alloca(m*sizeof(long));
 137   for(i=0;i<m;i++)ilsp[i]=vorbis_coslook_i(lsp[i]/M_PI*65536.+.5);
 138
 139   i=0;
 140   while(i<n){
 141     int j,k=map[i];
 142     unsigned long pi=46341; /* 2**-.5 in 0.16 */
 143     unsigned long qi=46341;
 144     int qexp=0,shift;
 145     long wi=vorbis_coslook_i(k*65536/ln);
 146
 147     pi*=labs(ilsp[0]-wi);
 148     qi*=labs(ilsp[1]-wi);
 149
 150     for(j=2;j<m;j+=2){
 151       if(!(shift=MLOOP_1[(pi|qi)>>25]))
 152         if(!(shift=MLOOP_2[(pi|qi)>>19]))
 153           shift=MLOOP_3[(pi|qi)>>16];
 154       pi=(pi>>shift)*labs(ilsp[j]-wi);
 155       qi=(qi>>shift)*labs(ilsp[j+1]-wi);
 156       qexp+=shift;
 157     }
 158     if(!(shift=MLOOP_1[(pi|qi)>>25]))
 159       if(!(shift=MLOOP_2[(pi|qi)>>19]))
 160         shift=MLOOP_3[(pi|qi)>>16];
 161     pi>>=shift;
 162     qi>>=shift;
 163     qexp+=shift-7*m;
 164
 165     /* pi,qi normalized collectively, both tracked using qexp */
 166
 167     /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
 168        worth tracking step by step */
 169
 170     pi=((pi*pi)>>16);
 171     qi=((qi*qi)>>16);
 172     qexp=qexp*2+m;
 173
 174     qi*=(1<<14)-wi;
 175     pi*=(1<<14)+wi;
 176
 177     qi=(qi+pi)>>14;
 178
 179     /* we've let the normalization drift because it wasn't important;
 180        however, for the lookup, things must be normalized again.  We
 181        need at most one right shift or a number of left shifts */
 182
 183     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
 184       qi>>=1; qexp++;
 185     }else
 186       while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
 187         qi<<=1; qexp--;
 188       }
 189
 190     amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
 191                             vorbis_invsqlook_i(qi,qexp)-
 192                                                       /*  m.8, m+n<=8 */
 193                             ampoffseti);              /*  8.12[0]     */
 194
 195     curve[i]=amp;
 196     while(map[++i]==k)curve[i]=amp;
 197   }
 198 }
 199
 200 #else
 201
 202 /* old, nonoptimized but simple version for any poor sap who needs to
 203    figure out what the hell this code does, or wants the other tiny
 204    fraction of a dB precision */
 205
 206 /* side effect: changes *lsp to cosines of lsp */
 207 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
 208                             float amp,float ampoffset){
 209   int i;
 210   float wdel=M_PI/ln;
 211   for(i=0;i<m;i++)lsp[i]=2*cos(lsp[i]);
 212
 213   i=0;
 214   while(i<n){
 215     int j,k=map[i];
 216     float p=.5;
 217     float q=.5;
 218     float w=2*cos(wdel*k);
 219     for(j=0;j<m;j+=2){
 220       p *= w-lsp[j];
 221       q *= w-lsp[j+1];
 222     }
 223     p*=p*(2.+w);
 224     q*=q*(2.-w);
 225     q=fromdB(amp/sqrt(p+q)-ampoffset);
 226
 227     curve[i]=q;
 228     while(map[++i]==k)curve[i]=q;
 229   }
 230 }
 231
 232 #endif
 233 #endif
 234
 235 static void cheby(float *g, int ord) {
 236   int i, j;
 237
 238   g[0] *= 0.5;
 239   for(i=2; i<= ord; i++) {
 240     for(j=ord; j >= i; j--) {
 241       g[j-2] -= g[j];
 242       g[j] += g[j];
 243     }
 244   }
 245 }
 246
 247 static int comp(const void *a,const void *b){
 248   if(*(float *)a<*(float *)b)
 249     return(1);
 250   else
 251     return(-1);
 252 }
 253
 254 /* This is one of those 'mathemeticians should not write code' kind of
 255    cases.  Newton's method of polishing roots is straightforward
 256    enough... except in those cases where it just fails in the real
 257    world.  In our case below, we're worried about a local mini/maxima
 258    shooting a root estimation off to infinity, or the new estimation
 259    chaotically oscillating about convergence (shouldn't actually be a
 260    problem in our usage.
 261
 262    Maehly's modification (zero suppression, to prevent two tenative
 263    roots from collapsing to the same actual root) similarly can
 264    temporarily shoot a root off toward infinity.  It would come
 265    back... if it were not for the fact that machine representation has
 266    limited dynamic range and resolution.  This too is guarded by
 267    limiting delta.
 268
 269    Last problem is convergence criteria; we don't know what a 'double'
 270    is on our hardware/compiler, and the convergence limit is bounded
 271    by roundoff noise.  So, we hack convergence:
 272
 273    Require at most 1e-6 mean squared error for all zeroes.  When
 274    converging, start the clock ticking at 1e-6; limit our polishing to
 275    as many more iterations as took us to get this far, 100 max.
 276
 277    Past max iters, quit when MSE is no longer decreasing *or* we go
 278    below ~1e-20 MSE, whichever happens first. */
 279
 280 static void Newton_Raphson_Maehly(float *a,int ord,float *r){
 281   int i, k, count=0, maxiter=0;
 282   double error=1.,besterror=1.;
 283   double *root=alloca(ord*sizeof(double));
 284
 285   for(i=0; i<ord;i++) root[i] = 2.0 * (i+0.5) / ord - 1.0;
 286
 287   while(error>1.e-20){
 288     error=0;
 289
 290     for(i=0; i<ord; i++) { /* Update each point. */
 291       double ac=0.,pp=0.,delta;
 292       double rooti=root[i];
 293       double p=a[ord];
 294       for(k=ord-1; k>= 0; k--) {
 295
 296         pp= pp* rooti + p;
 297         p = p * rooti+ a[k];
 298         if (k != i) ac += 1./(rooti - root[k]);
 299       }
 300       ac=p*ac;
 301
 302       delta = p/(pp-ac);
 303
 304       /* don't allow the correction to scream off into infinity if we
 305          happened to polish right at a local mini/maximum */
 306
 307       if(delta<-3)delta=-3;
 308       if(delta>3.)delta=3.; /* 3 is not a random choice; it's large
 309                                enough to make sure the first pass
 310                                can't accidentally limit two poles to
 311                                the same value in a fatal nonelastic
 312                                collision.  */
 313
 314       root[i] -= delta;
 315       error += delta*delta;
 316     }
 317
 318     if(maxiter && count>maxiter && error>=besterror)break;
 319
 320     /* anything to help out the polisher; converge using doubles */
 321     if(!count || error<besterror){
 322       for(i=0; i<ord; i++) r[i]=root[i];
 323       besterror=error;
 324       if(error<1.e-6){ /* rough minimum criteria */
 325         maxiter=count*2+10;
 326         if(maxiter>100)maxiter=100;
 327       }
 328     }
 329
 330     count++;
 331   }
 332
 333   /* Replaced the original bubble sort with a real sort.  With your
 334      help, we can eliminate the bubble sort in our lifetime. --Monty */
 335
 336   qsort(r,ord,sizeof(float),comp);
 337
 338 }
 339
 340 /* Convert lpc coefficients to lsp coefficients */
 341 void vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
 342   int order2=m/2;
 343   float *g1=alloca(sizeof(float)*(order2+1));
 344   float *g2=alloca(sizeof(float)*(order2+1));
 345   float *g1r=alloca(sizeof(float)*(order2+1));
 346   float *g2r=alloca(sizeof(float)*(order2+1));
 347   int i;
 348
 349   /* Compute the lengths of the x polynomials. */
 350   /* Compute the first half of K & R F1 & F2 polynomials. */
 351   /* Compute half of the symmetric and antisymmetric polynomials. */
 352   /* Remove the roots at +1 and -1. */
 353
 354   g1[order2] = 1.0;
 355   for(i=0;i<order2;i++) g1[order2-i-1] = lpc[i]+lpc[m-i-1];
 356   g2[order2] = 1.0;
 357   for(i=0;i<order2;i++) g2[order2-i-1] = lpc[i]-lpc[m-i-1];
 358
 359   for(i=0; i<order2;i++) g1[order2-i-1] -= g1[order2-i];
 360   for(i=0; i<order2;i++) g2[order2-i-1] += g2[order2-i];
 361
 362   /* Convert into polynomials in cos(alpha) */
 363   cheby(g1,order2);
 364   cheby(g2,order2);
 365
 366   /* Find the roots of the 2 even polynomials.*/
 367
 368   Newton_Raphson_Maehly(g1,order2,g1r);
 369   Newton_Raphson_Maehly(g2,order2,g2r);
 370
 371   for(i=0;i<m;i+=2){
 372     lsp[i] = acos(g1r[i/2]);
 373     lsp[i+1] = acos(g2r[i/2]);
 374   }
 375 }