1use crate::Real;
2
3pub const fn tdb_minus_tt(seconds_since_j2000_tt: Real) -> Real {
8 const J2000_SEC_PER_MILLENNIUM: f64 = 31_557_600_000.0;
10
11 let t = seconds_since_j2000_tt / J2000_SEC_PER_MILLENNIUM; let mut correction = f!(0.0);
13
14 let g = f!(6283.075849991) * t + f!(6.240054195);
16 let e = f!(0.0167086342) - f!(0.0004203654) * t - f!(0.0000126734) * t * t
17 + f!(0.0000001444) * t * t * t
18 - f!(0.0000000002) * t * t * t * t
19 + f!(0.0000000003) * t * t * t * t * t;
20 let k = f!(0.09897232);
21 let varpi = f!(-0.00000257) - f!(0.05551247) * t;
22 correction += k * e * sin(g + varpi + f!(0.01671) * sin(g));
23
24 let lte440_terms: [(f64, f64, f64); 12] = [
26 (0.00012630813184, 77713.771468120, 5.18472464), (0.00001937467715, 5753.384884897, 1.33855843), (0.00001370088760, 12566.151699983, 3.07602294), (0.00000747520418, 5574.656149776, 3.32446352), (0.00000424397312, 4320.34946237, 3.43186281), (0.00000376051430, 377.97977422, 0.92358639), (0.00000293368121, 161002.466707021, 1.09317212), (0.00000267752983, 6208.659051973, 1.51225314), (0.00000236687890, 71430.993657045, 5.21748801), (0.00000185820098, 211.334300759, 2.56843762), (0.00000109742615, 3929.675646567, 4.67635157), (0.00000108850698, 7859.351293133, 2.99248981), ];
39
40 let mut i = 0;
41 while i < 12 {
42 let (amp, freq, phase) = lte440_terms[i];
43 correction += amp * sin(freq * t + phase);
44 i += 1;
45 }
46
47 correction += f!(0.00000000065) * sin(f!(6069.776754) * t + f!(4.021194));
49 correction += f!(0.00000000033) * sin(f!(213.299095) * t + f!(5.543132));
50 correction += f!(-0.00000000196) * sin(f!(6208.294251) * t + f!(5.696701));
51 correction += f!(-0.00000000173) * sin(f!(74.781599) * t + f!(2.435900));
52 correction += f!(0.00000003638) * t * t; correction
55}
56
57pub(crate) const fn clamp_i128_to_i64(x: i128) -> i64 {
59 if x > i64::MAX as i128 {
60 i64::MAX
61 } else if x < i64::MIN as i128 {
62 i64::MIN
63 } else {
64 x as i64
65 }
66}
67
68pub const fn sin(x: Real) -> Real {
72 const PI: Real = f!(core::f64::consts::PI);
73 const TWO_PI: Real = f!(2.0) * PI;
74
75 let mut x = x % TWO_PI;
77 if x < f!(0.0) {
78 x += TWO_PI;
79 }
80 if x > PI {
81 x -= TWO_PI;
82 }
83
84 let sign = if x < f!(0.0) { f!(-1.0) } else { f!(1.0) };
86 let x = x.abs();
87 let x = if x > PI / f!(2.0) { PI - x } else { x };
88
89 let y = x * x;
92
93 let p = f!(-1.0) / f!(121645100408832000.0); let p = p * y + f!(1.0) / f!(355687428096000.0); let p = p * y + f!(-1.0) / f!(1307674368000.0); let p = p * y + f!(1.0) / f!(6227020800.0); let p = p * y + f!(-1.0) / f!(39916800.0); let p = p * y + f!(1.0) / f!(362880.0); let p = p * y + f!(-1.0) / f!(5040.0); let p = p * y + f!(1.0) / f!(120.0); let p = p * y + f!(-1.0) / f!(6.0); let p = p * y + f!(1.0); sign * (x * p)
105}
106
107pub const fn floor_f(x: Real) -> Real {
112 if x.is_nan() || x.is_infinite() {
113 x
114 } else if x == f!(0.0) {
115 x } else {
117 let i = x as i64;
118 let truncated = f!(i);
119 if x >= f!(0.0) || truncated == x {
120 truncated
121 } else {
122 truncated - f!(1.0)
123 }
124 }
125}
126
127const LN2_HI: f64 = 6.93147180369123816490e-01; const LN2_LO: f64 = 1.90821492927058770002e-10; const LG1: f64 = 6.666666666666735130e-01; const LG2: f64 = 3.999999999940941908e-01; const LG3: f64 = 2.857142874366239149e-01; const LG4: f64 = 2.222219843214978396e-01; const LG5: f64 = 1.818357216161805012e-01; const LG6: f64 = 1.531383769920937332e-01; const LG7: f64 = 1.479819860511658591e-01; pub const fn log(mut x: f64) -> f64 {
139 let x1p54 = f64::from_bits(0x4350000000000000); let mut ui = x.to_bits();
142 let mut hx: u32 = (ui >> 32) as u32;
143 let mut k: i32 = 0;
144
145 if (hx < 0x00100000) || ((hx >> 31) != 0) {
146 if ui << 1 == 0 {
148 return -1. / (x * x); }
150 if hx >> 31 != 0 {
151 return (x - x) / 0.0; }
153 k -= 54;
155 x *= x1p54;
156 ui = x.to_bits();
157 hx = (ui >> 32) as u32;
158 } else if hx >= 0x7ff00000 {
159 return x;
160 } else if hx == 0x3ff00000 && ui << 32 == 0 {
161 return 0.;
162 }
163
164 hx += 0x3ff00000 - 0x3fe6a09e;
166 k += ((hx >> 20) as i32) - 0x3ff;
167 hx = (hx & 0x000fffff) + 0x3fe6a09e;
168 ui = ((hx as u64) << 32) | (ui & 0xffffffff);
169 x = f64::from_bits(ui);
170
171 let f: f64 = x - 1.0;
172 let hfsq: f64 = 0.5 * f * f;
173 let s: f64 = f / (2.0 + f);
174 let z: f64 = s * s;
175 let w: f64 = z * z;
176 let t1: f64 = w * (LG2 + w * (LG4 + w * LG6));
177 let t2: f64 = z * (LG1 + w * (LG3 + w * (LG5 + w * LG7)));
178 let r: f64 = t2 + t1;
179 let dk: f64 = k as f64;
180 s * (hfsq + r) + dk * LN2_LO - hfsq + f + dk * LN2_HI
181}
182
183const RSQRT_TAB: [u16; 128] = [
188 0xb451, 0xb2f0, 0xb196, 0xb044, 0xaef9, 0xadb6, 0xac79, 0xab43, 0xaa14, 0xa8eb, 0xa7c8, 0xa6aa,
189 0xa592, 0xa480, 0xa373, 0xa26b, 0xa168, 0xa06a, 0x9f70, 0x9e7b, 0x9d8a, 0x9c9d, 0x9bb5, 0x9ad1,
190 0x99f0, 0x9913, 0x983a, 0x9765, 0x9693, 0x95c4, 0x94f8, 0x9430, 0x936b, 0x92a9, 0x91ea, 0x912e,
191 0x9075, 0x8fbe, 0x8f0a, 0x8e59, 0x8daa, 0x8cfe, 0x8c54, 0x8bac, 0x8b07, 0x8a64, 0x89c4, 0x8925,
192 0x8889, 0x87ee, 0x8756, 0x86c0, 0x862b, 0x8599, 0x8508, 0x8479, 0x83ec, 0x8361, 0x82d8, 0x8250,
193 0x81c9, 0x8145, 0x80c2, 0x8040, 0xff02, 0xfd0e, 0xfb25, 0xf947, 0xf773, 0xf5aa, 0xf3ea, 0xf234,
194 0xf087, 0xeee3, 0xed47, 0xebb3, 0xea27, 0xe8a3, 0xe727, 0xe5b2, 0xe443, 0xe2dc, 0xe17a, 0xe020,
195 0xdecb, 0xdd7d, 0xdc34, 0xdaf1, 0xd9b3, 0xd87b, 0xd748, 0xd61a, 0xd4f1, 0xd3cd, 0xd2ad, 0xd192,
196 0xd07b, 0xcf69, 0xce5b, 0xcd51, 0xcc4a, 0xcb48, 0xca4a, 0xc94f, 0xc858, 0xc764, 0xc674, 0xc587,
197 0xc49d, 0xc3b7, 0xc2d4, 0xc1f4, 0xc116, 0xc03c, 0xbf65, 0xbe90, 0xbdbe, 0xbcef, 0xbc23, 0xbb59,
198 0xba91, 0xb9cc, 0xb90a, 0xb84a, 0xb78c, 0xb6d0, 0xb617, 0xb560,
199];
200
201#[inline]
202const fn mul32(a: u32, b: u32) -> u32 {
203 ((a as u64).wrapping_mul(b as u64) >> 32) as u32
204}
205
206#[inline]
207const fn mul64(a: u64, b: u64) -> u64 {
208 let ahi = a >> 32;
209 let alo = a & 0xffffffff;
210 let bhi = b >> 32;
211 let blo = b & 0xffffffff;
212 ahi.wrapping_mul(bhi)
213 .wrapping_add(ahi.wrapping_mul(blo) >> 32)
214 .wrapping_add(alo.wrapping_mul(bhi) >> 32)
215}
216
217pub const fn sqrt(x: f64) -> f64 {
221 let mut ix = x.to_bits();
222 let mut top = ix >> 52;
223
224 if top.wrapping_sub(0x001) >= 0x7fe {
226 if ix << 1 == 0 {
227 return x; }
229 if ix == 0x7ff0_0000_0000_0000 {
230 return x; }
232 if ix > 0x7ff0_0000_0000_0000 {
233 let nan_bits = 0x7ff8_0000_0000_0000 | (ix & 0x8000_0000_0000_0000);
235 return f64::from_bits(nan_bits);
236 }
237 let scale = f64::from_bits(0x4330_0000_0000_0000); ix = (x * scale).to_bits();
240 top = (ix >> 52).wrapping_sub(52);
241 }
242
243 let even = top & 1;
244 let mut m = (ix << 11) | 0x8000_0000_0000_0000u64;
245 if even != 0 {
246 m >>= 1;
247 }
248 let top = (top.wrapping_add(0x3ff)) >> 1; let three: u64 = 0xc000_0000;
253 let i = ((ix >> 46) % 128) as usize;
254 let mut r: u64 = (RSQRT_TAB[i] as u64) << 16;
255
256 let mut s: u64 = mul32((m >> 32) as u32, r as u32) as u64;
257 let mut d: u64 = mul32(s as u32, r as u32) as u64;
258 let mut u: u64 = three - d;
259 r = (mul32(r as u32, u as u32) << 1) as u64;
260 s = (mul32(s as u32, u as u32) << 1) as u64;
261
262 d = mul32(s as u32, r as u32) as u64;
263 u = three - d;
264 r = (mul32(r as u32, u as u32) << 1) as u64;
265
266 r <<= 32;
267 s = mul64(m, r);
268 d = mul64(s, r);
269 u = (three << 32) - d;
270 s = mul64(s, u);
271
272 s = (s - 2) >> 9;
274
275 let d0 = (m << 42).wrapping_sub(s.wrapping_mul(s));
276 let d1 = s.wrapping_sub(d0);
277 let _d2 = d1.wrapping_add(s).wrapping_add(1);
278
279 if (d1 >> 63) != 0 {
280 s = s.wrapping_add(1);
281 }
282 s &= 0x000f_ffff_ffff_ffff;
283 s |= (top as u64) << 52;
284
285 f64::from_bits(s)
286}
287
288const SPLIT: f64 = 134217728. + 1.; const fn sq(x: f64) -> (f64, f64) {
291 let xh: f64;
292 let xl: f64;
293 let xc: f64;
294
295 xc = x * SPLIT;
296 xh = x - xc + xc;
297 xl = x - xh;
298 let hi = x * x;
299 let lo = xh * xh - hi + 2. * xh * xl + xl * xl;
300 (hi, lo)
301}
302
303pub const fn hypot(mut x: f64, mut y: f64) -> f64 {
304 let x1p700 = f64::from_bits(0x6bb0000000000000); let x1p_700 = f64::from_bits(0x1430000000000000); let mut uxi = x.to_bits();
308 let mut uyi = y.to_bits();
309 let uti;
310 let ex: i64;
311 let ey: i64;
312 let mut z: f64;
313
314 uxi &= -1i64 as u64 >> 1;
316 uyi &= -1i64 as u64 >> 1;
317 if uxi < uyi {
318 uti = uxi;
319 uxi = uyi;
320 uyi = uti;
321 }
322
323 ex = (uxi >> 52) as i64;
325 ey = (uyi >> 52) as i64;
326 x = f64::from_bits(uxi);
327 y = f64::from_bits(uyi);
328 if ey == 0x7ff {
330 return y;
331 }
332 if ex == 0x7ff || uyi == 0 {
333 return x;
334 }
335 if ex - ey > 64 {
338 return x + y;
339 }
340
341 z = 1.;
344 if ex > 0x3ff + 510 {
345 z = x1p700;
346 x *= x1p_700;
347 y *= x1p_700;
348 } else if ey < 0x3ff - 450 {
349 z = x1p_700;
350 x *= x1p700;
351 y *= x1p700;
352 }
353 let (hx, lx) = sq(x);
354 let (hy, ly) = sq(y);
355 z * sqrt(ly + lx + hy + hx)
356}
357
358#[cfg(feature = "std")]
359#[cfg(test)]
360mod tests {
361 use super::sqrt;
362 use std::{eprintln, f64, vec, vec::Vec};
363
364 #[test]
365 fn test_special_cases() {
366 assert_eq!(sqrt(0.0), 0.0);
367 assert_eq!(sqrt(-0.0), -0.0);
368 assert!(sqrt(f64::INFINITY).is_infinite() && sqrt(f64::INFINITY) > 0.0);
369 assert!(sqrt(f64::NEG_INFINITY).is_nan());
370 assert!(sqrt(-1.0).is_nan());
371 assert!(sqrt(f64::NAN).is_nan());
372 }
374
375 #[test]
376 fn test_perfect_squares() {
377 for i in 0..100u32 {
378 let x = (i * i) as f64;
379 let r = sqrt(x);
380 assert!((r - i as f64).abs() < 1e-10 || r.is_nan());
381 }
382 }
383
384 #[test]
385 fn test_random_vs_std() {
386 let mut failures = 0u32;
388 let mut state: u64 = 0x123456789abcdef0;
389 for _ in 0..5_000_000 {
390 state = state.wrapping_mul(6364136223846793005).wrapping_add(1);
391 let bits = (state & 0x000f_ffff_ffff_ffff) | 0x3ff0_0000_0000_0000; let val = f64::from_bits(bits);
393 let r1 = sqrt(val);
394 let r2 = val.sqrt();
395 if r1.to_bits() != r2.to_bits() {
396 failures += 1;
397 if failures < 3 {
398 eprintln!(
399 "Mismatch at {:016x}: ours={:016x} std={:016x}",
400 bits,
401 r1.to_bits(),
402 r2.to_bits()
403 );
404 }
405 }
406 }
407 assert_eq!(
408 failures, 0,
409 "Found {} mismatches in 5M random normals [1,2)",
410 failures
411 );
412 }
413
414 #[test]
415 fn test_subnormals_random() {
416 let mut failures = 0u32;
418 let mut state: u64 = 0xdeadbeefcafebabe;
419 for _ in 0..100_000 {
420 state = state.wrapping_mul(6364136223846793005).wrapping_add(1);
421 let bits = state & 0x000f_ffff_ffff_ffff; let val = f64::from_bits(bits);
424 if val == 0.0 {
425 continue;
426 } let r1 = sqrt(val);
428 let r2 = val.sqrt();
429 if r1.to_bits() != r2.to_bits() {
430 failures += 1;
431 if failures < 3 {
432 eprintln!(
433 "Subnormal mismatch at {:016x}: ours={:016x} std={:016x}",
434 bits,
435 r1.to_bits(),
436 r2.to_bits()
437 );
438 }
439 }
440 }
441 assert_eq!(
442 failures, 0,
443 "Found {} mismatches in 100k random subnormals",
444 failures
445 );
446 }
447
448 #[test]
449 fn test_boundaries() {
450 let boundaries: [f64; 8] = [
452 f64::MIN_POSITIVE, f64::from_bits(0x0010_0000_0000_0000), f64::from_bits(0x000f_ffff_ffff_ffff), 2.0f64.powi(-1074), f64::MAX, f64::from_bits(0x7fe0_0000_0000_0000), 2.0f64.powi(1023), 2.0f64.powi(-1022) * (1.0 + f64::EPSILON), ];
461 for &x in &boundaries {
462 let r1 = sqrt(x);
463 let r2 = x.sqrt();
464 assert_eq!(r1.to_bits(), r2.to_bits(), "Boundary mismatch for {:e}", x);
465 if x > 0.0 && x.is_finite() && x > 1e-200 {
467 let xx = x * x;
468 if xx.is_finite() && xx.is_normal() {
469 let r = sqrt(xx);
470 let rel = ((r - x).abs() / x).max(0.0);
471 assert!(
472 rel < 1e-14 || r.is_nan(),
473 "sqrt(x*x) not close to x for {}",
474 x
475 );
476 }
477 }
478 }
479 }
480
481 #[test]
482 fn test_known_hard_cases() {
483 let cases: &[f64] = &[
485 2.0,
486 0.5,
487 4.0,
488 9.0,
489 0.0,
490 f64::INFINITY,
491 1.0e-300, f64::from_bits(0x0010_0000_0000_0001), 1.0 + f64::EPSILON, f64::from_bits(0x7fefffffffffffff), ];
496 for &x in cases {
497 let r = sqrt(x);
498 assert_eq!(r.to_bits(), x.sqrt().to_bits(), "Bit mismatch for {:e}", x);
500 }
501 }
502
503 fn next_up(x: f64) -> f64 {
505 if x.is_nan() || x == f64::INFINITY {
506 return x;
507 }
508 if x == 0.0 {
509 return f64::from_bits(1);
510 }
511 let bits = x.to_bits();
512 if x > 0.0 {
513 f64::from_bits(bits + 1)
514 } else {
515 f64::from_bits(bits - 1)
516 }
517 }
518 fn next_down(x: f64) -> f64 {
519 if x.is_nan() || x == f64::NEG_INFINITY {
520 return x;
521 }
522 if x == -0.0 || x == 0.0 {
523 return f64::from_bits(0x8000_0000_0000_0001);
524 }
525 let bits = x.to_bits();
526 if x > 0.0 {
527 f64::from_bits(bits - 1)
528 } else {
529 f64::from_bits(bits + 1)
530 }
531 }
532
533 #[test]
534 fn test_powers_of_two() {
535 for exp in -1074i32..=1023 {
537 let x = if exp >= -1022 {
538 2.0f64.powi(exp)
539 } else {
540 f64::from_bits(1u64 << (exp + 1074))
542 };
543 if !x.is_finite() || x == 0.0 {
544 continue;
545 }
546 let r1 = sqrt(x);
547 let r2 = x.sqrt();
548 assert_eq!(
549 r1.to_bits(),
550 r2.to_bits(),
551 "Power-of-2 mismatch for 2^{}",
552 exp
553 );
554 if exp % 2 == 0 {
556 let expected_exp = exp / 2;
557 if expected_exp >= -1022 {
558 let expected = 2.0f64.powi(expected_exp);
559 assert_eq!(
560 r1.to_bits(),
561 expected.to_bits(),
562 "Even power-of-2 not exact for 2^{}",
563 exp
564 );
565 }
566 }
567 }
568 }
569
570 #[test]
571 fn test_nextafter_edges() {
572 let mut edges: Vec<f64> = vec![
574 f64::from_bits(1), f64::from_bits(0x0000_0000_0000_0002), next_down(f64::MIN_POSITIVE), f64::MIN_POSITIVE, next_up(f64::MIN_POSITIVE),
579 next_down(1.0),
580 1.0,
581 next_up(1.0),
582 next_down(f64::MAX),
583 f64::MAX,
584 ];
585 edges.push(next_up(-f64::MIN_POSITIVE)); for &x in &edges {
588 let r1 = sqrt(x);
589 let r2 = x.sqrt();
590 assert_eq!(
591 r1.to_bits(),
592 r2.to_bits(),
593 "nextafter edge mismatch for {:e} (bits {:016x})",
594 x,
595 x.to_bits()
596 );
597 }
598 }
599
600 #[test]
601 fn test_negative_subnormals() {
602 let mut state: u64 = 0xfeedface_deadbeef;
604 for _ in 0..10_000 {
605 state = state.wrapping_mul(6364136223846793005).wrapping_add(1);
606 let bits = (state & 0x000f_ffff_ffff_ffff) | 0x8000_0000_0000_0000; let val = f64::from_bits(bits);
608 if val == 0.0 {
609 continue;
610 }
611 let r = sqrt(val);
612 assert!(
613 r.is_nan(),
614 "Negative subnormal did not produce NaN: {:e}",
615 val
616 );
617 assert!(
619 r.to_bits() & 0x8000_0000_0000_0000 != 0,
620 "NaN sign bit not set for negative subnormal"
621 );
622 }
623 }
624}