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.
636 lines
12 KiB
636 lines
12 KiB
/**
|
|
* This code is available at
|
|
* http://www.mindspring.com/~pfilandr/C/fs_math/
|
|
* and it is believed to be public domain.
|
|
*/
|
|
|
|
/* BEGIN fs_math.c */
|
|
|
|
#include "libs/fs_math.h"
|
|
|
|
#include <float.h>
|
|
/*
|
|
** pi == (atan(1.0 / 3) + atan(1.0 / 2)) * 4
|
|
*/
|
|
static double fs_pi(void);
|
|
static long double fs_pil(void);
|
|
|
|
double fs_sqrt(double x)
|
|
{
|
|
int n;
|
|
double a, b;
|
|
|
|
if (x > 0 && DBL_MAX >= x) {
|
|
for (n = 0; x > 2; x /= 4) {
|
|
++n;
|
|
}
|
|
while (0.5 > x) {
|
|
--n;
|
|
x *= 4;
|
|
}
|
|
a = x;
|
|
b = (1 + x) / 2;
|
|
do {
|
|
x = b;
|
|
b = (a / x + x) / 2;
|
|
} while (x > b);
|
|
while (n > 0) {
|
|
x *= 2;
|
|
--n;
|
|
}
|
|
while (0 > n) {
|
|
x /= 2;
|
|
++n;
|
|
}
|
|
} else {
|
|
if (x != 0) {
|
|
x = DBL_MAX;
|
|
}
|
|
}
|
|
return x;
|
|
}
|
|
|
|
double fs_log(double x)
|
|
{
|
|
int n;
|
|
double a, b, c, epsilon;
|
|
static double A, B, C;
|
|
static int initialized;
|
|
|
|
if (x > 0 && DBL_MAX >= x) {
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
A = fs_sqrt(2);
|
|
B = A / 2;
|
|
C = fs_log(A);
|
|
}
|
|
for (n = 0; x > A; x /= 2) {
|
|
++n;
|
|
}
|
|
while (B > x) {
|
|
--n;
|
|
x *= 2;
|
|
}
|
|
a = (x - 1) / (x + 1);
|
|
x = C * n + a;
|
|
c = a * a;
|
|
n = 1;
|
|
epsilon = DBL_EPSILON * x;
|
|
if (0 > a) {
|
|
if (epsilon > 0) {
|
|
epsilon = -epsilon;
|
|
}
|
|
do {
|
|
n += 2;
|
|
a *= c;
|
|
b = a / n;
|
|
x += b;
|
|
} while (epsilon > b);
|
|
} else {
|
|
if (0 > epsilon) {
|
|
epsilon = -epsilon;
|
|
}
|
|
do {
|
|
n += 2;
|
|
a *= c;
|
|
b = a / n;
|
|
x += b;
|
|
} while (b > epsilon);
|
|
}
|
|
x *= 2;
|
|
} else {
|
|
x = -DBL_MAX;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
double fs_log10(double x)
|
|
{
|
|
static double log_10;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
log_10 = fs_log(10);
|
|
}
|
|
return x > 0 && DBL_MAX >= x ? fs_log(x) / log_10 : fs_log(x);
|
|
}
|
|
|
|
double fs_exp(double x)
|
|
{
|
|
unsigned n, square;
|
|
double b, e;
|
|
static double x_max, x_min, epsilon;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
x_max = fs_log(DBL_MAX);
|
|
x_min = fs_log(DBL_MIN);
|
|
epsilon = DBL_EPSILON / 4;
|
|
}
|
|
if (x_max >= x && x >= x_min) {
|
|
for (square = 0; x > 1; x /= 2) {
|
|
++square;
|
|
}
|
|
while (-1 > x) {
|
|
++square;
|
|
x /= 2;
|
|
}
|
|
e = b = n = 1;
|
|
do {
|
|
b /= n++;
|
|
b *= x;
|
|
e += b;
|
|
b /= n++;
|
|
b *= x;
|
|
e += b;
|
|
} while (b > epsilon);
|
|
while (square-- != 0) {
|
|
e *= e;
|
|
}
|
|
} else {
|
|
e = x > 0 ? DBL_MAX : 0;
|
|
}
|
|
return e;
|
|
}
|
|
|
|
double fs_modf(double value, double *iptr)
|
|
{
|
|
double a, b;
|
|
const double c = value;
|
|
|
|
if (0 > c) {
|
|
value = -value;
|
|
}
|
|
if (DBL_MAX >= value) {
|
|
for (*iptr = 0; value >= 1; value -= b) {
|
|
a = value / 2;
|
|
b = 1;
|
|
while (a >= b) {
|
|
b *= 2;
|
|
}
|
|
*iptr += b;
|
|
}
|
|
} else {
|
|
*iptr = value;
|
|
value = 0;
|
|
}
|
|
if (0 > c) {
|
|
*iptr = -*iptr;
|
|
value = -value;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
double fs_fmod(double x, double y)
|
|
{
|
|
double a, b;
|
|
const double c = x;
|
|
|
|
if (0 > c) {
|
|
x = -x;
|
|
}
|
|
if (0 > y) {
|
|
y = -y;
|
|
}
|
|
if (y != 0 && DBL_MAX >= y && DBL_MAX >= x) {
|
|
while (x >= y) {
|
|
a = x / 2;
|
|
b = y;
|
|
while (a >= b) {
|
|
b *= 2;
|
|
}
|
|
x -= b;
|
|
}
|
|
} else {
|
|
x = 0;
|
|
}
|
|
return 0 > c ? -x : x;
|
|
}
|
|
|
|
double fs_pow(double x, double y)
|
|
{
|
|
double p = 0;
|
|
|
|
if (0 > x && fs_fmod(y, 1) == 0) {
|
|
if (fs_fmod(y, 2) == 0) {
|
|
p = fs_exp(fs_log(-x) * y);
|
|
} else {
|
|
p = -fs_exp(fs_log(-x) * y);
|
|
}
|
|
} else {
|
|
if (x != 0 || 0 >= y) {
|
|
p = fs_exp(fs_log( x) * y);
|
|
}
|
|
}
|
|
return p;
|
|
}
|
|
|
|
static double fs_pi(void)
|
|
{
|
|
unsigned n;
|
|
double a, b, epsilon;
|
|
static double p;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
epsilon = DBL_EPSILON / 4;
|
|
n = 1;
|
|
a = 3;
|
|
do {
|
|
a /= 9;
|
|
b = a / n;
|
|
n += 2;
|
|
a /= 9;
|
|
b -= a / n;
|
|
n += 2;
|
|
p += b;
|
|
} while (b > epsilon);
|
|
epsilon = DBL_EPSILON / 2;
|
|
n = 1;
|
|
a = 2;
|
|
do {
|
|
a /= 4;
|
|
b = a / n;
|
|
n += 2;
|
|
a /= 4;
|
|
b -= a / n;
|
|
n += 2;
|
|
p += b;
|
|
} while (b > epsilon);
|
|
p *= 4;
|
|
}
|
|
return p;
|
|
}
|
|
|
|
double fs_cos(double x)
|
|
{
|
|
unsigned n;
|
|
int negative, sine;
|
|
double a, b, c;
|
|
static double pi, two_pi, half_pi, third_pi, epsilon;
|
|
static int initialized;
|
|
|
|
if (0 > x) {
|
|
x = -x;
|
|
}
|
|
if (DBL_MAX >= x) {
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
pi = fs_pi();
|
|
two_pi = 2 * pi;
|
|
half_pi = pi / 2;
|
|
third_pi = pi / 3;
|
|
epsilon = DBL_EPSILON / 2;
|
|
}
|
|
if (x > two_pi) {
|
|
x = fs_fmod(x, two_pi);
|
|
}
|
|
if (x > pi) {
|
|
x = two_pi - x;
|
|
}
|
|
if (x > half_pi) {
|
|
x = pi - x;
|
|
negative = 1;
|
|
} else {
|
|
negative = 0;
|
|
}
|
|
if (x > third_pi) {
|
|
x = half_pi - x;
|
|
sine = 1;
|
|
} else {
|
|
sine = 0;
|
|
}
|
|
c = x * x;
|
|
x = n = 0;
|
|
a = 1;
|
|
do {
|
|
b = a;
|
|
a *= c;
|
|
a /= ++n;
|
|
a /= ++n;
|
|
b -= a;
|
|
a *= c;
|
|
a /= ++n;
|
|
a /= ++n;
|
|
x += b;
|
|
} while (b > epsilon);
|
|
if (sine) {
|
|
x = fs_sqrt((1 - x) * (1 + x));
|
|
}
|
|
if (negative) {
|
|
x = -x;
|
|
}
|
|
} else {
|
|
x = -DBL_MAX;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
double fs_log2(double x)
|
|
{
|
|
static double log_2;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
log_2 = fs_log(2);
|
|
}
|
|
return x > 0 && DBL_MAX >= x ? fs_log(x) / log_2 : fs_log(x);
|
|
}
|
|
|
|
double fs_exp2(double x)
|
|
{
|
|
static double log_2;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
log_2 = fs_log(2);
|
|
}
|
|
return fs_exp(x * log_2);
|
|
}
|
|
|
|
long double fs_powl(long double x, long double y)
|
|
{
|
|
long double p;
|
|
|
|
if (0 > x && fs_fmodl(y, 1) == 0) {
|
|
if (fs_fmodl(y, 2) == 0) {
|
|
p = fs_expl(fs_logl(-x) * y);
|
|
} else {
|
|
p = -fs_expl(fs_logl(-x) * y);
|
|
}
|
|
} else {
|
|
if (x != 0 || 0 >= y) {
|
|
p = fs_expl(fs_logl( x) * y);
|
|
} else {
|
|
p = 0;
|
|
}
|
|
}
|
|
return p;
|
|
}
|
|
|
|
long double fs_sqrtl(long double x)
|
|
{
|
|
long int n;
|
|
long double a, b;
|
|
|
|
if (x > 0 && LDBL_MAX >= x) {
|
|
for (n = 0; x > 2; x /= 4) {
|
|
++n;
|
|
}
|
|
while (0.5 > x) {
|
|
--n;
|
|
x *= 4;
|
|
}
|
|
a = x;
|
|
b = (1 + x) / 2;
|
|
do {
|
|
x = b;
|
|
b = (a / x + x) / 2;
|
|
} while (x > b);
|
|
while (n > 0) {
|
|
x *= 2;
|
|
--n;
|
|
}
|
|
while (0 > n) {
|
|
x /= 2;
|
|
++n;
|
|
}
|
|
} else {
|
|
if (x != 0) {
|
|
x = LDBL_MAX;
|
|
}
|
|
}
|
|
return x;
|
|
}
|
|
|
|
long double fs_logl(long double x)
|
|
{
|
|
long int n;
|
|
long double a, b, c, epsilon;
|
|
static long double A, B, C;
|
|
static int initialized;
|
|
|
|
if (x > 0 && LDBL_MAX >= x) {
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
B = 1.5;
|
|
do {
|
|
A = B;
|
|
B = 1 / A + A / 2;
|
|
} while (A > B);
|
|
B /= 2;
|
|
C = fs_logl(A);
|
|
}
|
|
for (n = 0; x > A; x /= 2) {
|
|
++n;
|
|
}
|
|
while (B > x) {
|
|
--n;
|
|
x *= 2;
|
|
}
|
|
a = (x - 1) / (x + 1);
|
|
x = C * n + a;
|
|
c = a * a;
|
|
n = 1;
|
|
epsilon = LDBL_EPSILON * x;
|
|
if (0 > a) {
|
|
if (epsilon > 0) {
|
|
epsilon = -epsilon;
|
|
}
|
|
do {
|
|
n += 2;
|
|
a *= c;
|
|
b = a / n;
|
|
x += b;
|
|
} while (epsilon > b);
|
|
} else {
|
|
if (0 > epsilon) {
|
|
epsilon = -epsilon;
|
|
}
|
|
do {
|
|
n += 2;
|
|
a *= c;
|
|
b = a / n;
|
|
x += b;
|
|
} while (b > epsilon);
|
|
}
|
|
x *= 2;
|
|
} else {
|
|
x = -LDBL_MAX;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
long double fs_expl(long double x)
|
|
{
|
|
long unsigned n, square;
|
|
long double b, e;
|
|
static long double x_max, x_min, epsilon;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
x_max = fs_logl(LDBL_MAX);
|
|
x_min = fs_logl(LDBL_MIN);
|
|
epsilon = LDBL_EPSILON / 4;
|
|
}
|
|
if (x_max >= x && x >= x_min) {
|
|
for (square = 0; x > 1; x /= 2) {
|
|
++square;
|
|
}
|
|
while (-1 > x) {
|
|
++square;
|
|
x /= 2;
|
|
}
|
|
e = b = n = 1;
|
|
do {
|
|
b /= n++;
|
|
b *= x;
|
|
e += b;
|
|
b /= n++;
|
|
b *= x;
|
|
e += b;
|
|
} while (b > epsilon);
|
|
while (square-- != 0) {
|
|
e *= e;
|
|
}
|
|
} else {
|
|
e = x > 0 ? LDBL_MAX : 0;
|
|
}
|
|
return e;
|
|
}
|
|
|
|
static long double fs_pil(void)
|
|
{
|
|
long unsigned n;
|
|
long double a, b, epsilon;
|
|
static long double p;
|
|
static int initialized;
|
|
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
epsilon = LDBL_EPSILON / 4;
|
|
n = 1;
|
|
a = 3;
|
|
do {
|
|
a /= 9;
|
|
b = a / n;
|
|
n += 2;
|
|
a /= 9;
|
|
b -= a / n;
|
|
n += 2;
|
|
p += b;
|
|
} while (b > epsilon);
|
|
epsilon = LDBL_EPSILON / 2;
|
|
n = 1;
|
|
a = 2;
|
|
do {
|
|
a /= 4;
|
|
b = a / n;
|
|
n += 2;
|
|
a /= 4;
|
|
b -= a / n;
|
|
n += 2;
|
|
p += b;
|
|
} while (b > epsilon);
|
|
p *= 4;
|
|
}
|
|
return p;
|
|
}
|
|
|
|
long double fs_cosl(long double x)
|
|
{
|
|
long unsigned n;
|
|
int negative, sine;
|
|
long double a, b, c;
|
|
static long double pi, two_pi, half_pi, third_pi, epsilon;
|
|
static int initialized;
|
|
|
|
if (0 > x) {
|
|
x = -x;
|
|
}
|
|
if (LDBL_MAX >= x) {
|
|
if (!initialized) {
|
|
initialized = 1;
|
|
pi = fs_pil();
|
|
two_pi = 2 * pi;
|
|
half_pi = pi / 2;
|
|
third_pi = pi / 3;
|
|
epsilon = LDBL_EPSILON / 2;
|
|
}
|
|
if (x > two_pi) {
|
|
x = fs_fmodl(x, two_pi);
|
|
}
|
|
if (x > pi) {
|
|
x = two_pi - x;
|
|
}
|
|
if (x > half_pi) {
|
|
x = pi - x;
|
|
negative = 1;
|
|
} else {
|
|
negative = 0;
|
|
}
|
|
if (x > third_pi) {
|
|
x = half_pi - x;
|
|
sine = 1;
|
|
} else {
|
|
sine = 0;
|
|
}
|
|
c = x * x;
|
|
x = n = 0;
|
|
a = 1;
|
|
do {
|
|
b = a;
|
|
a *= c;
|
|
a /= ++n;
|
|
a /= ++n;
|
|
b -= a;
|
|
a *= c;
|
|
a /= ++n;
|
|
a /= ++n;
|
|
x += b;
|
|
} while (b > epsilon);
|
|
if (sine) {
|
|
x = fs_sqrtl((1 - x) * (1 + x));
|
|
}
|
|
if (negative) {
|
|
x = -x;
|
|
}
|
|
} else {
|
|
x = -LDBL_MAX;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
long double fs_fmodl(long double x, long double y)
|
|
{
|
|
long double a, b;
|
|
const long double c = x;
|
|
|
|
if (0 > c) {
|
|
x = -x;
|
|
}
|
|
if (0 > y) {
|
|
y = -y;
|
|
}
|
|
if (y != 0 && LDBL_MAX >= y && LDBL_MAX >= x) {
|
|
while (x >= y) {
|
|
a = x / 2;
|
|
b = y;
|
|
while (a >= b) {
|
|
b *= 2;
|
|
}
|
|
x -= b;
|
|
}
|
|
} else {
|
|
x = 0;
|
|
}
|
|
return 0 > c ? -x : x;
|
|
}
|
|
|
|
/* END fs_math.c */
|