mirror of
https://github.com/DrBeef/ioq3quest.git
synced 2024-11-23 12:32:09 +00:00
454 lines
12 KiB
C
454 lines
12 KiB
C
/********************************************************************
|
|
* *
|
|
* 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 19453 2015-03-02 22:35:34Z 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
|
|
separately 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;
|
|
|
|
while(c--){
|
|
q*=ftmp[0]-w;
|
|
p*=ftmp[1]-w;
|
|
ftmp+=2;
|
|
}
|
|
|
|
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 *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;
|
|
}
|
|
|
|
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);
|
|
}
|