sidereon_core/
frame.rs

1//! Frame-tagged position types.
2//!
3//! Every position value encodes its reference frame and datum in the **type
4//! name**, never a bare `position_m` that hides the frame. SP3 products give
5//! satellite states
6//! in the ITRF/IGS-realization ECEF frame, in meters; that fact is carried by
7//! [`ItrfPositionM`] so a consumer cannot accidentally mix it with, say, a
8//! GCRS/TEME state from the core crate (which is in kilometers).
9
10use core::fmt;
11
12use crate::astro::math::vec3::{cross3, unit3};
13
14/// Error returned when constructing frame-tagged values from invalid inputs.
15#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
16pub enum FrameValueError {
17    /// A frame value component was non-finite or outside its documented domain.
18    #[error("invalid frame value {field}: {reason}")]
19    InvalidInput {
20        field: &'static str,
21        reason: &'static str,
22    },
23}
24
25const fn invalid_input(field: &'static str, reason: &'static str) -> FrameValueError {
26    FrameValueError::InvalidInput { field, reason }
27}
28
29/// Geocentric local vertical at a receiver ECEF position: the position vector
30/// normalized (the spherical radial direction, out from the geocenter).
31///
32/// PARITY / FRAME: this is the GEOCENTRIC up (`position / |position|`), **not**
33/// the geodetic ellipsoid normal. The two differ by up to ~0.19 deg, which
34/// shifts low-elevation variance weighting and antenna projections; the GNSS
35/// baseline/PPP code paths were captured against the geocentric definition
36/// (Elixir `local_up/1`), so this is deliberately the geocentric variant. A
37/// geodetic ENU rotation (built from geodetic latitude/longitude) is a separate
38/// construction and stays with its caller. Normalization is reciprocal-multiply
39/// (`scale3(v, 1/n)` via [`unit3`]) to preserve the callers' 0-ULP goldens; a
40/// zero-length position degenerates to `+Z`.
41pub(crate) fn geocentric_up(position_ecef_m: [f64; 3]) -> [f64; 3] {
42    unit3(position_ecef_m).unwrap_or([0.0, 0.0, 1.0])
43}
44
45/// East unit of the geocentric local frame for a given local `up`:
46/// `normalize(Z x up)`, degenerating to `+X` when `up` is parallel to `Z`.
47pub(crate) fn geocentric_east(up: [f64; 3]) -> [f64; 3] {
48    unit3(cross3([0.0, 0.0, 1.0], up)).unwrap_or([1.0, 0.0, 0.0])
49}
50
51/// Geocentric local North-East-Up basis at a receiver ECEF position, returned as
52/// `(north, east, up)`.
53///
54/// `up` is [`geocentric_up`], `east` is [`geocentric_east`], and
55/// `north = normalize(up x east)`. This is the single shared geocentric basis
56/// builder: the PPP correction tables, the static PPP solver, and the RTK
57/// baseline-filter antenna model all consume it (each previously kept a
58/// byte-identical copy). See [`geocentric_up`] for the geocentric-vs-geodetic
59/// distinction.
60pub(crate) fn geocentric_neu_basis(position_ecef_m: [f64; 3]) -> ([f64; 3], [f64; 3], [f64; 3]) {
61    let up = geocentric_up(position_ecef_m);
62    let east = geocentric_east(up);
63    let north = unit3(cross3(up, east)).unwrap_or([0.0, 0.0, 1.0]);
64    (north, east, up)
65}
66
67/// A position in the ITRF / IGS-realization Earth-Centered-Earth-Fixed frame,
68/// expressed in **meters**.
69///
70/// SP3 ephemerides are published in an IGS realization of the ITRF (e.g.
71/// `IGS14`, `IGS20`); the exact realization string is carried on the SP3
72/// header ([`crate::sp3::Sp3Header::coordinate_system`]), while this type fixes
73/// the *kind* of frame (ITRF/IGS ECEF) and the *unit* (meters) in the type
74/// system. There is intentionally no implicit conversion to the core crate's
75/// kilometer states; conversion happens explicitly at an API boundary.
76#[derive(Debug, Clone, Copy, PartialEq)]
77pub struct ItrfPositionM {
78    /// ECEF X coordinate in meters.
79    pub x_m: f64,
80    /// ECEF Y coordinate in meters.
81    pub y_m: f64,
82    /// ECEF Z coordinate in meters.
83    pub z_m: f64,
84}
85
86impl ItrfPositionM {
87    /// Construct an ITRF ECEF position from meter components.
88    pub const fn new(x_m: f64, y_m: f64, z_m: f64) -> Result<Self, FrameValueError> {
89        if !x_m.is_finite() {
90            return Err(invalid_input("x_m", "must be finite"));
91        }
92        if !y_m.is_finite() {
93            return Err(invalid_input("y_m", "must be finite"));
94        }
95        if !z_m.is_finite() {
96            return Err(invalid_input("z_m", "must be finite"));
97        }
98        Ok(Self { x_m, y_m, z_m })
99    }
100
101    /// The components as a `[x, y, z]` meter array.
102    pub const fn as_array(self) -> [f64; 3] {
103        [self.x_m, self.y_m, self.z_m]
104    }
105}
106
107impl fmt::Display for ItrfPositionM {
108    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
109        write!(
110            f,
111            "ITRF[{:.3}, {:.3}, {:.3}] m",
112            self.x_m, self.y_m, self.z_m
113        )
114    }
115}
116
117/// A geodetic (ellipsoidal) position on the WGS84 datum.
118///
119/// Latitude and longitude are geodetic angles in **radians**; height is the
120/// ellipsoidal height in **meters**. This is the receiver-position type the
121/// atmospheric (ionosphere/troposphere) models take: they need the geodetic
122/// latitude/longitude of the antenna and, for the troposphere, the height.
123///
124/// The frame and units are fixed in the type so a caller cannot mix this with
125/// the ECEF [`ItrfPositionM`] or pass degrees where radians are expected.
126/// Longitude is positive east; latitude is positive north.
127#[derive(Debug, Clone, Copy, PartialEq)]
128pub struct Wgs84Geodetic {
129    /// Geodetic latitude in radians, positive north, range `[-pi/2, pi/2]`.
130    pub lat_rad: f64,
131    /// Geodetic longitude in radians, positive east, range `[-pi, pi]`.
132    pub lon_rad: f64,
133    /// Ellipsoidal height above the WGS84 ellipsoid in meters.
134    pub height_m: f64,
135}
136
137impl Wgs84Geodetic {
138    /// Construct a WGS84 geodetic position from radians and meters.
139    pub const fn new(lat_rad: f64, lon_rad: f64, height_m: f64) -> Result<Self, FrameValueError> {
140        if !lat_rad.is_finite() {
141            return Err(invalid_input("lat_rad", "must be finite"));
142        }
143        if !lon_rad.is_finite() {
144            return Err(invalid_input("lon_rad", "must be finite"));
145        }
146        if !height_m.is_finite() {
147            return Err(invalid_input("height_m", "must be finite"));
148        }
149        if lat_rad < -core::f64::consts::FRAC_PI_2 || lat_rad > core::f64::consts::FRAC_PI_2 {
150            return Err(invalid_input("lat_rad", "must be in [-pi/2, pi/2]"));
151        }
152        if lon_rad < -core::f64::consts::PI || lon_rad > core::f64::consts::PI {
153            return Err(invalid_input("lon_rad", "must be in [-pi, pi]"));
154        }
155        Ok(Self {
156            lat_rad,
157            lon_rad,
158            height_m,
159        })
160    }
161}
162
163impl fmt::Display for Wgs84Geodetic {
164    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
165        write!(
166            f,
167            "WGS84[lat {:.6} rad, lon {:.6} rad, h {:.3} m]",
168            self.lat_rad, self.lon_rad, self.height_m
169        )
170    }
171}
172
173/// A velocity in the ITRF / IGS-realization ECEF frame, in **meters per
174/// second**.
175///
176/// Present only when the SP3 file is a velocity product (`#?V...` header /
177/// `V`-records). The frame/unit are fixed in the type for the same reason as
178/// [`ItrfPositionM`].
179#[derive(Debug, Clone, Copy, PartialEq)]
180pub struct ItrfVelocityMS {
181    /// ECEF X velocity in meters per second.
182    pub vx_m_s: f64,
183    /// ECEF Y velocity in meters per second.
184    pub vy_m_s: f64,
185    /// ECEF Z velocity in meters per second.
186    pub vz_m_s: f64,
187}
188
189impl ItrfVelocityMS {
190    /// Construct an ITRF ECEF velocity from meter-per-second components.
191    pub const fn new(vx_m_s: f64, vy_m_s: f64, vz_m_s: f64) -> Result<Self, FrameValueError> {
192        if !vx_m_s.is_finite() {
193            return Err(invalid_input("vx_m_s", "must be finite"));
194        }
195        if !vy_m_s.is_finite() {
196            return Err(invalid_input("vy_m_s", "must be finite"));
197        }
198        if !vz_m_s.is_finite() {
199            return Err(invalid_input("vz_m_s", "must be finite"));
200        }
201        Ok(Self {
202            vx_m_s,
203            vy_m_s,
204            vz_m_s,
205        })
206    }
207
208    /// The components as a `[vx, vy, vz]` m/s array.
209    pub const fn as_array(self) -> [f64; 3] {
210        [self.vx_m_s, self.vy_m_s, self.vz_m_s]
211    }
212}
213
214#[cfg(test)]
215mod tests {
216    use super::*;
217    use crate::astro::math::vec3::{norm3, scale3};
218
219    fn bits(v: [f64; 3]) -> [u64; 3] {
220        [v[0].to_bits(), v[1].to_bits(), v[2].to_bits()]
221    }
222
223    /// The explicit clamp-and-match recipe the PPP/RTK modules previously
224    /// copy-pasted. The shared builder must reproduce it bit-for-bit.
225    fn reference_neu(position_ecef_m: [f64; 3]) -> ([f64; 3], [f64; 3], [f64; 3]) {
226        let up = match norm3(position_ecef_m) {
227            n if n > 0.0 => scale3(position_ecef_m, 1.0 / n),
228            _ => [0.0, 0.0, 1.0],
229        };
230        let cross = cross3([0.0, 0.0, 1.0], up);
231        let east = if cross == [0.0, 0.0, 0.0] {
232            [1.0, 0.0, 0.0]
233        } else {
234            match norm3(cross) {
235                n if n > 0.0 => scale3(cross, 1.0 / n),
236                _ => [1.0, 0.0, 0.0],
237            }
238        };
239        let north = match norm3(cross3(up, east)) {
240            n if n > 0.0 => scale3(cross3(up, east), 1.0 / n),
241            _ => [0.0, 0.0, 1.0],
242        };
243        (north, east, up)
244    }
245
246    fn assert_close3(actual: [f64; 3], expected: [f64; 3], tol: f64) {
247        for (a, e) in actual.into_iter().zip(expected) {
248            assert!(
249                (a - e).abs() <= tol,
250                "component {a:?} differs from {e:?} by more than {tol}"
251            );
252        }
253    }
254
255    #[test]
256    fn constructors_reject_invalid_frame_values() {
257        assert_eq!(
258            ItrfPositionM::new(f64::NAN, 0.0, 0.0),
259            Err(FrameValueError::InvalidInput {
260                field: "x_m",
261                reason: "must be finite"
262            })
263        );
264        assert_eq!(
265            ItrfVelocityMS::new(0.0, f64::INFINITY, 0.0),
266            Err(FrameValueError::InvalidInput {
267                field: "vy_m_s",
268                reason: "must be finite"
269            })
270        );
271        assert_eq!(
272            Wgs84Geodetic::new(core::f64::consts::FRAC_PI_2 + 1.0e-12, 0.0, 0.0),
273            Err(FrameValueError::InvalidInput {
274                field: "lat_rad",
275                reason: "must be in [-pi/2, pi/2]"
276            })
277        );
278        assert_eq!(
279            Wgs84Geodetic::new(0.0, -core::f64::consts::PI - 1.0e-12, 0.0),
280            Err(FrameValueError::InvalidInput {
281                field: "lon_rad",
282                reason: "must be in [-pi, pi]"
283            })
284        );
285        assert_eq!(
286            Wgs84Geodetic::new(0.0, 0.0, f64::NAN),
287            Err(FrameValueError::InvalidInput {
288                field: "height_m",
289                reason: "must be finite"
290            })
291        );
292    }
293
294    #[test]
295    fn constructors_preserve_valid_frame_values() {
296        let position = ItrfPositionM::new(1.0, -2.0, 3.5).expect("valid ITRF position");
297        assert_eq!(position.as_array(), [1.0, -2.0, 3.5]);
298
299        let velocity = ItrfVelocityMS::new(0.1, -0.2, 0.3).expect("valid ITRF velocity");
300        assert_eq!(velocity.as_array(), [0.1, -0.2, 0.3]);
301
302        let geodetic =
303            Wgs84Geodetic::new(-0.5, core::f64::consts::PI, -12.0).expect("valid geodetic");
304        assert_eq!(geodetic.lat_rad.to_bits(), (-0.5_f64).to_bits());
305        assert_eq!(geodetic.lon_rad.to_bits(), core::f64::consts::PI.to_bits());
306        assert_eq!(geodetic.height_m.to_bits(), (-12.0_f64).to_bits());
307
308        let west_antimeridian =
309            Wgs84Geodetic::new(0.25, -core::f64::consts::PI, 42.0).expect("valid geodetic");
310        assert_eq!(
311            west_antimeridian.lon_rad.to_bits(),
312            (-core::f64::consts::PI).to_bits()
313        );
314    }
315
316    #[test]
317    fn geocentric_basis_matches_reference_recipe_to_the_bit() {
318        // A representative mid-latitude ECEF receiver position (metres).
319        let position = [4_027_894.0, 307_045.0, 4_919_474.0];
320        let (north, east, up) = geocentric_neu_basis(position);
321        let (rn, re, ru) = reference_neu(position);
322        assert_eq!(bits(north), bits(rn));
323        assert_eq!(bits(east), bits(re));
324        assert_eq!(bits(up), bits(ru));
325        // up = normalized position; east and north are orthogonal unit vectors.
326        assert_eq!(bits(up), bits(geocentric_up(position)));
327        assert_eq!(bits(east), bits(geocentric_east(up)));
328    }
329
330    #[test]
331    fn geocentric_basis_is_right_handed_enu_and_points_north() {
332        let (north, east, up) = geocentric_neu_basis([6_378_137.0, 0.0, 0.0]);
333        assert_close3(up, [1.0, 0.0, 0.0], 1.0e-15);
334        assert_close3(east, [0.0, 1.0, 0.0], 1.0e-15);
335        assert_close3(north, [0.0, 0.0, 1.0], 1.0e-15);
336
337        let (north, east, up) = geocentric_neu_basis([4_027_894.0, 307_045.0, 4_919_474.0]);
338        assert_close3(cross3(up, east), north, 1.0e-15);
339        assert_close3(cross3(east, north), up, 1.0e-15);
340    }
341
342    #[test]
343    fn geocentric_basis_handles_degenerate_positions() {
344        // Zero-length position degenerates to +Z up, +X east.
345        let (north, east, up) = geocentric_neu_basis([0.0, 0.0, 0.0]);
346        assert_eq!(up, [0.0, 0.0, 1.0]);
347        assert_eq!(east, [1.0, 0.0, 0.0]);
348        // north = normalize(up x east) = [0,0,1] x [1,0,0] = [0,1,0].
349        assert_eq!(north, [0.0, 1.0, 0.0]);
350        // A purely polar position (up parallel to +Z) keeps east at +X.
351        assert_eq!(geocentric_east([0.0, 0.0, 1.0]), [1.0, 0.0, 0.0]);
352    }
353}
sidereon_core/frame.rs

sidereon_core/
frame.rs
