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