diff options
Diffstat (limited to 'libFDK/src/FDK_trigFcts.cpp')
| -rw-r--r-- | libFDK/src/FDK_trigFcts.cpp | 607 |
1 files changed, 607 insertions, 0 deletions
diff --git a/libFDK/src/FDK_trigFcts.cpp b/libFDK/src/FDK_trigFcts.cpp new file mode 100644 index 0000000..a198dc6 --- /dev/null +++ b/libFDK/src/FDK_trigFcts.cpp @@ -0,0 +1,607 @@ +/*************************** Fraunhofer IIS FDK Tools ********************** + + (C) Copyright Fraunhofer IIS (2005) + All Rights Reserved + + Please be advised that this software and/or program delivery is + Confidential Information of Fraunhofer and subject to and covered by the + + Fraunhofer IIS Software Evaluation Agreement + between Google Inc. and Fraunhofer + effective and in full force since March 1, 2012. + + You may use this software and/or program only under the terms and + conditions described in the above mentioned Fraunhofer IIS Software + Evaluation Agreement. Any other and/or further use requires a separate agreement. + + + $Id$ + Author(s): Haricharan Lakshman, Manuel Jander + Description: Trigonometric functions fixed point fractional implementation. + + This software and/or program is protected by copyright law and international + treaties. Any reproduction or distribution of this software and/or program, + or any portion of it, may result in severe civil and criminal penalties, and + will be prosecuted to the maximum extent possible under law. + +******************************************************************************/ + +#include "FDK_trigFcts.h" + +#include "fixpoint_math.h" + + +// #define CORDIC_SINCOS + + +#define IMPROVE_ATAN2_ACCURACY 1 // 0 --> 59 dB SNR 1 --> 65 dB SNR +#define MINSFTAB 7 +#define MAXSFTAB 25 + +#if IMPROVE_ATAN2_ACCURACY +static const FIXP_DBL f_atan_expand_range[MAXSFTAB-(MINSFTAB-1)] = +{ + /***************************************************************************** + * + * Table holds fixp_atan() output values which are outside of input range + * of fixp_atan() to improve SNR of fixp_atan2(). + * + * This Table might also be used in fixp_atan() [todo] so there a wider input + * range can be covered, too. + * + * Matlab (generate table): + * for scl = 7:25 % MINSFTAB .. MAXSFTAB + * at=atan(0.5 *(2^scl)); % 0.5 because get in 'middle' area of current scale level 'scl' + * at/2 % div at by ATO_SCALE + * end + * + * Table divided by 2=ATO_SCALE <-- SF=ATO_SF + *****************************************************************************/ + FL2FXCONST_DBL(7.775862990872099e-001), FL2FXCONST_DBL(7.814919928673978e-001), FL2FXCONST_DBL(7.834450483314648e-001), + FL2FXCONST_DBL(7.844216021392089e-001), FL2FXCONST_DBL(7.849098823026687e-001), FL2FXCONST_DBL(7.851540227918509e-001), + FL2FXCONST_DBL(7.852760930873737e-001), FL2FXCONST_DBL(7.853371282415015e-001), FL2FXCONST_DBL(7.853676458193612e-001), + FL2FXCONST_DBL(7.853829046083906e-001), FL2FXCONST_DBL(7.853905340029177e-001), FL2FXCONST_DBL(7.853943487001828e-001), + FL2FXCONST_DBL(7.853962560488155e-001), FL2FXCONST_DBL(7.853972097231319e-001), FL2FXCONST_DBL(7.853976865602901e-001), + FL2FXCONST_DBL(7.853979249788692e-001), FL2FXCONST_DBL(7.853980441881587e-001), FL2FXCONST_DBL(7.853981037928035e-001), + FL2FXCONST_DBL(7.853981335951259e-001) + // pi/4 = 0.785398163397448 = pi/2/ATO_SCALE +}; +#endif + +FIXP_DBL fixp_atan2(FIXP_DBL y, FIXP_DBL x) +{ + FIXP_DBL q; + FIXP_DBL at; // atan out + FIXP_DBL at2; // atan2 out + FIXP_DBL ret = FL2FXCONST_DBL(-1.0f); + INT sf,sfo,stf; + + // --- division + + if (y > FL2FXCONST_DBL(0.0f)) + { + if (x > FL2FXCONST_DBL(0.0f)) { + q = fDivNormHighPrec( y, x, &sf); // both pos. + } + else if (x < FL2FXCONST_DBL(0.0f)) { + q = -fDivNormHighPrec( y,-x, &sf); // x neg. + } + else {//(x ==FL2FXCONST_DBL(0.0f)) + q = FL2FXCONST_DBL(+1.0f); // y/x = pos/zero = +Inf + sf = 0; + } + } + else if (y < FL2FXCONST_DBL(0.0f)) + { + if (x > FL2FXCONST_DBL(0.0f)) { + q = -fDivNormHighPrec(-y, x, &sf); // y neg. + } + else if (x < FL2FXCONST_DBL(0.0f)) { + q = fDivNormHighPrec(-y,-x, &sf); // both neg. + } + else {//(x ==FL2FXCONST_DBL(0.0f)) + q = FL2FXCONST_DBL(-1.0f); // y/x = neg/zero = -Inf + sf = 0; + } + } + else { // (y ==FL2FXCONST_DBL(0.0f)) + q = FL2FXCONST_DBL(0.0f); + sf = 0; + } + sfo = sf; + //write2file(fpF__h,(float)y/(float)x); + //write2file(fpC__h,(float) q * (float)FDKpow(2,sfo)); // SNR 86 dB But range not limited to [-64..64] which is the input range of fixp_atan() + + // --- atan() + + if ( sfo > ATI_SF ) { + // --- could not calc fixp_atan() here bec of input data out of range + // ==> therefore give back boundary values + + #if IMPROVE_ATAN2_ACCURACY + if (sfo > MAXSFTAB) sfo = MAXSFTAB; + #endif + + if ( q > FL2FXCONST_DBL(0.0f) ) { + #if IMPROVE_ATAN2_ACCURACY + at = +f_atan_expand_range[sfo-ATI_SF-1]; + #else + at = FL2FXCONST_DBL( +M_PI/2 / ATO_SCALE); + #endif + } + else if ( q < FL2FXCONST_DBL(0.0f) ) { + #if IMPROVE_ATAN2_ACCURACY + at = -f_atan_expand_range[sfo-ATI_SF-1]; + #else + at = FL2FXCONST_DBL( -M_PI/2 / ATO_SCALE); + #endif + } + else { // q== FL2FXCONST_DBL(0.0f) + at = FL2FXCONST_DBL( 0.0f ); + } + }else{ + // --- calc of fixp_atan() is possible; input data within range + // ==> set q on fixed scale level as desired from fixp_atan() + stf = sfo - ATI_SF; + if (stf > 0) q = q << (INT)fMin( stf,DFRACT_BITS-1); + else q = q >> (INT)fMin(-stf,DFRACT_BITS-1); + //write2file(fpF__e,(float)y/(float)x); + //write2file(fpC__e,(float)q * (float)FDKpow(2,ATI_SF)); // 88.9 dB + at = fixp_atan(q); // ATO_SF + } + //write2file(fpF__g,(float)FDKatan( (float)y/(float)x )); + //write2file(fpC__g,(float) at * (float)FDKpow(2,ATO_SF) ); // todo dB + + // --- atan2() + + at2 = at >> (AT2O_SF - ATO_SF); // now AT2O_SF for atan2 + if ( x > FL2FXCONST_DBL(0.0f) ) { + ret = at2; + } + else if ( x < FL2FXCONST_DBL(0.0f) ) { + if ( y >= FL2FXCONST_DBL(0.0f) ) { + ret = at2 + FL2FXCONST_DBL( M_PI / AT2O_SCALE); + } else { + ret = at2 - FL2FXCONST_DBL( M_PI / AT2O_SCALE); + } + } + else { + // x == 0 + if ( y > FL2FXCONST_DBL(0.0f) ) { + ret = FL2FXCONST_DBL( +M_PI/2 / AT2O_SCALE); + } + else if ( y < FL2FXCONST_DBL(0.0f) ) { + ret = FL2FXCONST_DBL( -M_PI/2 / AT2O_SCALE); + } + else if ( y == FL2FXCONST_DBL(0.0f) ) { + ret = FL2FXCONST_DBL(0.0f); + } + } + //write2file(fpC__f,(float)ret * (float)FDKpow(2,AT2O_SF)); // 65 dB + //write2file(fpF__f,(float)FDKatan2(y,x)); + return ret; +} + + +FIXP_DBL fixp_atan(FIXP_DBL x) +{ + INT sign; + FIXP_DBL result, temp; + + // SNR of fixp_atan() = 56 dB + FIXP_DBL ONEBY3P56 = (FIXP_DBL)0x26800000; // 1.0/3.56 in q31 + FIXP_DBL P281 = (FIXP_DBL)0x00013000; // 0.281 in q18 + FIXP_DBL ONEP571 = (FIXP_DBL)0x6487ef00; // 1.571 in q30 + + if (x < FIXP_DBL(0)) { + sign = 1; + x = - x ; + } else { + sign = 0; + } + + /* calc of arctan */ + if(x < ( Q(Q_ATANINP)-FL2FXCONST_DBL(0.00395)) ) + { + INT res_e; + + temp = fPow2(x); // q25 * q25 - (DFRACT_BITS-1) = q19 + temp = fMult(temp, ONEBY3P56); // q19 * q31 - (DFRACT_BITS-1) = q19 + temp = temp + Q(19); // q19 + q19 = q19 + result = fDivNorm(x, temp, &res_e); + result = scaleValue(result, (Q_ATANOUT-Q_ATANINP+19-DFRACT_BITS+1) + res_e ); + } + else if( x < FL2FXCONST_DBL(1.28/64.0) ) + { + FIXP_DBL delta_fix; + FIXP_DBL PI_BY_4 = FL2FXCONST_DBL(3.1415926/4.0) >> 1; /* pi/4 in q30 */ + + delta_fix = (x - FL2FXCONST_DBL(1.0/64.0)) << 5; /* q30 */ + result = PI_BY_4 + (delta_fix >> 1) - (fPow2Div2(delta_fix)); + } + else + { + INT res_e; + + temp = fPow2Div2(x); // q25 * q25 - (DFRACT_BITS-1) - 1 = q18 + temp = temp + P281; // q18 + q18 = q18 + result = fDivNorm(x, temp, &res_e); + result = scaleValue(result, (Q_ATANOUT-Q_ATANINP+18-DFRACT_BITS+1) + res_e ); + result = ONEP571 - result; // q30 + q30 = q30 + } + if (sign) { + result = -result; + } + + return(result); +} + + +#ifndef CORDIC_SINCOS + +#include "FDK_tools_rom.h" + +FIXP_DBL fixp_cos(FIXP_DBL x, int scale) +{ + FIXP_DBL residual, error, sine, cosine; + + residual = fixp_sin_cos_residual_inline(x, scale, &sine, &cosine); + error = fMult(sine, residual); + + return cosine - error; +} + +FIXP_DBL fixp_sin(FIXP_DBL x, int scale) +{ + FIXP_DBL residual, error, sine, cosine; + + residual = fixp_sin_cos_residual_inline(x, scale, &sine, &cosine); + error = fMult(cosine, residual); + + return sine + error; +} + +void fixp_cos_sin (FIXP_DBL x, int scale, FIXP_DBL *cos, FIXP_DBL *sin) +{ + FIXP_DBL residual, error0, error1, sine, cosine; + + residual = fixp_sin_cos_residual_inline(x, scale, &sine, &cosine); + error0 = fMult(sine, residual); + error1 = fMult(cosine, residual); + *cos = cosine - error0; + *sin = sine + error1; +} + +#else /* #ifndef CORDIC_SINCOS */ + +/* + // Matlab + function v = cordic(beta,n) + % This function computes v = [cos(beta), sin(beta)] (beta in radians) + % using n iterations. Increasing n will increase the precision. + + if beta < -pi/2 | beta > pi/2 + if beta < 0 + v = cordic(beta + pi, n); + else + v = cordic(beta - pi, n); + end + v = -v; % flip the sign for second or third quadrant + return + end + + % Initialization of tables of constants used by CORDIC + % need a table of arctangents of negative powers of two, in radians: + % angles = atan(2.^-(0:27)); + angles = [ ... + 0.78539816339745 0.46364760900081 0.24497866312686 0.12435499454676 ... + 0.06241880999596 0.03123983343027 0.01562372862048 0.00781234106010 ... + 0.00390623013197 0.00195312251648 0.00097656218956 0.00048828121119 ... + 0.00024414062015 0.00012207031189 0.00006103515617 0.00003051757812 ... + 0.00001525878906 0.00000762939453 0.00000381469727 0.00000190734863 ... + 0.00000095367432 0.00000047683716 0.00000023841858 0.00000011920929 ... + 0.00000005960464 0.00000002980232 0.00000001490116 0.00000000745058 ]; + % and a table of products of reciprocal lengths of vectors [1, 2^-j]: + Kvalues = [ ... + 0.70710678118655 0.63245553203368 0.61357199107790 0.60883391251775 ... + 0.60764825625617 0.60735177014130 0.60727764409353 0.60725911229889 ... + 0.60725447933256 0.60725332108988 0.60725303152913 0.60725295913894 ... + 0.60725294104140 0.60725293651701 0.60725293538591 0.60725293510314 ... + 0.60725293503245 0.60725293501477 0.60725293501035 0.60725293500925 ... + 0.60725293500897 0.60725293500890 0.60725293500889 0.60725293500888 ]; + Kn = Kvalues(min(n, length(Kvalues))); + + % Initialize loop variables: + v = [1;0]; % start with 2-vector cosine and sine of zero + poweroftwo = 1; + angle = angles(1); + + % Iterations + for j = 0:n-1; + if beta < 0 + sigma = -1; + else + sigma = 1; + end + factor = sigma * poweroftwo; + R = [1, -factor; factor, 1]; + v = R * v; % 2-by-2 matrix multiply + beta = beta - sigma * angle; % update the remaining angle + poweroftwo = poweroftwo / 2; + % update the angle from table, or eventually by just dividing by two + if j+2 > length(angles) + angle = angle / 2; + else + angle = angles(j+2); + end + end + + % Adjust length of output vector to be [cos(beta), sin(beta)]: + v = v * Kn; + return + + // C++ + #define C1 + #define C2 + #define TYPE double + + TYPE fixp_cordic(TYPE in, + int n, + int type) + { + int i; + TYPE c,s,a; + TYPE x,y,v; + TYPE b; + + c = 0.5; + s = 0.0; + b = 1.0; + + for (i=0; i<n; i++) { + a = angleValues[i]; + x = b * s; + y = b * c; + + if (in < 0.0) { + c = c + x; + s = s - y; + in = in + a; + } + else { + c = c - x; + s = s + y; + in = in - a; + } + b = b / 2; + } + + if (type == 0) + v = c * kValues[n-1]*2; + else + v = s * kValues[n-1]*2; + + return (v); + } + + TYPE fixp_cos(TYPE in, + int n, + int scale) + { + TYPE v; + INT sign = 0; + + while ( (in < -M_PI/2) || (in > M_PI/2) ) { + if (in < 0) + in = in + M_PI; + else + in = in - M_PI; + + if (sign == 0) + sign = 1; + else + sign = 0; + } + + v = fixp_cordic(in,n,0); + + if (sign) + v = -v; + + return (v); + } + + TYPE fixp_sin(TYPE in, + int n, + int scale) + { + TYPE v; + INT sign = 0; + + while ( (in < -M_PI/2) || (in > M_PI/2) ) { + if (in < 0) + in = in + M_PI; + else + in = in - M_PI; + + if (sign == 0) + sign = 1; + else + sign = 0; + } + + v = fixp_cordic(in,n,1); + + if (sign) + v = -v; + + return (v); + } +*/ + + +#define SF_C1 1 +#define C1(x) FL2FXCONST_DBL(x/(1<<SF_C1)) +#define C2(x) FL2FXCONST_DBL(x) +#define M_PI_4 FL2FXCONST_DBL(M_PI/4) +#define ITER (DFRACT_BITS-1) + +/* + for (i=0; i<DFRACT_BITS; i++) { + angleValues[i] = FDKatan(FDKpow(2.0,-i)); + } +*/ +const FIXP_DBL angleValues[DFRACT_BITS] = { + C1(7.853981633974483e-001), C1(4.636476090008061e-001), C1(2.449786631268641e-001), C1(1.243549945467614e-001), + C1(6.241880999595735e-002), C1(3.123983343026828e-002), C1(1.562372862047683e-002), C1(7.812341060101111e-003), + C1(3.906230131966972e-003), C1(1.953122516478819e-003), C1(9.765621895593195e-004), C1(4.882812111948983e-004), + C1(2.441406201493618e-004), C1(1.220703118936702e-004), C1(6.103515617420877e-005), C1(3.051757811552610e-005), + C1(1.525878906131576e-005), C1(7.629394531101970e-006), C1(3.814697265606496e-006), C1(1.907348632810187e-006), + C1(9.536743164059608e-007), C1(4.768371582030888e-007), C1(2.384185791015580e-007), C1(1.192092895507807e-007), + C1(5.960464477539055e-008), C1(2.980232238769530e-008), C1(1.490116119384766e-008), C1(7.450580596923828e-009), + C1(3.725290298461914e-009), C1(1.862645149230957e-009), C1(9.313225746154785e-010), C1(4.656612873077393e-010) +}; + +/* + for (i=0; i<DFRACT_BITS; i++) { + kValues[i] = 1.0; + for (j=0; j<=i; j++) { + val = FDKcos(FDKatan(FDKpow(2.0,-j))); + kValues[i] *= val; + } + } +*/ +const FIXP_DBL kValues[DFRACT_BITS] = { + C2(7.071067811865476e-001), C2(6.324555320336759e-001), C2(6.135719910778964e-001), C2(6.088339125177524e-001), + C2(6.076482562561683e-001), C2(6.073517701412960e-001), C2(6.072776440935261e-001), C2(6.072591122988928e-001), + C2(6.072544793325625e-001), C2(6.072533210898753e-001), C2(6.072530315291345e-001), C2(6.072529591389450e-001), + C2(6.072529410413973e-001), C2(6.072529365170104e-001), C2(6.072529353859136e-001), C2(6.072529351031395e-001), + C2(6.072529350324459e-001), C2(6.072529350147725e-001), C2(6.072529350103542e-001), C2(6.072529350092496e-001), + C2(6.072529350089735e-001), C2(6.072529350089044e-001), C2(6.072529350088872e-001), C2(6.072529350088829e-001), + C2(6.072529350088818e-001), C2(6.072529350088816e-001), C2(6.072529350088814e-001), C2(6.072529350088814e-001), + C2(6.072529350088814e-001), C2(6.072529350088814e-001), C2(6.072529350088814e-001), C2(6.072529350088814e-001) +}; + +inline +static FIXP_DBL fixp_cordic(FIXP_DBL in, + INT type) +{ + int i; + FIXP_DBL c,s,a; + FIXP_DBL x,y; + int n = ITER; + + in = fMult(in,M_PI_4); + c = kValues[n-1] >> 1; + s = FL2FXCONST_DBL(0.0f); + + for (i=0; i<n; i++) { + a = angleValues[i]; + x = s >> i; + y = c >> i; + + if (in < FL2FXCONST_DBL(0.0)) { + c = c + x; + s = s - y; + in = in + a; + } + else { + c = c - x; + s = s + y; + in = in - a; + } + } + + /* 1 headroom bit */ + if (type == 0) + return (c); + else + return (s); +} + +/* + in: argument + scale: scalefactor of argument has to be in the range of 0, ... ,DFRACT_BITS-1 + type: calculate cosine 0 + calculate sine 1 +*/ +static FIXP_DBL fixp_sincos(FIXP_DBL in, + INT scale, + INT type) +{ + FIXP_DBL v; + INT sign = 0; + FIXP_DBL pi,pi_2; + + if (scale < 2) { + in = in >> (2-scale); + scale = 2; + } + + pi = M_PI_4 >> (scale-2); + pi_2 = M_PI_4 >> (scale-1); + + /* move signal into the range of -pi/2 to +pi/2 */ + while ( (in < (-pi_2)) || (in > (pi_2)) ) { + if (in < FL2FXCONST_DBL(0.0)) + in = in + pi; + else + in = in - pi; + + sign = ~sign; + } + + /* scale signal with 2/pi => unscaled signal is in the range of [-1.0,...,+1.0[ */ + in = fMult(in,FL2FXCONST_DBL(2/M_PI)) << scale; + + v = fixp_cordic(in,type); + + if (sign) + v = -v; + + /* compensate 1 headroom bit */ + if ( v <= FL2FXCONST_DBL(-0.5f) ) + v = (FIXP_DBL) (MINVAL_DBL+1); + else + v = SATURATE_LEFT_SHIFT(v,1,DFRACT_BITS); + + return (v); +} + +/* + in: argument + scale: scalefactor of argument has to be in the range of 0, ... ,DFRACT_BITS-1 +*/ +FIXP_DBL fixp_cos(FIXP_DBL in, + INT scale) + +{ + FDK_ASSERT ((scale >= 0) && (scale < DFRACT_BITS)); + + if (in == FL2FXCONST_DBL(0.0f)) + return /*FL2FXCONST_DBL(1.0)*/ (FIXP_DBL)MAXVAL_DBL; + + return (fixp_sincos(in,scale,0)); +} + +/* + in: argument + scale: scalefactor of argument has to be in the range of 0, ... ,DFRACT_BITS-1 +*/ +FIXP_DBL fixp_sin(FIXP_DBL in, + INT scale) + +{ + FDK_ASSERT ((scale >= 0) && (scale < DFRACT_BITS)); + + if (in == FL2FXCONST_DBL(0.0f)) + return FL2FXCONST_DBL(0.0f); + + return (fixp_sincos(in,scale,1)); +} + +#endif /* #ifndef CORDIC_SINCOS */ + + + + |