lib/lsp.c

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