diff --git a/fixmath/fixmath.c b/fixmath/fixmath.c new file mode 100644 index 0000000000000000000000000000000000000000..99d9fe66a4988680e363a25eb9491fb62d53782c --- /dev/null +++ b/fixmath/fixmath.c @@ -0,0 +1,120 @@ +#include "fixmath.h" + +#include <stdint> + +fix_t fix_emul(fix_t x, fix_t y) { + uint64_t big_x = x.raw; + uint64_t big_y = y.raw; + return (fix_t) { .raw = (big_x * big_y) >> 16 } +} + +fix_t fix_ediv(fix_t x, fix_t y) { + uint64_t big_x = x.raw; + uint64_t big_y = y.raw; + return (fix_t) { .raw = (big_x << 16) / big_y } +} + +fix_t fix_esqrt(fix_t x) { + // special case sqrt(0) = 0 + if (x.raw == 0) { + return x; + } + + // calculate an initial estimate for x + + // use __builtin_clz to count leading zeros + // highest set bit position is 31 - result + int hsb_pos = 31 - __builtin_clz(x.raw); + + // shift until in range [0.5, 2) (get distance from hsb and desired value) + int shift = msb_pos - 16; + if (shift % 2 != 0) { + ++shift; + } + + fix_t a; + if (shift >= 0) { + a = { .raw = x.raw >> shift }; + } else { + a = { .raw = x.raw << -shift }; + } + int n = shift / 2; + + // initial estimate is (0.5 + 0.5*a) * 2^n + fix_t y = { .raw = fix_eadd(half, fix_emul(half, a)).raw << n }; + + // peform a few iterations of newton's method + for (int i = 0; i < 5; i++) { + y = fix_ediv(fix_eadd(y, fix_ediv(x, y)), half); + } + + return y; +} + +void fix_eto_string(fix_t x, char[] buf) { + uint32_t raw = x.raw; + int index = -1; + + // if the number is negative then invert it and add a '-' + if (raw & (1 << 31)) { + buf[++index] = '-'; + raw = -raw; + } + + // + while (raw != 0) { + uint32_t rem = raw % 10; + raw /= 10; + + } +} + +// TODO find a way to eliminate this repetition! +fix_unit_t fix_umul(fix_unit_t x, fix_unit_t y) { + uint64_t big_x = x.raw; + uint64_t big_y = y.raw; + return (fix_unit_t) { .raw = (big_x * big_y) >> 31 } +} + +fix_unit_t fix_udiv(fix_unit_t x, fix_unit_t y) { + uint64_t big_x = x.raw; + uint64_t big_y = y.raw; + return (fix_unit_t) { .raw = (big_x << 31) / big_y } +} + +fix_unit_t fix_usqrt(fix_unit_t x) { + // special case sqrt(0) = 0 + if (x.raw == 0) { + return x; + } + + // calculate an initial estimate for x + + // use __builtin_clz to count leading zeros + // highest set bit position is 31 - result + int hsb_pos = 31 - __builtin_clz(x.raw); + + // shift until in range [0.5, 2) (get distance from hsb and desired value) + int shift = msb_pos - 31; + if (shift % 2 != 0) { + ++shift; + } + + fix_t a; + if (shift >= 0) { + a = { .raw = x.raw >> shift }; + } else { + a = { .raw = x.raw << -shift }; + } + int n = shift / 2; + + // initial estimate is (0.5 + 0.5*a) * 2^n + fix_t y = { .raw = fix_uadd(half, fix_umul(half, a)).raw << n }; + + // peform a few iterations of newton's method + for (int i = 0; i < 5; i++) { + y = fix_udiv(fix_uadd(y, fix_udiv(x, y)), half); + } + + return y; +} diff --git a/fixmath/fixmath.h b/fixmath/fixmath.h new file mode 100644 index 0000000000000000000000000000000000000000..119711e433629d8b05ac2314e7345ba28cdb62e6 --- /dev/null +++ b/fixmath/fixmath.h @@ -0,0 +1,72 @@ +#pragma once + +#include <stdint> + + +// standard, even type (16.16) with range [-32768, 32768) +typedef struct { + uint32_t raw; +} fix_t; + +// unit type (1.31) with range [-1, 1) +typedef struct { + uint32_t raw; +} fix_unit_t; + +// fix_t functions +static inline fix_t fix_eadd(fix_t x, fix_t y) { + return (fix_t) { .raw = x.raw + y.raw }; +} + +static inline fix_t fix_esub(fix_t x, fix_t y) { + return (fix_t) { .raw = x.raw - y.raw }; +} + +static inline fix_t fix_eneg(fix_t x) { + return (fix_t) { .raw = -x.raw }; +} + +fix_t fix_emul(fix_t x, fix_t y); +fix_t fix_ediv(fix_t x, fix_t y); + +fix_t fix_esqrt(fix_t x); + +static inline fix_t fix_efrom_int(int x) { + return (fix_t) { .raw = x << 16 } +} +void fix_eto_string(fix_t x, char[] buf); + +// fix_unit_t functions +static inline fix_unit_t fix_uadd(fix_unit_t x, fix_unit_t y) { + return (fix_unit_t) { .raw = x.raw + y.raw }; +} + +static inline fix_unit_t fix_usub(fix_unit_t x, fix_unit_t y) { + return (fix_unit_t) { .raw = x.raw - y.raw }; +} + +static inline fix_unit_t fix_eneg(fix_t x) { + return (fix_unit_t) { .raw = -x.raw }; +} + +fix_unit_t fix_umul(fix_unit_t x, fix_unit_t y); +fix_unit_t fix_udiv(fix_unit_t x, fix_unit_t y); + +fix_unit_t fix_usqrt(fix_unit_t x); + +static inline fix_unit_t fix_ufrom_reciprocal(int x) { + return (fix_unit_t) { .raw = (1 << 31) / x }; +} + +void fix_uto_string(fix_t x, char[] buf); + +// generic macros +# define fix_add(x, y) _Generic(x, fix_t: fix_eadd, fix_unit_t: fix_uadd)(x, y) +# define fix_sub(x, y) _Generic(x, fix_t: fix_esub, fix_unit_t: fix_usub)(x, y) +# define fix_mul(x, y) _Generic(x, fix_t: fix_emul, fix_unit_t: fix_umul)(x, y) +# define fix_div(x, y) _Generic(x, fix_t: fix_ediv, fix_unit_t: fix_udiv)(x, y) + +# define fix_sqrt(x) _Generic(x, fix_t: fix_esqrt, fix_unit_t: fix_usqrt)(x) + +# define fix_to_string(x, buf) _Generic(x, fix_t: fix_eto_string, fix_unit_t: fix_uto_string)(x, buf) +