1const MANT_MASK: u64 = 0x000f_ffff_ffff_ffff;
2const ONE_EXP: u64 = 0x3ff0_0000_0000_0000;
3
4const PIO2_P1: f64 = f64::from_bits(0x3ff921fb54400000);
5const PIO2_P2: f64 = f64::from_bits(0x3dd0b4611a600000);
6const PIO2_P3: f64 = f64::from_bits(0x3ba3198a2e000000);
7const PIO2_P4: f64 = f64::from_bits(0x397b839a252049c1);
8const PI_DD: Dd = Dd {
9 hi: f64::from_bits(0x400921fb54442d18),
10 lo: f64::from_bits(0x3ca1a62633145c07),
11};
12const HALF_PI_DD: Dd = Dd {
13 hi: f64::from_bits(0x3ff921fb54442d18),
14 lo: f64::from_bits(0x3c91a62633145c07),
15};
16const QUARTER_PI_DD: Dd = Dd {
17 hi: f64::from_bits(0x3fe921fb54442d18),
18 lo: f64::from_bits(0x3c81a62633145c07),
19};
20const THREE_QUARTER_PI_DD: Dd = Dd {
21 hi: f64::from_bits(0x4002d97c7f3321d2),
22 lo: f64::from_bits(0x3c9a79394c9e8a0a),
23};
24const LN2_HI: f64 = f64::from_bits(0x3fe62e42fec00000);
25const LN2_MID: f64 = f64::from_bits(0x3dfd1cf79a800000);
26const LN2_LO: f64 = f64::from_bits(0x3c0e4f1d9cc01f98);
27const LN2_DD: Dd = Dd {
28 hi: f64::from_bits(0x3fe62e42fefa39ef),
29 lo: f64::from_bits(0x3c7abc9e3b39803f),
30};
31const THIRD_DD: Dd = Dd {
32 hi: f64::from_bits(0x3fd5555555555555),
33 lo: f64::from_bits(0x3c75555555555555),
34};
35const FIFTH_DD: Dd = Dd {
36 hi: f64::from_bits(0x3fc999999999999a),
37 lo: f64::from_bits(0xbc6999999999999a),
38};
39const ATAN_TABLE: [Dd; 8] = [
40 Dd {
41 hi: f64::from_bits(0x3fbfd5ba9aac2f6e),
42 lo: f64::from_bits(0xbc4cd37686760c17),
43 },
44 Dd {
45 hi: f64::from_bits(0x3fcf5b75f92c80dd),
46 lo: f64::from_bits(0x3c68ab6e3cf7afbd),
47 },
48 Dd {
49 hi: f64::from_bits(0x3fd6f61941e4def1),
50 lo: f64::from_bits(0xbc7c63aae6f6e918),
51 },
52 Dd {
53 hi: f64::from_bits(0x3fddac670561bb4f),
54 lo: f64::from_bits(0x3c7a2b7f222f65e2),
55 },
56 Dd {
57 hi: f64::from_bits(0x3fe1e00babdefeb4),
58 lo: f64::from_bits(0xbc5928df287a668f),
59 },
60 Dd {
61 hi: f64::from_bits(0x3fe4978fa3269ee1),
62 lo: f64::from_bits(0x3c72419a87f2a458),
63 },
64 Dd {
65 hi: f64::from_bits(0x3fe700a7c5784634),
66 lo: f64::from_bits(0xbc78c34d25aadef6),
67 },
68 Dd {
69 hi: f64::from_bits(0x3fe921fb54442d18),
70 lo: f64::from_bits(0x3c81a62633145c07),
71 },
72];
73const TWO_OVER_PI: [u64; 20] = [
74 0xa2f9836e4e441529,
75 0xfc2757d1f534ddc0,
76 0xdb6295993c439041,
77 0xfe5163abdebbc561,
78 0xb7246e3a424dd2e0,
79 0x06492eea09d1921c,
80 0xfe1deb1cb129a73e,
81 0xe88235f52ebb4484,
82 0xe99c7026b45f7e41,
83 0x3991d639835339f4,
84 0x9c845f8bbdf9283b,
85 0x1ff897ffde05980f,
86 0xef2f118b5a0a6d1f,
87 0x6d367ecf27cb09b7,
88 0x4f463f669e5fea2d,
89 0x7527bac7ebe5f17b,
90 0x3d0739f78a5292ea,
91 0x6bfb5fb11f8d5d08,
92 0x56033046fc7b6bab,
93 0xf0cfbc209af4361d,
94];
95
96const INV_LN2: f64 = core::f64::consts::LOG2_E;
97const TWO_OVER_PI_F64: f64 = core::f64::consts::FRAC_2_PI;
98const CODY_WAITE_LIMIT: f64 = 1.0e6;
99const QUARTER_PI: f64 = core::f64::consts::FRAC_PI_4;
100const TWO_POW_54: f64 = 18014398509481984.0;
101const TWO_POW_53: f64 = 9007199254740992.0;
102
103const fn factorial(n: usize) -> f64 {
104 let mut f = 1.0;
105 let mut i = 2;
106 while i <= n {
107 f *= i as f64;
108 i += 1;
109 }
110 f
111}
112
113const fn exp_tail() -> [f64; 14] {
114 let mut c = [0.0; 14];
115 let mut i = 0;
116 while i < 14 {
117 c[i] = 1.0 / factorial(i + 2);
118 i += 1;
119 }
120 c
121}
122
123const fn sin_tail() -> [f64; 9] {
124 let mut c = [0.0; 9];
125 let mut i = 0;
126 while i < 9 {
127 let sign = if i % 2 == 0 { -1.0 } else { 1.0 };
128 c[i] = sign / factorial(2 * i + 3);
129 i += 1;
130 }
131 c
132}
133
134const fn cos_tail() -> [f64; 10] {
135 let mut c = [0.0; 10];
136 let mut i = 0;
137 while i < 10 {
138 let sign = if i % 2 == 0 { 1.0 } else { -1.0 };
139 c[i] = sign / factorial(2 * i + 4);
140 i += 1;
141 }
142 c
143}
144
145const fn ln_tail() -> [f64; 12] {
146 let mut c = [0.0; 12];
147 let mut i = 0;
148 while i < 12 {
149 c[i] = 1.0 / (7 + 2 * i) as f64;
150 i += 1;
151 }
152 c
153}
154
155const fn atan_tail() -> [f64; 9] {
156 let mut c = [0.0; 9];
157 let mut i = 0;
158 while i < 9 {
159 let sign = if i % 2 == 0 { -1.0 } else { 1.0 };
160 c[i] = sign / (3 + 2 * i) as f64;
161 i += 1;
162 }
163 c
164}
165
166const EXP_TAIL: [f64; 14] = exp_tail();
167const SIN_TAIL: [f64; 9] = sin_tail();
168const COS_TAIL: [f64; 10] = cos_tail();
169const LN_TAIL: [f64; 12] = ln_tail();
170const ATAN_TAIL: [f64; 9] = atan_tail();
171
172#[derive(Clone, Copy)]
173struct Dd {
174 hi: f64,
175 lo: f64,
176}
177
178impl Dd {
179 const ONE: Dd = Dd { hi: 1.0, lo: 0.0 };
180 const ZERO: Dd = Dd { hi: 0.0, lo: 0.0 };
181
182 fn of(hi: f64) -> Dd {
183 Dd { hi, lo: 0.0 }
184 }
185
186 fn neg(self) -> Dd {
187 Dd {
188 hi: -self.hi,
189 lo: -self.lo,
190 }
191 }
192}
193
194fn two_sum(a: f64, b: f64) -> (f64, f64) {
195 let s = a + b;
196 let bb = s - a;
197 (s, (a - (s - bb)) + (b - bb))
198}
199
200fn fast_two_sum(a: f64, b: f64) -> (f64, f64) {
201 let s = a + b;
202 (s, b - (s - a))
203}
204
205fn split(a: f64) -> (f64, f64) {
206 let t = 134217729.0 * a;
207 let hi = t - (t - a);
208 (hi, a - hi)
209}
210
211fn two_prod(a: f64, b: f64) -> (f64, f64) {
212 let p = a * b;
213 let (ah, al) = split(a);
214 let (bh, bl) = split(b);
215 (p, ((ah * bh - p) + ah * bl + al * bh) + al * bl)
216}
217
218fn dd_add(a: Dd, b: Dd) -> Dd {
219 let (s1, s2) = two_sum(a.hi, b.hi);
220 let (t1, t2) = two_sum(a.lo, b.lo);
221 let (s1, s2) = fast_two_sum(s1, s2 + t1);
222 let (hi, lo) = fast_two_sum(s1, s2 + t2);
223 Dd { hi, lo }
224}
225
226fn dd_sub(a: Dd, b: Dd) -> Dd {
227 dd_add(a, b.neg())
228}
229
230fn dd_add_f(a: Dd, b: f64) -> Dd {
231 let (s1, s2) = two_sum(a.hi, b);
232 let (hi, lo) = fast_two_sum(s1, s2 + a.lo);
233 Dd { hi, lo }
234}
235
236fn dd_mul(a: Dd, b: Dd) -> Dd {
237 let (p1, p2) = two_prod(a.hi, b.hi);
238 let (hi, lo) = fast_two_sum(p1, p2 + (a.hi * b.lo + a.lo * b.hi));
239 Dd { hi, lo }
240}
241
242fn dd_mul_f(a: Dd, b: f64) -> Dd {
243 let (p1, p2) = two_prod(a.hi, b);
244 let (hi, lo) = fast_two_sum(p1, p2 + a.lo * b);
245 Dd { hi, lo }
246}
247
248fn dd_div(a: Dd, b: Dd) -> Dd {
249 let q1 = a.hi / b.hi;
250 let r = dd_sub(a, dd_mul_f(b, q1));
251 let q2 = r.hi / b.hi;
252 let r = dd_sub(r, dd_mul_f(b, q2));
253 let q3 = r.hi / b.hi;
254 let (q1, q2) = fast_two_sum(q1, q2);
255 dd_add_f(Dd { hi: q1, lo: q2 }, q3)
256}
257
258fn pow2i(n: i32) -> f64 {
259 f64::from_bits(((n + 1023) as u64) << 52)
260}
261
262fn scalbn(mut y: f64, mut n: i32) -> f64 {
263 if n > 1023 {
264 y *= pow2i(1023);
265 n -= 1023;
266 if n > 1023 {
267 y *= pow2i(1023);
268 n -= 1023;
269 if n > 1023 {
270 n = 1023;
271 }
272 }
273 } else if n < -1022 {
274 y *= pow2i(-1022) * TWO_POW_53;
275 n += 1022 - 53;
276 if n < -1022 {
277 y *= pow2i(-1022) * TWO_POW_53;
278 n += 1022 - 53;
279 if n < -1022 {
280 n = -1022;
281 }
282 }
283 }
284 y * pow2i(n)
285}
286
287fn exp_parts(x: f64, xl: f64) -> f64 {
288 if x > 710.0 {
289 return f64::INFINITY;
290 }
291 if x < -746.0 {
292 return 0.0;
293 }
294 let k = (x * INV_LN2 + 0.5).floor();
295 let r1 = x - k * LN2_HI;
296 let (r, e) = two_sum(r1, -(k * LN2_MID));
297 let small = (e - k * LN2_LO) + xl;
298 let r2 = r * r;
299 let mut q = 0.0;
300 for c in EXP_TAIL.iter().rev() {
301 q = q * r + c;
302 }
303 let g = r + r2 * q;
304 let (eh, el) = two_sum(1.0, g);
305 scalbn(eh + (el + eh * small), k as i32)
306}
307
308pub fn exp(x: f64) -> f64 {
309 if x.is_nan() {
310 return f64::NAN;
311 }
312 exp_parts(x, 0.0)
313}
314
315fn ln_dd(x: f64) -> Dd {
316 let mut bits = x.to_bits();
317 let mut k = 0i32;
318 if bits >> 52 == 0 {
319 bits = (x * TWO_POW_54).to_bits();
320 k = -54;
321 }
322 k += ((bits >> 52) & 0x7ff) as i32 - 1023;
323 let mut m = f64::from_bits((bits & MANT_MASK) | ONE_EXP);
324 if m > core::f64::consts::SQRT_2 {
325 m *= 0.5;
326 k += 1;
327 }
328 let f = m - 1.0;
329 let (p, pe) = two_sum(m, 1.0);
330 let s = dd_div(Dd::of(f), Dd { hi: p, lo: pe });
331 let z = dd_mul(s, s);
332 let zh = z.hi;
333 let mut t = 0.0;
334 for c in LN_TAIL.iter().rev() {
335 t = t * zh + c;
336 }
337 let inner = dd_add(Dd::ONE, dd_mul(z, THIRD_DD));
338 let inner = dd_add(inner, dd_mul(dd_mul(z, z), FIFTH_DD));
339 let inner = dd_add_f(inner, zh * zh * zh * t);
340 let lnm = dd_mul(dd_mul_f(s, 2.0), inner);
341 if k == 0 {
342 lnm
343 } else {
344 dd_add(dd_mul_f(LN2_DD, k as f64), lnm)
345 }
346}
347
348pub fn ln(x: f64) -> f64 {
349 if x.is_nan() || x < 0.0 {
350 return f64::NAN;
351 }
352 if x == 0.0 {
353 return f64::NEG_INFINITY;
354 }
355 if x == f64::INFINITY {
356 return x;
357 }
358 ln_dd(x).hi
359}
360
361fn is_integer(y: f64) -> bool {
362 y.floor() == y
363}
364
365fn is_odd_integer(y: f64) -> bool {
366 y.abs() < TWO_POW_53 && is_integer(y) && !is_integer(y * 0.5)
367}
368
369fn pow_signed(v: f64, negative: bool) -> f64 {
370 if negative { -v } else { v }
371}
372
373pub fn pow(x: f64, y: f64) -> f64 {
374 if y == 0.0 || x == 1.0 {
375 return 1.0;
376 }
377 if x.is_nan() || y.is_nan() {
378 return f64::NAN;
379 }
380 let y_integer = is_integer(y);
381 let odd = is_odd_integer(y);
382 if x == 0.0 {
383 let negative_zero = x.is_sign_negative() && odd;
384 return if y < 0.0 {
385 pow_signed(f64::INFINITY, negative_zero)
386 } else {
387 pow_signed(0.0, negative_zero)
388 };
389 }
390 if y.is_infinite() {
391 let a = x.abs();
392 return if a == 1.0 {
393 1.0
394 } else if (a > 1.0) == (y > 0.0) {
395 f64::INFINITY
396 } else {
397 0.0
398 };
399 }
400 if x.is_infinite() {
401 let negative = x < 0.0 && odd;
402 return if y > 0.0 {
403 pow_signed(f64::INFINITY, negative)
404 } else {
405 pow_signed(0.0, negative)
406 };
407 }
408 if x < 0.0 && !y_integer {
409 return f64::NAN;
410 }
411 let negative = x < 0.0 && odd;
412 let a = x.abs();
413 if y == 1.0 {
414 return x;
415 }
416 if a.to_bits() & MANT_MASK == 0 && a.to_bits() >> 52 != 0 && y.abs() < 4096.0 && y_integer {
417 let e = ((a.to_bits() >> 52) as i32 - 1023) as f64 * y;
418 let v = if e > 1100.0 {
419 f64::INFINITY
420 } else if e < -1100.0 {
421 0.0
422 } else {
423 scalbn(1.0, e as i32)
424 };
425 return pow_signed(v, negative);
426 }
427 let l = ln_dd(a);
428 let estimate = y * l.hi;
429 let v = if estimate > 1000.0 {
430 f64::INFINITY
431 } else if estimate < -1000.0 {
432 0.0
433 } else {
434 let t = dd_mul_f(l, y);
435 exp_parts(t.hi, t.lo)
436 };
437 pow_signed(v, negative)
438}
439
440fn sin_kernel(r: f64, rl: f64) -> f64 {
441 let z = r * r;
442 let mut p = 0.0;
443 for c in SIN_TAIL.iter().rev() {
444 p = p * z + c;
445 }
446 let tail = r * z * p + rl * (1.0 - 0.5 * z);
447 r + tail
448}
449
450fn cos_kernel(r: f64, rl: f64) -> f64 {
451 let z = r * r;
452 let mut q = 0.0;
453 for c in COS_TAIL.iter().rev() {
454 q = q * z + c;
455 }
456 let hz = 0.5 * z;
457 let w = 1.0 - hz;
458 let err = (1.0 - w) - hz;
459 w + (z * z * q - r * rl + err)
460}
461
462fn reduce_small(ax: f64) -> (u32, f64, f64) {
463 let k = (ax * TWO_OVER_PI_F64 + 0.5).floor();
464 let r1 = ax - k * PIO2_P1;
465 let (s, e) = two_sum(r1, -(k * PIO2_P2));
466 let rl = (e - k * PIO2_P3) - k * PIO2_P4;
467 let (r, rl) = fast_two_sum(s, rl);
468 ((k as u64 & 3) as u32, r, rl)
469}
470
471fn two_over_pi_bit(j: i32) -> u64 {
472 if !(1..=1280).contains(&j) {
473 return 0;
474 }
475 let idx = (j - 1) as usize;
476 (TWO_OVER_PI[idx / 64] >> (63 - idx % 64)) & 1
477}
478
479fn reduce_large(ax: f64) -> (u32, f64, f64) {
480 let bits = ax.to_bits();
481 let m = (bits & MANT_MASK) | (1 << 52);
482 let e = ((bits >> 52) & 0x7ff) as i32 - 1075;
483 let mut v = [0u64; 4];
484 for p in 0..194i32 {
485 if two_over_pi_bit(e + 192 - p) == 1 {
486 v[(p / 64) as usize] |= 1 << (p % 64);
487 }
488 }
489 let mut prod = [0u64; 4];
490 let mut carry = 0u128;
491 for i in 0..4 {
492 let t = (m as u128) * (v[i] as u128) + carry;
493 prod[i] = t as u64;
494 carry = t >> 64;
495 }
496 let mut n = (prod[3] & 3) as u32;
497 let mut f = [prod[0], prod[1], prod[2]];
498 let mut negative = false;
499 if f[2] >> 63 == 1 {
500 negative = true;
501 n = (n + 1) & 3;
502 let mut carry = true;
503 for limb in f.iter_mut() {
504 let (s, c) = (!*limb).overflowing_add(carry as u64);
505 *limb = s;
506 carry = c;
507 }
508 }
509 let lz = if f[2] != 0 {
510 f[2].leading_zeros()
511 } else if f[1] != 0 {
512 64 + f[1].leading_zeros()
513 } else if f[0] != 0 {
514 128 + f[0].leading_zeros()
515 } else {
516 return (n, 0.0, 0.0);
517 };
518 let words = (lz / 64) as usize;
519 let bitshift = lz % 64;
520 let mut out = [0u64; 3];
521 for (i, slot) in out.iter_mut().enumerate() {
522 let mut w = 0u64;
523 if i >= words {
524 w |= f[i - words] << bitshift;
525 if bitshift > 0 && i > words {
526 w |= f[i - words - 1] >> (64 - bitshift);
527 }
528 }
529 *slot = w;
530 }
531 let top = out[2] >> 11;
532 let next = ((out[2] & 0x7ff) << 42) | (out[1] >> 22);
533 let scale = pow2i(-(53 + lz as i32));
534 let (hi, lo) = fast_two_sum(top as f64 * scale, next as f64 * pow2i(-(106 + lz as i32)));
535 let r = dd_mul(Dd { hi, lo }, HALF_PI_DD);
536 if negative {
537 (n, -r.hi, -r.lo)
538 } else {
539 (n, r.hi, r.lo)
540 }
541}
542
543fn reduce(ax: f64) -> (u32, f64, f64) {
544 if ax <= QUARTER_PI {
545 (0, ax, 0.0)
546 } else if ax < CODY_WAITE_LIMIT {
547 reduce_small(ax)
548 } else {
549 reduce_large(ax)
550 }
551}
552
553pub fn sin(x: f64) -> f64 {
554 if !x.is_finite() {
555 return f64::NAN;
556 }
557 if x == 0.0 {
558 return x;
559 }
560 let (q, r, rl) = reduce(x.abs());
561 let v = match q {
562 0 => sin_kernel(r, rl),
563 1 => cos_kernel(r, rl),
564 2 => -sin_kernel(r, rl),
565 _ => -cos_kernel(r, rl),
566 };
567 if x < 0.0 { -v } else { v }
568}
569
570pub fn cos(x: f64) -> f64 {
571 if !x.is_finite() {
572 return f64::NAN;
573 }
574 let (q, r, rl) = reduce(x.abs());
575 match q {
576 0 => cos_kernel(r, rl),
577 1 => -sin_kernel(r, rl),
578 2 => -cos_kernel(r, rl),
579 _ => sin_kernel(r, rl),
580 }
581}
582
583fn atan_unit(t: f64) -> Dd {
584 if t == 0.0 {
585 return Dd::ZERO;
586 }
587 let k = (t * 8.0 + 0.5).floor() as usize;
588 let (base, u) = if k == 0 {
589 (Dd::ZERO, Dd::of(t))
590 } else {
591 let c = k as f64 / 8.0;
592 let (p, pe) = two_prod(t, c);
593 let den = dd_add_f(Dd { hi: p, lo: pe }, 1.0);
594 (ATAN_TABLE[k - 1], dd_div(Dd::of(t - c), den))
595 };
596 let s = u.hi;
597 let z = s * s;
598 let mut p = 0.0;
599 for c in ATAN_TAIL.iter().rev() {
600 p = p * z + c;
601 }
602 let ser = s * z * p + u.lo;
603 dd_add_f(dd_add_f(base, s), ser)
604}
605
606pub fn atan2(y: f64, x: f64) -> f64 {
607 if x.is_nan() || y.is_nan() {
608 return f64::NAN;
609 }
610 let negative = y.is_sign_negative();
611 let signed = |v: Dd| if negative { -v.hi } else { v.hi };
612 if y == 0.0 {
613 return if x.is_sign_negative() {
614 signed(PI_DD)
615 } else {
616 signed(Dd::ZERO)
617 };
618 }
619 if x == 0.0 {
620 return signed(HALF_PI_DD);
621 }
622 if x.is_infinite() {
623 return if y.is_infinite() {
624 signed(if x > 0.0 {
625 QUARTER_PI_DD
626 } else {
627 THREE_QUARTER_PI_DD
628 })
629 } else if x > 0.0 {
630 signed(Dd::ZERO)
631 } else {
632 signed(PI_DD)
633 };
634 }
635 if y.is_infinite() {
636 return signed(HALF_PI_DD);
637 }
638 let ay = y.abs();
639 let ax = x.abs();
640 let swap = ay > ax;
641 let t = if swap { ax / ay } else { ay / ax };
642 let mut a = atan_unit(t);
643 if swap {
644 a = dd_sub(HALF_PI_DD, a);
645 }
646 if x < 0.0 {
647 a = dd_sub(PI_DD, a);
648 }
649 signed(a)
650}
651
652pub fn sqrt(x: f64) -> f64 {
653 if x.is_nan() {
654 return f64::NAN;
655 }
656 x.sqrt()
657}
658
659pub fn sinf(x: f32) -> f32 {
660 sin(x as f64) as f32
661}
662
663pub fn cosf(x: f32) -> f32 {
664 cos(x as f64) as f32
665}
666
667pub fn atan2f(y: f32, x: f32) -> f32 {
668 atan2(y as f64, x as f64) as f32
669}
670
671pub fn expf(x: f32) -> f32 {
672 exp(x as f64) as f32
673}
674
675pub fn lnf(x: f32) -> f32 {
676 ln(x as f64) as f32
677}
678
679pub fn powf(x: f32, y: f32) -> f32 {
680 pow(x as f64, y as f64) as f32
681}
682
683pub fn sqrtf(x: f32) -> f32 {
684 if x.is_nan() {
685 return f32::NAN;
686 }
687 x.sqrt()
688}