1#[inline]
31fn high_word(x: f64) -> u32 {
32 (x.to_bits() >> 32) as u32
33}
34
35#[inline]
36fn low_word(x: f64) -> u32 {
37 x.to_bits() as u32
38}
39
40#[inline]
42fn with_low_word(x: f64, low: u32) -> f64 {
43 let u = (x.to_bits() & 0xffff_ffff_0000_0000) | (low as u64);
44 f64::from_bits(u)
45}
46
47#[inline]
53fn sin_kernel(x: f64, y: f64, iy: i32) -> f64 {
54 const S1: f64 = -1.66666666666666324348e-01;
55 const S2: f64 = 8.33333333332248946124e-03;
56 const S3: f64 = -1.98412698298579493134e-04;
57 const S4: f64 = 2.75573137070700676789e-06;
58 const S5: f64 = -2.50507602534068634195e-08;
59 const S6: f64 = 1.58969099521155010221e-10;
60
61 let z = x * x;
62 let w = z * z;
63 let r = S2 + z * (S3 + z * S4) + z * w * (S5 + z * S6);
64 let v = z * x;
65 if iy == 0 {
66 x + v * (S1 + z * r)
67 } else {
68 x - ((z * (0.5 * y - v * r) - y) - v * S1)
69 }
70}
71
72#[inline]
74fn cos_kernel(x: f64, y: f64) -> f64 {
75 const C1: f64 = 4.16666666666666019037e-02;
76 const C2: f64 = -1.38888888888741095749e-03;
77 const C3: f64 = 2.48015872894767294178e-05;
78 const C4: f64 = -2.75573143513906633035e-07;
79 const C5: f64 = 2.08757232129817482790e-09;
80 const C6: f64 = -1.13596475577881948265e-11;
81
82 let z = x * x;
83 let w = z * z;
84 let r = z * (C1 + z * (C2 + z * C3)) + w * w * (C4 + z * (C5 + z * C6));
85 let hz = 0.5 * z;
86 let w1 = 1.0 - hz;
87 w1 + (((1.0 - w1) - hz) + (z * r - x * y))
88}
89
90#[inline]
92fn tan_kernel(mut x: f64, mut y: f64, odd: i32) -> f64 {
93 const T: [f64; 13] = [
94 3.33333333333334091986e-01,
95 1.33333333333201242699e-01,
96 5.39682539762260521377e-02,
97 2.18694882948595424599e-02,
98 8.86323982359930005737e-03,
99 3.59207910759131235356e-03,
100 1.45620945432529025516e-03,
101 5.88041240820264096874e-04,
102 2.46463134818469906812e-04,
103 7.81794442939557092300e-05,
104 7.14072491382608190305e-05,
105 -1.85586374855275456654e-05,
106 2.59073051863633712884e-05,
107 ];
108 const PIO4: f64 = 7.85398163397448278999e-01;
109 const PIO4LO: f64 = 3.06161699786838301793e-17;
110
111 let hx = high_word(x);
112 let big = (hx & 0x7fff_ffff) >= 0x3FE5_9428; let mut sign = false;
114 if big {
115 sign = (hx >> 31) != 0;
116 if sign {
117 x = -x;
118 y = -y;
119 }
120 x = (PIO4 - x) + (PIO4LO - y);
121 y = 0.0;
122 }
123
124 let z = x * x;
125 let w = z * z;
126 let r = T[1] + w * (T[3] + w * (T[5] + w * (T[7] + w * (T[9] + w * T[11]))));
127 let v =
128 z * (T[2] + w * (T[4] + w * (T[6] + w * (T[8] + w * (T[10] + w * T[12])))));
129 let s = z * x;
130 let rr = y + z * (s * (r + v) + y) + s * T[0];
131 let ww = x + rr;
132 if big {
133 let s2 = 1.0 - 2.0 * (odd as f64);
134 let vv = s2 - 2.0 * (x + (rr - ww * ww / (ww + s2)));
135 return if sign { -vv } else { vv };
136 }
137 if odd == 0 {
138 return ww;
139 }
140 let w0 = with_low_word(ww, 0);
142 let vv = rr - (w0 - x);
143 let aa = -1.0 / ww;
144 let a0 = with_low_word(aa, 0);
145 a0 + aa * (1.0 + a0 * w0 + a0 * vv)
146}
147
148fn rem_pio2(x: f64) -> (i32, [f64; 2]) {
155 const PIO2_1: f64 = 1.57079632673412561417e+00;
156 const PIO2_1T: f64 = 6.07710050650619224932e-11;
157 const HALF_PI: f64 = 1.57079632679489661923132169163975144;
158
159 let ux = x.to_bits();
160 let sign = (ux >> 63) != 0;
161 let ix = ((ux >> 32) & 0x7fff_ffff) as u32;
162 let mut y = [0.0f64; 2];
163
164 if ix <= 0x400f_6a7a {
165 if (ix & 0xf_ffff) != 0x9_21fb {
167 if ix <= 0x4002_d97c {
169 if !sign {
171 let z = x - PIO2_1;
172 y[0] = z - PIO2_1T;
173 y[1] = (z - y[0]) - PIO2_1T;
174 return (1, y);
175 }
176 let z = x + PIO2_1;
177 y[0] = z + PIO2_1T;
178 y[1] = (z - y[0]) + PIO2_1T;
179 return (-1, y);
180 }
181 if !sign {
182 let z = x - 2.0 * PIO2_1;
183 y[0] = z - 2.0 * PIO2_1T;
184 y[1] = (z - y[0]) - 2.0 * PIO2_1T;
185 return (2, y);
186 }
187 let z = x + 2.0 * PIO2_1;
188 y[0] = z + 2.0 * PIO2_1T;
189 y[1] = (z - y[0]) + 2.0 * PIO2_1T;
190 return (-2, y);
191 }
192 return rem_pio2_medium(x, ix);
194 }
195
196 if ix <= 0x401c_463b {
197 if ix <= 0x4015_fdbc {
199 if ix == 0x4012_d97c {
201 return rem_pio2_medium(x, ix);
202 }
203 if !sign {
204 let z = x - 3.0 * PIO2_1;
205 y[0] = z - 3.0 * PIO2_1T;
206 y[1] = (z - y[0]) - 3.0 * PIO2_1T;
207 return (3, y);
208 }
209 let z = x + 3.0 * PIO2_1;
210 y[0] = z + 3.0 * PIO2_1T;
211 y[1] = (z - y[0]) + 3.0 * PIO2_1T;
212 return (-3, y);
213 }
214 if ix == 0x4019_21fb {
215 return rem_pio2_medium(x, ix);
216 }
217 if !sign {
218 let z = x - 4.0 * PIO2_1;
219 y[0] = z - 4.0 * PIO2_1T;
220 y[1] = (z - y[0]) - 4.0 * PIO2_1T;
221 return (4, y);
222 }
223 let z = x + 4.0 * PIO2_1;
224 y[0] = z + 4.0 * PIO2_1T;
225 y[1] = (z - y[0]) + 4.0 * PIO2_1T;
226 return (-4, y);
227 }
228
229 if ix < 0x4139_21fb {
230 return rem_pio2_medium(x, ix);
232 }
233
234 if ix >= 0x7ff0_0000 {
235 let v = x - x;
237 y[0] = v;
238 y[1] = v;
239 return (0, y);
240 }
241
242 let quotient = (x / HALF_PI).round();
245 let q = quotient as i32;
246 y[0] = x - quotient * HALF_PI;
247 y[1] = 0.0;
248 (q, y)
249}
250
251fn rem_pio2_medium(x: f64, ix: u32) -> (i32, [f64; 2]) {
253 const TOINT: f64 = 1.5 / f64::EPSILON;
254 const PIO4: f64 = 7.85398163397448278999e-01; const INVPIO2: f64 = 6.36619772367581382433e-01;
256 const PIO2_1: f64 = 1.57079632673412561417e+00;
257 const PIO2_1T: f64 = 6.07710050650619224932e-11;
258 const PIO2_2: f64 = 6.07710050630396597660e-11;
259 const PIO2_2T: f64 = 2.02226624879595063154e-21;
260 const PIO2_3: f64 = 2.02226624871116645580e-21;
261 const PIO2_3T: f64 = 8.47842766036889956997e-32;
262
263 let mut y = [0.0f64; 2];
264 let mut fn_ = x * INVPIO2 + TOINT - TOINT;
265 let mut n = fn_ as i32;
266 let mut r = x - fn_ * PIO2_1;
267 let mut w = fn_ * PIO2_1T;
268
269 if r - w < -PIO4 {
270 n -= 1;
271 fn_ -= 1.0;
272 r = x - fn_ * PIO2_1;
273 w = fn_ * PIO2_1T;
274 } else if r - w > PIO4 {
275 n += 1;
276 fn_ += 1.0;
277 r = x - fn_ * PIO2_1;
278 w = fn_ * PIO2_1T;
279 }
280
281 y[0] = r - w;
282 let uy0 = y[0].to_bits();
283 let ey = ((uy0 >> 52) & 0x7ff) as i32;
284 let ex = (ix >> 20) as i32;
285
286 if ex - ey > 16 {
287 let t = r;
288 w = fn_ * PIO2_2;
289 r = t - w;
290 w = fn_ * PIO2_2T - ((t - r) - w);
291 y[0] = r - w;
292 let uy0_2 = y[0].to_bits();
293 let ey2 = ((uy0_2 >> 52) & 0x7ff) as i32;
294 if ex - ey2 > 49 {
295 let t2 = r;
296 w = fn_ * PIO2_3;
297 r = t2 - w;
298 w = fn_ * PIO2_3T - ((t2 - r) - w);
299 y[0] = r - w;
300 }
301 }
302
303 y[1] = (r - y[0]) - w;
304 (n, y)
305}
306
307pub fn sin(x: f64) -> f64 {
313 let ix = ((x.to_bits() >> 32) & 0x7fff_ffff) as u32;
314 if ix <= 0x3fe9_21fb {
315 if ix < 0x3e50_0000 {
317 return x; }
319 return sin_kernel(x, 0.0, 0);
320 }
321 if ix >= 0x7ff0_0000 {
322 return x - x; }
324 let (n, y) = rem_pio2(x);
325 match n & 3 {
326 0 => sin_kernel(y[0], y[1], 1),
327 1 => cos_kernel(y[0], y[1]),
328 2 => -sin_kernel(y[0], y[1], 1),
329 _ => -cos_kernel(y[0], y[1]),
330 }
331}
332
333pub fn cos(x: f64) -> f64 {
335 let ix = ((x.to_bits() >> 32) & 0x7fff_ffff) as u32;
336 if ix <= 0x3fe9_21fb {
337 if ix < 0x3e46_a09e {
339 return 1.0;
340 }
341 return cos_kernel(x, 0.0);
342 }
343 if ix >= 0x7ff0_0000 {
344 return x - x; }
346 let (n, y) = rem_pio2(x);
347 match n & 3 {
348 0 => cos_kernel(y[0], y[1]),
349 1 => -sin_kernel(y[0], y[1], 1),
350 2 => -cos_kernel(y[0], y[1]),
351 _ => sin_kernel(y[0], y[1], 1),
352 }
353}
354
355pub fn tan(x: f64) -> f64 {
357 let ix = high_word(x) & 0x7fff_ffff;
358 if ix <= 0x3fe9_21fb {
359 if ix < 0x3e40_0000 {
361 return x;
362 }
363 return tan_kernel(x, 0.0, 0);
364 }
365 if ix >= 0x7ff0_0000 {
366 return x - x; }
368 let (n, y) = rem_pio2(x);
369 tan_kernel(y[0], y[1], n & 1)
370}
371
372pub fn acos(x: f64) -> f64 {
374 const PIO2_HI: f64 = 1.57079632679489655800e+00;
375 const PIO2_LO: f64 = 6.12323399573676603587e-17;
376 const PS0: f64 = 1.66666666666666657415e-01;
377 const PS1: f64 = -3.25565818622400915405e-01;
378 const PS2: f64 = 2.01212532134862925881e-01;
379 const PS3: f64 = -4.00555345006794114027e-02;
380 const PS4: f64 = 7.91534994289814532176e-04;
381 const PS5: f64 = 3.47933107596021167570e-05;
382 const QS1: f64 = -2.40339491173441421878e+00;
383 const QS2: f64 = 2.02094576023350569471e+00;
384 const QS3: f64 = -6.88283971605453293030e-01;
385 const QS4: f64 = 7.70381505559019352791e-02;
386
387 #[inline]
388 fn r(z: f64) -> f64 {
389 let p = z * (PS0 + z * (PS1 + z * (PS2 + z * (PS3 + z * (PS4 + z * PS5)))));
390 let q = 1.0 + z * (QS1 + z * (QS2 + z * (QS3 + z * QS4)));
391 p / q
392 }
393
394 let xx = x.to_bits();
395 let hx = (xx >> 32) as u32;
396 let ix = hx & 0x7fff_ffff;
397
398 if ix >= 0x3ff0_0000 {
399 let lx = xx as u32;
400 if (ix.wrapping_sub(0x3ff0_0000) | lx) == 0 {
401 if (hx >> 31) != 0 {
402 return 2.0 * PIO2_HI + f64::from_bits(0x3987_0000_0000_0000); }
404 return 0.0;
405 }
406 return 0.0 / (x - x); }
408
409 if ix < 0x3fe0_0000 {
410 if ix <= 0x3c60_0000 {
412 return PIO2_HI + f64::from_bits(0x3987_0000_0000_0000);
413 }
414 return PIO2_HI - (x - (PIO2_LO - x * r(x * x)));
415 }
416
417 if (hx >> 31) != 0 {
418 let z = (1.0 + x) * 0.5;
420 let s = z.sqrt();
421 let w = r(z) * s - PIO2_LO;
422 return 2.0 * (PIO2_HI - (s + w));
423 }
424
425 let z = (1.0 - x) * 0.5;
427 let s = z.sqrt();
428 let df = f64::from_bits(s.to_bits() & 0xffff_ffff_0000_0000);
429 let c = (z - df * df) / (s + df);
430 let w = r(z) * s + c;
431 2.0 * (df + w)
432}
433
434pub fn asin(x: f64) -> f64 {
436 const HALF_PI: f64 = 1.57079632679489661923132169163975144;
437 if !x.is_finite() || x < -1.0 || x > 1.0 {
438 return f64::NAN;
439 }
440 if x == 1.0 {
441 return HALF_PI;
442 }
443 if x == -1.0 {
444 return -HALF_PI;
445 }
446 HALF_PI - acos(x)
447}
448
449pub fn atan(x: f64) -> f64 {
451 const ATANHI: [f64; 4] = [
452 4.63647609000806093515e-01,
453 7.85398163397448278999e-01,
454 9.82793723247329054082e-01,
455 1.57079632679489655800e+00,
456 ];
457 const ATANLO: [f64; 4] = [
458 2.26987774529616870924e-17,
459 3.06161699786838301793e-17,
460 1.39033110312309984516e-17,
461 6.12323399573676603587e-17,
462 ];
463 const AT: [f64; 11] = [
464 3.33333333333329318027e-01,
465 -1.99999999998764832476e-01,
466 1.42857142725034663711e-01,
467 -1.11111104054623557880e-01,
468 9.09088713343650656196e-02,
469 -7.69187620504482999495e-02,
470 6.66107313738753120669e-02,
471 -5.83357013379057348645e-02,
472 4.97687799461593236017e-02,
473 -3.65315727442169155270e-02,
474 1.62858201153657823623e-02,
475 ];
476
477 let mut ix = high_word(x);
478 let sign = ix >> 31;
479 ix &= 0x7fff_ffff;
480
481 if ix >= 0x4410_0000 {
482 if x.is_nan() {
484 return x;
485 }
486 let z = ATANHI[3] + f64::from_bits(0x3987_0000_0000_0000); return if sign != 0 { -z } else { z };
488 }
489
490 let mut x = x;
491 let id: i32;
492 if ix < 0x3fdc_0000 {
493 if ix < 0x3e40_0000 {
495 return x; }
497 id = -1;
498 } else {
499 x = x.abs();
500 if ix < 0x3ff3_0000 {
501 if ix < 0x3fe6_0000 {
503 id = 0;
505 x = (2.0 * x - 1.0) / (2.0 + x);
506 } else {
507 id = 1;
509 x = (x - 1.0) / (x + 1.0);
510 }
511 } else if ix < 0x4003_8000 {
512 id = 2;
514 x = (x - 1.5) / (1.0 + 1.5 * x);
515 } else {
516 id = 3;
518 x = -1.0 / x;
519 }
520 }
521
522 let z = x * x;
523 let w = z * z;
524 let s1 = z * (AT[0] + w * (AT[2] + w * (AT[4] + w * (AT[6] + w * (AT[8] + w * AT[10])))));
525 let s2 = w * (AT[1] + w * (AT[3] + w * (AT[5] + w * (AT[7] + w * AT[9]))));
526
527 if id < 0 {
528 return x - x * (s1 + s2);
529 }
530 let zz =
531 ATANHI[id as usize] - (x * (s1 + s2) - ATANLO[id as usize] - x);
532 if sign != 0 {
533 -zz
534 } else {
535 zz
536 }
537}
538
539pub fn atan2(y: f64, x: f64) -> f64 {
541 const PI: f64 = 3.1415926535897931160E+00;
542 const PI_LO: f64 = 1.2246467991473531772E-16;
543
544 if x.is_nan() || y.is_nan() {
545 return x + y;
546 }
547
548 let mut ix = high_word(x);
549 let mut iy = high_word(y);
550 let lx = low_word(x);
551 let ly = low_word(y);
552
553 if (ix.wrapping_sub(0x3ff0_0000) | lx) == 0 {
554 return atan(y); }
556
557 let m = ((iy >> 31) & 1) | ((ix >> 30) & 2);
558 ix &= 0x7fff_ffff;
559 iy &= 0x7fff_ffff;
560
561 if (iy | ly) == 0 {
562 return match m {
564 0 | 1 => y,
565 2 => PI,
566 _ => -PI,
567 };
568 }
569 if (ix | lx) == 0 {
570 return if (m & 1) != 0 { -PI / 2.0 } else { PI / 2.0 };
572 }
573
574 if ix == 0x7ff0_0000 {
575 if iy == 0x7ff0_0000 {
577 return match m {
579 0 => PI / 4.0,
580 1 => -PI / 4.0,
581 2 => 3.0 * PI / 4.0,
582 _ => -3.0 * PI / 4.0,
583 };
584 }
585 return match m {
586 0 => 0.0,
587 1 => -0.0,
588 2 => PI,
589 _ => -PI,
590 };
591 }
592
593 if ix + (64 << 20) < iy || iy == 0x7ff0_0000 {
594 return if (m & 1) != 0 { -PI / 2.0 } else { PI / 2.0 };
596 }
597
598 let z;
599 if (m & 2) != 0 && iy + (64 << 20) < ix {
600 z = 0.0; } else {
602 z = atan((y / x).abs());
603 }
604
605 match m {
606 0 => z,
607 1 => -z,
608 2 => PI - (z - PI_LO),
609 _ => (z - PI_LO) - PI,
610 }
611}
612
613#[cfg(test)]
618mod tests {
619 use super::*;
620
621 const PI: f64 = std::f64::consts::PI;
622 const FRAC_PI_2: f64 = std::f64::consts::FRAC_PI_2;
623 const FRAC_PI_4: f64 = std::f64::consts::FRAC_PI_4;
624
625 #[test]
626 fn test_sin_basic() {
627 assert_eq!(sin(0.0), 0.0);
628 assert!((sin(FRAC_PI_2) - 1.0).abs() < 1e-15);
629 assert!((sin(PI)).abs() < 1e-15);
630 assert!((sin(-FRAC_PI_2) + 1.0).abs() < 1e-15);
631 }
632
633 #[test]
634 fn test_cos_basic() {
635 assert_eq!(cos(0.0), 1.0);
636 assert!((cos(FRAC_PI_2)).abs() < 1e-15);
637 assert!((cos(PI) + 1.0).abs() < 1e-15);
638 }
639
640 #[test]
641 fn test_tan_basic() {
642 assert_eq!(tan(0.0), 0.0);
643 assert!((tan(FRAC_PI_4) - 1.0).abs() < 1e-15);
644 }
645
646 #[test]
647 fn test_acos_basic() {
648 assert_eq!(acos(1.0), 0.0);
649 assert!((acos(0.0) - FRAC_PI_2).abs() < 1e-15);
650 assert!((acos(-1.0) - PI).abs() < 1e-15);
651 }
652
653 #[test]
654 fn test_asin_basic() {
655 assert_eq!(asin(0.0), 0.0);
656 assert!((asin(1.0) - FRAC_PI_2).abs() < 1e-15);
657 }
658
659 #[test]
660 fn test_atan_basic() {
661 assert_eq!(atan(0.0), 0.0);
662 assert!((atan(1.0) - FRAC_PI_4).abs() < 1e-15);
663 }
664
665 #[test]
666 fn test_atan2_basic() {
667 assert!((atan2(1.0, 1.0) - FRAC_PI_4).abs() < 1e-15);
668 assert!((atan2(0.0, 1.0)).abs() < 1e-15);
669 assert!((atan2(1.0, 0.0) - FRAC_PI_2).abs() < 1e-15);
670 }
671
672 #[test]
675 fn test_agreement_with_std() {
676 let test_values: Vec<f64> = (-100..=100)
677 .map(|i| i as f64 * 0.1)
678 .collect();
679
680 for &v in &test_values {
681 let our_sin = sin(v);
682 let std_sin = v.sin();
683 assert!(
684 (our_sin - std_sin).abs() < 1e-12,
685 "sin({v}): ours={our_sin}, std={std_sin}"
686 );
687
688 let our_cos = cos(v);
689 let std_cos = v.cos();
690 assert!(
691 (our_cos - std_cos).abs() < 1e-12,
692 "cos({v}): ours={our_cos}, std={std_cos}"
693 );
694
695 let our_tan = tan(v);
696 let std_tan = v.tan();
697 if std_tan.abs() < 1e6 {
699 assert!(
700 (our_tan - std_tan).abs() < 1e-10,
701 "tan({v}): ours={our_tan}, std={std_tan}"
702 );
703 }
704 }
705 }
706
707 #[test]
708 fn test_acos_agreement_with_std() {
709 for i in -100..=100 {
710 let v = i as f64 * 0.01; let our = acos(v);
712 let std_val = v.acos();
713 assert!(
714 (our - std_val).abs() < 1e-14,
715 "acos({v}): ours={our}, std={std_val}"
716 );
717 }
718 }
719
720 #[test]
721 fn test_atan_agreement_with_std() {
722 let test_values: Vec<f64> = (-100..=100)
723 .map(|i| i as f64 * 0.1)
724 .collect();
725 for &v in &test_values {
726 let our = atan(v);
727 let std_val = v.atan();
728 assert!(
729 (our - std_val).abs() < 1e-14,
730 "atan({v}): ours={our}, std={std_val}"
731 );
732 }
733 }
734
735 #[test]
736 fn test_atan2_agreement_with_std() {
737 let vals = [-10.0, -1.0, -0.1, 0.0, 0.1, 1.0, 10.0];
738 for &y in &vals {
739 for &x in &vals {
740 if x == 0.0 && y == 0.0 {
741 continue;
742 }
743 let our = atan2(y, x);
744 let std_val = y.atan2(x);
745 assert!(
746 (our - std_val).abs() < 1e-14,
747 "atan2({y}, {x}): ours={our}, std={std_val}"
748 );
749 }
750 }
751 }
752
753 #[test]
754 fn test_special_values() {
755 assert!(sin(f64::NAN).is_nan());
757 assert!(cos(f64::NAN).is_nan());
758 assert!(tan(f64::NAN).is_nan());
759
760 assert!(sin(f64::INFINITY).is_nan());
762 assert!(cos(f64::INFINITY).is_nan());
763 assert!(tan(f64::INFINITY).is_nan());
764 }
765}