1use anise::astro::Aberration;
20use anise::constants::frames::SUN_J2000;
21use anise::errors::OrientationSnafu;
22use anise::prelude::Almanac;
23use serde::{Deserialize, Serialize};
24use serde_dhall::StaticType;
25use snafu::ResultExt;
26use typed_builder::TypedBuilder;
27
28use crate::cosmic::{AstroPhysicsSnafu, Epoch, Frame, Orbit};
29use crate::dynamics::{
30 AccelModel, DynamicsAlmanacSnafu, DynamicsAstroSnafu, DynamicsError, DynamicsPlanetarySnafu,
31};
32use crate::linalg::{Matrix3, U7, Vector3, Vector4};
33use hyperdual::linalg::norm;
34use hyperdual::{OHyperdual, hyperspace_from_vector};
35use std::fmt;
36
37#[cfg(feature = "python")]
38use pyo3::prelude::*;
39#[cfg(feature = "python")]
40use pyo3::types::PyType;
41
42#[derive(Clone, Debug, Serialize, Deserialize, StaticType, TypedBuilder, PartialEq)]
52#[cfg_attr(feature = "python", pyclass(from_py_object, get_all, set_all))]
53pub struct SolidTides {
54 pub frame: Frame,
56 pub k2: f64,
58 pub k3: f64,
60 pub perturbers: Vec<TidalPerturber>,
62 pub correction: Option<Aberration>,
64}
65
66#[derive(Clone, Debug, Serialize, Deserialize, StaticType, TypedBuilder, PartialEq)]
71#[cfg_attr(feature = "python", pyclass(from_py_object, get_all, set_all))]
72pub struct TidalPerturber {
73 pub frame: Frame,
75 pub compute_degree_3: bool,
78}
79
80impl TidalPerturber {
81 fn compute_pert(
82 &self,
83 epoch: Epoch,
84 tidal_model: &SolidTides,
85 almanac: &Almanac,
86 ab_corr: Option<Aberration>,
87 delta_c: &mut [[f64; 4]; 4],
88 delta_s: &mut [[f64; 4]; 4],
89 ) -> Result<(), DynamicsError> {
90 let radius_km = almanac
91 .transform(self.frame, tidal_model.frame, epoch, ab_corr)
92 .context(DynamicsAlmanacSnafu {
93 action: "Moon position in ECEF",
94 })?
95 .radius_km;
96
97 let r_body = radius_km.norm();
98 let s_body = radius_km.x / r_body;
99 let t_body = radius_km.y / r_body;
100 let u_body = radius_km.z / r_body;
101
102 let sin_phi = u_body;
103 let cos_phi = (1.0 - sin_phi.powi(2)).max(0.0).sqrt();
104 let cos_lambda = if cos_phi > 1e-12 {
105 s_body / cos_phi
106 } else {
107 1.0
108 };
109 let sin_lambda = if cos_phi > 1e-12 {
110 t_body / cos_phi
111 } else {
112 0.0
113 };
114
115 let p20 = 0.5 * (3.0 * sin_phi.powi(2) - 1.0) * 5.0f64.sqrt();
117 let p21 = 3.0 * sin_phi * cos_phi * (5.0 / 3.0f64).sqrt();
118 let p22 = 3.0 * cos_phi.powi(2) * (5.0 / 12.0f64).sqrt();
119
120 let p30 = 0.5 * (5.0 * sin_phi.powi(3) - 3.0 * sin_phi) * 7.0f64.sqrt();
121 let p31 = 1.5 * (5.0 * sin_phi.powi(2) - 1.0) * cos_phi * (7.0 / 6.0f64).sqrt();
122 let p32 = 15.0 * sin_phi * cos_phi.powi(2) * (7.0 / 60.0f64).sqrt();
123 let p33 = 15.0 * cos_phi.powi(3) * (7.0 / 360.0f64).sqrt();
124
125 let primary_mu_km3_s2 = tidal_model
126 .frame
127 .mu_km3_s2()
128 .context(AstroPhysicsSnafu)
129 .context(DynamicsAstroSnafu)?;
130
131 let primary_eq_radius_km = tidal_model
132 .frame
133 .mean_equatorial_radius_km()
134 .context(AstroPhysicsSnafu)
135 .context(DynamicsAstroSnafu)?;
136
137 let secondary_mu_km3_s2 = self
138 .frame
139 .mu_km3_s2()
140 .context(AstroPhysicsSnafu)
141 .context(DynamicsAstroSnafu)?;
142
143 let gm_ratio = secondary_mu_km3_s2 / primary_mu_km3_s2;
144 let r_ratio = primary_eq_radius_km / r_body;
145
146 let m = if self.compute_degree_3 { 3 } else { 2 };
147
148 for n in 2..=m {
149 let kn = if n == 2 {
150 tidal_model.k2
151 } else {
152 tidal_model.k3
153 };
154 let common = kn / (2.0 * n as f64 + 1.0) * gm_ratio * r_ratio.powi(n as i32 + 1);
155
156 for m in 0..=n {
157 let p_nm = match (n, m) {
158 (2, 0) => p20,
159 (2, 1) => p21,
160 (2, 2) => p22,
161 (3, 0) => p30,
162 (3, 1) => p31,
163 (3, 2) => p32,
164 (3, 3) => p33,
165 _ => 0.0,
166 };
167
168 let (cos_ml, sin_ml) = match m {
169 0 => (1.0, 0.0),
170 1 => (cos_lambda, sin_lambda),
171 2 => (
172 cos_lambda.powi(2) - sin_lambda.powi(2),
173 2.0 * sin_lambda * cos_lambda,
174 ),
175 3 => (
176 cos_lambda * (cos_lambda.powi(2) - 3.0 * sin_lambda.powi(2)),
177 sin_lambda * (3.0 * cos_lambda.powi(2) - sin_lambda.powi(2)),
178 ),
179 _ => (0.0, 0.0),
180 };
181
182 delta_c[n][m] += common * p_nm * cos_ml;
183 delta_s[n][m] += common * p_nm * sin_ml;
184 }
185 }
186
187 Ok(())
188 }
189}
190
191impl SolidTides {
192 pub fn earth_moon_system(
195 mut earth_frame: Frame,
196 mut moon_frame: Frame,
197 almanac: &Almanac,
198 ab_corr: Option<Aberration>,
199 ) -> Result<Self, DynamicsError> {
200 let mut sun_j2k = almanac
201 .frame_info(SUN_J2000)
202 .context(DynamicsPlanetarySnafu {
203 action: "fetching sun frame",
204 })?;
205
206 for frame in [&mut earth_frame, &mut moon_frame, &mut sun_j2k] {
208 if frame.mu_km3_s2.is_none() || frame.mean_equatorial_radius_km().is_err() {
209 *frame = almanac.frame_info(*frame).context(DynamicsPlanetarySnafu {
210 action: "fetching frame",
211 })?;
212 }
213
214 frame
216 .mu_km3_s2()
217 .context(AstroPhysicsSnafu)
218 .context(DynamicsAstroSnafu)?;
219
220 frame
222 .mean_equatorial_radius_km()
223 .context(AstroPhysicsSnafu)
224 .context(DynamicsAstroSnafu)?;
225 }
226
227 let me = Self::builder()
228 .k2(0.3019)
229 .k3(0.093)
230 .frame(earth_frame)
231 .perturbers(vec![
232 TidalPerturber::builder()
233 .frame(moon_frame)
234 .compute_degree_3(true)
235 .build(),
236 TidalPerturber::builder()
237 .frame(sun_j2k)
238 .compute_degree_3(false)
239 .build(),
240 ])
241 .correction(ab_corr)
242 .build();
243
244 Ok(me)
245 }
246
247 fn accumulate_deltas(
249 &self,
250 epoch: Epoch,
251 almanac: &Almanac,
252 ) -> Result<([[f64; 4]; 4], [[f64; 4]; 4]), DynamicsError> {
253 let mut delta_c = [[0.0f64; 4]; 4];
254 let mut delta_s = [[0.0f64; 4]; 4];
255
256 for pert in &self.perturbers {
257 pert.compute_pert(
258 epoch,
259 self,
260 almanac,
261 self.correction,
262 &mut delta_c,
263 &mut delta_s,
264 )?;
265 }
266
267 Ok((delta_c, delta_s))
268 }
269}
270
271#[allow(clippy::needless_range_loop)]
272impl AccelModel for SolidTides {
273 fn eom(&self, osc: &Orbit, almanac: &Almanac) -> Result<Vector3<f64>, DynamicsError> {
274 let (delta_c, delta_s) = self.accumulate_deltas(osc.epoch, almanac)?;
275
276 let state = almanac
278 .transform_to(*osc, self.frame, None)
279 .context(DynamicsAlmanacSnafu {
280 action: "transforming into solid tides frame",
281 })?;
282
283 let r_ = state.rmag_km();
285 let s_ = state.radius_km.x / r_;
286 let t_ = state.radius_km.y / r_;
287 let u_ = state.radius_km.z / r_;
288
289 let mut a_nm = [[0.0f64; 6]; 6];
291 a_nm[0][0] = 1.0;
292 for n in 1..=4 {
293 a_nm[n][n] = (1.0 + 1.0 / (2.0 * n as f64)).sqrt() * a_nm[n - 1][n - 1];
294 }
295 a_nm[1][0] = u_ * 3.0f64.sqrt();
296 for n in 1..=4 {
297 a_nm[n + 1][n] = (2.0 * n as f64 + 3.0).sqrt() * u_ * a_nm[n][n];
298 }
299
300 let b_nm = |n: usize, m: usize| {
301 (((2.0 * n as f64 + 1.0) * (2.0 * n as f64 - 1.0))
302 / ((n as f64 + m as f64) * (n as f64 - m as f64)))
303 .sqrt()
304 };
305 let c_nm = |n: usize, m: usize| {
306 (((2.0 * n as f64 + 1.0) * (n as f64 + m as f64 - 1.0) * (n as f64 - m as f64 - 1.0))
307 / ((n as f64 - m as f64) * (n as f64 + m as f64) * (2.0 * n as f64 - 3.0)))
308 .sqrt()
309 };
310
311 for m in 0..=3 {
312 for n in (m + 2)..=4 {
313 a_nm[n][m] = u_ * b_nm(n, m) * a_nm[n - 1][m] - c_nm(n, m) * a_nm[n - 2][m];
314 }
315 }
316
317 let mut r_m = [0.0f64; 4];
318 let mut i_m = [0.0f64; 4];
319 r_m[0] = 1.0;
320 i_m[0] = 0.0;
321 for m in 1..=3 {
322 r_m[m] = s_ * r_m[m - 1] - t_ * i_m[m - 1];
323 i_m[m] = s_ * i_m[m - 1] + t_ * r_m[m - 1];
324 }
325
326 let eq_radius_km = self
327 .frame
328 .mean_equatorial_radius_km()
329 .context(AstroPhysicsSnafu)
330 .context(DynamicsAstroSnafu)?;
331
332 let mu_km3_s2 = self
333 .frame
334 .mu_km3_s2()
335 .context(AstroPhysicsSnafu)
336 .context(DynamicsAstroSnafu)?;
337
338 let rho = eq_radius_km / r_;
339
340 let mut rho_np1 = mu_km3_s2 / r_ * rho;
341 let mut accel4 = Vector4::zeros();
342
343 let vr01 = |n: usize, m: usize| {
344 let mut val = ((n as f64 - m as f64) * (n as f64 + m as f64 + 1.0)).sqrt();
345 if m == 0 {
346 val /= 2.0f64.sqrt();
347 }
348 val
349 };
350 let vr11 = |n: usize, m: usize| {
351 let mut val = (((2.0 * n as f64 + 1.0)
352 * (n as f64 + m as f64 + 2.0)
353 * (n as f64 + m as f64 + 1.0))
354 / (2.0 * n as f64 + 3.0))
355 .sqrt();
356 if m == 0 {
357 val /= 2.0f64.sqrt();
358 }
359 val
360 };
361
362 let sqrt2 = 2.0f64.sqrt();
363
364 for n in 1..=3 {
365 rho_np1 *= rho;
366 if n < 2 {
367 continue;
368 } let mut sum = Vector4::zeros();
371 for m in 0..=n {
372 let c_val = delta_c[n][m];
373 let s_val = delta_s[n][m];
374
375 let d_ = (c_val * r_m[m] + s_val * i_m[m]) * sqrt2;
376 let e_ = if m == 0 {
377 0.0
378 } else {
379 (c_val * r_m[m - 1] + s_val * i_m[m - 1]) * sqrt2
380 };
381 let f_ = if m == 0 {
382 0.0
383 } else {
384 (s_val * r_m[m - 1] - c_val * i_m[m - 1]) * sqrt2
385 };
386
387 sum.x += (m as f64) * a_nm[n][m] * e_;
388 sum.y += (m as f64) * a_nm[n][m] * f_;
389 sum.z += vr01(n, m) * a_nm[n][m + 1] * d_;
390 sum.w -= vr11(n, m) * a_nm[n + 1][m + 1] * d_;
391 }
392 accel4 += (rho_np1 / eq_radius_km) * sum;
393 }
394
395 let accel_ecef = Vector3::new(
396 accel4.x + accel4.w * s_,
397 accel4.y + accel4.w * t_,
398 accel4.z + accel4.w * u_,
399 );
400
401 let dcm = almanac
402 .rotate(self.frame, osc.frame, osc.epoch)
403 .context(OrientationSnafu {
404 action: "rotating accel back to integration frame",
405 })
406 .context(DynamicsAlmanacSnafu {
407 action: "rotating accel back to integration frame",
408 })?
409 .rot_mat;
410
411 Ok(dcm * accel_ecef)
412 }
413
414 fn gradient(
415 &self,
416 osc: &Orbit,
417 almanac: &Almanac,
418 ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError> {
419 let (delta_c, delta_s) = self.accumulate_deltas(osc.epoch, almanac)?;
420
421 let state = almanac
423 .transform_to(*osc, self.frame, None)
424 .context(DynamicsAlmanacSnafu {
425 action: "transforming into gravity field frame",
426 })?;
427
428 let radius: Vector3<OHyperdual<f64, U7>> = hyperspace_from_vector(&state.radius_km);
429
430 let r_ = norm(&radius);
432 let s_ = radius[0] / r_;
433 let t_ = radius[1] / r_;
434 let u_ = radius[2] / r_;
435
436 let mut a_nm = [[OHyperdual::<f64, U7>::from(0.0); 6]; 6];
438 a_nm[0][0] = OHyperdual::from(1.0);
439 for n in 1..=4 {
440 a_nm[n][n] =
441 OHyperdual::from((1.0 + 1.0 / (2.0 * n as f64)).sqrt()) * a_nm[n - 1][n - 1];
442 }
443 a_nm[1][0] = u_ * OHyperdual::from(3.0f64.sqrt());
444 for n in 1..=4 {
445 a_nm[n + 1][n] = OHyperdual::from((2.0 * n as f64 + 3.0).sqrt()) * u_ * a_nm[n][n];
446 }
447
448 let b_nm = |n: usize, m: usize| {
449 (((2.0 * n as f64 + 1.0) * (2.0 * n as f64 - 1.0))
450 / ((n as f64 + m as f64) * (n as f64 - m as f64)))
451 .sqrt()
452 };
453 let c_nm = |n: usize, m: usize| {
454 (((2.0 * n as f64 + 1.0) * (n as f64 + m as f64 - 1.0) * (n as f64 - m as f64 - 1.0))
455 / ((n as f64 - m as f64) * (n as f64 + m as f64) * (2.0 * n as f64 - 3.0)))
456 .sqrt()
457 };
458
459 for m in 0..=3 {
460 for n in (m + 2)..=4 {
461 a_nm[n][m] = u_ * OHyperdual::from(b_nm(n, m)) * a_nm[n - 1][m]
462 - OHyperdual::from(c_nm(n, m)) * a_nm[n - 2][m];
463 }
464 }
465
466 let mut r_m = [OHyperdual::<f64, U7>::from(0.0); 4];
467 let mut i_m = [OHyperdual::<f64, U7>::from(0.0); 4];
468 r_m[0] = OHyperdual::from(1.0);
469 i_m[0] = OHyperdual::from(0.0);
470 for m in 1..=3 {
471 r_m[m] = s_ * r_m[m - 1] - t_ * i_m[m - 1];
472 i_m[m] = s_ * i_m[m - 1] + t_ * r_m[m - 1];
473 }
474
475 let real_eq_radius_km = self
476 .frame
477 .mean_equatorial_radius_km()
478 .context(AstroPhysicsSnafu)
479 .context(DynamicsAstroSnafu)?;
480
481 let real_mu_km3_s2 = self
482 .frame
483 .mu_km3_s2()
484 .context(AstroPhysicsSnafu)
485 .context(DynamicsAstroSnafu)?;
486
487 let eq_radius = OHyperdual::<f64, U7>::from(real_eq_radius_km);
488 let rho = eq_radius / r_;
489 let mut rho_np1 = OHyperdual::<f64, U7>::from(real_mu_km3_s2) / r_ * rho;
490
491 let mut a0 = OHyperdual::<f64, U7>::from(0.0);
492 let mut a1 = OHyperdual::<f64, U7>::from(0.0);
493 let mut a2 = OHyperdual::<f64, U7>::from(0.0);
494 let mut a3 = OHyperdual::<f64, U7>::from(0.0);
495
496 let vr01 = |n: usize, m: usize| {
497 let mut val = ((n as f64 - m as f64) * (n as f64 + m as f64 + 1.0)).sqrt();
498 if m == 0 {
499 val /= 2.0f64.sqrt();
500 }
501 val
502 };
503 let vr11 = |n: usize, m: usize| {
504 let mut val = (((2.0 * n as f64 + 1.0)
505 * (n as f64 + m as f64 + 2.0)
506 * (n as f64 + m as f64 + 1.0))
507 / (2.0 * n as f64 + 3.0))
508 .sqrt();
509 if m == 0 {
510 val /= 2.0f64.sqrt();
511 }
512 val
513 };
514 let sqrt2 = OHyperdual::<f64, U7>::from(2.0f64.sqrt());
515
516 for n in 1..=3 {
517 rho_np1 *= rho;
518 if n < 2 {
519 continue;
520 }
521
522 let mut sum0 = OHyperdual::from(0.0);
523 let mut sum1 = OHyperdual::from(0.0);
524 let mut sum2 = OHyperdual::from(0.0);
525 let mut sum3 = OHyperdual::from(0.0);
526
527 for m in 0..=n {
528 let c_val = OHyperdual::from(delta_c[n][m]);
529 let s_val = OHyperdual::from(delta_s[n][m]);
530
531 let d_ = (c_val * r_m[m] + s_val * i_m[m]) * sqrt2;
532 let e_ = if m == 0 {
533 OHyperdual::from(0.0)
534 } else {
535 (c_val * r_m[m - 1] + s_val * i_m[m - 1]) * sqrt2
536 };
537 let f_ = if m == 0 {
538 OHyperdual::from(0.0)
539 } else {
540 (s_val * r_m[m - 1] - c_val * i_m[m - 1]) * sqrt2
541 };
542
543 sum0 += OHyperdual::from(m as f64) * a_nm[n][m] * e_;
544 sum1 += OHyperdual::from(m as f64) * a_nm[n][m] * f_;
545 sum2 += OHyperdual::from(vr01(n, m)) * a_nm[n][m + 1] * d_;
546 sum3 += OHyperdual::from(vr11(n, m)) * a_nm[n + 1][m + 1] * d_;
547 }
548 let rr = rho_np1 / eq_radius;
549 a0 += rr * sum0;
550 a1 += rr * sum1;
551 a2 += rr * sum2;
552 a3 -= rr * sum3;
553 }
554
555 let accel_local = Vector3::new(a0 + a3 * s_, a1 + a3 * t_, a2 + a3 * u_);
556
557 let dcm = almanac
558 .rotate(self.frame, osc.frame, osc.epoch)
559 .context(OrientationSnafu {
560 action: "rotating accel back to integration frame",
561 })
562 .context(DynamicsAlmanacSnafu {
563 action: "rotating accel back to integration frame",
564 })?
565 .rot_mat;
566
567 let dx = dcm
568 * Vector3::new(
569 accel_local[0].real(),
570 accel_local[1].real(),
571 accel_local[2].real(),
572 );
573
574 let mut grad_local = Matrix3::zeros();
575 for i in 0..3 {
576 for j in 1..4 {
577 grad_local[(i, j - 1)] += accel_local[i][j];
578 }
579 }
580 let grad = dcm * grad_local * dcm.transpose();
581 Ok((dx, grad))
582 }
583}
584
585impl fmt::Display for SolidTides {
586 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
587 write!(
588 f,
589 "Solid tides for {} with {} (k2={:.6}, k3={:.6})",
590 self.frame,
591 self.perturbers
592 .iter()
593 .map(|pert| pert.to_string())
594 .collect::<Vec<String>>()
595 .join(", "),
596 self.k2,
597 self.k3
598 )
599 }
600}
601
602impl fmt::Display for TidalPerturber {
603 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
604 write!(
605 f,
606 "TidalPerturber of {} (3rd deg= {})",
607 self.frame, self.compute_degree_3
608 )
609 }
610}
611
612#[cfg(feature = "python")]
613#[cfg_attr(feature = "python", pymethods)]
614impl SolidTides {
615 #[new]
616 #[pyo3(signature=(frame, k2, k3, perturbers, correction=None))]
617 fn py_new(
618 frame: Frame,
619 k2: f64,
620 k3: f64,
621 perturbers: Vec<TidalPerturber>,
622 correction: Option<Aberration>,
623 ) -> Self {
624 Self {
625 frame,
626 k2,
627 k3,
628 perturbers,
629 correction,
630 }
631 }
632
633 #[classmethod]
642 #[pyo3(name = "earth_moon_system", signature=(earth_frame, moon_frame, almanac, correction=None))]
643 fn py_earth_moon_system(
644 _cls: &Bound<'_, PyType>,
645 earth_frame: Frame,
646 moon_frame: Frame,
647 almanac: &Almanac,
648 correction: Option<Aberration>,
649 ) -> Result<Self, DynamicsError> {
650 Self::earth_moon_system(earth_frame, moon_frame, almanac, correction)
651 }
652
653 fn __str__(&self) -> String {
654 format!("{self}")
655 }
656
657 fn __repr__(&self) -> String {
658 format!("{self} @ {self:p}")
659 }
660
661 fn __eq__(&self, other: &Self) -> bool {
662 self == other
663 }
664}
665
666#[cfg(feature = "python")]
667#[cfg_attr(feature = "python", pymethods)]
668impl TidalPerturber {
669 #[new]
670 fn py_new(frame: Frame, compute_degree_3: bool) -> Self {
671 Self {
672 frame,
673 compute_degree_3,
674 }
675 }
676
677 fn __str__(&self) -> String {
678 format!("{self}")
679 }
680
681 fn __repr__(&self) -> String {
682 format!("{self} @ {self:p}")
683 }
684
685 fn __eq__(&self, other: &Self) -> bool {
686 self == other
687 }
688}
689
690#[cfg(test)]
691mod tests {
692 use super::*;
693 use crate::cosmic::Orbit;
694 use anise::constants::frames::{EARTH_J2000, IAU_EARTH_FRAME, IAU_MOON_FRAME};
695 use std::path::PathBuf;
696 use std::str::FromStr;
697
698 #[test]
699 fn test_solid_tides_earth() {
700 let data_folder: PathBuf = [env!("CARGO_MANIFEST_DIR"), "../data/01_planetary"]
701 .iter()
702 .collect();
703 let mut almanac = Almanac::default();
704 almanac = almanac
706 .load(data_folder.join("de440s.bsp").to_str().unwrap())
707 .unwrap();
708 almanac = almanac
709 .load(data_folder.join("pck08.pca").to_str().unwrap())
710 .unwrap();
711
712 let epoch = Epoch::from_str("2024-01-01T12:00:00 UTC").unwrap();
713 let sc_orbit = Orbit::cartesian(7000.0, 0.0, 0.0, 0.0, 7.5, 0.0, epoch, EARTH_J2000);
714
715 let tides =
716 SolidTides::earth_moon_system(IAU_EARTH_FRAME, IAU_MOON_FRAME, &almanac.clone(), None)
717 .expect("could not init solid tides");
718 let acc = tides.eom(&sc_orbit, &almanac).unwrap();
719
720 println!("Solid tides acceleration: {:?}", acc);
721 assert!(acc.norm() > 0.0);
723 assert!(acc.norm() < 1e-6);
724
725 let (acc_grad, grad) = tides.gradient(&sc_orbit, &almanac).unwrap();
726 assert!((acc - acc_grad).norm() < 1e-12);
727 assert!(grad.norm() > 0.0);
728 }
729}