1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vorbis/lsp.c	Sat Feb 01 19:58:19 2014 +0200
     1.3 @@ -0,0 +1,454 @@
     1.4 +/********************************************************************
     1.5 + *                                                                  *
     1.6 + * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
     1.7 + * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
     1.8 + * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
     1.9 + * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
    1.10 + *                                                                  *
    1.11 + * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2009             *
    1.12 + * by the Xiph.Org Foundation http://www.xiph.org/                  *
    1.13 + *                                                                  *
    1.14 + ********************************************************************
    1.15 +
    1.16 +  function: LSP (also called LSF) conversion routines
    1.17 +  last mod: $Id: lsp.c 17538 2010-10-15 02:52:29Z tterribe $
    1.18 +
    1.19 +  The LSP generation code is taken (with minimal modification and a
    1.20 +  few bugfixes) from "On the Computation of the LSP Frequencies" by
    1.21 +  Joseph Rothweiler (see http://www.rothweiler.us for contact info).
    1.22 +  The paper is available at:
    1.23 +
    1.24 +  http://www.myown1.com/joe/lsf
    1.25 +
    1.26 + ********************************************************************/
    1.27 +
    1.28 +/* Note that the lpc-lsp conversion finds the roots of polynomial with
    1.29 +   an iterative root polisher (CACM algorithm 283).  It *is* possible
    1.30 +   to confuse this algorithm into not converging; that should only
    1.31 +   happen with absurdly closely spaced roots (very sharp peaks in the
    1.32 +   LPC f response) which in turn should be impossible in our use of
    1.33 +   the code.  If this *does* happen anyway, it's a bug in the floor
    1.34 +   finder; find the cause of the confusion (probably a single bin
    1.35 +   spike or accidental near-float-limit resolution problems) and
    1.36 +   correct it. */
    1.37 +
    1.38 +#include <math.h>
    1.39 +#include <string.h>
    1.40 +#include <stdlib.h>
    1.41 +#include "lsp.h"
    1.42 +#include "os.h"
    1.43 +#include "misc.h"
    1.44 +#include "lookup.h"
    1.45 +#include "scales.h"
    1.46 +
    1.47 +/* three possible LSP to f curve functions; the exact computation
    1.48 +   (float), a lookup based float implementation, and an integer
    1.49 +   implementation.  The float lookup is likely the optimal choice on
    1.50 +   any machine with an FPU.  The integer implementation is *not* fixed
    1.51 +   point (due to the need for a large dynamic range and thus a
    1.52 +   separately tracked exponent) and thus much more complex than the
    1.53 +   relatively simple float implementations. It's mostly for future
    1.54 +   work on a fully fixed point implementation for processors like the
    1.55 +   ARM family. */
    1.56 +
    1.57 +/* define either of these (preferably FLOAT_LOOKUP) to have faster
    1.58 +   but less precise implementation. */
    1.59 +#undef FLOAT_LOOKUP
    1.60 +#undef INT_LOOKUP
    1.61 +
    1.62 +#ifdef FLOAT_LOOKUP
    1.63 +#include "lookup.c" /* catch this in the build system; we #include for
    1.64 +                       compilers (like gcc) that can't inline across
    1.65 +                       modules */
    1.66 +
    1.67 +/* side effect: changes *lsp to cosines of lsp */
    1.68 +void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
    1.69 +                            float amp,float ampoffset){
    1.70 +  int i;
    1.71 +  float wdel=M_PI/ln;
    1.72 +  vorbis_fpu_control fpu;
    1.73 +
    1.74 +  vorbis_fpu_setround(&fpu);
    1.75 +  for(i=0;i<m;i++)lsp[i]=vorbis_coslook(lsp[i]);
    1.76 +
    1.77 +  i=0;
    1.78 +  while(i<n){
    1.79 +    int k=map[i];
    1.80 +    int qexp;
    1.81 +    float p=.7071067812f;
    1.82 +    float q=.7071067812f;
    1.83 +    float w=vorbis_coslook(wdel*k);
    1.84 +    float *ftmp=lsp;
    1.85 +    int c=m>>1;
    1.86 +
    1.87 +    while(c--){
    1.88 +      q*=ftmp[0]-w;
    1.89 +      p*=ftmp[1]-w;
    1.90 +      ftmp+=2;
    1.91 +    }
    1.92 +
    1.93 +    if(m&1){
    1.94 +      /* odd order filter; slightly assymetric */
    1.95 +      /* the last coefficient */
    1.96 +      q*=ftmp[0]-w;
    1.97 +      q*=q;
    1.98 +      p*=p*(1.f-w*w);
    1.99 +    }else{
   1.100 +      /* even order filter; still symmetric */
   1.101 +      q*=q*(1.f+w);
   1.102 +      p*=p*(1.f-w);
   1.103 +    }
   1.104 +
   1.105 +    q=frexp(p+q,&qexp);
   1.106 +    q=vorbis_fromdBlook(amp*
   1.107 +                        vorbis_invsqlook(q)*
   1.108 +                        vorbis_invsq2explook(qexp+m)-
   1.109 +                        ampoffset);
   1.110 +
   1.111 +    do{
   1.112 +      curve[i++]*=q;
   1.113 +    }while(map[i]==k);
   1.114 +  }
   1.115 +  vorbis_fpu_restore(fpu);
   1.116 +}
   1.117 +
   1.118 +#else
   1.119 +
   1.120 +#ifdef INT_LOOKUP
   1.121 +#include "lookup.c" /* catch this in the build system; we #include for
   1.122 +                       compilers (like gcc) that can't inline across
   1.123 +                       modules */
   1.124 +
   1.125 +static const int MLOOP_1[64]={
   1.126 +   0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
   1.127 +  14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
   1.128 +  15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
   1.129 +  15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
   1.130 +};
   1.131 +
   1.132 +static const int MLOOP_2[64]={
   1.133 +  0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
   1.134 +  8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
   1.135 +  9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
   1.136 +  9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
   1.137 +};
   1.138 +
   1.139 +static const int MLOOP_3[8]={0,1,2,2,3,3,3,3};
   1.140 +
   1.141 +
   1.142 +/* side effect: changes *lsp to cosines of lsp */
   1.143 +void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
   1.144 +                            float amp,float ampoffset){
   1.145 +
   1.146 +  /* 0 <= m < 256 */
   1.147 +
   1.148 +  /* set up for using all int later */
   1.149 +  int i;
   1.150 +  int ampoffseti=rint(ampoffset*4096.f);
   1.151 +  int ampi=rint(amp*16.f);
   1.152 +  long *ilsp=alloca(m*sizeof(*ilsp));
   1.153 +  for(i=0;i<m;i++)ilsp[i]=vorbis_coslook_i(lsp[i]/M_PI*65536.f+.5f);
   1.154 +
   1.155 +  i=0;
   1.156 +  while(i<n){
   1.157 +    int j,k=map[i];
   1.158 +    unsigned long pi=46341; /* 2**-.5 in 0.16 */
   1.159 +    unsigned long qi=46341;
   1.160 +    int qexp=0,shift;
   1.161 +    long wi=vorbis_coslook_i(k*65536/ln);
   1.162 +
   1.163 +    qi*=labs(ilsp[0]-wi);
   1.164 +    pi*=labs(ilsp[1]-wi);
   1.165 +
   1.166 +    for(j=3;j<m;j+=2){
   1.167 +      if(!(shift=MLOOP_1[(pi|qi)>>25]))
   1.168 +        if(!(shift=MLOOP_2[(pi|qi)>>19]))
   1.169 +          shift=MLOOP_3[(pi|qi)>>16];
   1.170 +      qi=(qi>>shift)*labs(ilsp[j-1]-wi);
   1.171 +      pi=(pi>>shift)*labs(ilsp[j]-wi);
   1.172 +      qexp+=shift;
   1.173 +    }
   1.174 +    if(!(shift=MLOOP_1[(pi|qi)>>25]))
   1.175 +      if(!(shift=MLOOP_2[(pi|qi)>>19]))
   1.176 +        shift=MLOOP_3[(pi|qi)>>16];
   1.177 +
   1.178 +    /* pi,qi normalized collectively, both tracked using qexp */
   1.179 +
   1.180 +    if(m&1){
   1.181 +      /* odd order filter; slightly assymetric */
   1.182 +      /* the last coefficient */
   1.183 +      qi=(qi>>shift)*labs(ilsp[j-1]-wi);
   1.184 +      pi=(pi>>shift)<<14;
   1.185 +      qexp+=shift;
   1.186 +
   1.187 +      if(!(shift=MLOOP_1[(pi|qi)>>25]))
   1.188 +        if(!(shift=MLOOP_2[(pi|qi)>>19]))
   1.189 +          shift=MLOOP_3[(pi|qi)>>16];
   1.190 +
   1.191 +      pi>>=shift;
   1.192 +      qi>>=shift;
   1.193 +      qexp+=shift-14*((m+1)>>1);
   1.194 +
   1.195 +      pi=((pi*pi)>>16);
   1.196 +      qi=((qi*qi)>>16);
   1.197 +      qexp=qexp*2+m;
   1.198 +
   1.199 +      pi*=(1<<14)-((wi*wi)>>14);
   1.200 +      qi+=pi>>14;
   1.201 +
   1.202 +    }else{
   1.203 +      /* even order filter; still symmetric */
   1.204 +
   1.205 +      /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
   1.206 +         worth tracking step by step */
   1.207 +
   1.208 +      pi>>=shift;
   1.209 +      qi>>=shift;
   1.210 +      qexp+=shift-7*m;
   1.211 +
   1.212 +      pi=((pi*pi)>>16);
   1.213 +      qi=((qi*qi)>>16);
   1.214 +      qexp=qexp*2+m;
   1.215 +
   1.216 +      pi*=(1<<14)-wi;
   1.217 +      qi*=(1<<14)+wi;
   1.218 +      qi=(qi+pi)>>14;
   1.219 +
   1.220 +    }
   1.221 +
   1.222 +
   1.223 +    /* we've let the normalization drift because it wasn't important;
   1.224 +       however, for the lookup, things must be normalized again.  We
   1.225 +       need at most one right shift or a number of left shifts */
   1.226 +
   1.227 +    if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
   1.228 +      qi>>=1; qexp++;
   1.229 +    }else
   1.230 +      while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
   1.231 +        qi<<=1; qexp--;
   1.232 +      }
   1.233 +
   1.234 +    amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
   1.235 +                            vorbis_invsqlook_i(qi,qexp)-
   1.236 +                                                      /*  m.8, m+n<=8 */
   1.237 +                            ampoffseti);              /*  8.12[0]     */
   1.238 +
   1.239 +    curve[i]*=amp;
   1.240 +    while(map[++i]==k)curve[i]*=amp;
   1.241 +  }
   1.242 +}
   1.243 +
   1.244 +#else
   1.245 +
   1.246 +/* old, nonoptimized but simple version for any poor sap who needs to
   1.247 +   figure out what the hell this code does, or wants the other
   1.248 +   fraction of a dB precision */
   1.249 +
   1.250 +/* side effect: changes *lsp to cosines of lsp */
   1.251 +void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
   1.252 +                            float amp,float ampoffset){
   1.253 +  int i;
   1.254 +  float wdel=M_PI/ln;
   1.255 +  for(i=0;i<m;i++)lsp[i]=2.f*cos(lsp[i]);
   1.256 +
   1.257 +  i=0;
   1.258 +  while(i<n){
   1.259 +    int j,k=map[i];
   1.260 +    float p=.5f;
   1.261 +    float q=.5f;
   1.262 +    float w=2.f*cos(wdel*k);
   1.263 +    for(j=1;j<m;j+=2){
   1.264 +      q *= w-lsp[j-1];
   1.265 +      p *= w-lsp[j];
   1.266 +    }
   1.267 +    if(j==m){
   1.268 +      /* odd order filter; slightly assymetric */
   1.269 +      /* the last coefficient */
   1.270 +      q*=w-lsp[j-1];
   1.271 +      p*=p*(4.f-w*w);
   1.272 +      q*=q;
   1.273 +    }else{
   1.274 +      /* even order filter; still symmetric */
   1.275 +      p*=p*(2.f-w);
   1.276 +      q*=q*(2.f+w);
   1.277 +    }
   1.278 +
   1.279 +    q=fromdB(amp/sqrt(p+q)-ampoffset);
   1.280 +
   1.281 +    curve[i]*=q;
   1.282 +    while(map[++i]==k)curve[i]*=q;
   1.283 +  }
   1.284 +}
   1.285 +
   1.286 +#endif
   1.287 +#endif
   1.288 +
   1.289 +static void cheby(float *g, int ord) {
   1.290 +  int i, j;
   1.291 +
   1.292 +  g[0] *= .5f;
   1.293 +  for(i=2; i<= ord; i++) {
   1.294 +    for(j=ord; j >= i; j--) {
   1.295 +      g[j-2] -= g[j];
   1.296 +      g[j] += g[j];
   1.297 +    }
   1.298 +  }
   1.299 +}
   1.300 +
   1.301 +static int comp(const void *a,const void *b){
   1.302 +  return (*(float *)a<*(float *)b)-(*(float *)a>*(float *)b);
   1.303 +}
   1.304 +
   1.305 +/* Newton-Raphson-Maehly actually functioned as a decent root finder,
   1.306 +   but there are root sets for which it gets into limit cycles
   1.307 +   (exacerbated by zero suppression) and fails.  We can't afford to
   1.308 +   fail, even if the failure is 1 in 100,000,000, so we now use
   1.309 +   Laguerre and later polish with Newton-Raphson (which can then
   1.310 +   afford to fail) */
   1.311 +
   1.312 +#define EPSILON 10e-7
   1.313 +static int Laguerre_With_Deflation(float *a,int ord,float *r){
   1.314 +  int i,m;
   1.315 +  double *defl=alloca(sizeof(*defl)*(ord+1));
   1.316 +  for(i=0;i<=ord;i++)defl[i]=a[i];
   1.317 +
   1.318 +  for(m=ord;m>0;m--){
   1.319 +    double new=0.f,delta;
   1.320 +
   1.321 +    /* iterate a root */
   1.322 +    while(1){
   1.323 +      double p=defl[m],pp=0.f,ppp=0.f,denom;
   1.324 +
   1.325 +      /* eval the polynomial and its first two derivatives */
   1.326 +      for(i=m;i>0;i--){
   1.327 +        ppp = new*ppp + pp;
   1.328 +        pp  = new*pp  + p;
   1.329 +        p   = new*p   + defl[i-1];
   1.330 +      }
   1.331 +
   1.332 +      /* Laguerre's method */
   1.333 +      denom=(m-1) * ((m-1)*pp*pp - m*p*ppp);
   1.334 +      if(denom<0)
   1.335 +        return(-1);  /* complex root!  The LPC generator handed us a bad filter */
   1.336 +
   1.337 +      if(pp>0){
   1.338 +        denom = pp + sqrt(denom);
   1.339 +        if(denom<EPSILON)denom=EPSILON;
   1.340 +      }else{
   1.341 +        denom = pp - sqrt(denom);
   1.342 +        if(denom>-(EPSILON))denom=-(EPSILON);
   1.343 +      }
   1.344 +
   1.345 +      delta  = m*p/denom;
   1.346 +      new   -= delta;
   1.347 +
   1.348 +      if(delta<0.f)delta*=-1;
   1.349 +
   1.350 +      if(fabs(delta/new)<10e-12)break;
   1.351 +    }
   1.352 +
   1.353 +    r[m-1]=new;
   1.354 +
   1.355 +    /* forward deflation */
   1.356 +
   1.357 +    for(i=m;i>0;i--)
   1.358 +      defl[i-1]+=new*defl[i];
   1.359 +    defl++;
   1.360 +
   1.361 +  }
   1.362 +  return(0);
   1.363 +}
   1.364 +
   1.365 +
   1.366 +/* for spit-and-polish only */
   1.367 +static int Newton_Raphson(float *a,int ord,float *r){
   1.368 +  int i, k, count=0;
   1.369 +  double error=1.f;
   1.370 +  double *root=alloca(ord*sizeof(*root));
   1.371 +
   1.372 +  for(i=0; i<ord;i++) root[i] = r[i];
   1.373 +
   1.374 +  while(error>1e-20){
   1.375 +    error=0;
   1.376 +
   1.377 +    for(i=0; i<ord; i++) { /* Update each point. */
   1.378 +      double pp=0.,delta;
   1.379 +      double rooti=root[i];
   1.380 +      double p=a[ord];
   1.381 +      for(k=ord-1; k>= 0; k--) {
   1.382 +
   1.383 +        pp= pp* rooti + p;
   1.384 +        p = p * rooti + a[k];
   1.385 +      }
   1.386 +
   1.387 +      delta = p/pp;
   1.388 +      root[i] -= delta;
   1.389 +      error+= delta*delta;
   1.390 +    }
   1.391 +
   1.392 +    if(count>40)return(-1);
   1.393 +
   1.394 +    count++;
   1.395 +  }
   1.396 +
   1.397 +  /* Replaced the original bubble sort with a real sort.  With your
   1.398 +     help, we can eliminate the bubble sort in our lifetime. --Monty */
   1.399 +
   1.400 +  for(i=0; i<ord;i++) r[i] = root[i];
   1.401 +  return(0);
   1.402 +}
   1.403 +
   1.404 +
   1.405 +/* Convert lpc coefficients to lsp coefficients */
   1.406 +int vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
   1.407 +  int order2=(m+1)>>1;
   1.408 +  int g1_order,g2_order;
   1.409 +  float *g1=alloca(sizeof(*g1)*(order2+1));
   1.410 +  float *g2=alloca(sizeof(*g2)*(order2+1));
   1.411 +  float *g1r=alloca(sizeof(*g1r)*(order2+1));
   1.412 +  float *g2r=alloca(sizeof(*g2r)*(order2+1));
   1.413 +  int i;
   1.414 +
   1.415 +  /* even and odd are slightly different base cases */
   1.416 +  g1_order=(m+1)>>1;
   1.417 +  g2_order=(m)  >>1;
   1.418 +
   1.419 +  /* Compute the lengths of the x polynomials. */
   1.420 +  /* Compute the first half of K & R F1 & F2 polynomials. */
   1.421 +  /* Compute half of the symmetric and antisymmetric polynomials. */
   1.422 +  /* Remove the roots at +1 and -1. */
   1.423 +
   1.424 +  g1[g1_order] = 1.f;
   1.425 +  for(i=1;i<=g1_order;i++) g1[g1_order-i] = lpc[i-1]+lpc[m-i];
   1.426 +  g2[g2_order] = 1.f;
   1.427 +  for(i=1;i<=g2_order;i++) g2[g2_order-i] = lpc[i-1]-lpc[m-i];
   1.428 +
   1.429 +  if(g1_order>g2_order){
   1.430 +    for(i=2; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+2];
   1.431 +  }else{
   1.432 +    for(i=1; i<=g1_order;i++) g1[g1_order-i] -= g1[g1_order-i+1];
   1.433 +    for(i=1; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+1];
   1.434 +  }
   1.435 +
   1.436 +  /* Convert into polynomials in cos(alpha) */
   1.437 +  cheby(g1,g1_order);
   1.438 +  cheby(g2,g2_order);
   1.439 +
   1.440 +  /* Find the roots of the 2 even polynomials.*/
   1.441 +  if(Laguerre_With_Deflation(g1,g1_order,g1r) ||
   1.442 +     Laguerre_With_Deflation(g2,g2_order,g2r))
   1.443 +    return(-1);
   1.444 +
   1.445 +  Newton_Raphson(g1,g1_order,g1r); /* if it fails, it leaves g1r alone */
   1.446 +  Newton_Raphson(g2,g2_order,g2r); /* if it fails, it leaves g2r alone */
   1.447 +
   1.448 +  qsort(g1r,g1_order,sizeof(*g1r),comp);
   1.449 +  qsort(g2r,g2_order,sizeof(*g2r),comp);
   1.450 +
   1.451 +  for(i=0;i<g1_order;i++)
   1.452 +    lsp[i*2] = acos(g1r[i]);
   1.453 +
   1.454 +  for(i=0;i<g2_order;i++)
   1.455 +    lsp[i*2+1] = acos(g2r[i]);
   1.456 +  return(0);
   1.457 +}