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