diff options
Diffstat (limited to 'libk/src/math')
-rw-r--r-- | libk/src/math/abs.c | 14 | ||||
-rw-r--r-- | libk/src/math/ceil.c | 59 | ||||
-rw-r--r-- | libk/src/math/copysign.c | 20 | ||||
-rw-r--r-- | libk/src/math/fact.c | 29 | ||||
-rw-r--r-- | libk/src/math/fclamp.c | 13 | ||||
-rw-r--r-- | libk/src/math/floor.c | 55 | ||||
-rw-r--r-- | libk/src/math/fma.c | 38 | ||||
-rw-r--r-- | libk/src/math/fmod.c | 129 | ||||
-rw-r--r-- | libk/src/math/frexp.c | 19 | ||||
-rw-r--r-- | libk/src/math/pow.c | 152 | ||||
-rw-r--r-- | libk/src/math/sqrt.c | 36 | ||||
-rw-r--r-- | libk/src/math/trig.c | 62 |
12 files changed, 626 insertions, 0 deletions
diff --git a/libk/src/math/abs.c b/libk/src/math/abs.c new file mode 100644 index 0000000..1757272 --- /dev/null +++ b/libk/src/math/abs.c @@ -0,0 +1,14 @@ +#include <math.h> +#include <stdint.h> + +float fabsf(float num) { + union {float f; uint32_t i;} u = {num}; + u.i &= 0x7fffffff; + return u.f; +} + +double fabs(double num) { + union {double f; uint64_t i;} u = {num}; + u.i &= -1ULL/2; + return u.f; +} diff --git a/libk/src/math/ceil.c b/libk/src/math/ceil.c new file mode 100644 index 0000000..17b667f --- /dev/null +++ b/libk/src/math/ceil.c @@ -0,0 +1,59 @@ +#include <stdint.h> +#include <float.h> +#include <math.h> + +#include "internal/libm.h" + +#define EPS DBL_EPSILON + +static const double toint = 1/EPS; + +double ceil(double x) +{ + union {double f; uint64_t i;} u = {x}; + int e = u.i >> 52 & 0x7ff; + double y; + + if (e >= 0x3ff+52 || x == 0) + return x; + + if (u.i >> 63) { + y = x - toint + toint - x; + } else { + y = x + toint - toint - x; + } + + if (e <= 0x3ff-1) { + FORCE_EVAL(y); + return u.i >> 63 ? -0.0 : 1; + } + + if (y < 0) + return x + y + 1; + return x + y; +} + +float ceilf(float x) { + union {float f; uint32_t i;} u = {x}; + int e = (int)(u.i >> 23 & 0xff) - 0x7f; + uint32_t m; + + if (e >= 23) + return x; + if (e >= 0) { + m = 0x007fffff >> e; + if ((u.i & m) == 0) + return x; + FORCE_EVAL(x + 0x1p120f); + if (u.i >> 31 == 0) + u.i += m; + u.i &= ~m; + } else { + FORCE_EVAL(x + 0x1p120f); + if (u.i >> 31) + u.f = -0.0; + else if (u.i << 1) + u.f = 1.0; + } + return u.f; +} diff --git a/libk/src/math/copysign.c b/libk/src/math/copysign.c new file mode 100644 index 0000000..fd5fd4f --- /dev/null +++ b/libk/src/math/copysign.c @@ -0,0 +1,20 @@ +#include <math.h> +#include <stdint.h> + +#define DMASK 0x8000000000000000 + +double copysign(double n, double s) { + union {double f; uint64_t i;} sb = {s}; + union {double f; uint64_t i;} nb = {n}; + nb.i = (nb.i & ~DMASK) | (sb.i & DMASK); + return nb.f; +} + +#define FMASK 0x80000000 + +float copysignf(float n, float s) { + union {float f; uint32_t i;} sb = {s}; + union {float f; uint32_t i;} nb = {n}; + nb.i = (nb.i & ~FMASK) | (sb.i & FMASK); + return nb.f; +} diff --git a/libk/src/math/fact.c b/libk/src/math/fact.c new file mode 100644 index 0000000..a146e4d --- /dev/null +++ b/libk/src/math/fact.c @@ -0,0 +1,29 @@ +#include <math.h> + +static unsigned int FACT_TABLE[] = { + 1, // 0 + 1, // 1 + 2, // 2 + 6, // 3 + 24, // 4 + 120, // 5 + 720, // 6 + 5040, // 7 + 40320, // 8 + 362880, // 9 + 3628800, // 10 + 39916800, // 11 + 479001600, // 12 +}; + +unsigned long long fact(unsigned int num) { + if (num < 13) { + return FACT_TABLE[num]; + } + unsigned long long l = FACT_TABLE[12]; + for (unsigned int i = 12; i < num;) { + i++; + l *= i; + } + return l; +} diff --git a/libk/src/math/fclamp.c b/libk/src/math/fclamp.c new file mode 100644 index 0000000..f757b48 --- /dev/null +++ b/libk/src/math/fclamp.c @@ -0,0 +1,13 @@ +#include <math.h> + +double fclamp(double x, double l, double h) { + if (x < l) return l; + if (x > h) return h; + return x; +} + +float fclampf(float x, float l, float h) { + if (x < l) return l; + if (x > h) return h; + return x; +} diff --git a/libk/src/math/floor.c b/libk/src/math/floor.c new file mode 100644 index 0000000..ce8e07e --- /dev/null +++ b/libk/src/math/floor.c @@ -0,0 +1,55 @@ +#include <stdint.h> +#include <float.h> +#include <math.h> + +#include "internal/libm.h" + +#define EPS DBL_EPSILON + +static const double toint = 1/EPS; + +double floor(double x) { + union {double f; uint64_t i;} u = {x}; + int e = u.i >> 52 & 0x7ff; + double y; + + if (e >= 0x3ff+52 || x == 0) + return x; + if (u.i >> 63) + y = x - toint + toint - x; + else + y = x + toint - toint - x; + if (e <= 0x3ff-1) { + return u.i >> 63 ? -1 : 0; + } + if (y > 0) { + FORCE_EVAL(y); + return x + y - 1; + } + return x + y; +} + +float floorf(float x) { + union {float f; uint32_t i;} u = {x}; + int e = (int)(u.i >> 23 & 0xff) - 0x7f; + uint32_t m; + + if (e >= 23) + return x; + if (e >= 0) { + m = 0x007fffff >> e; + if ((u.i & m) == 0) + return x; + FORCE_EVAL(x + 0x1p120f); + if (u.i >> 31) + u.i += m; + u.i &= ~m; + } else { + FORCE_EVAL(x + 0x1p120f); + if (u.i >> 31 == 0) + u.i = 0; + else if (u.i << 1) + u.f = -1.0; + } + return u.f; +} diff --git a/libk/src/math/fma.c b/libk/src/math/fma.c new file mode 100644 index 0000000..b26229e --- /dev/null +++ b/libk/src/math/fma.c @@ -0,0 +1,38 @@ +#include <stdint.h> +#include <math.h> + +double fma(double x, double y, double z) { + double xy, result; + union {double f; uint64_t i;} u; + int e; + + xy = (double)x * y; + result = xy + z; + u.f = result; + e = u.i>>52 & 0x7ff; + if ( + (u.i & 0x1fffffff) != 0x10000000 || + e == 0x7ff || + (result - xy == z && result - z == xy) + ) { + z = result; + return z; + } + + double err; + int neg = u.i >> 63; + if (neg == (z > xy)) + err = xy - result + z; + else + err = z - result + xy; + if (neg == (err < 0)) + u.i++; + else + u.i--; + z = u.f; + return z; +} + +float fmaf(float x, float y, float z) { + return fma(x, y, z); +} diff --git a/libk/src/math/fmod.c b/libk/src/math/fmod.c new file mode 100644 index 0000000..996f78b --- /dev/null +++ b/libk/src/math/fmod.c @@ -0,0 +1,129 @@ +#include <math.h> +#include <stdint.h> + +double fmod(double x, double 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; + + /* in the followings uxi should be ux.i, but then gcc wrongly adds */ + /* float load/store to inner loops ruining performance and code size */ + 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; + } + + /* normalize x and y */ + 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; + } + + /* x mod y */ + 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--); + + /* scale result */ + 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; +} + +float fmodf(float x, float y) { + union {float f; uint32_t i;} ux = {x}, uy = {y}; + int ex = ux.i>>23 & 0xff; + int ey = uy.i>>23 & 0xff; + uint32_t sx = ux.i & 0x80000000; + uint32_t i; + uint32_t uxi = ux.i; + + if (uy.i<<1 == 0 || isnan(y) || ex == 0xff) + return (x*y)/(x*y); + if (uxi<<1 <= uy.i<<1) { + if (uxi<<1 == uy.i<<1) + return 0*x; + return x; + } + + /* normalize x and y */ + if (!ex) { + for (i = uxi<<9; i>>31 == 0; ex--, i <<= 1); + uxi <<= -ex + 1; + } else { + uxi &= -1U >> 9; + uxi |= 1U << 23; + } + if (!ey) { + for (i = uy.i<<9; i>>31 == 0; ey--, i <<= 1); + uy.i <<= -ey + 1; + } else { + uy.i &= -1U >> 9; + uy.i |= 1U << 23; + } + + /* x mod y */ + for (; ex > ey; ex--) { + i = uxi - uy.i; + if (i >> 31 == 0) { + if (i == 0) + return 0*x; + uxi = i; + } + uxi <<= 1; + } + i = uxi - uy.i; + if (i >> 31 == 0) { + if (i == 0) + return 0*x; + uxi = i; + } + for (; uxi>>23 == 0; uxi <<= 1, ex--); + + /* scale result up */ + if (ex > 0) { + uxi -= 1U << 23; + uxi |= (uint32_t)ex << 23; + } else { + uxi >>= -ex + 1; + } + uxi |= sx; + ux.i = uxi; + return ux.f; +} diff --git a/libk/src/math/frexp.c b/libk/src/math/frexp.c new file mode 100644 index 0000000..16b8dd3 --- /dev/null +++ b/libk/src/math/frexp.c @@ -0,0 +1,19 @@ +#include <math.h> + +double frexp(double arg, int* exp) { + *exp = 0; + while (arg > 1) { + arg /= 2;; + (*exp)++; + } + return arg; +} + +float frexpf(float arg, int* exp) { + *exp = 0; + while (arg > 1) { + arg /= 2; + (*exp)++; + } + return arg; +} diff --git a/libk/src/math/pow.c b/libk/src/math/pow.c new file mode 100644 index 0000000..c850db7 --- /dev/null +++ b/libk/src/math/pow.c @@ -0,0 +1,152 @@ +#include <stdint.h> +#include <string.h> +#include <math.h> + +uint32_t float_as_uint32 (float a) { + uint32_t r; + memcpy (&r, &a, sizeof r); + return r; +} + +float uint32_as_float (uint32_t a) { + float r; + memcpy (&r, &a, sizeof r); + return r; +} + +void logf_ext (float a, float *loghi, float *loglo) { + const float LOG2_HI = 6.93147182e-1f; + const float LOG2_LO = -1.90465421e-9f; + const float SQRT_HALF = 0.70710678f; + float m, r, i, s, t, p, qhi, qlo; + int e; + + const float POW_TWO_M23 = 1.19209290e-7f; + const float POW_TWO_P23 = 8388608.0f; + const float FP32_MIN_NORM = 1.175494351e-38f; + i = 0.0f; + + if (a < FP32_MIN_NORM) { + a = a * POW_TWO_P23; + i = -23.0f; + } + + e = (float_as_uint32 (a) - float_as_uint32 (SQRT_HALF)) & 0xff800000; + m = uint32_as_float (float_as_uint32 (a) - e); + i = fmaf ((float)e, POW_TWO_M23, i); + + p = m + 1.0f; + m = m - 1.0f; + r = 1.0f / p; + qhi = r * m; + qlo = r * fmaf (qhi, -m, fmaf (qhi, -2.0f, m)); + + s = qhi * qhi; + r = 0.1293334961f; + r = fmaf (r, s, 0.1419928074f); + r = fmaf (r, s, 0.2000148296f); + r = fmaf (r, s, 0.3333332539f); + t = fmaf (qhi, qlo + qlo, fmaf (qhi, qhi, -s)); + p = s * qhi; + t = fmaf (s, qlo, fmaf (t, qhi, fmaf (s, qhi, -p))); + s = fmaf (r, p, fmaf (r, t, qlo)); + r = 2 * qhi; + + t = fmaf ( LOG2_HI, i, r); + p = fmaf (-LOG2_HI, i, t); + s = fmaf ( LOG2_LO, i, fmaf (2.f, s, r - p)); + *loghi = p = t + s; + *loglo = (t - p) + s; +} + +float expf_unchecked (float a) { + float f, j, r; + int i; + + j = fmaf (1.442695f, a, 12582912.f) - 12582912.f; + f = fmaf (j, -6.93145752e-1f, a); + f = fmaf (j, -1.42860677e-6f, f); + i = (int)j; + + r = 1.37805939e-3f; + r = fmaf (r, f, 8.37312452e-3f); + r = fmaf (r, f, 4.16695364e-2f); + r = fmaf (r, f, 1.66664720e-1f); + r = fmaf (r, f, 4.99999851e-1f); + r = fmaf (r, f, 1.00000000e+0f); + r = fmaf (r, f, 1.00000000e+0f); + + float s, t; + uint32_t ia = (i > 0) ? 0u : 0x83000000u; + s = uint32_as_float (0x7f000000u + ia); + t = uint32_as_float (((uint32_t)i << 23) - ia); + r = r * s; + r = r * t; + + return r; +} + +float powf_core (float a, float b) { + const float MAX_IEEE754_FLT = uint32_as_float (0x7f7fffff); + const float EXP_OVFL_BOUND = 88.7228394f; // 0x1.62e430p+6f; + const float EXP_OVFL_UNFL_F = 104.0f; + const float MY_INF_F = uint32_as_float (0x7f800000); + float lhi, llo, thi, tlo, phi, plo, r; + + logf_ext (a, &lhi, &llo); + + thi = lhi * b; + + if (fabsf (thi) > EXP_OVFL_UNFL_F) { + r = (thi < 0.0f) ? 0.0f : MY_INF_F; + } else { + tlo = fmaf (lhi, b, -thi); + tlo = fmaf (llo, b, +tlo); + phi = thi + tlo; + + if (phi == EXP_OVFL_BOUND) + phi = uint32_as_float (float_as_uint32 (phi) - 1); + + plo = (thi - phi) + tlo; + r = expf_unchecked (phi); + if (fabsf (r) <= MAX_IEEE754_FLT) { + r = fmaf (plo, r, r); + } + } + return r; +} + +float powf (float a, float b) { + const float MY_INF_F = uint32_as_float (0x7f800000); + const float MY_NAN_F = uint32_as_float (0xffc00000); + int expo_odd_int; + float r; + + expo_odd_int = fmaf (-2.0f, floorf (0.5f * b), b) == 1.0f; + if ((a == 1.0f) || (b == 0.0f)) { + r = 1.0f; + } else if (isnan (a) || isnan (b)) { + r = a + b; + } else if (isinf (b)) { + r = ((fabsf (a) < 1.0f) != (b < 0.0f)) ? 0.0f : MY_INF_F; + if (a == -1.0f) r = 1.0f; + } else if (isinf (a)) { + r = (b < 0.0f) ? 0.0f : MY_INF_F; + if ((a < 0.0f) && expo_odd_int) r = -r; + } else if (a == 0.0f) { + r = (expo_odd_int) ? (a + a) : 0.0f; + if (b < 0.0f) r = copysignf (MY_INF_F, r); + } else if ((a < 0.0f) && (b != floorf (b))) { + r = MY_NAN_F; + } else { + r = powf_core (fabsf (a), b); + if ((a < 0.0f) && expo_odd_int) { + r = -r; + } + } + return r; +} + +double pow (double a, double b) { + return powf(a, b); // who cares about precision in a kernel :3 +} diff --git a/libk/src/math/sqrt.c b/libk/src/math/sqrt.c new file mode 100644 index 0000000..b875079 --- /dev/null +++ b/libk/src/math/sqrt.c @@ -0,0 +1,36 @@ +#include <math.h> + +float sqrtf(float num) { + if (num < 0) + return NAN; + + if (num < 2) + return num; + + float y = num; + float z = (y + (num / y)) / 2; + + while (fabsf(y - z) >= 0.00001) { + y = z; + z = (y + (num / y)) / 2; + } + return z; +} + +double sqrt(double num) { + if (num < 0) + return NAN; + + if (num < 2) + return num; + + double y = num; + double z = (y + (num / y)) / 2; + + while (fabs(y - z) >= 0.00001) { + y = z; + z = (y + (num / y)) / 2; + } + return z; +} + diff --git a/libk/src/math/trig.c b/libk/src/math/trig.c new file mode 100644 index 0000000..f234d69 --- /dev/null +++ b/libk/src/math/trig.c @@ -0,0 +1,62 @@ +#include <math.h> + +double sin(double r) { + r = fmod(r + PI, PI * 2) - PI; + double d = r; + float mult = -1; + for (int i = 3; i < 11; i+= 2, mult = -mult) { + d += (pow(r, i)/fact(i)) * mult; + } + return d; +} + +double cos(double r) { + r = fmod(r + PI, PI * 2) - PI; + double d = 1; + float mult = -1; + for (int i = 2; i < 10; i+= 2, mult = -mult) { + d += (pow(r, i)/fact(i)) * mult; + } + return d; +} + +double tan(double r) { + return sin(r) / cos(r); +} + +double csc(double r) { + return 1 / sin(r); +} + +double sec(double r) { + return 1 / cos(r); +} + +double cot(double r) { + return cos(r) / sin(r); +} + +double sinh(double r) { + return (pow(E, r) - pow(E, -r)) / 2; +} + +double cosh(double r) { + return (pow(E, r) + pow(E, -r)) / 2; +} + +double tanh(double r) { + return sinh(r) / cosh(r); +} + +double csch(double r) { + return 1 / sinh(r); +} + +double sech(double r) { + return 1 / cosh(r); +} + +double coth(double r) { + return cosh(r) / sinh(r); +} + |