| /******************************************************************** |
| * * |
| * 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 OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2009 * |
| * by the Xiph.Org Foundation http://www.xiph.org/ * |
| * * |
| ******************************************************************** |
| |
| function: LSP (also called LSF) conversion routines |
| last mod: $Id: lsp.c 16227 2009-07-08 06:58:46Z xiphmont $ |
| |
| 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" |
| |
| /* 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 |
| seperately 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; |
| |
| do{ |
| q*=ftmp[0]-w; |
| p*=ftmp[1]-w; |
| ftmp+=2; |
| }while(--c); |
| |
| 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); |
| } |
| |
| 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); |
| } |
| 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; |
| |
| } |
| |
| |
| /* 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; |
| } |
| } |
| |
| #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; |
| } |
| } |
| |
| #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 (*(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 lastdelta=0.f; |
| 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; |
| lastdelta=delta; |
| } |
| |
| r[m-1]=new; |
| |
| /* forward deflation */ |
| |
| for(i=m;i>0;i--) |
| defl[i-1]+=new*defl[i]; |
| defl++; |
| |
| } |
| return(0); |
| } |
| |
| |
| /* 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--) { |
| |
| pp= pp* rooti + p; |
| p = p * rooti + a[k]; |
| } |
| |
| 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 */ |
| |
| for(i=0; i<ord;i++) r[i] = root[i]; |
| return(0); |
| } |
| |
| |
| /* Convert lpc coefficients to lsp coefficients */ |
| 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[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) */ |
| cheby(g1,g1_order); |
| cheby(g2,g2_order); |
| |
| /* Find the roots of the 2 even polynomials.*/ |
| 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); |
| } |
