1/* @title: Fixed Point Arithmetic */
2#pragma once
3#include <kassert.h>
4#include <types/types.h>
5
6#define FX_ONE ((fx32_32_t) (1LL << 32))
7#define FX_HALF ((fx32_32_t) (1LL << 31))
8
9/* Cast truncates towards zero, so we need to round */
10#define FX(x) ((fx32_32_t) ((x) * 4294967296.0 + ((x) < 0 ? -0.5 : 0.5)))
11#define FX_FROM_RATIO(n, d) ((fx32_32_t) (((__int128) (n) << 32) / (d)))
12
13/* strtol style */
14fx32_32_t fx_parse(const char *str, char **endptr);
15
16static inline fx32_32_t fx_add(fx32_32_t a, fx32_32_t b) {
17 return (fx32_32_t) (a + b);
18}
19
20static inline fx32_32_t fx_sub(fx32_32_t a, fx32_32_t b) {
21 return (fx32_32_t) (a - b);
22}
23
24static inline fx32_32_t fx_mul(fx32_32_t a, fx32_32_t b) {
25 bool neg = (a ^ b) < 0;
26 uint64_t ua = (uint64_t) (a < 0 ? -a : a);
27 uint64_t ub = (uint64_t) (b < 0 ? -b : b);
28
29 uint64_t a_hi = ua >> 32;
30 uint64_t a_lo = ua & 0xFFFFFFFFULL;
31 uint64_t b_hi = ub >> 32;
32 uint64_t b_lo = ub & 0xFFFFFFFFULL;
33
34 uint64_t p0 = a_lo * b_lo;
35 uint64_t p1 = a_lo * b_hi;
36 uint64_t p2 = a_hi * b_lo;
37 uint64_t p3 = a_hi * b_hi;
38
39 uint64_t mid = (p0 >> 32) + (p1 & 0xFFFFFFFFULL) + (p2 & 0xFFFFFFFFULL);
40 uint64_t high = p3 + (p1 >> 32) + (p2 >> 32) + (mid >> 32);
41
42 uint64_t q = (high << 32) | (mid & 0xFFFFFFFFULL);
43
44 return (fx32_32_t) (neg ? -(int64_t) q : (int64_t) q);
45}
46
47static inline fx32_32_t fx_div(fx32_32_t a, fx32_32_t b) {
48 kassert(b);
49 bool neg = (a ^ b) < 0;
50 uint64_t ua = (uint64_t) (a < 0 ? -a : a);
51 uint64_t ub = (uint64_t) (b < 0 ? -b : b);
52
53 uint64_t num_hi = ua >> 32;
54 uint64_t num_lo = ua << 32;
55 uint64_t q = 0;
56
57 for (int i = 63; i >= 0; i--) {
58 uint64_t bit = num_hi >> 63;
59 num_hi = (num_hi << 1) | (num_lo >> 63);
60 num_lo <<= 1;
61
62 if (bit || num_hi >= ub) {
63 num_hi -= ub;
64 q |= (1ULL << i);
65 }
66 }
67
68 return (fx32_32_t) (neg ? -(int64_t) q : (int64_t) q);
69}
70
71static inline fx32_32_t fx_from_int(int64_t x) {
72 return x << 32;
73}
74
75static inline int64_t fx_to_int(fx32_32_t x) {
76 return x >> 32;
77}
78
79static inline fx32_32_t fx_min(fx32_32_t a, fx32_32_t b) {
80 return a < b ? a : b;
81}
82
83static inline fx32_32_t fx_max(fx32_32_t a, fx32_32_t b) {
84 return a > b ? a : b;
85}
86
87static inline fx32_32_t fx_clamp(fx32_32_t x, fx32_32_t lo, fx32_32_t hi) {
88 return fx_max(a: lo, b: fx_min(a: x, b: hi));
89}
90
91static inline fx32_32_t fx_pow_i32(fx32_32_t base, int exp) {
92 fx32_32_t result = FX_ONE;
93 if (exp < 0) {
94 exp = -exp;
95 base = fx_div(FX_ONE, b: base);
96 }
97 while (exp) {
98 if (exp & 1)
99 result = fx_mul(a: result, b: base);
100 base = fx_mul(a: base, b: base);
101 exp >>= 1;
102 }
103 return result;
104}
105
106static inline fx32_32_t fx_sqrt(fx32_32_t x) {
107 if (x <= 0)
108 return 0;
109 fx32_32_t r = x;
110 for (int i = 0; i < 8; i++) {
111 r = (r + fx_div(a: x, b: (fx32_32_t) r)) >> 1;
112 }
113 return r;
114}
115
116/* Use two's complement to branchlessly floor for both signs */
117static inline fx32_32_t fx_ceil(fx32_32_t x) {
118 return (x + FX_ONE - 1) & ~(FX_ONE - 1);
119}
120
121static inline fx32_32_t fx_floor(fx32_32_t x) {
122 return x & ~(FX_ONE - 1);
123}
124
125static inline fx32_32_t fx_map(fx32_32_t value, fx32_32_t from_low,
126 fx32_32_t from_high, fx32_32_t to_low,
127 fx32_32_t to_high) {
128 fx32_32_t dist = value - from_low;
129 fx32_32_t to_range = to_high - to_low;
130 fx32_32_t from_range = from_high - from_low;
131 fx32_32_t num = to_low + fx_mul(a: dist, b: to_range);
132 return fx_div(a: num, b: from_range);
133}
134
135static inline fx32_32_t fx_lerp(fx32_32_t a, fx32_32_t b, fx32_32_t t) {
136 return a + fx_mul(a: t, b: b - a);
137}
138
139static inline fx32_32_t fx_round_up(fx32_32_t x, fx32_32_t multiple) {
140 return fx_mul(a: fx_div(a: x + multiple - FX_ONE, b: multiple), b: multiple);
141}
142
143static inline fx32_32_t fx_round_down(fx32_32_t x, fx32_32_t multiple) {
144 return fx_mul(a: fx_div(a: x, b: multiple), b: multiple);
145}
146