sidereon_core/inertial/
frames.rs

1//! WGS84 normal gravity and ECEF plumb-bob gravity vectors.
2
3use crate::astro::constants::earth::{
4    GM_EARTH_M3_S2, OMEGA_E_DOT_RAD_S, WGS84_A_M, WGS84_E2, WGS84_F,
5};
6use crate::astro::math::vec3::{norm3, scale3};
7use crate::frame::{itrf_to_geodetic, ItrfPositionM};
8
9use super::{invalid_input, validate_finite, validate_vec3, InertialError};
10
11/// WGS84 normal gravity at the equator in m/s^2.
12pub const WGS84_NORMAL_GRAVITY_EQUATOR_MPS2: f64 = 9.780_325_335_9;
13
14/// WGS84 normal gravity at the pole in m/s^2.
15pub const WGS84_NORMAL_GRAVITY_POLE_MPS2: f64 = 9.832_184_937_8;
16
17/// Somigliana normal-gravity formula constant derived from WGS84 table values.
18pub const WGS84_SOMIGLIANA_K: f64 = ((WGS84_A_M * (1.0 - WGS84_F))
19    * WGS84_NORMAL_GRAVITY_POLE_MPS2)
20    / (WGS84_A_M * WGS84_NORMAL_GRAVITY_EQUATOR_MPS2)
21    - 1.0;
22
23/// WGS84 Somigliana normal gravity magnitude at geodetic latitude and height.
24///
25/// The returned magnitude is positive downward along the geodetic normal. The
26/// height continuation is the WGS84 Taylor expression for normal gravity above
27/// the ellipsoid, using the same WGS84 rotation and `GM` constants as the rest
28/// of the core.
29pub fn normal_gravity_mps2(lat_rad: f64, height_m: f64) -> Result<f64, InertialError> {
30    validate_finite(lat_rad, "lat_rad")?;
31    validate_finite(height_m, "height_m")?;
32    if !(-core::f64::consts::FRAC_PI_2..=core::f64::consts::FRAC_PI_2).contains(&lat_rad) {
33        return Err(invalid_input("lat_rad", "must be in [-pi/2, pi/2]"));
34    }
35
36    let sin_lat = lat_rad.sin();
37    let sin2 = sin_lat * sin_lat;
38    let surface = WGS84_NORMAL_GRAVITY_EQUATOR_MPS2 * (1.0 + WGS84_SOMIGLIANA_K * sin2)
39        / (1.0 - WGS84_E2 * sin2).sqrt();
40    let b_m = WGS84_A_M * (1.0 - WGS84_F);
41    let m = OMEGA_E_DOT_RAD_S * OMEGA_E_DOT_RAD_S * WGS84_A_M * WGS84_A_M * b_m / GM_EARTH_M3_S2;
42    let height_scale = 1.0
43        - (2.0 / WGS84_A_M) * (1.0 + WGS84_F + m - 2.0 * WGS84_F * sin2) * height_m
44        + 3.0 * height_m * height_m / (WGS84_A_M * WGS84_A_M);
45    let gravity = surface * height_scale;
46    if gravity.is_finite() && gravity > 0.0 {
47        Ok(gravity)
48    } else {
49        Err(invalid_input(
50            "height_m",
51            "outside normal-gravity Taylor domain",
52        ))
53    }
54}
55
56/// WGS84 normal gravity vector in ECEF coordinates, in m/s^2.
57///
58/// The vector includes the centrifugal contribution through Somigliana normal
59/// gravity and points downward along the geodetic normal at the supplied ECEF
60/// position.
61pub fn gravity_ecef_mps2(position_ecef_m: [f64; 3]) -> Result<[f64; 3], InertialError> {
62    validate_vec3(position_ecef_m, "position_ecef_m")?;
63    if norm3(position_ecef_m) <= 0.0 {
64        return Err(invalid_input("position_ecef_m", "must be nonzero"));
65    }
66    let position = ItrfPositionM::new(position_ecef_m[0], position_ecef_m[1], position_ecef_m[2])
67        .map_err(|_| invalid_input("position_ecef_m", "must be finite"))?;
68    let geodetic = itrf_to_geodetic(position)
69        .map_err(|_| invalid_input("position_ecef_m", "must convert to WGS84 geodetic"))?;
70    let gravity = normal_gravity_mps2(geodetic.lat_rad, geodetic.height_m)?;
71    let normal = geodetic_surface_normal_ecef(geodetic.lat_rad, geodetic.lon_rad);
72    Ok(scale3(normal, -gravity))
73}
74
75fn geodetic_surface_normal_ecef(lat_rad: f64, lon_rad: f64) -> [f64; 3] {
76    let (sin_lat, cos_lat) = lat_rad.sin_cos();
77    let (sin_lon, cos_lon) = lon_rad.sin_cos();
78    [cos_lat * cos_lon, cos_lat * sin_lon, sin_lat]
79}
80
81#[cfg(test)]
82mod tests {
83    //! Provenance: WGS84 normal-gravity validation uses WGS84 Technical Report
84    //! TR8350.2, Chapter 4.2, Somigliana surface gravity, and WGS84 table
85    //! values gamma_e = 9.7803253359 m/s^2 and gamma_p = 9.8321849378 m/s^2.
86
87    use super::*;
88
89    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
90        assert!(
91            (actual - expected).abs() <= tolerance,
92            "actual {actual:.17e}, expected {expected:.17e}, tolerance {tolerance:.17e}"
93        );
94    }
95
96    #[test]
97    fn somigliana_hits_wgs84_equator_and_pole_constants() {
98        let equator = normal_gravity_mps2(0.0, 0.0).expect("equator");
99        assert_eq!(
100            equator.to_bits(),
101            WGS84_NORMAL_GRAVITY_EQUATOR_MPS2.to_bits()
102        );
103
104        let pole = normal_gravity_mps2(core::f64::consts::FRAC_PI_2, 0.0).expect("pole gravity");
105        assert_close(pole, WGS84_NORMAL_GRAVITY_POLE_MPS2, 2.0e-15);
106    }
107
108    #[test]
109    fn gravity_ecef_points_down_at_equator_and_pole() {
110        let equator = gravity_ecef_mps2([WGS84_A_M, 0.0, 0.0]).expect("equator vector");
111        assert_eq!(
112            equator[0].to_bits(),
113            (-WGS84_NORMAL_GRAVITY_EQUATOR_MPS2).to_bits()
114        );
115        assert_eq!(equator[1].to_bits(), (-0.0_f64).to_bits());
116        assert_eq!(equator[2].to_bits(), (-0.0_f64).to_bits());
117
118        let b_m = WGS84_A_M * (1.0 - WGS84_F);
119        let pole = gravity_ecef_mps2([0.0, 0.0, b_m]).expect("pole vector");
120        assert_close(pole[0], 0.0, 1.0e-15);
121        assert_close(pole[1], 0.0, 1.0e-15);
122        assert_close(pole[2], -WGS84_NORMAL_GRAVITY_POLE_MPS2, 2.0e-15);
123    }
124}
sidereon_core/inertial/frames.rs

sidereon_core/inertial/
frames.rs
