CoolPotOS/apps/libs/math.c

807 lines
18 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "../include/math.h"
#include "../include/ctype.h"
static const double ivln10hi =
4.34294481878168880939e-01, /* 0x3fdbcb7b, 0x15200000 */
ivln10lo = 2.50829467116452752298e-11, /* 0x3dbb9438, 0xca9aadd5 */
log10_2hi = 3.01029995663611771306e-01, /* 0x3FD34413, 0x509F6000 */
log10_2lo = 3.69423907715893078616e-13, /* 0x3D59FEF3, 0x11F12B36 */
Lg1 = 6.666666666666735130e-01, /* 3FE55555 55555593 */
Lg2 = 3.999999999940941908e-01, /* 3FD99999 9997FA04 */
Lg3 = 2.857142874366239149e-01, /* 3FD24924 94229359 */
Lg4 = 2.222219843214978396e-01, /* 3FCC71C5 1D8E78AF */
Lg5 = 1.818357216161805012e-01, /* 3FC74664 96CB03DE */
Lg6 = 1.531383769920937332e-01, /* 3FC39A09 D078C69F */
Lg7 = 1.479819860511658591e-01; /* 3FC2F112 DF3E5244 */
#define sub_ddmmss(sh, sl, ah, al, bh, bl) \
__asm__ ("subl %5,%1\n\tsbbl %3,%0" \
: "=r" ((USItype) (sh)), \
"=&r" ((USItype) (sl)) \
: "0" ((USItype) (ah)), \
"g" ((USItype) (bh)), \
"1" ((USItype) (al)), \
"g" ((USItype) (bl)))
#define umul_ppmm(w1, w0, u, v) \
__asm__ ("mull %3" \
: "=a" ((USItype) (w0)), \
"=d" ((USItype) (w1)) \
: "%0" ((USItype) (u)), \
"rm" ((USItype) (v)))
#define udiv_qrnnd(q, r, n1, n0, dv) \
__asm__ ("divl %4" \
: "=a" ((USItype) (q)), \
"=d" ((USItype) (r)) \
: "0" ((USItype) (n0)), \
"1" ((USItype) (n1)), \
"rm" ((USItype) (dv)))
#define count_leading_zeros(count, x) \
do { \
USItype __cbtmp; \
__asm__ ("bsrl %1,%0" \
: "=r" (__cbtmp) : "rm" ((USItype) (x))); \
(count) = __cbtmp ^ 31; \
} while (0)
static unsigned rand_seed = 1;
static unsigned short max_bit = 32;
static unsigned short randlevel = 1;
#define __negdi2(a) (-(a))
static UDWtype __udivmoddi4(UDWtype n, UDWtype d, UDWtype *rp) {
DWunion ww;
DWunion nn, dd;
DWunion rr;
UWtype d0, d1, n0, n1, n2;
UWtype q0, q1;
UWtype b, bm;
nn.ll = n;
dd.ll = d;
d0 = dd.s.low;
d1 = dd.s.high;
n0 = nn.s.low;
n1 = nn.s.high;
#if !defined(UDIV_NEEDS_NORMALIZATION)
if (d1 == 0) {
if (d0 > n1) {
/* 0q = nn / 0D */
udiv_qrnnd(q0, n0, n1, n0, d0);
q1 = 0;
/* Remainder in n0. */
} else {
/* qq = NN / 0d */
if (d0 == 0)
d0 = 1 / d0; /* Divide intentionally by zero. */
udiv_qrnnd(q1, n1, 0, n1, d0);
udiv_qrnnd(q0, n0, n1, n0, d0);
/* Remainder in n0. */
}
if (rp != 0) {
rr.s.low = n0;
rr.s.high = 0;
*rp = rr.ll;
}
}
#else /* UDIV_NEEDS_NORMALIZATION */
if (d1 == 0)
{
if (d0 > n1)
{
/* 0q = nn / 0D */
count_leading_zeros (bm, d0);
if (bm != 0)
{
/* Normalize, i.e. make the most significant bit of the
denominator set. */
d0 = d0 << bm;
n1 = (n1 << bm) | (n0 >> (W_TYPE_SIZE - bm));
n0 = n0 << bm;
}
udiv_qrnnd (q0, n0, n1, n0, d0);
q1 = 0;
/* Remainder in n0 >> bm. */
}
else
{
/* qq = NN / 0d */
if (d0 == 0)
d0 = 1 / d0; /* Divide intentionally by zero. */
count_leading_zeros (bm, d0);
if (bm == 0)
{
/* From (n1 >= d0) /\ (the most significant bit of d0 is set),
conclude (the most significant bit of n1 is set) /\ (the
leading quotient digit q1 = 1).
This special case is necessary, not an optimization.
(Shifts counts of W_TYPE_SIZE are undefined.) */
n1 -= d0;
q1 = 1;
}
else
{
/* Normalize. */
b = W_TYPE_SIZE - bm;
d0 = d0 << bm;
n2 = n1 >> b;
n1 = (n1 << bm) | (n0 >> b);
n0 = n0 << bm;
udiv_qrnnd (q1, n1, n2, n1, d0);
}
/* n1 != d0... */
udiv_qrnnd (q0, n0, n1, n0, d0);
/* Remainder in n0 >> bm. */
}
if (rp != 0)
{
rr.s.low = n0 >> bm;
rr.s.high = 0;
*rp = rr.ll;
}
}
#endif /* UDIV_NEEDS_NORMALIZATION */
else {
if (d1 > n1) {
/* 00 = nn / DD */
q0 = 0;
q1 = 0;
/* Remainder in n1n0. */
if (rp != 0) {
rr.s.low = n0;
rr.s.high = n1;
*rp = rr.ll;
}
} else {
/* 0q = NN / dd */
count_leading_zeros(bm, d1);
if (bm == 0) {
/* From (n1 >= d1) /\ (the most significant bit of d1 is set),
conclude (the most significant bit of n1 is set) /\ (the
quotient digit q0 = 0 or 1).
This special case is necessary, not an optimization. */
/* The condition on the next line takes advantage of that
n1 >= d1 (true due to program flow). */
if (n1 > d1 || n0 >= d0) {
q0 = 1;
sub_ddmmss(n1, n0, n1, n0, d1, d0);
} else
q0 = 0;
q1 = 0;
if (rp != 0) {
rr.s.low = n0;
rr.s.high = n1;
*rp = rr.ll;
}
} else {
UWtype m1, m0;
/* Normalize. */
b = W_TYPE_SIZE - bm;
d1 = (d1 << bm) | (d0 >> b);
d0 = d0 << bm;
n2 = n1 >> b;
n1 = (n1 << bm) | (n0 >> b);
n0 = n0 << bm;
udiv_qrnnd(q0, n1, n2, n1, d1);
umul_ppmm(m1, m0, q0, d0);
if (m1 > n1 || (m1 == n1 && m0 > n0)) {
q0--;
sub_ddmmss(m1, m0, m1, m0, d1, d0);
}
q1 = 0;
/* Remainder in (n1n0 - m1m0) >> bm. */
if (rp != 0) {
sub_ddmmss(n1, n0, n1, n0, m1, m0);
rr.s.low = (n1 << b) | (n0 >> bm);
rr.s.high = n1 >> bm;
*rp = rr.ll;
}
}
}
}
ww.s.low = q0;
ww.s.high = q1;
return ww.ll;
}
__attribute__((used)) long long __moddi3(long long u, long long v) {
int c = 0;
DWunion uu, vv;
DWtype w;
uu.ll = u;
vv.ll = v;
if (uu.s.high < 0) {
c = ~c;
uu.ll = __negdi2 (uu.ll);
}
if (vv.s.high < 0)
vv.ll = __negdi2 (vv.ll);
__udivmoddi4(uu.ll, vv.ll, (UDWtype *) &w);
if (c)
w = __negdi2 (w);
return w;
}
int rand() {
unsigned short i = 0;
while (i < randlevel)
rand_seed = rand_seed * 1103515245 + 12345, rand_seed <<= max_bit, i++;
return (rand_seed >>= max_bit);
}
void srand(unsigned seed) {
rand_seed = seed;
}
void smax(unsigned short max_b) {
max_b = (sizeof(unsigned long long) * 8) - (max_b % (sizeof(unsigned long long) * 8));
max_bit = (max_b == 0) ? (sizeof(unsigned long long) * 8 / 2) : (max_b);
}
void srandlevel(unsigned short randlevel_) {
if (randlevel_ != 0)
randlevel = randlevel_;
}
int32_t abs(int32_t x) {
return (x < 0) ? (-x) : (x);
}
double pow(double a, long long b) {
char t = 0;
if (b < 0)b = -b, t = 1;
double ans = 1;
while (b) {
if (b & 1)ans *= a;
a *= a;
b >>= 1;
}
if (t)return (1.0 / ans);
else return ans;
}
//快速整数平方
unsigned long long ull_pow(unsigned long long a, unsigned long long b) {
unsigned long long ans = 1;
while (b) {
if (b & 1)ans *= a;
a *= a;
b >>= 1;
}
return ans;
}
double sqrt(double x) {
if (x == 0)return 0.0;
double xk = 1.0, xk1 = 0.0;
while (xk != xk1)xk1 = xk, xk = (xk + x / xk) / 2.0;
return xk;
}
//快速求算数平方根(速度快,精度低)
float q_sqrt(float number) {
long i;
float x, y;
const float f = 1.5F;
x = number * 0.5F;
y = number;
i = *(long *) (&y);
i = 0x5f3759df - (i >> 1);
y = *(float *) (&i);
y = y * (f - (x * y * y));
y = y * (f - (x * y * y));
return number * y;
}
double mod(double x, double y) {
return x - (int32_t)(x / y) * y;
}
double sin(double x) {
x = mod(x, 2 * PI);
double sum = x;
double term = x;
int n = 1;
bool sign = true;
while (term > F64_EPSILON || term < -F64_EPSILON) {
n += 2;
term *= x * x / (n * (n - 1));
sum += sign ? -term : term;
sign = !sign;
}
return sum;
}
double cos(double x) {
x = mod(x, 2 * PI);
double sum = 1;
double term = 1;
int n = 0;
bool sign = true;
while (term > F64_EPSILON || term < -F64_EPSILON) {
n += 2;
term *= x * x / (n * (n - 1));
sum += sign ? -term : term;
sign = !sign;
}
return sum;
}
double tan(double x) {
return sin(x) / cos(x);
}
double asin(double x) {
double sum = x;
double term = x;
int n = 1;
while (term > F64_EPSILON || term < -F64_EPSILON) {
term *= (x * x * (2 * n - 1) * (2 * n - 1)) / (2 * n * (2 * n + 1));
sum += term;
n++;
}
return sum;
}
double acos(double x) {
return PI / 2 - asin(x);
}
double atan(double x) {
double sum = x;
double term = x;
int n = 1;
bool sign = true;
while (term > F64_EPSILON || term < -F64_EPSILON) {
term *= x * x * (2 * n - 1) / (2 * n + 1);
sum += sign ? -term : term;
sign = !sign;
n++;
}
return sum;
}
double atan2(double y, double x) {
if (x > 0) return atan(y / x);
if (x < 0 && y >= 0) return atan(y / x) + PI;
if (x < 0 && y < 0) return atan(y / x) - PI;
if (x == 0 && y > 0) return PI / 2;
if (x == 0 && y < 0) return -PI / 2;
return 0;
}
double floor(double x) {
int flag = 1;
if (fabs(x) == x) {
flag = 0;
}
if (flag) {
double r = (x); // 如果小于则返回x-1的整数部分
double s;
modf(r, &s);
return s - 1;
} else {
double r = (x); // 如果大于等于则返回x的整数部分
double s;
modf(r, &s);
return s;
}
}
double modf(double x, double *iptr) {
union {
double f;
uint64_t i;
} u = {x};
uint64_t mask;
int e = (int) (u.i >> 52 & 0x7ff) - 0x3ff;
/* no fractional part */
if (e >= 52) {
*iptr = x;
if (e == 0x400 && u.i << 12 != 0) /* nan */
return x;
u.i &= 1ULL << 63;
return u.f;
}
/* no integral part*/
if (e < 0) {
u.i &= 1ULL << 63;
*iptr = u.f;
return x;
}
mask = -1ULL >> 12 >> e;
if ((u.i & mask) == 0) {
*iptr = x;
u.i &= 1ULL << 63;
return u.f;
}
u.i &= ~mask;
*iptr = u.f;
return x - u.f;
}
double fabs(double x) {
union {
double f;
uint64_t i;
} u = {x};
u.i &= -1ULL / 2;
return u.f;
}
double ceil(double x) {
int flag = 0;
if (fabs(x) == x) {
flag = 1;
}
if (flag) {
double r = (x); // 如果小于则返回x-1的整数部分
double s;
modf(r, &s);
return s + 1;
} else {
double r = (x); // 如果大于等于则返回x的整数部分
double s;
modf(r, &s);
return s;
}
}
#define __negdi2(a) (-(a))
long long __divdi3(long long u, long long v) {
int c = 0;
DWunion uu, vv;
DWtype w;
uu.ll = u;
vv.ll = v;
if (uu.s.high < 0) {
c = ~c;
uu.ll = __negdi2 (uu.ll);
}
if (vv.s.high < 0) {
c = ~c;
vv.ll = __negdi2 (vv.ll);
}
w = __udivmoddi4(uu.ll, vv.ll, (UDWtype *) 0);
if (c)
w = __negdi2 (w);
return w;
}
double frexp(double x, int *e) {
union {
double d;
uint64_t i;
} y = {x};
int ee = y.i >> 52 & 0x7ff;
if (!ee) {
if (x) {
x = frexp(x * 0x1p64, e);
*e -= 64;
} else
*e = 0;
return x;
} else if (ee == 0x7ff) {
return x;
}
*e = ee - 0x3fe;
y.i &= 0x800fffffffffffffull;
y.i |= 0x3fe0000000000000ull;
return y.d;
}
double scalbn(double x, int n) {
union {
double f;
uint64_t i;
} u;
double y = x;
if (n > 1023) {
y *= 0x1p1023;
n -= 1023;
if (n > 1023) {
y *= 0x1p1023;
n -= 1023;
if (n > 1023)
n = 1023;
}
} else if (n < -1022) {
y *= 0x1p-1022;
n += 1022;
if (n < -1022) {
y *= 0x1p-1022;
n += 1022;
if (n < -1022)
n = -1022;
}
}
u.i = (uint64_t)(0x3ff + n) << 52;
x = y * u.f;
return x;
}
double scalbln(double x, long n) {
if (n > INT_MAX)
n = INT_MAX;
else if (n < INT_MIN)
n = INT_MIN;
return scalbn(x, n);
}
double ldexp(double x, int n) { return scalbn(x, n); }
float scalbnf(float x, int n) {
union {
float f;
uint32_t i;
} u;
float y = x;
if (n > 127) {
y *= 0x1p127f;
n -= 127;
if (n > 127) {
y *= 0x1p127f;
n -= 127;
if (n > 127)
n = 127;
}
} else if (n < -126) {
y *= 0x1p-126f * 0x1p24f;
n += 126 - 24;
if (n < -126) {
y *= 0x1p-126f * 0x1p24f;
n += 126 - 24;
if (n < -126)
n = -126;
}
}
u.i = (uint32_t)(0x7f + n) << 23;
x = y * u.f;
return x;
}
double fmod(double x, double y) {
return x - (x / y) * y;
/*
union {
double f;
uint64_t i;
} ux = {x}, uy = {y};
int ex = ux.i >> 52 & 0x7ff;
int ey = uy.i >> 52 & 0x7ff;
int sx = ux.i >> 63;
uint64_t i;
uint64_t uxi = ux.i;
if (uy.i << 1 == 0 || isnan(y) || ex == 0x7ff)
return (x * y) / (x * y);
if (uxi << 1 <= uy.i << 1) {
if (uxi << 1 == uy.i << 1)
return 0 * x;
return x;
}
if (!ex) {
for (i = uxi << 12; i >> 63 == 0; ex--, i <<= 1);
uxi <<= -ex + 1;
} else {
uxi &= -1ULL >> 12;
uxi |= 1ULL << 52;
}
if (!ey) {
for (i = uy.i << 12; i >> 63 == 0; ey--, i <<= 1);
uy.i <<= -ey + 1;
} else {
uy.i &= -1ULL >> 12;
uy.i |= 1ULL << 52;
}
for (; ex > ey; ex--) {
i = uxi - uy.i;
if (i >> 63 == 0) {
if (i == 0)
return 0 * x;
uxi = i;
}
uxi <<= 1;
}
i = uxi - uy.i;
if (i >> 63 == 0) {
if (i == 0)
return 0 * x;
uxi = i;
}
for (; uxi >> 52 == 0; uxi <<= 1, ex--);
if (ex > 0) {
uxi -= 1ULL << 52;
uxi |= (uint64_t) ex << 52;
} else {
uxi >>= -ex + 1;
}
uxi |= (uint64_t) sx << 63;
ux.i = uxi;
return ux.f;
*/
}
double exp(double x) {
x = 1.0 + x / 256;
x *= x;
x *= x;
x *= x;
x *= x;
x *= x;
x *= x;
x *= x;
x *= x;
return x;
}
double log10(double x) {
union {
double f;
uint64_t i;
} u = {x};
double hfsq, f, s, z, R, w, t1, t2, dk, y, hi, lo, val_hi, val_lo;
uint32_t hx;
int k;
hx = u.i >> 32;
k = 0;
if (hx < 0x00100000 || hx >> 31) {
if (u.i << 1 == 0)
return -1 / (x * x); /* log(+-0)=-inf */
if (hx >> 31)
return (x - x) / 0.0; /* log(-#) = NaN */
/* subnormal number, scale x up */
k -= 54;
x *= 0x1p54;
u.f = x;
hx = u.i >> 32;
} else if (hx >= 0x7ff00000) {
return x;
} else if (hx == 0x3ff00000 && u.i << 32 == 0)
return 0;
/* reduce x into [sqrt(2)/2, sqrt(2)] */
hx += 0x3ff00000 - 0x3fe6a09e;
k += (int) (hx >> 20) - 0x3ff;
hx = (hx & 0x000fffff) + 0x3fe6a09e;
u.i = (uint64_t) hx << 32 | (u.i & 0xffffffff);
x = u.f;
f = x - 1.0;
hfsq = 0.5 * f * f;
s = f / (2.0 + f);
z = s * s;
w = z * z;
t1 = w * (Lg2 + w * (Lg4 + w * Lg6));
t2 = z * (Lg1 + w * (Lg3 + w * (Lg5 + w * Lg7)));
R = t2 + t1;
/* See log2.c for details. */
/* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
hi = f - hfsq;
u.f = hi;
u.i &= (uint64_t) - 1 << 32;
hi = u.f;
lo = f - hi - hfsq + s * (hfsq + R);
/* val_hi+val_lo ~ log10(1+f) + k*log10(2) */
val_hi = hi * ivln10hi;
dk = k;
y = dk * log10_2hi;
val_lo = dk * log10_2lo + (lo + hi) * ivln10lo + lo * ivln10hi;
/*
* Extra precision in for adding y is not strictly needed
* since there is no very large cancellation near x = sqrt(2) or
* x = 1/sqrt(2), but we do it anyway since it costs little on CPUs
* with some parallelism and it reduces the error for many args.
*/
w = y + val_hi;
val_lo += (y - w) + val_hi;
val_hi = w;
return val_lo + val_hi;
}
double log2(float x) {
long *a;
double o;
a = (long *) &x;
o = (double) *a;
o = o / POW223 - 126.928071372;
return o;
}
double log(double a) {
int N = 15;//我们取了前15+1项来估算
int k, nk;
double x, xx, y;
x = (a - 1) / (a + 1);
xx = x * x;
nk = 2 * N + 1;
y = 1.0 / nk;
for (k = N; k > 0; k--) {
nk = nk - 2;
y = 1.0 / nk + xx * y;
}
return 2.0 * x * y;
}
unsigned long long __udivdi3(unsigned long long u, unsigned long long v) {
return __udivmoddi4(u, v, (UDWtype *) 0);
}
unsigned long long __umoddi3(unsigned long long u, unsigned long long v) {
UDWtype w;
__udivmoddi4(u, v, &w);
return w;
}