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