394 wiersze
7.9 KiB
C
394 wiersze
7.9 KiB
C
#include "cmPrefix.h"
|
|
#include "cmGlobal.h"
|
|
#include "cmFloatTypes.h"
|
|
#include "cmMath.h"
|
|
#include <sys/types.h> // u_char
|
|
|
|
// TODO: rewrite to avoid copying
|
|
// this code comes via csound source ...
|
|
double cmX80ToDouble( unsigned char rate[10] )
|
|
{
|
|
char sign;
|
|
short exp = 0;
|
|
unsigned long mant1 = 0;
|
|
unsigned long mant0 = 0;
|
|
double val;
|
|
unsigned char* p = (unsigned char*)rate;
|
|
|
|
exp = *p++;
|
|
exp <<= 8;
|
|
exp |= *p++;
|
|
sign = (exp & 0x8000) ? 1 : 0;
|
|
exp &= 0x7FFF;
|
|
|
|
mant1 = *p++;
|
|
mant1 <<= 8;
|
|
mant1 |= *p++;
|
|
mant1 <<= 8;
|
|
mant1 |= *p++;
|
|
mant1 <<= 8;
|
|
mant1 |= *p++;
|
|
|
|
mant0 = *p++;
|
|
mant0 <<= 8;
|
|
mant0 |= *p++;
|
|
mant0 <<= 8;
|
|
mant0 |= *p++;
|
|
mant0 <<= 8;
|
|
mant0 |= *p++;
|
|
|
|
/* special test for all bits zero meaning zero
|
|
- else pow(2,-16383) bombs */
|
|
if (mant1 == 0 && mant0 == 0 && exp == 0 && sign == 0)
|
|
return 0.0;
|
|
else {
|
|
val = ((double)mant0) * pow(2.0,-63.0);
|
|
val += ((double)mant1) * pow(2.0,-31.0);
|
|
val *= pow(2.0,((double) exp) - 16383.0);
|
|
return sign ? -val : val;
|
|
}
|
|
}
|
|
|
|
// TODO: rewrite to avoid copying
|
|
/*
|
|
* Convert double to IEEE 80 bit floating point
|
|
* Should be portable to all C compilers.
|
|
* 19aug91 aldel/dpwe covered for MSB bug in Ultrix 'cc'
|
|
*/
|
|
|
|
void cmDoubleToX80(double val, unsigned char rate[10])
|
|
{
|
|
char sign = 0;
|
|
short exp = 0;
|
|
unsigned long mant1 = 0;
|
|
unsigned long mant0 = 0;
|
|
unsigned char* p = (unsigned char*)rate;
|
|
|
|
if (val < 0.0) { sign = 1; val = -val; }
|
|
|
|
if (val != 0.0) /* val identically zero -> all elements zero */
|
|
{
|
|
exp = (short)(log(val)/log(2.0) + 16383.0);
|
|
val *= pow(2.0, 31.0+16383.0-(double)exp);
|
|
mant1 =((unsigned)val);
|
|
val -= ((double)mant1);
|
|
val *= pow(2.0, 32.0);
|
|
mant0 =((double)val);
|
|
}
|
|
|
|
*p++ = ((sign<<7)|(exp>>8));
|
|
*p++ = (u_char)(0xFF & exp);
|
|
*p++ = (u_char)(0xFF & (mant1>>24));
|
|
*p++ = (u_char)(0xFF & (mant1>>16));
|
|
*p++ = (u_char)(0xFF & (mant1>> 8));
|
|
*p++ = (u_char)(0xFF & (mant1));
|
|
*p++ = (u_char)(0xFF & (mant0>>24));
|
|
*p++ = (u_char)(0xFF & (mant0>>16));
|
|
*p++ = (u_char)(0xFF & (mant0>> 8));
|
|
*p++ = (u_char)(0xFF & (mant0));
|
|
|
|
}
|
|
|
|
bool cmIsPowerOfTwo( unsigned x )
|
|
{
|
|
return !( (x < 2) || (x & (x-1)) );
|
|
}
|
|
|
|
unsigned cmNextPowerOfTwo( unsigned val )
|
|
{
|
|
unsigned i;
|
|
unsigned mask = 1;
|
|
unsigned msb = 0;
|
|
unsigned cnt = 0;
|
|
|
|
// if val is a power of two return it
|
|
if( cmIsPowerOfTwo(val) )
|
|
return val;
|
|
|
|
// next pow of zero is 2
|
|
if( val == 0 )
|
|
return 2;
|
|
|
|
// if the next power of two can't be represented in 32 bits
|
|
if( val > 0x80000000)
|
|
{
|
|
assert(0);
|
|
return 0;
|
|
}
|
|
|
|
// find most sig. bit that is set - the number with only the next msb set is next pow 2
|
|
for(i=0; i<31; i++,mask<<=1)
|
|
if( mask & val )
|
|
{
|
|
msb = i;
|
|
cnt++;
|
|
}
|
|
|
|
|
|
return 1 << (msb + 1);
|
|
}
|
|
|
|
unsigned cmNearPowerOfTwo( unsigned i )
|
|
{
|
|
unsigned vh = cmNextPowerOfTwo(i);
|
|
|
|
if( vh == 2 )
|
|
return vh;
|
|
|
|
unsigned vl = vh / 2;
|
|
|
|
if( vh - i < i - vl )
|
|
return vh;
|
|
return vl;
|
|
}
|
|
|
|
bool cmIsOddU( unsigned v ) { return v % 2 == 1; }
|
|
bool cmIsEvenU( unsigned v ) { return !cmIsOddU(v); }
|
|
unsigned cmNextOddU( unsigned v ) { return cmIsOddU(v) ? v : v+1; }
|
|
unsigned cmPrevOddU( unsigned v ) { return cmIsOddU(v) ? v : v-1; }
|
|
unsigned cmNextEvenU( unsigned v ) { return cmIsEvenU(v) ? v : v+1; }
|
|
unsigned cmPrevEvenU( unsigned v ) { return cmIsEvenU(v) ? v : v-1; }
|
|
|
|
// modified bessel function of first kind, order 0
|
|
// ref: orfandis appendix B io.m
|
|
double cmBessel0( double x )
|
|
{
|
|
double eps = pow(10.0,-9.0);
|
|
double n = 1.0;
|
|
double S = 1.0;
|
|
double D = 1.0;
|
|
|
|
while(D > eps*S)
|
|
{
|
|
double T = x /(2.0*n);
|
|
n = n+1;
|
|
D = D * pow(T,2.0);
|
|
S = S + D;
|
|
}
|
|
|
|
return S;
|
|
|
|
}
|
|
|
|
//=================================================================
|
|
// The following elliptic-related function approximations come from
|
|
// Parks & Burrus, Digital Filter Design, Appendix program 9, pp. 317-326
|
|
// which in turn draws directly on other sources
|
|
|
|
// calculate complete elliptic integral (quarter period) K
|
|
// given *complimentary* modulus kc
|
|
cmReal_t cmEllipK( cmReal_t kc )
|
|
{
|
|
cmReal_t a = 1, b = kc, c = 1, tmp;
|
|
|
|
while( c > cmReal_EPSILON )
|
|
{
|
|
c = 0.5*(a-b);
|
|
tmp = 0.5*(a+b);
|
|
b = sqrt(a*b);
|
|
a = tmp;
|
|
}
|
|
|
|
return M_PI/(2*a);
|
|
}
|
|
|
|
// calculate elliptic modulus k
|
|
// given ratio of complete elliptic integrals r = K/K'
|
|
// (solves the "degree equation" for fixed N = K*K1'/K'K1)
|
|
cmReal_t cmEllipDeg( cmReal_t r )
|
|
{
|
|
cmReal_t q,a,b,c,d;
|
|
a = b = c = 1;
|
|
d = q = exp(-M_PI*r);
|
|
|
|
while( c > cmReal_EPSILON )
|
|
{
|
|
a = a + 2*c*d;
|
|
c = c*d*d;
|
|
b = b + c;
|
|
d = d*q;
|
|
}
|
|
|
|
return 4*sqrt(q)*pow(b/a,2);
|
|
}
|
|
|
|
// calculate arc elliptic tangent u (elliptic integral of the 1st kind)
|
|
// given argument x = sc(u,k) and *complimentary* modulus kc
|
|
cmReal_t cmEllipArcSc( cmReal_t x, cmReal_t kc )
|
|
{
|
|
cmReal_t a = 1, b = kc, y = 1/x, tmp;
|
|
unsigned L = 0;
|
|
|
|
while( true )
|
|
{
|
|
tmp = a*b;
|
|
a += b;
|
|
b = 2*sqrt(tmp);
|
|
y -= tmp/y;
|
|
if( y == 0 )
|
|
y = sqrt(tmp) * 1E-10;
|
|
if( fabs(a-b)/a < cmReal_EPSILON )
|
|
break;
|
|
L *= 2;
|
|
if( y < 0 )
|
|
L++;
|
|
}
|
|
|
|
if( y < 0 )
|
|
L++;
|
|
|
|
return (atan(a/y) + M_PI*L)/a;
|
|
}
|
|
|
|
// calculate Jacobi elliptic functions sn, cn, and dn
|
|
// given argument u and *complimentary* modulus kc
|
|
cmRC_t cmEllipJ( cmReal_t u, cmReal_t kc, cmReal_t* sn, cmReal_t* cn, cmReal_t* dn )
|
|
{
|
|
assert( sn != NULL || cn != NULL || dn != NULL );
|
|
|
|
if( u == 0 )
|
|
{
|
|
if( sn != NULL ) *sn = 0;
|
|
if( cn != NULL ) *cn = 1;
|
|
if( dn != NULL ) *dn = 1;
|
|
return cmOkRC;
|
|
}
|
|
|
|
int i;
|
|
cmReal_t a,b,c,d,e,tmp,_sn,_cn,_dn;
|
|
cmReal_t aa[16], bb[16];
|
|
|
|
a = 1;
|
|
b = kc;
|
|
|
|
for( i = 0; i < 16; i++ )
|
|
{
|
|
aa[i] = a;
|
|
bb[i] = b;
|
|
tmp = (a+b)/2;
|
|
b = sqrt(a*b);
|
|
a = tmp;
|
|
if( (a-b)/a < cmReal_EPSILON )
|
|
break;
|
|
}
|
|
|
|
c = a/tan(u*a);
|
|
d = 1;
|
|
|
|
for( ; i >= 0; i-- )
|
|
{
|
|
e = c*c/a;
|
|
c = c*d;
|
|
a = aa[i];
|
|
d = (e + bb[i]) / (e+a);
|
|
}
|
|
|
|
_sn = 1/sqrt(1+c*c);
|
|
_cn = _sn*c;
|
|
_dn = d;
|
|
|
|
if( sn != NULL ) *sn = _sn;
|
|
if( cn != NULL ) *cn = _cn;
|
|
if( dn != NULL ) *dn = _dn;
|
|
|
|
return cmOkRC;
|
|
}
|
|
|
|
//=================================================================
|
|
// bilinear transform
|
|
// z = (2*sr + s)/(2*sr - s)
|
|
cmRC_t cmBlt( unsigned n, cmReal_t sr, cmReal_t* rp, cmReal_t* ip )
|
|
{
|
|
unsigned i;
|
|
cmReal_t a = 2*sr,
|
|
tr, ti, td;
|
|
|
|
for( i = 0; i < n; i++ )
|
|
{
|
|
tr = rp[i];
|
|
ti = ip[i];
|
|
td = pow(a-tr, 2) + ti*ti;
|
|
rp[i] = (a*a - tr*tr - ti*ti)/td;
|
|
ip[i] = 2*a*ti/td;
|
|
if( tr < -1E15 )
|
|
rp[i] = 0;
|
|
if( fabs(ti) > 1E15 )
|
|
ip[i] = 0;
|
|
}
|
|
|
|
return cmOkRC;
|
|
}
|
|
|
|
unsigned cmHzToMidi( double hz )
|
|
{
|
|
|
|
float midi = 12.0 * log2(hz/13.75) + 9;
|
|
|
|
if( midi < 0 )
|
|
midi = 0;
|
|
if( midi > 127 )
|
|
midi = 127;
|
|
|
|
return (unsigned)lround(midi);
|
|
}
|
|
|
|
float cmMidiToHz( unsigned midi )
|
|
{
|
|
double m = midi <= 127 ? midi : 127;
|
|
|
|
return (float)( 13.75 * pow(2.0,(m - 9.0)/12.0));
|
|
}
|
|
|
|
|
|
//=================================================================
|
|
// Floating point byte swapping
|
|
|
|
// Unions used to type-pun the swapping functions and thereby
|
|
// avoid strict aliasing problems with -O2. Using unions for
|
|
// this purpose is apparently legal under C99 but not C++.
|
|
|
|
typedef union
|
|
{
|
|
unsigned u;
|
|
float f;
|
|
} _cmMathU_t;
|
|
|
|
typedef union
|
|
{
|
|
unsigned long long u;
|
|
double f;
|
|
} _cmMathUL_t;
|
|
|
|
unsigned cmFfSwapFloatToUInt( float v )
|
|
{
|
|
assert( sizeof(float) == sizeof(unsigned));
|
|
_cmMathU_t u;
|
|
u.f=v;
|
|
return cmSwap32(u.u);
|
|
}
|
|
|
|
float cmFfSwapUIntToFloat( unsigned v )
|
|
{
|
|
assert( sizeof(float) == sizeof(unsigned));
|
|
_cmMathU_t u;
|
|
|
|
u.u = cmSwap32(v);
|
|
return u.f;
|
|
}
|
|
|
|
unsigned long long cmFfSwapDoubleToULLong( double v )
|
|
{
|
|
assert( sizeof(double) == sizeof(unsigned long long));
|
|
_cmMathUL_t u;
|
|
u.f = v;
|
|
return cmSwap64(u.u);
|
|
}
|
|
|
|
double cmFfSwapULLongToDouble( unsigned long long v )
|
|
{
|
|
assert( sizeof(double) == sizeof(unsigned long long));
|
|
_cmMathUL_t u;
|
|
u.u = cmSwap64(v);
|
|
return u.f;
|
|
}
|