1#include <math/fixed.h>
2#include <math/fixed_extended.h>
3
4fx32_32_t fx_exp(fx32_32_t x) {
5 int64_t k = fx_to_int(x: fx_mul(a: x, FX(1.44269504089)));
6 fx32_32_t k_ln2 = fx_mul(a: fx_from_int(x: k), FX(0.69314718056));
7 fx32_32_t r = x - k_ln2;
8 static const fx32_32_t coeffs[] = {FX(1.0), FX(1.0), FX(0.5), FX(1.0 / 6.0),
9 FX(1.0 / 24.0)};
10 fx32_32_t poly = fx_poly_eval(x: r, c: coeffs, n: 5);
11 if (k >= 0)
12 return poly << k;
13 else
14 return poly >> -k;
15}
16
17fx32_32_t fx_poly_eval(fx32_32_t x, const fx32_32_t *c, int n) {
18 fx32_32_t r = c[n - 1];
19 for (int i = n - 2; i >= 0; i--) {
20 r = fx_mul(a: r, b: x) + c[i];
21 }
22 return r;
23}
24
25static const fx32_32_t FX_LN2 = FX(0.69314718056);
26
27fx32_32_t fx_ln(fx32_32_t x) {
28 if (x <= 0)
29 return INT64_MIN;
30 int64_t k = 0;
31 /* Normalize x into [1,2) */
32 while (x < FX(1.0)) {
33 x <<= 1;
34 k--;
35 }
36 while (x >= FX(2.0)) {
37 x >>= 1;
38 k++;
39 }
40 /* z = (x - 1) / (x + 1) */
41 fx32_32_t num = x - FX(1.0);
42 fx32_32_t den = x + FX(1.0);
43 fx32_32_t z = fx_div(a: num, b: den);
44 /* Polynomial: z + z^3/3 + z^5/5 */
45 fx32_32_t z2 = fx_mul(a: z, b: z);
46 fx32_32_t z3 = fx_mul(a: z2, b: z);
47 fx32_32_t z5 = fx_mul(a: z3, b: z2);
48 fx32_32_t series = z + fx_mul(a: z3, b: fx_div(FX(1.0), FX(3.0))) +
49 fx_mul(a: z5, b: fx_div(FX(1.0), FX(5.0)));
50 fx32_32_t ln_m = fx_mul(a: series, FX(2.0));
51 return ln_m + fx_mul(a: fx_from_int(x: k), b: FX_LN2);
52}
53
54static const fx32_32_t cordic_atan[] = {
55 FX(0.7853981633974483), FX(0.4636476090008061), FX(0.2449786631268641),
56 FX(0.1243549945467614), FX(0.0624188099959574), FX(0.0312398334302683),
57 FX(0.0156237286204768), FX(0.0078123410601011), FX(0.0039062301319669),
58 FX(0.0019531225164788), FX(0.0009765621895593), FX(0.0004882812111949),
59 FX(0.0002441406201494), FX(0.0001220703118937), FX(0.0000610351561742),
60 FX(0.0000305175781166), FX(0.0000152587890863), FX(0.0000076293945430),
61 FX(0.0000038146972715), FX(0.0000019073486358), FX(0.0000009536743179),
62 FX(0.0000004768371590), FX(0.0000002384185795), FX(0.0000001192092898),
63 FX(0.0000000596046449), FX(0.0000000298023224), FX(0.0000000149011612),
64 FX(0.0000000074505806), FX(0.0000000037252903), FX(0.0000000018626452),
65 FX(0.0000000009313226), FX(0.0000000004656613),
66};
67
68static const fx32_32_t FX_CORDIC_K = FX(0.6072529350088812561694);
69
70static void fx_cordic(fx32_32_t angle, fx32_32_t *out_sin, fx32_32_t *out_cos) {
71 fx32_32_t x = FX_CORDIC_K;
72 fx32_32_t y = 0;
73 fx32_32_t z = angle;
74 for (int i = 0; i < 32; i++) {
75 fx32_32_t x_new, y_new;
76 if (z >= 0) {
77 x_new = x - (y >> i);
78 y_new = y + (x >> i);
79 z -= cordic_atan[i];
80 } else {
81 x_new = x + (y >> i);
82 y_new = y - (x >> i);
83 z += cordic_atan[i];
84 }
85 x = x_new;
86 y = y_new;
87 }
88 *out_sin = y;
89 *out_cos = x;
90}
91
92fx32_32_t fx_sin(fx32_32_t angle) {
93 fx32_32_t s, c;
94 fx_cordic(angle, out_sin: &s, out_cos: &c);
95 return s;
96}
97
98fx32_32_t fx_cos(fx32_32_t angle) {
99 fx32_32_t s, c;
100 fx_cordic(angle, out_sin: &s, out_cos: &c);
101 return c;
102}
103
104void fx_sincos(fx32_32_t angle, fx32_32_t *sin_out, fx32_32_t *cos_out) {
105 fx_cordic(angle, out_sin: sin_out, out_cos: cos_out);
106}
107
108static fx32_32_t _fx_reduce_pi(fx32_32_t angle) {
109 static const fx32_32_t TWO_PI_R = FX(0.15915494309189535);
110
111 int64_t cycles = fx_to_int(x: fx_mul(a: angle, b: TWO_PI_R));
112 angle -= fx_mul(a: fx_from_int(x: cycles), FX(2.0 * FP_PI));
113
114 if (angle > FX_PI)
115 angle -= FX(2.0 * FP_PI);
116 if (angle < -FX_PI)
117 angle += FX(2.0 * FP_PI);
118 return angle;
119}
120
121fx32_32_t fx_sin_t(fx32_32_t angle) {
122
123 angle = _fx_reduce_pi(angle);
124
125 int negate = 0;
126 if (angle < 0) {
127 angle = -angle;
128 negate ^= 1;
129 }
130 if (angle > FX(FP_PI / 2.0)) {
131 angle = FX_PI - angle;
132 }
133
134 fx32_32_t x2 = fx_mul(a: angle, b: angle);
135 fx32_32_t x3 = fx_mul(a: x2, b: angle);
136 fx32_32_t x5 = fx_mul(a: x3, b: x2);
137 fx32_32_t x7 = fx_mul(a: x5, b: x2);
138 fx32_32_t x9 = fx_mul(a: x7, b: x2);
139 fx32_32_t x11 = fx_mul(a: x9, b: x2);
140 fx32_32_t x13 = fx_mul(a: x11, b: x2);
141
142 fx32_32_t result =
143 angle - fx_mul(a: x3, FX(1.0 / 6.0)) + fx_mul(a: x5, FX(1.0 / 120.0)) -
144 fx_mul(a: x7, FX(1.0 / 5040.0)) + fx_mul(a: x9, FX(1.0 / 362880.0)) -
145 fx_mul(a: x11, FX(1.0 / 39916800.0)) + fx_mul(a: x13, FX(1.0 / 6227020800.0));
146
147 return negate ? -result : result;
148}
149
150fx32_32_t fx_cos_t(fx32_32_t angle) {
151
152 angle = _fx_reduce_pi(angle);
153
154 /* fold: cos(-x) = cos(x); cos(pi - x) = -cos(x) */
155 int negate = 0;
156 if (angle < 0)
157 angle = -angle;
158 if (angle > FX(FP_PI / 2.0)) {
159 angle = FX_PI - angle;
160 negate = 1;
161 }
162
163 fx32_32_t x2 = fx_mul(a: angle, b: angle);
164 fx32_32_t x4 = fx_mul(a: x2, b: x2);
165 fx32_32_t x6 = fx_mul(a: x4, b: x2);
166 fx32_32_t x8 = fx_mul(a: x6, b: x2);
167 fx32_32_t x10 = fx_mul(a: x8, b: x2);
168 fx32_32_t x12 = fx_mul(a: x10, b: x2);
169
170 fx32_32_t result =
171 FX(1.0) - fx_mul(a: x2, FX(1.0 / 2.0)) + fx_mul(a: x4, FX(1.0 / 24.0)) -
172 fx_mul(a: x6, FX(1.0 / 720.0)) + fx_mul(a: x8, FX(1.0 / 40320.0)) -
173 fx_mul(a: x10, FX(1.0 / 3628800.0)) + fx_mul(a: x12, FX(1.0 / 479001600.0));
174
175 return negate ? -result : result;
176}
177
178void fx_sincos_t(fx32_32_t angle, fx32_32_t *sin_out, fx32_32_t *cos_out) {
179 angle = _fx_reduce_pi(angle);
180
181 int sin_neg = 0, cos_neg = 0;
182 if (angle < 0) {
183 angle = -angle;
184 sin_neg ^= 1;
185 }
186 if (angle > FX(FP_PI / 2.0)) {
187 angle = FX_PI - angle;
188 cos_neg = 1;
189 }
190
191 fx32_32_t x2 = fx_mul(a: angle, b: angle);
192 fx32_32_t x3 = fx_mul(a: x2, b: angle);
193 fx32_32_t x4 = fx_mul(a: x3, b: angle);
194 fx32_32_t x5 = fx_mul(a: x4, b: angle);
195 fx32_32_t x6 = fx_mul(a: x5, b: angle);
196 fx32_32_t x7 = fx_mul(a: x6, b: angle);
197 fx32_32_t x8 = fx_mul(a: x7, b: angle);
198 fx32_32_t x9 = fx_mul(a: x8, b: angle);
199 fx32_32_t x10 = fx_mul(a: x9, b: angle);
200 fx32_32_t x11 = fx_mul(a: x10, b: angle);
201 fx32_32_t x12 = fx_mul(a: x11, b: angle);
202 fx32_32_t x13 = fx_mul(a: x12, b: angle);
203
204 fx32_32_t s =
205 angle - fx_mul(a: x3, FX(1.0 / 6.0)) + fx_mul(a: x5, FX(1.0 / 120.0)) -
206 fx_mul(a: x7, FX(1.0 / 5040.0)) + fx_mul(a: x9, FX(1.0 / 362880.0)) -
207 fx_mul(a: x11, FX(1.0 / 39916800.0)) + fx_mul(a: x13, FX(1.0 / 6227020800.0));
208
209 fx32_32_t c =
210 FX(1.0) - fx_mul(a: x2, FX(1.0 / 2.0)) + fx_mul(a: x4, FX(1.0 / 24.0)) -
211 fx_mul(a: x6, FX(1.0 / 720.0)) + fx_mul(a: x8, FX(1.0 / 40320.0)) -
212 fx_mul(a: x10, FX(1.0 / 3628800.0)) + fx_mul(a: x12, FX(1.0 / 479001600.0));
213
214 *sin_out = sin_neg ? -s : s;
215 *cos_out = cos_neg ? -c : c;
216}
217
218fx32_32_t fx_ln_t(fx32_32_t x) {
219 static const fx32_32_t LN2 = FX(0.69314718055994530941);
220
221 if (x <= 0)
222 return INT64_MIN;
223
224 int64_t k = 0;
225 while (x < FX(1.0)) {
226 x <<= 1;
227 k--;
228 }
229 while (x >= FX(2.0)) {
230 x >>= 1;
231 k++;
232 }
233
234 fx32_32_t z = fx_div(a: x - FX(1.0), b: x + FX(1.0));
235 fx32_32_t z2 = fx_mul(a: z, b: z);
236
237 fx32_32_t series =
238 FX(2.0 / 1.0) +
239 fx_mul(
240 a: z2,
241 FX(2.0 / 3.0) +
242 fx_mul(
243 a: z2,
244 FX(2.0 / 5.0) +
245 fx_mul(
246 a: z2,
247 FX(2.0 / 7.0) +
248 fx_mul(
249 a: z2,
250 FX(2.0 / 9.0) +
251 fx_mul(
252 a: z2,
253 FX(2.0 / 11.0) +
254 fx_mul(
255 a: z2,
256 FX(2.0 / 13.0) +
257 fx_mul(a: z2,
258 FX(2.0 /
259 15.0))))))));
260
261 return fx_mul(a: z, b: series) + fx_mul(a: fx_from_int(x: k), b: LN2);
262}
263
264fx32_32_t fx_exp_t(fx32_32_t x) {
265 static const fx32_32_t LN2 = FX(0.69314718055994530941);
266 static const fx32_32_t LOG2_E = FX(1.44269504088896340735);
267
268 int64_t k = fx_to_int(x: fx_mul(a: x, b: LOG2_E));
269 fx32_32_t r = x - fx_mul(a: fx_from_int(x: k), b: LN2);
270
271 fx32_32_t poly =
272 FX(1.0) +
273 fx_mul(
274 a: r,
275 FX(1.0) +
276 fx_mul(
277 a: r,
278 FX(1.0 / 2.0) +
279 fx_mul(
280 a: r,
281 FX(1.0 / 6.0) +
282 fx_mul(
283 a: r,
284 FX(1.0 / 24.0) +
285 fx_mul(
286 a: r,
287 FX(1.0 / 120.0) +
288 fx_mul(
289 a: r,
290 FX(1.0 / 720.0) +
291 fx_mul(
292 a: r,
293 FX(1.0 /
294 5040.0))))))));
295
296 if (k >= 0)
297 return poly << k;
298 else
299 return poly >> -k;
300}
301
302fx32_32_t fx_parse(const char *str, char **endptr) {
303 const char *s = str;
304
305 while (*s == ' ' || *s == '\t')
306 s++;
307
308 bool neg = false;
309 if (*s == '+' || *s == '-') {
310 neg = (*s == '-');
311 s++;
312 }
313
314 bool any_digit = false;
315
316 uint64_t ipart = 0;
317 while (*s >= '0' && *s <= '9') {
318 ipart = ipart * 10 + (uint64_t) (*s - '0');
319 any_digit = true;
320 s++;
321 }
322
323 uint64_t frac_num = 0; /* accumulated fractional digits */
324 uint64_t frac_den = 1; /* 10^(fractional digit count), <= 10^18 */
325 if (*s == '.') {
326 s++;
327 int frac_digits = 0;
328 while (*s >= '0' && *s <= '9') {
329 if (frac_digits < 18) {
330 frac_num = frac_num * 10 + (uint64_t) (*s - '0');
331 frac_den *= 10;
332 frac_digits++;
333 }
334 any_digit = true;
335 s++;
336 }
337 }
338
339 if (!any_digit) {
340 if (endptr)
341 *endptr = (char *) str;
342 return 0;
343 }
344
345 fx32_32_t result = (fx32_32_t) (ipart << 32);
346 if (frac_den > 1)
347 result += (fx32_32_t) (((uint128_t) frac_num << 32) / frac_den);
348
349 if (endptr)
350 *endptr = (char *) s;
351
352 return neg ? -result : result;
353}
354