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