use crate::mathtypes::Vector3;
use serde::{Deserialize, Serialize};
#[derive(Debug, Clone, Copy, PartialEq, Default, Serialize, Deserialize)]
pub struct EcomParams {
pub d0: f64,
pub y0: f64,
pub b0: f64,
pub dc: f64,
pub ds: f64,
pub yc: f64,
pub ys: f64,
pub bc: f64,
pub bs: f64,
pub d2c: f64,
pub d2s: f64,
pub d4c: f64,
pub d4s: f64,
pub sun_relative: bool,
}
impl EcomParams {
pub const fn reduced(d0: f64, y0: f64, b0: f64, bc: f64, bs: f64) -> Self {
Self {
d0,
y0,
b0,
bc,
bs,
dc: 0.0,
ds: 0.0,
yc: 0.0,
ys: 0.0,
d2c: 0.0,
d2s: 0.0,
d4c: 0.0,
d4s: 0.0,
sun_relative: false,
}
}
#[allow(clippy::too_many_arguments)]
pub const fn ecom1(
d0: f64,
y0: f64,
b0: f64,
dc: f64,
ds: f64,
yc: f64,
ys: f64,
bc: f64,
bs: f64,
) -> Self {
Self {
d0,
y0,
b0,
dc,
ds,
yc,
ys,
bc,
bs,
d2c: 0.0,
d2s: 0.0,
d4c: 0.0,
d4s: 0.0,
sun_relative: false,
}
}
#[allow(clippy::too_many_arguments)]
pub const fn ecom2(
d0: f64,
y0: f64,
b0: f64,
b1c: f64,
b1s: f64,
d2c: f64,
d2s: f64,
d4c: f64,
d4s: f64,
) -> Self {
Self {
d0,
y0,
b0,
bc: b1c,
bs: b1s,
dc: 0.0,
ds: 0.0,
yc: 0.0,
ys: 0.0,
d2c,
d2s,
d4c,
d4s,
sun_relative: true,
}
}
pub fn is_zero(&self) -> bool {
self.d0 == 0.0
&& self.y0 == 0.0
&& self.b0 == 0.0
&& self.dc == 0.0
&& self.ds == 0.0
&& self.yc == 0.0
&& self.ys == 0.0
&& self.bc == 0.0
&& self.bs == 0.0
&& self.d2c == 0.0
&& self.d2s == 0.0
&& self.d4c == 0.0
&& self.d4s == 0.0
}
fn has_harmonics(&self) -> bool {
self.dc != 0.0
|| self.ds != 0.0
|| self.yc != 0.0
|| self.ys != 0.0
|| self.bc != 0.0
|| self.bs != 0.0
|| self.d2c != 0.0
|| self.d2s != 0.0
|| self.d4c != 0.0
|| self.d4s != 0.0
}
}
pub fn dyb_basis(
pos_gcrf: &Vector3,
vel_gcrf: &Vector3,
sun_gcrf: &Vector3,
) -> (Vector3, Vector3, Vector3) {
let e_d = (*sun_gcrf - *pos_gcrf).normalize();
let r_hat = pos_gcrf.normalize();
let c = e_d.cross(&r_hat);
let e_y = if c.norm_squared() < 1e-12 {
let h_hat = pos_gcrf.cross(vel_gcrf).normalize();
h_hat.cross(&r_hat).normalize()
} else {
c.normalize()
};
let e_b = e_d.cross(&e_y);
(e_d, e_y, e_b)
}
pub fn orbit_angle(
pos_gcrf: &Vector3,
vel_gcrf: &Vector3,
sun_gcrf: &Vector3,
sun_relative: bool,
) -> f64 {
let h_hat = pos_gcrf.cross(vel_gcrf).normalize();
let reference: Vector3 = if sun_relative {
*sun_gcrf
} else {
let z: Vector3 = numeris::vector![0.0, 0.0, 1.0];
let node = z.cross(&h_hat);
if node.norm() < 1e-9 {
numeris::vector![1.0, 0.0, 0.0]
} else {
node
}
};
let in_plane = reference - h_hat * reference.dot(&h_hat);
let r_hat = pos_gcrf.normalize();
h_hat
.dot(&in_plane.cross(&r_hat))
.atan2(in_plane.dot(&r_hat))
}
pub fn ecom_accel(
p: &EcomParams,
pos_gcrf: &Vector3,
vel_gcrf: &Vector3,
sun_gcrf: &Vector3,
shadow: f64,
) -> Vector3 {
let (e_d, e_y, e_b) = dyb_basis(pos_gcrf, vel_gcrf, sun_gcrf);
let (mut d, mut y, mut b) = (p.d0, p.y0, p.b0);
if p.has_harmonics() {
let phi = orbit_angle(pos_gcrf, vel_gcrf, sun_gcrf, p.sun_relative);
let (s1, c1) = phi.sin_cos();
let c2 = c1 * c1 - s1 * s1;
let s2 = 2.0 * s1 * c1;
let c4 = c2 * c2 - s2 * s2;
let s4 = 2.0 * s2 * c2;
d += p.dc * c1 + p.ds * s1 + p.d2c * c2 + p.d2s * s2 + p.d4c * c4 + p.d4s * s4;
y += p.yc * c1 + p.ys * s1;
b += p.bc * c1 + p.bs * s1;
}
(e_d * d + e_y * y + e_b * b) * shadow
}
#[cfg(test)]
mod tests {
use super::*;
use std::f64::consts::PI;
const AU: f64 = 1.495978707e11;
fn sample_geometry() -> (Vector3, Vector3, Vector3) {
let pos: Vector3 = numeris::vector![1.5e7, 2.0e7, 1.0e7];
let vel: Vector3 = numeris::vector![-2.5e3, 1.5e3, 1.0e3];
let sun: Vector3 = numeris::vector![0.6 * AU, 0.7 * AU, 0.3 * AU];
(pos, vel, sun)
}
#[test]
fn dyb_is_orthonormal_and_right_handed() {
let (pos, vel, sun) = sample_geometry();
let (d, y, b) = dyb_basis(&pos, &vel, &sun);
for e in [&d, &y, &b] {
assert!((e.norm() - 1.0).abs() < 1e-14);
}
assert!(d.dot(&y).abs() < 1e-14);
assert!(d.dot(&b).abs() < 1e-14);
assert!(y.dot(&b).abs() < 1e-14);
assert!((d.cross(&y) - b).norm() < 1e-14);
assert!(d.dot(&(sun - pos)) > 0.0);
assert!(y.dot(&pos).abs() / pos.norm() < 1e-14);
}
#[test]
fn d0_only_is_constant_sun_direction() {
let (pos, vel, sun) = sample_geometry();
let p = EcomParams {
d0: -1e-7,
..Default::default()
};
let a = ecom_accel(&p, &pos, &vel, &sun, 1.0);
let (e_d, _, _) = dyb_basis(&pos, &vel, &sun);
assert!((a - e_d * -1e-7).norm() < 1e-22);
assert!(a.dot(&(sun - pos)) < 0.0);
}
#[test]
fn all_axes_scaled_by_shadow() {
let (pos, vel, sun) = sample_geometry();
let p = EcomParams::reduced(-1e-7, 2e-9, 3e-9, 0.0, 0.0);
let lit = ecom_accel(&p, &pos, &vel, &sun, 1.0);
let half = ecom_accel(&p, &pos, &vel, &sun, 0.5);
let dark = ecom_accel(&p, &pos, &vel, &sun, 0.0);
assert!(dark.norm() < 1e-30);
assert!((half - lit * 0.5).norm() < 1e-22);
}
#[test]
fn dyb_basis_sun_exactly_radial() {
let sun: Vector3 = numeris::vector![AU, 0.0, 0.0];
let pos: Vector3 = numeris::vector![2.66e7, 0.0, 0.0];
let vel: Vector3 = numeris::vector![0.0, 3.9e3, 0.0];
let (e_d, e_y, e_b) = dyb_basis(&pos, &vel, &sun);
assert!(e_y.as_slice().iter().all(|v| v.is_finite()));
assert!((e_y - numeris::vector![0.0, 1.0, 0.0]).norm() < 1e-12);
assert!((e_d.dot(&e_y)).abs() < 1e-12 && (e_b.dot(&e_y)).abs() < 1e-12);
}
#[test]
fn delta_u_noon_and_midnight() {
let sun: Vector3 = numeris::vector![AU, 0.0, 0.0];
let noon: Vector3 = numeris::vector![2.66e7, 0.0, 0.0];
let vel_noon: Vector3 = numeris::vector![0.0, 0.0, 3.9e3];
assert!(orbit_angle(&noon, &vel_noon, &sun, true).abs() < 1e-12);
let midnight: Vector3 = numeris::vector![-2.66e7, 0.0, 0.0];
let vel_mid: Vector3 = numeris::vector![0.0, 0.0, -3.9e3];
assert!((orbit_angle(&midnight, &vel_mid, &sun, true).abs() - PI).abs() < 1e-12);
let q: Vector3 = numeris::vector![0.0, 0.0, 2.66e7];
let vel_q: Vector3 = numeris::vector![-3.9e3, 0.0, 0.0];
assert!((orbit_angle(&q, &vel_q, &sun, true) - PI / 2.0).abs() < 1e-12);
}
#[test]
fn argument_of_latitude_matches_kepler() {
use crate::kepler::{Anomaly, Kepler};
let inc = 55.0_f64.to_radians();
let raan = 200.0_f64.to_radians();
for (argp_deg, ta_deg) in [
(30.0_f64, 100.0_f64),
(250.0, 0.0),
(0.0, 359.0),
(170.0, 200.0),
] {
let argp = argp_deg.to_radians();
let ta = ta_deg.to_radians();
let k = Kepler::new(2.656e7, 0.01, inc, raan, argp, Anomaly::True(ta));
let (r, v) = k.to_pv();
let sun: Vector3 = numeris::vector![AU, 0.0, 0.0];
let u = orbit_angle(&r, &v, &sun, false);
let expected = (argp + ta).rem_euclid(2.0 * PI);
let got = u.rem_euclid(2.0 * PI);
let diff = (got - expected + PI).rem_euclid(2.0 * PI) - PI;
assert!(
diff.abs() < 1e-9,
"argp {argp_deg} ta {ta_deg}: got {got} expected {expected}"
);
}
}
#[test]
fn harmonics_are_periodic() {
let (pos, vel, sun) = sample_geometry();
let h = pos.cross(&vel).normalize();
let pos2 = -pos + h * (2.0 * pos.dot(&h));
let vel2 = -vel + h * (2.0 * vel.dot(&h));
let phi1 = orbit_angle(&pos, &vel, &sun, true);
let phi2 = orbit_angle(&pos2, &vel2, &sun, true);
let d = (phi2 - phi1 - PI + PI).rem_euclid(2.0 * PI) - PI;
assert!(d.abs() < 1e-9, "phi1 {phi1} phi2 {phi2}");
}
#[test]
fn d0_only_reproduces_cannonball() {
use crate::orbitprop::{propagate, PropSettings, SatPropertiesSimple};
use crate::{Duration, Instant};
let cr_a_over_m = 0.02;
let t0 = Instant::from_datetime(2024, 1, 15, 0, 0, 0.0).unwrap();
let t1 = t0 + Duration::from_days(1.0);
let settings = PropSettings {
abs_error: 1e-12,
rel_error: 1e-12,
use_spaceweather: false,
..PropSettings::default()
};
let state: crate::orbitprop::SimpleState =
numeris::vector![1.5e7, 2.0e7, 1.0e7, -2.5e3, 1.5e3, 1.0e3];
let cannon = SatPropertiesSimple::new(0.0, cr_a_over_m);
let ecom = SatPropertiesSimple::new(0.0, 0.0).with_ecom(EcomParams {
d0: -4.56e-6 * cr_a_over_m,
..Default::default()
});
let a = propagate(&state, &t0, &t1, &settings, Some(&cannon)).unwrap();
let b = propagate(&state, &t0, &t1, &settings, Some(&ecom)).unwrap();
let dr = (a.state_end.block::<3, 1>(0, 0) - b.state_end.block::<3, 1>(0, 0)).norm();
assert!(dr < 1e-3, "cannonball vs D0-only ECOM differ by {dr} m");
let none = SatPropertiesSimple::new(0.0, 0.0);
let c = propagate(&state, &t0, &t1, &settings, Some(&none)).unwrap();
let dr0 = (a.state_end.block::<3, 1>(0, 0) - c.state_end.block::<3, 1>(0, 0)).norm();
assert!(dr0 > 10.0, "SRP had no effect: {dr0} m");
}
#[test]
fn is_zero_and_constructors() {
assert!(EcomParams::default().is_zero());
let r = EcomParams::reduced(-1e-7, 1e-9, 2e-9, 3e-9, 4e-9);
assert!(!r.is_zero());
assert!(!r.sun_relative);
assert_eq!(r.bc, 3e-9);
let e2 = EcomParams::ecom2(-1e-7, 0.0, 0.0, 1e-9, 2e-9, 3e-9, 4e-9, 5e-9, 6e-9);
assert!(e2.sun_relative);
assert_eq!((e2.bc, e2.bs, e2.d2c, e2.d4s), (1e-9, 2e-9, 3e-9, 6e-9));
let e1 = EcomParams::ecom1(-1e-7, 0.0, 0.0, 1e-9, 2e-9, 3e-9, 4e-9, 5e-9, 6e-9);
assert!(!e1.sun_relative);
assert_eq!((e1.dc, e1.ys, e1.bs), (1e-9, 4e-9, 6e-9));
}
}