use crate::rinex::EpochUtc;
use serde::Serialize;
pub const BAD_CLOCK_US: f64 = 999_999.999_999;
#[derive(Clone, Debug, PartialEq, Serialize)]
pub struct Sp3SatState {
pub sat: String,
pub pos_m: [f64; 3],
pub clock_us: f64,
#[serde(skip_serializing_if = "Option::is_none")]
pub vel_m_s: Option<[f64; 3]>,
}
impl Sp3SatState {
pub fn clock_is_bad(&self) -> bool {
(self.clock_us - BAD_CLOCK_US).abs() < 1e-6
}
}
#[derive(Clone, Debug, Serialize)]
pub struct Sp3Epoch {
pub time: EpochUtc,
pub sats: Vec<Sp3SatState>,
}
#[derive(Clone, Debug, Serialize)]
pub struct Sp3Header {
pub version: char,
pub has_velocity: bool,
pub start: EpochUtc,
pub num_epochs: usize,
pub sat_ids: Vec<String>,
}
#[derive(Clone, Debug, Serialize)]
pub struct Sp3File {
pub header: Sp3Header,
pub epochs: Vec<Sp3Epoch>,
}
impl Sp3File {
pub fn observed_satellites(&self) -> Vec<String> {
let mut seen = Vec::new();
for epoch in &self.epochs {
for s in &epoch.sats {
if !seen.contains(&s.sat) {
seen.push(s.sat.clone());
}
}
}
seen
}
pub fn position_of(&self, sat: &str, idx: usize) -> Option<[f64; 3]> {
self.epochs
.get(idx)?
.sats
.iter()
.find(|s| s.sat == sat)
.map(|s| s.pos_m)
}
pub fn from_propagators(
ids: &[String],
sats: &[crate::orbit::Propagator],
start: EpochUtc,
start_jd_ut1: f64,
step_s: f64,
num_epochs: usize,
) -> Self {
let start_jd_cal = crate::timescales::julian_date(
start.year,
start.month,
start.day,
start.hour,
start.minute,
start.second,
);
let mut epochs = Vec::with_capacity(num_epochs);
for i in 0..num_epochs {
let t = i as f64 * step_s;
let jd_ut1 = start_jd_ut1 + t / 86_400.0;
let civil = crate::timescales::civil_from_jd(start_jd_cal + t / 86_400.0);
let mut states = Vec::with_capacity(sats.len());
for (id, sat) in ids.iter().zip(sats.iter()) {
let ecef = crate::frames::teme_to_ecef(sat.position_eci(t), jd_ut1);
states.push(Sp3SatState {
sat: id.clone(),
pos_m: ecef,
clock_us: BAD_CLOCK_US,
vel_m_s: None,
});
}
epochs.push(Sp3Epoch {
time: EpochUtc {
year: civil.year,
month: civil.month,
day: civil.day,
hour: civil.hour,
minute: civil.minute,
second: civil.second,
},
sats: states,
});
}
Sp3File {
header: Sp3Header {
version: 'c',
has_velocity: false,
start,
num_epochs,
sat_ids: ids.to_vec(),
},
epochs,
}
}
pub fn to_sp3_string(&self) -> String {
let mut out = String::new();
let s = &self.header.start;
out.push_str(&format!(
"#cP{:4} {:2} {:2} {:2} {:2} {:11.8} {:6} ORBIT IGS14 HLM KSHANA\n",
s.year, s.month, s.day, s.hour, s.minute, s.second, self.header.num_epochs,
));
let mut plus = format!("+ {:2} ", self.header.sat_ids.len());
for id in &self.header.sat_ids {
plus.push_str(id);
}
out.push_str(&plus);
out.push('\n');
for epoch in &self.epochs {
let t = &epoch.time;
out.push_str(&format!(
"* {:4} {:2} {:2} {:2} {:2} {:11.8}\n",
t.year, t.month, t.day, t.hour, t.minute, t.second,
));
for st in &epoch.sats {
out.push_str(&format!(
"P{}{:14.6}{:14.6}{:14.6}{:14.6}\n",
st.sat,
st.pos_m[0] / 1000.0,
st.pos_m[1] / 1000.0,
st.pos_m[2] / 1000.0,
st.clock_us,
));
}
}
out.push_str("EOF\n");
out
}
pub fn interpolator(&self, sat: &str) -> Option<Sp3Interpolator> {
let s0 = self.header.start;
let jd0 = crate::timescales::julian_date(
s0.year, s0.month, s0.day, s0.hour, s0.minute, s0.second,
);
let (mut t_s, mut x, mut y, mut z) = (Vec::new(), Vec::new(), Vec::new(), Vec::new());
for epoch in &self.epochs {
if let Some(st) = epoch.sats.iter().find(|s| s.sat == sat) {
let e = epoch.time;
let jd = crate::timescales::julian_date(
e.year, e.month, e.day, e.hour, e.minute, e.second,
);
t_s.push((jd - jd0) * 86_400.0);
x.push(st.pos_m[0]);
y.push(st.pos_m[1]);
z.push(st.pos_m[2]);
}
}
if t_s.len() < 2 {
return None;
}
Some(Sp3Interpolator {
sat: sat.to_string(),
t_s,
x,
y,
z,
start_jd_ut1: jd0,
})
}
}
const SP3_INTERP_ORDER: usize = 9;
fn lagrange(xs: &[f64], ys: &[f64], x: f64) -> f64 {
let mut sum = 0.0;
for j in 0..xs.len() {
let mut term = ys[j];
for k in 0..xs.len() {
if k != j {
term *= (x - xs[k]) / (xs[j] - xs[k]);
}
}
sum += term;
}
sum
}
#[derive(Clone, Debug)]
pub struct Sp3Interpolator {
pub sat: String,
t_s: Vec<f64>,
x: Vec<f64>,
y: Vec<f64>,
z: Vec<f64>,
start_jd_ut1: f64,
}
impl Sp3Interpolator {
fn window(&self, t: f64) -> (usize, usize) {
let n = self.t_s.len();
let npts = (SP3_INTERP_ORDER + 1).min(n);
let mut i = 0;
while i < n && self.t_s[i] < t {
i += 1;
}
let start = i.saturating_sub(npts / 2).min(n - npts);
(start, start + npts)
}
pub fn position_ecef(&self, t: f64) -> [f64; 3] {
let (a, b) = self.window(t);
let xs = &self.t_s[a..b];
[
lagrange(xs, &self.x[a..b], t),
lagrange(xs, &self.y[a..b], t),
lagrange(xs, &self.z[a..b], t),
]
}
pub fn position_teme(&self, t: f64) -> [f64; 3] {
crate::frames::ecef_to_teme(self.position_ecef(t), self.start_jd_ut1 + t / 86_400.0)
}
pub fn approx_period_s(&self) -> f64 {
let r = (self.x[0].powi(2) + self.y[0].powi(2) + self.z[0].powi(2)).sqrt();
std::f64::consts::TAU * (r * r * r / crate::orbit::MU_EARTH).sqrt()
}
}
fn parse_epoch(tokens: &[&str]) -> Result<EpochUtc, String> {
if tokens.len() < 6 {
return Err(format!("epoch needs 6 time fields, got {}", tokens.len()));
}
let p_i = |s: &str, what: &str| s.parse::<i64>().map_err(|_| format!("bad {what}: {s:?}"));
Ok(EpochUtc {
year: p_i(tokens[0], "year")? as i32,
month: p_i(tokens[1], "month")? as u32,
day: p_i(tokens[2], "day")? as u32,
hour: p_i(tokens[3], "hour")? as u32,
minute: p_i(tokens[4], "minute")? as u32,
second: tokens[5]
.parse::<f64>()
.map_err(|_| format!("bad second: {:?}", tokens[5]))?,
})
}
fn parse_record_values(body: &str) -> Result<[f64; 4], String> {
let nums: Vec<f64> = body
.split_whitespace()
.take(4)
.map(|t| t.parse::<f64>().map_err(|_| format!("bad number: {t:?}")))
.collect::<Result<_, _>>()?;
if nums.len() < 4 {
return Err(format!("record needs 4 values, got {}", nums.len()));
}
Ok([nums[0], nums[1], nums[2], nums[3]])
}
pub fn parse_sp3(text: &str) -> Result<Sp3File, String> {
let mut lines = text.lines();
let first = lines.next().ok_or("empty SP3 input")?;
if !first.starts_with('#') {
return Err(format!("first line is not an SP3 header: {first:?}"));
}
let version = first
.chars()
.nth(1)
.filter(|c| *c == 'c' || *c == 'd')
.ok_or_else(|| format!("unsupported SP3 version in {first:?}"))?;
let has_velocity = first.chars().nth(2) == Some('V');
let rest: Vec<&str> = first.get(3..).unwrap_or("").split_whitespace().collect();
let start = parse_epoch(&rest)?;
let num_epochs: usize = rest
.get(6)
.ok_or("missing epoch count on header line 1")?
.parse()
.map_err(|_| "bad epoch count")?;
let mut sat_ids = Vec::new();
let mut epochs: Vec<Sp3Epoch> = Vec::new();
let mut current: Option<Sp3Epoch> = None;
for line in lines {
if line.starts_with("++") || line.starts_with("%") || line.starts_with("/*") {
continue;
}
if let Some(rest) = line.strip_prefix('+') {
let ids = rest.get(8..).unwrap_or("");
let bytes = ids.as_bytes();
let mut i = 0;
while i + 3 <= bytes.len() {
let chunk = &ids[i..i + 3];
let c0 = chunk.as_bytes()[0];
if c0.is_ascii_uppercase() && chunk[1..].bytes().all(|b| b.is_ascii_digit()) {
sat_ids.push(chunk.to_string());
}
i += 3;
}
continue;
}
if line.starts_with("EOF") {
break;
}
if let Some(rest) = line.strip_prefix('*') {
if let Some(e) = current.take() {
epochs.push(e);
}
let tokens: Vec<&str> = rest.split_whitespace().collect();
current = Some(Sp3Epoch {
time: parse_epoch(&tokens)?,
sats: Vec::new(),
});
continue;
}
let is_pos = line.starts_with('P');
let is_vel = line.starts_with('V');
if is_pos || is_vel {
let sat = line
.get(1..4)
.map(|s| s.trim().to_string())
.filter(|s| !s.is_empty())
.ok_or_else(|| format!("record has no satellite id: {line:?}"))?;
let vals = parse_record_values(line.get(4..).unwrap_or(""))?;
let epoch = current
.as_mut()
.ok_or("position record before any epoch header")?;
if is_pos {
epoch.sats.push(Sp3SatState {
sat,
pos_m: [vals[0] * 1000.0, vals[1] * 1000.0, vals[2] * 1000.0],
clock_us: vals[3],
vel_m_s: None,
});
} else if let Some(state) = epoch.sats.iter_mut().rev().find(|s| s.sat == sat) {
state.vel_m_s = Some([vals[0] * 0.1, vals[1] * 0.1, vals[2] * 0.1]);
}
}
}
if let Some(e) = current.take() {
epochs.push(e);
}
if epochs.is_empty() {
return Err("SP3 file contained no epoch records".into());
}
Ok(Sp3File {
header: Sp3Header {
version,
has_velocity,
start,
num_epochs,
sat_ids,
},
epochs,
})
}
#[cfg(test)]
mod tests {
use super::*;
const SAMPLE: &str = "\
#cP2023 1 1 0 0 0.00000000 2 ORBIT IGS14 HLM IGS
## 2244 172800.00000000 900.00000000 59945 0.0000000000000
+ 2 G01G02 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
++ 2 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
%c G cc GPS ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc
%f 1.2500000 1.025000000 0.00000000000 0.000000000000000
%i 0 0 0 0 0 0 0 0 0
/* SYNTHETIC SP3 FIXTURE FOR TESTING
* 2023 1 1 0 0 0.00000000
PG01 15000.000000 -5000.000000 20000.000000 123.456789
PG02 -10000.000000 18000.000000 -8000.000000 999999.999999
* 2023 1 1 0 15 0.00000000
PG01 15100.000000 -5100.000000 20100.000000 124.000000
PG02 -10100.000000 18100.000000 -8100.000000 -46.000000
EOF";
#[test]
fn parses_header_fields() {
let f = parse_sp3(SAMPLE).expect("parses");
assert_eq!(f.header.version, 'c');
assert!(!f.header.has_velocity);
assert_eq!(f.header.num_epochs, 2);
assert_eq!(f.header.start.year, 2023);
assert_eq!(f.header.start.month, 1);
assert_eq!(f.header.start.day, 1);
assert_eq!(f.header.sat_ids, vec!["G01", "G02"]);
}
#[test]
fn parses_epochs_and_positions_km_to_m() {
let f = parse_sp3(SAMPLE).expect("parses");
assert_eq!(f.epochs.len(), 2);
let p = f.position_of("G01", 0).expect("G01 at epoch 0");
assert_eq!(p, [15_000_000.0, -5_000_000.0, 20_000_000.0]);
let g01 = &f.epochs[0].sats[0];
assert_eq!(g01.sat, "G01");
assert!((g01.clock_us - 123.456789).abs() < 1e-6);
assert_eq!(f.epochs[1].time.minute, 15);
assert_eq!(
f.position_of("G02", 1).unwrap(),
[-10_100_000.0, 18_100_000.0, -8_100_000.0]
);
}
#[test]
fn flags_the_bad_clock_sentinel() {
let f = parse_sp3(SAMPLE).unwrap();
let g02 = &f.epochs[0].sats[1];
assert!(g02.clock_is_bad());
assert!(!f.epochs[0].sats[0].clock_is_bad());
}
#[test]
fn observed_satellites_lists_each_once() {
let f = parse_sp3(SAMPLE).unwrap();
assert_eq!(f.observed_satellites(), vec!["G01", "G02"]);
}
#[test]
fn parses_a_velocity_product() {
let vfile = "\
#dV2023 1 1 0 0 0.00000000 1 ORBIT IGS20 HLM IGS
+ 1 G01 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
* 2023 1 1 0 0 0.00000000
PG01 15000.000000 -5000.000000 20000.000000 123.456789
VG01 -8000.000000 3000.000000 39000.000000 0.000000
EOF";
let f = parse_sp3(vfile).expect("parses V file");
assert_eq!(f.header.version, 'd');
assert!(f.header.has_velocity);
let v = f.epochs[0].sats[0].vel_m_s.expect("velocity present");
assert_eq!(v, [-800.0, 300.0, 3900.0]);
}
#[test]
fn rejects_non_sp3_and_empty_input() {
assert!(parse_sp3("").is_err());
assert!(parse_sp3("not an sp3 file").is_err());
assert!(parse_sp3("#cP2023 1 1 0 0 0.00000000 0 ORBIT").is_err());
}
#[test]
fn write_then_read_round_trips() {
let a = parse_sp3(SAMPLE).unwrap();
let text = a.to_sp3_string();
let b = parse_sp3(&text).expect("written SP3 re-parses");
assert_eq!(a.header.version, b.header.version);
assert_eq!(a.header.num_epochs, b.header.num_epochs);
assert_eq!(a.observed_satellites(), b.observed_satellites());
assert_eq!(a.position_of("G01", 0), b.position_of("G01", 0));
assert_eq!(a.position_of("G02", 1), b.position_of("G02", 1));
assert!(b.epochs[0].sats[1].clock_is_bad()); assert_eq!(b.epochs[1].time.minute, 15);
}
#[test]
fn lagrange_is_exact_for_a_low_degree_polynomial() {
let f = |x: f64| 2.0 * x * x * x - x + 5.0;
let xs = [0.0, 1.0, 2.0, 3.0];
let ys = xs.map(f);
assert!((lagrange(&xs, &ys, 1.5) - f(1.5)).abs() < 1e-9);
}
#[test]
fn interpolator_reproduces_the_nodes_and_a_kepler_orbit() {
use crate::orbit::{Orbit, Propagator};
let a = 26_560_000.0;
let orbit = Orbit::keplerian(a, 0.01, 0.9, 0.3, 0.2, 0.4);
let sats = vec![Propagator::Kepler(orbit)];
let ids = vec!["G01".to_string()];
let start = EpochUtc {
year: 2023,
month: 1,
day: 1,
hour: 0,
minute: 0,
second: 0.0,
};
let jd = crate::timescales::julian_date(2023, 1, 1, 0, 0, 0.0);
let f = Sp3File::from_propagators(&ids, &sats, start, jd, 900.0, 20);
let interp = f.interpolator("G01").expect("interpolator builds");
let t_node = 9.0 * 900.0;
let node = interp.position_teme(t_node);
let truth_node = orbit.position_eci(t_node);
for k in 0..3 {
assert!((node[k] - truth_node[k]).abs() < 1.0, "node axis {k}");
}
let t_mid = 9.5 * 900.0;
let mid = interp.position_teme(t_mid);
let truth_mid = orbit.position_eci(t_mid);
let err = ((mid[0] - truth_mid[0]).powi(2)
+ (mid[1] - truth_mid[1]).powi(2)
+ (mid[2] - truth_mid[2]).powi(2))
.sqrt();
assert!(err < 100.0, "midpoint interpolation error {err:.3} m");
let p: Propagator = interp.into();
assert!(matches!(p, Propagator::Sp3Precise(_)));
let r = (p.position_eci(t_mid)[0].powi(2)
+ p.position_eci(t_mid)[1].powi(2)
+ p.position_eci(t_mid)[2].powi(2))
.sqrt();
assert!((r - a).abs() < 400_000.0, "radius {r:.0} m");
assert!(
(4.2e4..4.4e4).contains(&p.period_s()),
"period {} s",
p.period_s()
);
}
#[test]
fn interpolator_needs_at_least_two_epochs() {
let f = parse_sp3(SAMPLE).unwrap();
assert!(f.interpolator("G01").is_some()); assert!(f.interpolator("G99").is_none()); }
#[test]
fn builds_an_sp3_from_propagated_orbits() {
use crate::orbit::{Orbit, Propagator};
let a = 26_560_000.0;
let sats = vec![
Propagator::Kepler(Orbit::new(a, 0.96, 0.0, 0.0)),
Propagator::Kepler(Orbit::new(a, 0.96, std::f64::consts::PI, 1.0)),
];
let ids = vec!["G01".to_string(), "G02".to_string()];
let start = EpochUtc {
year: 2023,
month: 1,
day: 1,
hour: 0,
minute: 0,
second: 0.0,
};
let jd = crate::timescales::julian_date(2023, 1, 1, 0, 0, 0.0);
let f = Sp3File::from_propagators(&ids, &sats, start, jd, 900.0, 3);
assert_eq!(f.header.sat_ids, ids);
assert_eq!(f.epochs.len(), 3);
let p = f.position_of("G01", 0).unwrap();
let r = (p[0].powi(2) + p[1].powi(2) + p[2].powi(2)).sqrt();
assert!((r - a).abs() < 1.0, "radius {r:.0} m");
assert!(f.epochs[0].sats[0].clock_is_bad());
assert_eq!(f.epochs[2].time.minute, 30);
let reparsed = parse_sp3(&f.to_sp3_string()).expect("built SP3 re-parses");
assert_eq!(reparsed.observed_satellites(), ids);
}
}