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