You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
114 lines
3.9 KiB
114 lines
3.9 KiB
|
|
#include "mathDSP_F32.h"
|
|
#include <math.h>
|
|
|
|
/* acos_f32(x) Bob Larkin 2020
|
|
* This acos(x) approximation is intended as being fast, resonably accurate, and
|
|
* with continuous function and derivative (slope) between -1 and 1 x value.
|
|
* It is the result of a "Chebychev-zero" fit to the true values, and is a 7th
|
|
* order polynomial for the full (-1.0, 1.0) range.
|
|
* Max error from -0.99 to 0.99 is < 0.018/Pi (1.0 deg)
|
|
* Error at -1 or +1 is 0.112/Pi (6.4 deg)
|
|
* For acos, speed and accuracy are in conflict near x = +/- 1, but that
|
|
* is not where communications phase detectors are normally used.
|
|
* Using T3.6 this function, by itself, measures as 0.18 uSec
|
|
*
|
|
* Thanks to Bob K3KHF for ideas on minimizing errors with acos().
|
|
* RSL 5 April 2020.
|
|
*/
|
|
float mathDSP_F32::acos_f32(float x) {
|
|
float w;
|
|
// These next two error checks use 0.056 uSec per call
|
|
if(x > 1.00000) return 0.0f;
|
|
if(x < -1.00000) return MF_PI;
|
|
w = x * x;
|
|
return 1.5707963268f+(x*((-0.97090f)+w*((-0.529008f)+w*(1.00279f-w*0.961446))));
|
|
}
|
|
|
|
/* *** Not currently used, but possible substitute for acosf(x) ***
|
|
* Apparently based on Handbook of Mathematical Functions
|
|
* M. Abramowitz and I.A. Stegun, Ed. Check before using.
|
|
* https://developer.download.nvidia.com/cg/acos.html
|
|
* Absolute error <= 6.7e-5, good, but not as good as acosf()
|
|
* T3.6 this measures 0.51 uSec (0.23 uSec from sqrtf() ),
|
|
* better than acosf(x) by a factor of 2.
|
|
*/
|
|
float mathDSP_F32::approxAcos(float x) {
|
|
if(x > 0.999999) return 0.0f;
|
|
if(x < -0.999999) return M_PI; // 3.14159265358979f;
|
|
float negate = float(x < 0);
|
|
x = fabsf(x);
|
|
float ret = -0.0187293f;
|
|
ret = ret * x;
|
|
ret = ret + 0.0742610f;
|
|
ret = ret * x;
|
|
ret = ret - 0.2121144f;
|
|
ret = ret * x;
|
|
ret = ret + 1.5707288f;
|
|
ret = ret * sqrtf(1.0f-x);
|
|
ret = ret - 2 * negate * ret;
|
|
return negate * MF_PI + ret;
|
|
}
|
|
|
|
/* Polynomial approximating arctangenet on iput range (-1, 1)
|
|
* giving result in a range of approximately (-pi/4, pi/4)
|
|
* Max error < 0.005 radians (or 0.29 degrees)
|
|
*
|
|
* Directly from www.dsprelated.com/showarticle/1052.php
|
|
* Thank you Nic Taylor---nice work.
|
|
*/
|
|
float mathDSP_F32::fastAtan2(float y, float x) {
|
|
if (x != 0.0f) {
|
|
if (fabsf(x) > fabsf(y)) {
|
|
const float z = y / x;
|
|
if (x > 0.0)
|
|
// atan2(y,x) = atan(y/x) if x > 0
|
|
return _Atan(z);
|
|
else if (y >= 0.0)
|
|
// atan2(y,x) = atan(y/x) + PI if x < 0, y >= 0
|
|
return _Atan(z) + M_PI;
|
|
else
|
|
// atan2(y,x) = atan(y/x) - PI if x < 0, y < 0
|
|
return _Atan(z) - M_PI;
|
|
}
|
|
else { // Use property atan(y/x) = PI/2-atan(x/y) if |y/x| > 1.
|
|
const float z = x / y;
|
|
if (y > 0.0)
|
|
// atan2(y,x) = PI/2 - atan(x/y) if |y/x| > 1, y > 0
|
|
return -_Atan(z) + M_PI_2;
|
|
else
|
|
// atan2(y,x) = -PI/2 - atan(x/y) if |y/x| > 1, y < 0
|
|
return -_Atan(z) - M_PI_2;
|
|
}
|
|
}
|
|
else {
|
|
if (y > 0.0f) // x = 0, y > 0
|
|
return M_PI_2;
|
|
else if (y < 0.0f) // x = 0, y < 0
|
|
return -M_PI_2;
|
|
}
|
|
return 0.0f; // x,y = 0. Undefined, stay finite.
|
|
}
|
|
|
|
/* float i0f(float x) Returns the modified Bessel function Io(x).
|
|
* Algorithm is based on Abromowitz and Stegun, Handbook of Mathematical
|
|
* Functions, and Press, et. al., Numerical Recepies in C.
|
|
* All in 32-bit floating point
|
|
*/
|
|
float mathDSP_F32::i0f(float x) {
|
|
float af, bf, cf;
|
|
if( (af=fabsf(x)) < 3.75f ) {
|
|
cf = x/3.75f;
|
|
cf = cf*cf;
|
|
bf=1.0f+cf*(3.515623f+cf*(3.089943f+cf*(1.20675f+cf*(0.265973f+
|
|
cf*(0.0360768f+cf*0.0045813f)))));
|
|
}
|
|
else {
|
|
cf = 3.75f/af;
|
|
bf=(expf(af)/sqrtf(af))*(0.3989423f+cf*(0.0132859f+cf*(0.0022532f+
|
|
cf*(-0.0015756f+cf*(0.0091628f+cf*(-0.0205771f+cf*(0.0263554f+
|
|
cf*(-0.0164763f+cf*0.0039238f))))))));
|
|
}
|
|
return bf;
|
|
}
|
|
|
|
|