use std::f64::consts::PI;
use std::fs::File;
use std::io::{BufRead, BufReader};
use std::path::Path;
pub type Mat3 = [[f64; 3]; 3];
type Iar80 = Vec<[i32; 5]>;
type Rar80 = Vec<[f64; 4]>;
type Trip = (f64, f64, f64);
type TripOpt = Option<(Trip, Trip, Trip)>;
type EopReturn = Option<(f64, f64, f64, f64, f64, f64, f64, f64, f64)>;
fn mat_identity() -> Mat3 {
[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
}
fn mat_transpose(a: &Mat3) -> Mat3 {
let mut r = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
r[i][j] = a[j][i];
}
}
r
}
fn mat_mul(a: &Mat3, b: &Mat3) -> Mat3 {
let mut r = [[0.0; 3]; 3];
for i in 0..3 {
for j in 0..3 {
let mut s = 0.0;
for k in 0..3 {
s += a[i][k] * b[k][j];
}
r[i][j] = s;
}
}
r
}
fn mat_vec_mul(a: &Mat3, v: (f64, f64, f64)) -> (f64, f64, f64) {
(
a[0][0] * v.0 + a[0][1] * v.1 + a[0][2] * v.2,
a[1][0] * v.0 + a[1][1] * v.1 + a[1][2] * v.2,
a[2][0] * v.0 + a[2][1] * v.1 + a[2][2] * v.2,
)
}
fn cross(a: (f64, f64, f64), b: (f64, f64, f64)) -> (f64, f64, f64) {
(
a.1 * b.2 - a.2 * b.1,
a.2 * b.0 - a.0 * b.2,
a.0 * b.1 - a.1 * b.0,
)
}
const EARTH_ROT: f64 = 7.292115e-5;
pub fn gstime(jdut1: f64) -> f64 {
let twopi = 2.0 * PI;
let deg2rad = PI / 180.0;
let tut1 = (jdut1 - 2451545.0) / 36525.0;
let mut temp = -6.2e-6 * tut1 * tut1 * tut1
+ 0.093104 * tut1 * tut1
+ (876600.0 * 3600.0 + 8640184.812866) * tut1
+ 67310.54841;
temp = (temp * deg2rad / 240.0) % twopi;
if temp < 0.0 {
temp += twopi;
}
temp
}
fn fundarg(ttt: f64) -> (f64, f64, f64, f64, f64) {
let deg2rad = PI / 180.0;
let ttt2 = ttt * ttt;
let _ttt3 = ttt2 * ttt;
let l = ((((0.064) * ttt + 31.310) * ttt + 1717915922.6330) * ttt) / 3600.0 + 134.96298139;
let l1 = ((((-0.012) * ttt - 0.577) * ttt + 129596581.2240) * ttt) / 3600.0 + 357.52772333;
let f = ((((0.011) * ttt - 13.257) * ttt + 1739527263.1370) * ttt) / 3600.0 + 93.27191028;
let d = ((((0.019) * ttt - 6.891) * ttt + 1602961601.3280) * ttt) / 3600.0 + 297.85036306;
let omega = ((((0.008) * ttt + 7.455) * ttt - 6962890.5390) * ttt) / 3600.0 + 125.04452222;
(
(l % 360.0) * deg2rad,
(l1 % 360.0) * deg2rad,
(f % 360.0) * deg2rad,
(d % 360.0) * deg2rad,
(omega % 360.0) * deg2rad,
)
}
fn iau80in() -> Option<(Iar80, Rar80)> {
let path = Path::new("refs/SGP8/nut80.dat");
let file = File::open(path).ok()?;
let reader = BufReader::new(file);
let mut iar80 = Vec::new();
let mut rar80 = Vec::new();
for l in reader.lines().map_while(Result::ok) {
let s = l.trim();
if s.is_empty() {
continue;
}
let parts: Vec<&str> = s.split_whitespace().collect();
if parts.len() < 9 {
continue;
}
let a = [
parts[0].parse::<i32>().unwrap_or(0),
parts[1].parse::<i32>().unwrap_or(0),
parts[2].parse::<i32>().unwrap_or(0),
parts[3].parse::<i32>().unwrap_or(0),
parts[4].parse::<i32>().unwrap_or(0),
];
let b = [
parts[5].parse::<f64>().unwrap_or(0.0),
parts[6].parse::<f64>().unwrap_or(0.0),
parts[7].parse::<f64>().unwrap_or(0.0),
parts[8].parse::<f64>().unwrap_or(0.0),
];
iar80.push(a);
rar80.push(b);
}
let convrt = 0.0001 * PI / (180.0 * 3600.0);
let mut rar80_conv = Vec::new();
for r in rar80 {
rar80_conv.push([r[0] * convrt, r[1] * convrt, r[2] * convrt, r[3] * convrt]);
}
Some((iar80, rar80_conv))
}
pub fn nutation(ttt: f64, ddpsi: f64, ddeps: f64) -> Option<(f64, f64, f64, f64, Mat3)> {
let deg2rad = PI / 180.0;
let (iar80, rar80) = iau80in()?;
let ttt2 = ttt * ttt;
let ttt3 = ttt2 * ttt;
let mut meaneps = -46.8150 * ttt - 0.00059 * ttt2 + 0.001813 * ttt3 + 84381.448;
meaneps = (meaneps / 3600.0) % 360.0;
if meaneps < 0.0 {
meaneps += 360.0;
}
meaneps *= deg2rad;
let (l, l1, f, d, omega) = fundarg(ttt);
let mut deltapsi = 0.0;
let mut deltaeps = 0.0;
let n = iar80.len().min(rar80.len());
for i in 0..n {
let tempval = (iar80[i][0] as f64) * l
+ (iar80[i][1] as f64) * l1
+ (iar80[i][2] as f64) * f
+ (iar80[i][3] as f64) * d
+ (iar80[i][4] as f64) * omega;
deltapsi += (rar80[i][0] + rar80[i][1] * ttt) * tempval.sin();
deltaeps += (rar80[i][2] + rar80[i][3] * ttt) * tempval.cos();
}
let mut deltapsi = (deltapsi + ddpsi) % (2.0 * PI);
if deltapsi < -PI {
deltapsi += 2.0 * PI;
}
let mut deltaeps = (deltaeps + ddeps) % (2.0 * PI);
if deltaeps < -PI {
deltaeps += 2.0 * PI;
}
let trueeps = meaneps + deltaeps;
let cospsi = deltapsi.cos();
let sinpsi = deltapsi.sin();
let coseps = meaneps.cos();
let sineps = meaneps.sin();
let costrueeps = trueeps.cos();
let sintrueeps = trueeps.sin();
let mut nut = mat_identity();
nut[0][0] = cospsi;
nut[0][1] = costrueeps * sinpsi;
nut[0][2] = sintrueeps * sinpsi;
nut[1][0] = -coseps * sinpsi;
nut[1][1] = costrueeps * coseps * cospsi + sintrueeps * sineps;
nut[1][2] = sintrueeps * coseps * cospsi - sineps * costrueeps;
nut[2][0] = -sineps * sinpsi;
nut[2][1] = costrueeps * sineps * cospsi - sintrueeps * coseps;
nut[2][2] = sintrueeps * sineps * cospsi + costrueeps * coseps;
Some((deltapsi, trueeps, meaneps, omega, nut))
}
pub fn precess(ttt: f64) -> (Mat3, f64, f64, f64, f64) {
let convrt = PI / (180.0 * 3600.0);
let ttt2 = ttt * ttt;
let ttt3 = ttt2 * ttt;
let mut prec = mat_identity();
let psia = 5038.7784 * ttt - 1.07259 * ttt2 - 0.001147 * ttt3;
let wa = 84381.448 + 0.05127 * ttt2 - 0.007726 * ttt3;
let ea = 84381.448 - 46.8150 * ttt - 0.00059 * ttt2 + 0.001813 * ttt3;
let xa = 10.5526 * ttt - 2.38064 * ttt2 - 0.001125 * ttt3;
let zeta = 2306.2181 * ttt + 0.30188 * ttt2 + 0.017998 * ttt3;
let theta = 2004.3109 * ttt - 0.42665 * ttt2 - 0.041833 * ttt3;
let z = 2306.2181 * ttt + 1.09468 * ttt2 + 0.018203 * ttt3;
let psia = psia * convrt;
let wa = wa * convrt;
let ea = ea * convrt;
let xa = xa * convrt;
let zeta = zeta * convrt;
let theta = theta * convrt;
let z = z * convrt;
let coszeta = zeta.cos();
let sinzeta = zeta.sin();
let costheta = theta.cos();
let sintheta = theta.sin();
let cosz = z.cos();
let sinz = z.sin();
prec[0][0] = coszeta * costheta * cosz - sinzeta * sinz;
prec[0][1] = coszeta * costheta * sinz + sinzeta * cosz;
prec[0][2] = coszeta * sintheta;
prec[1][0] = -sinzeta * costheta * cosz - coszeta * sinz;
prec[1][1] = -sinzeta * costheta * sinz + coszeta * cosz;
prec[1][2] = -sinzeta * sintheta;
prec[2][0] = -sintheta * cosz;
prec[2][1] = -sintheta * sinz;
prec[2][2] = costheta;
(prec, psia, wa, ea, xa)
}
pub fn polarm(xp: f64, yp: f64) -> Mat3 {
let mut pm = mat_identity();
let cosxp = xp.cos();
let sinxp = xp.sin();
let cosyp = yp.cos();
let sinyp = yp.sin();
pm[0][0] = cosxp;
pm[0][1] = 0.0;
pm[0][2] = -sinxp;
pm[1][0] = sinxp * sinyp;
pm[1][1] = cosyp;
pm[1][2] = cosxp * sinyp;
pm[2][0] = sinxp * cosyp;
pm[2][1] = -sinyp;
pm[2][2] = cosxp * cosyp;
pm
}
pub fn sidereal(
jdut1: f64,
deltapsi: f64,
meaneps: f64,
omega: f64,
lod: f64,
eqeterms: i32,
) -> (Mat3, Mat3) {
let mut st = mat_identity();
let mut stdot = [[0.0; 3]; 3];
let gmst = gstime(jdut1);
let ast = if jdut1 > 2450449.5 && eqeterms > 0 {
gmst + deltapsi * meaneps.cos()
+ 0.00264 * PI / (3600.0 * 180.0) * omega.sin()
+ 0.000063 * PI / (3600.0 * 180.0) * (2.0 * omega).sin()
} else {
gmst + deltapsi * meaneps.cos()
};
let mut ast = ast % (2.0 * PI);
if ast < 0.0 {
ast += 2.0 * PI;
}
let thetasa = EARTH_ROT * (1.0 - lod / 86400.0);
let omegaearth = thetasa;
st[0][0] = ast.cos();
st[0][1] = -ast.sin();
st[0][2] = 0.0;
st[1][0] = ast.sin();
st[1][1] = ast.cos();
st[1][2] = 0.0;
st[2][0] = 0.0;
st[2][1] = 0.0;
st[2][2] = 1.0;
stdot[0][0] = -omegaearth * ast.sin();
stdot[0][1] = -omegaearth * ast.cos();
stdot[0][2] = 0.0;
stdot[1][0] = omegaearth * ast.cos();
stdot[1][1] = -omegaearth * ast.sin();
stdot[1][2] = 0.0;
stdot[2][0] = 0.0;
stdot[2][1] = 0.0;
stdot[2][2] = 0.0;
(st, stdot)
}
pub fn teme2eci(
rteme: Trip,
vteme: Trip,
ateme: Trip,
ttt: f64,
ddpsi: f64,
ddeps: f64,
) -> TripOpt {
let (prec, _psia, _wa, _ea, _xa) = precess(ttt);
let (deltapsi, trueeps, _meaneps, _omega, nut) = nutation(ttt, ddpsi, ddeps)?;
let eqeg = deltapsi * trueeps.cos();
let mut eqe = mat_identity();
let eqeg = eqeg % (2.0 * PI);
eqe[0][0] = eqeg.cos();
eqe[0][1] = eqeg.sin();
eqe[0][2] = 0.0;
eqe[1][0] = -eqeg.sin();
eqe[1][1] = eqeg.cos();
eqe[1][2] = 0.0;
eqe[2][0] = 0.0;
eqe[2][1] = 0.0;
eqe[2][2] = 1.0;
let tmp = mat_mul(&prec, &nut);
let tm = mat_mul(&tmp, &mat_transpose(&eqe));
let reci = mat_vec_mul(&tm, rteme);
let veci = mat_vec_mul(&tm, vteme);
let aeci = mat_vec_mul(&tm, ateme);
Some((reci, veci, aeci))
}
#[allow(clippy::too_many_arguments)]
pub fn teme2ecef(
rteme: Trip,
vteme: Trip,
ateme: Trip,
ttt: f64,
jdut1: f64,
lod: f64,
xp: f64,
yp: f64,
eqeterms: i32,
) -> TripOpt {
let gmst = gstime(jdut1);
let mut omega =
125.04452222 + (-6962890.5390 * ttt + 7.455 * ttt * ttt + 0.008 * ttt * ttt * ttt) / 3600.0;
omega = (omega % 360.0) * PI / 180.0;
let mut gmstg = if jdut1 > 2450449.5 && eqeterms > 0 {
gmst + 0.00264 * PI / (3600.0 * 180.0) * omega.sin()
+ 0.000063 * PI / (3600.0 * 180.0) * (2.0 * omega).sin()
} else {
gmst
};
gmstg %= 2.0 * PI;
if gmstg < 0.0 {
gmstg += 2.0 * PI;
}
let mut st = mat_identity();
st[0][0] = gmstg.cos();
st[0][1] = -gmstg.sin();
st[0][2] = 0.0;
st[1][0] = gmstg.sin();
st[1][1] = gmstg.cos();
st[1][2] = 0.0;
st[2][0] = 0.0;
st[2][1] = 0.0;
st[2][2] = 1.0;
let pm = polarm(xp, yp);
let rpef = mat_vec_mul(&mat_transpose(&st), rteme);
let recef = mat_vec_mul(&mat_transpose(&pm), rpef);
let thetasa = EARTH_ROT * (1.0 - lod / 86400.0);
let omegaearth = (0.0, 0.0, thetasa);
let vpef = {
let st_t = mat_transpose(&st);
let vtemp = mat_vec_mul(&st_t, vteme);
let cross_term = cross(omegaearth, rpef);
(
vtemp.0 - cross_term.0,
vtemp.1 - cross_term.1,
vtemp.2 - cross_term.2,
)
};
let vecef = mat_vec_mul(&mat_transpose(&pm), vpef);
let temp = cross(omegaearth, rpef);
let aecef = {
let st_t = mat_transpose(&st);
let aterm = mat_vec_mul(&st_t, ateme);
let cross2 = cross(omegaearth, temp);
let cross3 = cross(omegaearth, vpef);
(
aterm.0 - cross2.0 - 2.0 * cross3.0,
aterm.1 - cross2.1 - 2.0 * cross3.1,
aterm.2 - cross2.2 - 2.0 * cross3.2,
)
};
Some((recef, vecef, aecef))
}
#[allow(clippy::too_many_arguments)]
pub fn ecef2tod(
recef: Trip,
vecef: Trip,
aecef: Trip,
ttt: f64,
jdut1: f64,
lod: f64,
xp: f64,
yp: f64,
eqeterms: i32,
ddpsi: f64,
ddeps: f64,
) -> TripOpt {
let (deltapsi, _trueeps, meaneps, omega, _nut) = nutation(ttt, ddpsi, ddeps)?;
let (st, _stdot) = sidereal(jdut1, deltapsi, meaneps, omega, lod, eqeterms);
let pm = polarm(xp, yp);
let thetasa = EARTH_ROT * (1.0 - lod / 86400.0);
let omegaearth = (0.0, 0.0, thetasa);
let rpef = mat_vec_mul(&pm, recef);
let rtod = mat_vec_mul(&st, rpef);
let vpef = mat_vec_mul(&pm, vecef);
let vtod = mat_vec_mul(
&st,
(
vpef.0 + cross(omegaearth, rpef).0,
vpef.1 + cross(omegaearth, rpef).1,
vpef.2 + cross(omegaearth, rpef).2,
),
);
let temp = cross(omegaearth, rpef);
let atod = {
let ae_term = mat_vec_mul(&pm, aecef);
let cross_term = cross(omegaearth, temp);
let cross2 = cross(omegaearth, vpef);
mat_vec_mul(
&st,
(
ae_term.0 + cross_term.0 + 2.0 * cross2.0,
ae_term.1 + cross_term.1 + 2.0 * cross2.1,
ae_term.2 + cross_term.2 + 2.0 * cross2.2,
),
)
};
Some((rtod, vtod, atod))
}
pub fn read_eop_for_mjd(mjd_utc: f64) -> EopReturn {
let path = Path::new("refs/SGP8/EOP-All.txt");
let file = File::open(path).ok()?;
let reader = BufReader::new(file);
let mut in_observed = false;
let mjd_search = mjd_utc.floor() as i32;
for l in reader.lines().map_while(Result::ok) {
let s = l.trim();
if s.starts_with("BEGIN OBSERVED") {
in_observed = true;
continue;
}
if !in_observed {
continue;
}
if s.is_empty() {
continue;
}
let parts: Vec<&str> = s.split_whitespace().collect();
if parts.len() < 13 {
continue;
}
let mjd = parts[3].parse::<i32>().ok()?;
if mjd == mjd_search {
let x = parts[4].parse::<f64>().ok()? / 3600.0 / 180.0 * PI; let y = parts[5].parse::<f64>().ok()? / 3600.0 / 180.0 * PI;
let ut1_utc = parts[6].parse::<f64>().ok()?;
let lod = parts[7].parse::<f64>().ok()?;
let dpsi = parts[8].parse::<f64>().ok()? / 3600.0 / 180.0 * PI;
let deps = parts[9].parse::<f64>().ok()? / 3600.0 / 180.0 * PI;
let dx = parts[10].parse::<f64>().ok()? / 3600.0 / 180.0 * PI;
let dy = parts[11].parse::<f64>().ok()? / 3600.0 / 180.0 * PI;
let tai_utc = parts[12].parse::<f64>().ok()?;
return Some((x, y, ut1_utc, lod, dpsi, deps, dx, dy, tai_utc));
}
}
None
}