lsp.c - vx32 - Local 9vx git repository for patches.
(HTM) git clone git://r-36.net/vx32
(DIR) Log
(DIR) Files
(DIR) Refs
---
lsp.c (12341B)
---
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 1919 2005-07-24 14:18:04Z baford $
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 }