X-Git-Url: http://review.tizen.org/git/?a=blobdiff_plain;f=lib%2Flsp.c;h=85880545159506a892dfcf80c0188f23f1ac4ab9;hb=a9eb99a5bd6f2d7da02d6cd13a428baf3a1bf48c;hp=0448de76b3e59b9d4f630bc0cb77b7694ea7c5d9;hpb=117b96ee4431075adc24c09afd0b6251c8ababd6;p=platform%2Fupstream%2Flibvorbis.git

diff --git a/lib/lsp.c b/lib/lsp.c
index 0448de7..8588054 100644
--- a/lib/lsp.c
+++ b/lib/lsp.c
@@ -1,165 +1,453 @@
 /********************************************************************
  *                                                                  *
- * THIS FILE IS PART OF THE Ogg Vorbis SOFTWARE CODEC SOURCE CODE.  *
- * USE, DISTRIBUTION AND REPRODUCTION OF THIS SOURCE IS GOVERNED BY *
- * THE GNU PUBLIC LICENSE 2, WHICH IS INCLUDED WITH THIS SOURCE.    *
- * PLEASE READ THESE TERMS DISTRIBUTING.                            *
+ * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
+ * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
+ * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
+ * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
  *                                                                  *
- * THE OggSQUISH SOURCE CODE IS (C) COPYRIGHT 1994-1999             *
- * by 1999 Monty <monty@xiph.org> and The XIPHOPHORUS Company       *
- * http://www.xiph.org/                                             *
+ * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2009             *
+ * by the Xiph.Org Foundation http://www.xiph.org/                  *
  *                                                                  *
  ********************************************************************
 
   function: LSP (also called LSF) conversion routines
-  author: Monty <monty@xiph.org>
-  modifications by: Monty
-  last modification date: Aug 03 1999
 
-  The LSP generation code is taken (with minimal modification) from
-  "On the Computation of the LSP Frequencies" by Joseph Rothweiler
-  <rothwlr@altavista.net>, available at:
-  
-  http://www2.xtdl.com/~rothwlr/lsfpaper/lsfpage.html 
+  The LSP generation code is taken (with minimal modification and a
+  few bugfixes) from "On the Computation of the LSP Frequencies" by
+  Joseph Rothweiler (see http://www.rothweiler.us for contact info).
+  The paper is available at:
+
+  http://www.myown1.com/joe/lsf
 
  ********************************************************************/
 
+/* Note that the lpc-lsp conversion finds the roots of polynomial with
+   an iterative root polisher (CACM algorithm 283).  It *is* possible
+   to confuse this algorithm into not converging; that should only
+   happen with absurdly closely spaced roots (very sharp peaks in the
+   LPC f response) which in turn should be impossible in our use of
+   the code.  If this *does* happen anyway, it's a bug in the floor
+   finder; find the cause of the confusion (probably a single bin
+   spike or accidental near-float-limit resolution problems) and
+   correct it. */
+
 #include <math.h>
 #include <string.h>
 #include <stdlib.h>
+#include "lsp.h"
+#include "os.h"
+#include "misc.h"
+#include "lookup.h"
+#include "scales.h"
 
-void vorbis_lsp_to_lpc(double *lsp,double *lpc,int m){ 
-  int i,j,m2=m/2;
-  double O[m2],E[m2];
-  double A,Ae[m2+1],Ao[m2+1];
-  double B,Be[m2],Bo[m2];
-  double temp;
-
-  /* even/odd roots setup */
-  for(i=0;i<m2;i++){
-    O[i]=-2.*cos(lsp[i*2]);
-    E[i]=-2.*cos(lsp[i*2+1]);
-  }
+/* three possible LSP to f curve functions; the exact computation
+   (float), a lookup based float implementation, and an integer
+   implementation.  The float lookup is likely the optimal choice on
+   any machine with an FPU.  The integer implementation is *not* fixed
+   point (due to the need for a large dynamic range and thus a
+   separately tracked exponent) and thus much more complex than the
+   relatively simple float implementations. It's mostly for future
+   work on a fully fixed point implementation for processors like the
+   ARM family. */
+
+/* define either of these (preferably FLOAT_LOOKUP) to have faster
+   but less precise implementation. */
+#undef FLOAT_LOOKUP
+#undef INT_LOOKUP
+
+#ifdef FLOAT_LOOKUP
+#include "lookup.c" /* catch this in the build system; we #include for
+                       compilers (like gcc) that can't inline across
+                       modules */
+
+/* side effect: changes *lsp to cosines of lsp */
+void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
+                            float amp,float ampoffset){
+  int i;
+  float wdel=M_PI/ln;
+  vorbis_fpu_control fpu;
+
+  vorbis_fpu_setround(&fpu);
+  for(i=0;i<m;i++)lsp[i]=vorbis_coslook(lsp[i]);
+
+  i=0;
+  while(i<n){
+    int k=map[i];
+    int qexp;
+    float p=.7071067812f;
+    float q=.7071067812f;
+    float w=vorbis_coslook(wdel*k);
+    float *ftmp=lsp;
+    int c=m>>1;
+
+    while(c--){
+      q*=ftmp[0]-w;
+      p*=ftmp[1]-w;
+      ftmp+=2;
+    }
+
+    if(m&1){
+      /* odd order filter; slightly assymetric */
+      /* the last coefficient */
+      q*=ftmp[0]-w;
+      q*=q;
+      p*=p*(1.f-w*w);
+    }else{
+      /* even order filter; still symmetric */
+      q*=q*(1.f+w);
+      p*=p*(1.f-w);
+    }
 
-  /* set up impulse response */
-  for(j=0;j<m2;j++){
-    Ae[j]=0.;
-    Ao[j]=1.;
-    Be[j]=0.;
-    Bo[j]=1.;
+    q=frexp(p+q,&qexp);
+    q=vorbis_fromdBlook(amp*
+                        vorbis_invsqlook(q)*
+                        vorbis_invsq2explook(qexp+m)-
+                        ampoffset);
+
+    do{
+      curve[i++]*=q;
+    }while(map[i]==k);
   }
-  Ao[j]=1.;
-  Ae[j]=1.;
-
-  /* run impulse response */
-  for(i=1;i<m+1;i++){
-    A=B=0.;
-    for(j=0;j<m2;j++){
-      temp=O[j]*Ao[j]+Ae[j];
-      Ae[j]=Ao[j];
-      Ao[j]=A;
-      A+=temp;
-
-      temp=E[j]*Bo[j]+Be[j];
-      Be[j]=Bo[j];
-      Bo[j]=B;
-      B+=temp;
+  vorbis_fpu_restore(fpu);
+}
+
+#else
+
+#ifdef INT_LOOKUP
+#include "lookup.c" /* catch this in the build system; we #include for
+                       compilers (like gcc) that can't inline across
+                       modules */
+
+static const int MLOOP_1[64]={
+   0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
+  14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
+  15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
+  15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
+};
+
+static const int MLOOP_2[64]={
+  0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
+  8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
+  9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
+  9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
+};
+
+static const int MLOOP_3[8]={0,1,2,2,3,3,3,3};
+
+
+/* side effect: changes *lsp to cosines of lsp */
+void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
+                            float amp,float ampoffset){
+
+  /* 0 <= m < 256 */
+
+  /* set up for using all int later */
+  int i;
+  int ampoffseti=rint(ampoffset*4096.f);
+  int ampi=rint(amp*16.f);
+  long *ilsp=alloca(m*sizeof(*ilsp));
+  for(i=0;i<m;i++)ilsp[i]=vorbis_coslook_i(lsp[i]/M_PI*65536.f+.5f);
+
+  i=0;
+  while(i<n){
+    int j,k=map[i];
+    unsigned long pi=46341; /* 2**-.5 in 0.16 */
+    unsigned long qi=46341;
+    int qexp=0,shift;
+    long wi=vorbis_coslook_i(k*65536/ln);
+
+    qi*=labs(ilsp[0]-wi);
+    pi*=labs(ilsp[1]-wi);
+
+    for(j=3;j<m;j+=2){
+      if(!(shift=MLOOP_1[(pi|qi)>>25]))
+        if(!(shift=MLOOP_2[(pi|qi)>>19]))
+          shift=MLOOP_3[(pi|qi)>>16];
+      qi=(qi>>shift)*labs(ilsp[j-1]-wi);
+      pi=(pi>>shift)*labs(ilsp[j]-wi);
+      qexp+=shift;
+    }
+    if(!(shift=MLOOP_1[(pi|qi)>>25]))
+      if(!(shift=MLOOP_2[(pi|qi)>>19]))
+        shift=MLOOP_3[(pi|qi)>>16];
+
+    /* pi,qi normalized collectively, both tracked using qexp */
+
+    if(m&1){
+      /* odd order filter; slightly assymetric */
+      /* the last coefficient */
+      qi=(qi>>shift)*labs(ilsp[j-1]-wi);
+      pi=(pi>>shift)<<14;
+      qexp+=shift;
+
+      if(!(shift=MLOOP_1[(pi|qi)>>25]))
+        if(!(shift=MLOOP_2[(pi|qi)>>19]))
+          shift=MLOOP_3[(pi|qi)>>16];
+
+      pi>>=shift;
+      qi>>=shift;
+      qexp+=shift-14*((m+1)>>1);
+
+      pi=((pi*pi)>>16);
+      qi=((qi*qi)>>16);
+      qexp=qexp*2+m;
+
+      pi*=(1<<14)-((wi*wi)>>14);
+      qi+=pi>>14;
+
+    }else{
+      /* even order filter; still symmetric */
+
+      /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
+         worth tracking step by step */
+
+      pi>>=shift;
+      qi>>=shift;
+      qexp+=shift-7*m;
+
+      pi=((pi*pi)>>16);
+      qi=((qi*qi)>>16);
+      qexp=qexp*2+m;
+
+      pi*=(1<<14)-wi;
+      qi*=(1<<14)+wi;
+      qi=(qi+pi)>>14;
+
     }
-    lpc[i-1]=(A+Ao[j]+B-Ae[j])/2;
-    Ao[j]=A;
-    Ae[j]=B;
+
+
+    /* we've let the normalization drift because it wasn't important;
+       however, for the lookup, things must be normalized again.  We
+       need at most one right shift or a number of left shifts */
+
+    if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
+      qi>>=1; qexp++;
+    }else
+      while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
+        qi<<=1; qexp--;
+      }
+
+    amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
+                            vorbis_invsqlook_i(qi,qexp)-
+                                                      /*  m.8, m+n<=8 */
+                            ampoffseti);              /*  8.12[0]     */
+
+    curve[i]*=amp;
+    while(map[++i]==k)curve[i]*=amp;
   }
 }
 
-static void kw(double *r,int n) {
-  double s[n/2+1];
-  double c[n+1];
-  int i, j, k;
-  
-  s[0] = 1.0;
-  s[1] = -2.0;
-  s[2] = 2.0;
-  for(i=3;i<=n/2;i++) s[i] = s[i-2];
-  
-  for(k=0;k<=n;k++) {
-    c[k] = r[k];
-    j = 1;
-    for(i=k+2;i<=n;i+=2) {
-      c[k] += s[j]*r[i];
-      s[j] -= s[j-1];
-      j++;
+#else
+
+/* old, nonoptimized but simple version for any poor sap who needs to
+   figure out what the hell this code does, or wants the other
+   fraction of a dB precision */
+
+/* side effect: changes *lsp to cosines of lsp */
+void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
+                            float amp,float ampoffset){
+  int i;
+  float wdel=M_PI/ln;
+  for(i=0;i<m;i++)lsp[i]=2.f*cos(lsp[i]);
+
+  i=0;
+  while(i<n){
+    int j,k=map[i];
+    float p=.5f;
+    float q=.5f;
+    float w=2.f*cos(wdel*k);
+    for(j=1;j<m;j+=2){
+      q *= w-lsp[j-1];
+      p *= w-lsp[j];
+    }
+    if(j==m){
+      /* odd order filter; slightly assymetric */
+      /* the last coefficient */
+      q*=w-lsp[j-1];
+      p*=p*(4.f-w*w);
+      q*=q;
+    }else{
+      /* even order filter; still symmetric */
+      p*=p*(2.f-w);
+      q*=q*(2.f+w);
     }
+
+    q=fromdB(amp/sqrt(p+q)-ampoffset);
+
+    curve[i]*=q;
+    while(map[++i]==k)curve[i]*=q;
   }
-  for(k=0;k<=n;k++) r[k] = c[k];
 }
 
+#endif
+#endif
+
+static void cheby(float *g, int ord) {
+  int i, j;
+
+  g[0] *= .5f;
+  for(i=2; i<= ord; i++) {
+    for(j=ord; j >= i; j--) {
+      g[j-2] -= g[j];
+      g[j] += g[j];
+    }
+  }
+}
 
 static int comp(const void *a,const void *b){
-  return(*(double *)a<*(double *)b);
+  return (*(float *)a<*(float *)b)-(*(float *)a>*(float *)b);
+}
+
+/* Newton-Raphson-Maehly actually functioned as a decent root finder,
+   but there are root sets for which it gets into limit cycles
+   (exacerbated by zero suppression) and fails.  We can't afford to
+   fail, even if the failure is 1 in 100,000,000, so we now use
+   Laguerre and later polish with Newton-Raphson (which can then
+   afford to fail) */
+
+#define EPSILON 10e-7
+static int Laguerre_With_Deflation(float *a,int ord,float *r){
+  int i,m;
+  double *defl=alloca(sizeof(*defl)*(ord+1));
+  for(i=0;i<=ord;i++)defl[i]=a[i];
+
+  for(m=ord;m>0;m--){
+    double new=0.f,delta;
+
+    /* iterate a root */
+    while(1){
+      double p=defl[m],pp=0.f,ppp=0.f,denom;
+
+      /* eval the polynomial and its first two derivatives */
+      for(i=m;i>0;i--){
+        ppp = new*ppp + pp;
+        pp  = new*pp  + p;
+        p   = new*p   + defl[i-1];
+      }
+
+      /* Laguerre's method */
+      denom=(m-1) * ((m-1)*pp*pp - m*p*ppp);
+      if(denom<0)
+        return(-1);  /* complex root!  The LPC generator handed us a bad filter */
+
+      if(pp>0){
+        denom = pp + sqrt(denom);
+        if(denom<EPSILON)denom=EPSILON;
+      }else{
+        denom = pp - sqrt(denom);
+        if(denom>-(EPSILON))denom=-(EPSILON);
+      }
+
+      delta  = m*p/denom;
+      new   -= delta;
+
+      if(delta<0.f)delta*=-1;
+
+      if(fabs(delta/new)<10e-12)break;
+    }
+
+    r[m-1]=new;
+
+    /* forward deflation */
+
+    for(i=m;i>0;i--)
+      defl[i-1]+=new*defl[i];
+    defl++;
+
+  }
+  return(0);
 }
 
-/* CACM algorithm 283. */
-static void cacm283(double *a,int ord,double *r){
-  int i, k;
-  double val, p, delta, error;
-  double rooti;
-
-  for(i=0; i<ord;i++) r[i] = 2.0 * (i+0.5) / ord - 1.0;
-  
-  for(error=1 ; error > 1.e-12; ) {
-    error = 0;
-    for( i=0; i<ord; i++) {  /* Update each point. */
-      rooti = r[i];
-      val = a[ord];
-      p = a[ord];
+
+/* for spit-and-polish only */
+static int Newton_Raphson(float *a,int ord,float *r){
+  int i, k, count=0;
+  double error=1.f;
+  double *root=alloca(ord*sizeof(*root));
+
+  for(i=0; i<ord;i++) root[i] = r[i];
+
+  while(error>1e-20){
+    error=0;
+
+    for(i=0; i<ord; i++) { /* Update each point. */
+      double pp=0.,delta;
+      double rooti=root[i];
+      double p=a[ord];
       for(k=ord-1; k>= 0; k--) {
-	val = val * rooti + a[k];
-	if (k != i) p *= rooti - r[k];
+
+        pp= pp* rooti + p;
+        p = p * rooti + a[k];
       }
-      delta = val/p;
-      r[i] -= delta;
-      error += delta*delta;
+
+      delta = p/pp;
+      root[i] -= delta;
+      error+= delta*delta;
     }
+
+    if(count>40)return(-1);
+
+    count++;
   }
-  
+
   /* Replaced the original bubble sort with a real sort.  With your
      help, we can eliminate the bubble sort in our lifetime. --Monty */
-  
-  qsort(r,ord,sizeof(double),comp);
 
+  for(i=0; i<ord;i++) r[i] = root[i];
+  return(0);
 }
 
+
 /* Convert lpc coefficients to lsp coefficients */
-void vorbis_lpc_to_lsp(double *lpc,double *lsp,int m){
-  int order2=m/2;
-  double g1[order2+1], g2[order2+1];
-  double g1r[order2+1], g2r[order2+1];
+int vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
+  int order2=(m+1)>>1;
+  int g1_order,g2_order;
+  float *g1=alloca(sizeof(*g1)*(order2+1));
+  float *g2=alloca(sizeof(*g2)*(order2+1));
+  float *g1r=alloca(sizeof(*g1r)*(order2+1));
+  float *g2r=alloca(sizeof(*g2r)*(order2+1));
   int i;
 
+  /* even and odd are slightly different base cases */
+  g1_order=(m+1)>>1;
+  g2_order=(m)  >>1;
+
   /* Compute the lengths of the x polynomials. */
   /* Compute the first half of K & R F1 & F2 polynomials. */
   /* Compute half of the symmetric and antisymmetric polynomials. */
   /* Remove the roots at +1 and -1. */
-  
-  g1[order2] = 1.0;
-  for(i=0;i<order2;i++) g1[order2-i-1] = lpc[i]+lpc[m-i-1];
-  g2[order2] = 1.0;
-  for(i=0;i<order2;i++) g2[order2-i-1] = lpc[i]-lpc[m-i-1];
-  
-  for(i=0; i<order2;i++) g1[order2-i-1] -= g1[order2-i];
-  for(i=0; i<order2;i++) g2[order2-i-1] += g2[order2-i];
+
+  g1[g1_order] = 1.f;
+  for(i=1;i<=g1_order;i++) g1[g1_order-i] = lpc[i-1]+lpc[m-i];
+  g2[g2_order] = 1.f;
+  for(i=1;i<=g2_order;i++) g2[g2_order-i] = lpc[i-1]-lpc[m-i];
+
+  if(g1_order>g2_order){
+    for(i=2; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+2];
+  }else{
+    for(i=1; i<=g1_order;i++) g1[g1_order-i] -= g1[g1_order-i+1];
+    for(i=1; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+1];
+  }
 
   /* Convert into polynomials in cos(alpha) */
-  kw(g1,order2);
-  kw(g2,order2);
+  cheby(g1,g1_order);
+  cheby(g2,g2_order);
 
   /* Find the roots of the 2 even polynomials.*/
-  
-  cacm283(g1,order2,g1r);
-  cacm283(g2,order2,g2r);
-  
-  for(i=0;i<m;i+=2){
-    lsp[i] = acos(g1r[i/2]*.5);
-    lsp[i+1] = acos(g2r[i/2]*.5);
-  }
+  if(Laguerre_With_Deflation(g1,g1_order,g1r) ||
+     Laguerre_With_Deflation(g2,g2_order,g2r))
+    return(-1);
+
+  Newton_Raphson(g1,g1_order,g1r); /* if it fails, it leaves g1r alone */
+  Newton_Raphson(g2,g2_order,g2r); /* if it fails, it leaves g2r alone */
+
+  qsort(g1r,g1_order,sizeof(*g1r),comp);
+  qsort(g2r,g2_order,sizeof(*g2r),comp);
+
+  for(i=0;i<g1_order;i++)
+    lsp[i*2] = acos(g1r[i]);
+
+  for(i=0;i<g2_order;i++)
+    lsp[i*2+1] = acos(g2r[i]);
+  return(0);
 }