]> git.jsancho.org Git - lugaru.git/blob - libvorbis-1.0.1/lib/lsp.c
Link against non-SSE2 libGLU.
[lugaru.git] / libvorbis-1.0.1 / lib / lsp.c
1 /********************************************************************
2  *                                                                  *
3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
7  *                                                                  *
8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2002             *
9  * by the XIPHOPHORUS Company http://www.xiph.org/                  *
10  *                                                                  *
11  ********************************************************************
12
13   function: LSP (also called LSF) conversion routines
14   last mod: $Id: lsp.c,v 1.24 2002/10/16 07:44:21 xiphmont Exp $
15
16   The LSP generation code is taken (with minimal modification and a
17   few bugfixes) from "On the Computation of the LSP Frequencies" by
18   Joseph Rothweiler (see http://www.rothweiler.us for contact info).
19   The paper is available at:
20
21   http://www.myown1.com/joe/lsf
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   vorbis_fpu_control fpu;
69   
70   vorbis_fpu_setround(&fpu);
71   for(i=0;i<m;i++)lsp[i]=vorbis_coslook(lsp[i]);
72
73   i=0;
74   while(i<n){
75     int k=map[i];
76     int qexp;
77     float p=.7071067812f;
78     float q=.7071067812f;
79     float w=vorbis_coslook(wdel*k);
80     float *ftmp=lsp;
81     int c=m>>1;
82
83     do{
84       q*=ftmp[0]-w;
85       p*=ftmp[1]-w;
86       ftmp+=2;
87     }while(--c);
88
89     if(m&1){
90       /* odd order filter; slightly assymetric */
91       /* the last coefficient */
92       q*=ftmp[0]-w;
93       q*=q;
94       p*=p*(1.f-w*w);
95     }else{
96       /* even order filter; still symmetric */
97       q*=q*(1.f+w);
98       p*=p*(1.f-w);
99     }
100
101     q=frexp(p+q,&qexp);
102     q=vorbis_fromdBlook(amp*             
103                         vorbis_invsqlook(q)*
104                         vorbis_invsq2explook(qexp+m)- 
105                         ampoffset);
106
107     do{
108       curve[i++]*=q;
109     }while(map[i]==k);
110   }
111   vorbis_fpu_restore(fpu);
112 }
113
114 #else
115
116 #ifdef INT_LOOKUP
117 #include "lookup.c" /* catch this in the build system; we #include for
118                        compilers (like gcc) that can't inline across
119                        modules */
120
121 static int MLOOP_1[64]={
122    0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
123   14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
124   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
125   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
126 };
127
128 static int MLOOP_2[64]={
129   0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
130   8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
131   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
132   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
133 };
134
135 static int MLOOP_3[8]={0,1,2,2,3,3,3,3};
136
137
138 /* side effect: changes *lsp to cosines of lsp */
139 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
140                             float amp,float ampoffset){
141
142   /* 0 <= m < 256 */
143
144   /* set up for using all int later */
145   int i;
146   int ampoffseti=rint(ampoffset*4096.f);
147   int ampi=rint(amp*16.f);
148   long *ilsp=alloca(m*sizeof(*ilsp));
149   for(i=0;i<m;i++)ilsp[i]=vorbis_coslook_i(lsp[i]/M_PI*65536.f+.5f);
150
151   i=0;
152   while(i<n){
153     int j,k=map[i];
154     unsigned long pi=46341; /* 2**-.5 in 0.16 */
155     unsigned long qi=46341;
156     int qexp=0,shift;
157     long wi=vorbis_coslook_i(k*65536/ln);
158
159     qi*=labs(ilsp[0]-wi);
160     pi*=labs(ilsp[1]-wi);
161
162     for(j=3;j<m;j+=2){
163       if(!(shift=MLOOP_1[(pi|qi)>>25]))
164         if(!(shift=MLOOP_2[(pi|qi)>>19]))
165           shift=MLOOP_3[(pi|qi)>>16];
166       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
167       pi=(pi>>shift)*labs(ilsp[j]-wi);
168       qexp+=shift;
169     }
170     if(!(shift=MLOOP_1[(pi|qi)>>25]))
171       if(!(shift=MLOOP_2[(pi|qi)>>19]))
172         shift=MLOOP_3[(pi|qi)>>16];
173
174     /* pi,qi normalized collectively, both tracked using qexp */
175
176     if(m&1){
177       /* odd order filter; slightly assymetric */
178       /* the last coefficient */
179       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
180       pi=(pi>>shift)<<14;
181       qexp+=shift;
182
183       if(!(shift=MLOOP_1[(pi|qi)>>25]))
184         if(!(shift=MLOOP_2[(pi|qi)>>19]))
185           shift=MLOOP_3[(pi|qi)>>16];
186       
187       pi>>=shift;
188       qi>>=shift;
189       qexp+=shift-14*((m+1)>>1);
190
191       pi=((pi*pi)>>16);
192       qi=((qi*qi)>>16);
193       qexp=qexp*2+m;
194
195       pi*=(1<<14)-((wi*wi)>>14);
196       qi+=pi>>14;
197
198     }else{
199       /* even order filter; still symmetric */
200
201       /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
202          worth tracking step by step */
203       
204       pi>>=shift;
205       qi>>=shift;
206       qexp+=shift-7*m;
207
208       pi=((pi*pi)>>16);
209       qi=((qi*qi)>>16);
210       qexp=qexp*2+m;
211       
212       pi*=(1<<14)-wi;
213       qi*=(1<<14)+wi;
214       qi=(qi+pi)>>14;
215       
216     }
217     
218
219     /* we've let the normalization drift because it wasn't important;
220        however, for the lookup, things must be normalized again.  We
221        need at most one right shift or a number of left shifts */
222
223     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
224       qi>>=1; qexp++; 
225     }else
226       while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
227         qi<<=1; qexp--; 
228       }
229
230     amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
231                             vorbis_invsqlook_i(qi,qexp)- 
232                                                       /*  m.8, m+n<=8 */
233                             ampoffseti);              /*  8.12[0]     */
234
235     curve[i]*=amp;
236     while(map[++i]==k)curve[i]*=amp;
237   }
238 }
239
240 #else 
241
242 /* old, nonoptimized but simple version for any poor sap who needs to
243    figure out what the hell this code does, or wants the other
244    fraction of a dB precision */
245
246 /* side effect: changes *lsp to cosines of lsp */
247 void vorbis_lsp_to_curve(float *curve,int *map,int n,int ln,float *lsp,int m,
248                             float amp,float ampoffset){
249   int i;
250   float wdel=M_PI/ln;
251   for(i=0;i<m;i++)lsp[i]=2.f*cos(lsp[i]);
252
253   i=0;
254   while(i<n){
255     int j,k=map[i];
256     float p=.5f;
257     float q=.5f;
258     float w=2.f*cos(wdel*k);
259     for(j=1;j<m;j+=2){
260       q *= w-lsp[j-1];
261       p *= w-lsp[j];
262     }
263     if(j==m){
264       /* odd order filter; slightly assymetric */
265       /* the last coefficient */
266       q*=w-lsp[j-1];
267       p*=p*(4.f-w*w);
268       q*=q;
269     }else{
270       /* even order filter; still symmetric */
271       p*=p*(2.f-w);
272       q*=q*(2.f+w);
273     }
274
275     q=fromdB(amp/sqrt(p+q)-ampoffset);
276
277     curve[i]*=q;
278     while(map[++i]==k)curve[i]*=q;
279   }
280 }
281
282 #endif
283 #endif
284
285 static void cheby(float *g, int ord) {
286   int i, j;
287
288   g[0] *= .5f;
289   for(i=2; i<= ord; i++) {
290     for(j=ord; j >= i; j--) {
291       g[j-2] -= g[j];
292       g[j] += g[j]; 
293     }
294   }
295 }
296
297 static int comp(const void *a,const void *b){
298   return (*(float *)a<*(float *)b)-(*(float *)a>*(float *)b);
299 }
300
301 /* Newton-Raphson-Maehly actually functioned as a decent root finder,
302    but there are root sets for which it gets into limit cycles
303    (exacerbated by zero suppression) and fails.  We can't afford to
304    fail, even if the failure is 1 in 100,000,000, so we now use
305    Laguerre and later polish with Newton-Raphson (which can then
306    afford to fail) */
307
308 #define EPSILON 10e-7
309 static int Laguerre_With_Deflation(float *a,int ord,float *r){
310   int i,m;
311   double lastdelta=0.f;
312   double *defl=alloca(sizeof(*defl)*(ord+1));
313   for(i=0;i<=ord;i++)defl[i]=a[i];
314
315   for(m=ord;m>0;m--){
316     double new=0.f,delta;
317
318     /* iterate a root */
319     while(1){
320       double p=defl[m],pp=0.f,ppp=0.f,denom;
321       
322       /* eval the polynomial and its first two derivatives */
323       for(i=m;i>0;i--){
324         ppp = new*ppp + pp;
325         pp  = new*pp  + p;
326         p   = new*p   + defl[i-1];
327       }
328       
329       /* Laguerre's method */
330       denom=(m-1) * ((m-1)*pp*pp - m*p*ppp);
331       if(denom<0)
332         return(-1);  /* complex root!  The LPC generator handed us a bad filter */
333
334       if(pp>0){
335         denom = pp + sqrt(denom);
336         if(denom<EPSILON)denom=EPSILON;
337       }else{
338         denom = pp - sqrt(denom);
339         if(denom>-(EPSILON))denom=-(EPSILON);
340       }
341
342       delta  = m*p/denom;
343       new   -= delta;
344
345       if(delta<0.f)delta*=-1;
346
347       if(fabs(delta/new)<10e-12)break; 
348       lastdelta=delta;
349     }
350
351     r[m-1]=new;
352
353     /* forward deflation */
354     
355     for(i=m;i>0;i--)
356       defl[i-1]+=new*defl[i];
357     defl++;
358
359   }
360   return(0);
361 }
362
363
364 /* for spit-and-polish only */
365 static int Newton_Raphson(float *a,int ord,float *r){
366   int i, k, count=0;
367   double error=1.f;
368   double *root=alloca(ord*sizeof(*root));
369
370   for(i=0; i<ord;i++) root[i] = r[i];
371   
372   while(error>1e-20){
373     error=0;
374     
375     for(i=0; i<ord; i++) { /* Update each point. */
376       double pp=0.,delta;
377       double rooti=root[i];
378       double p=a[ord];
379       for(k=ord-1; k>= 0; k--) {
380
381         pp= pp* rooti + p;
382         p = p * rooti + a[k];
383       }
384
385       delta = p/pp;
386       root[i] -= delta;
387       error+= delta*delta;
388     }
389     
390     if(count>40)return(-1);
391      
392     count++;
393   }
394
395   /* Replaced the original bubble sort with a real sort.  With your
396      help, we can eliminate the bubble sort in our lifetime. --Monty */
397
398   for(i=0; i<ord;i++) r[i] = root[i];
399   return(0);
400 }
401
402
403 /* Convert lpc coefficients to lsp coefficients */
404 int vorbis_lpc_to_lsp(float *lpc,float *lsp,int m){
405   int order2=(m+1)>>1;
406   int g1_order,g2_order;
407   float *g1=alloca(sizeof(*g1)*(order2+1));
408   float *g2=alloca(sizeof(*g2)*(order2+1));
409   float *g1r=alloca(sizeof(*g1r)*(order2+1));
410   float *g2r=alloca(sizeof(*g2r)*(order2+1));
411   int i;
412
413   /* even and odd are slightly different base cases */
414   g1_order=(m+1)>>1;
415   g2_order=(m)  >>1;
416
417   /* Compute the lengths of the x polynomials. */
418   /* Compute the first half of K & R F1 & F2 polynomials. */
419   /* Compute half of the symmetric and antisymmetric polynomials. */
420   /* Remove the roots at +1 and -1. */
421   
422   g1[g1_order] = 1.f;
423   for(i=1;i<=g1_order;i++) g1[g1_order-i] = lpc[i-1]+lpc[m-i];
424   g2[g2_order] = 1.f;
425   for(i=1;i<=g2_order;i++) g2[g2_order-i] = lpc[i-1]-lpc[m-i];
426   
427   if(g1_order>g2_order){
428     for(i=2; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+2];
429   }else{
430     for(i=1; i<=g1_order;i++) g1[g1_order-i] -= g1[g1_order-i+1];
431     for(i=1; i<=g2_order;i++) g2[g2_order-i] += g2[g2_order-i+1];
432   }
433
434   /* Convert into polynomials in cos(alpha) */
435   cheby(g1,g1_order);
436   cheby(g2,g2_order);
437
438   /* Find the roots of the 2 even polynomials.*/
439   if(Laguerre_With_Deflation(g1,g1_order,g1r) ||
440      Laguerre_With_Deflation(g2,g2_order,g2r))
441     return(-1);
442
443   Newton_Raphson(g1,g1_order,g1r); /* if it fails, it leaves g1r alone */
444   Newton_Raphson(g2,g2_order,g2r); /* if it fails, it leaves g2r alone */
445
446   qsort(g1r,g1_order,sizeof(*g1r),comp);
447   qsort(g2r,g2_order,sizeof(*g2r),comp);
448
449   for(i=0;i<g1_order;i++)
450     lsp[i*2] = acos(g1r[i]);
451
452   for(i=0;i<g2_order;i++)
453     lsp[i*2+1] = acos(g2r[i]);
454   return(0);
455 }