ApsisRS
/
apsis-rs
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

physics: add WGS84 ellipsoid and geodetic ECEF conversion

This commit is contained in:
ApsisRS contributors 2026-09-26 13:22:56 +08:00
parent f38e8a6097
commit 7e822d7ce4
6 changed files with 587 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
crates/physics/apsis-geodesy/Cargo.toml
View File

@ -11,3 +11,5 @@ publish = false
workspace = true
[dependencies]
apsis-core.workspace = true
apsis-math.workspace = true

136
crates/physics/apsis-geodesy/src/ellipsoid.rs Normal file
View File

@ -0,0 +1,136 @@
//! Reference ellipsoids.
/// A biaxial reference ellipsoid: an ellipse of revolution about its minor
/// axis.
///
/// The shape is fixed by two numbers. This crate stores the semi-major axis
/// and the inverse flattening, which are the two defining parameters of the
/// WGS 84 ellipsoid in the standard. Every other quantity is derived, so
/// there is no risk of the parameters disagreeing with each other.
///
/// Lengths are in meters and angles are in radians.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Ellipsoid {
semi_major_axis: f64,
inverse_flattening: f64,
}
impl Ellipsoid {
/// Creates an ellipsoid from its semi-major axis and inverse
/// flattening.
///
/// Lengths are in meters. The inverse flattening must be positive and
/// finite; a value of infinity describes a sphere.
pub const fn new(semi_major_axis: f64, inverse_flattening: f64) -> Self {
Self {
semi_major_axis,
inverse_flattening,
}
}
/// Returns the semi-major axis, in meters.
pub const fn semi_major_axis(self) -> f64 {
self.semi_major_axis
}
/// Returns the inverse flattening.
pub const fn inverse_flattening(self) -> f64 {
self.inverse_flattening
}
/// Returns the flattening.
///
/// This is zero for a sphere.
pub fn flattening(self) -> f64 {
1.0 / self.inverse_flattening
}
/// Returns the semi-minor axis, in meters.
pub fn semi_minor_axis(self) -> f64 {
self.semi_major_axis * (1.0 - self.flattening())
}
/// Returns the first eccentricity squared, a dimensionless number in
/// `[0, 1)`.
///
/// It is `2f - f^2` for flattening `f`.
pub fn eccentricity_squared(self) -> f64 {
let flattening = self.flattening();
2.0 * flattening - flattening * flattening
}
/// Returns the first eccentricity, a dimensionless number in `[0, 1)`.
pub fn eccentricity(self) -> f64 {
self.eccentricity_squared().sqrt()
}
/// Returns the second eccentricity squared, a dimensionless number.
///
/// It is `e^2 / (1 - e^2)`, which appears in the closed form for
/// converting a geodetic position to Cartesian coordinates.
pub fn second_eccentricity_squared(self) -> f64 {
let squared = self.eccentricity_squared();
squared / (1.0 - squared)
}
/// Returns the polar radius of curvature, in meters.
///
/// This is the radius of curvature in the meridian at the pole, equal
/// to `a / sqrt(1 - e^2)`.
pub fn polar_radius_of_curvature(self) -> f64 {
self.semi_major_axis / (1.0 - self.eccentricity_squared()).sqrt()
}
/// Returns the radius of curvature in the prime vertical at the given
/// geodetic latitude, in meters.
///
/// This is the distance from the surface to the polar axis measured
/// along the ellipsoid normal, and it grows from the semi-major axis at
/// the equator to [`Ellipsoid::polar_radius_of_curvature`] at the pole.
///
/// The latitude is in radians.
pub fn prime_vertical_radius(self, latitude: f64) -> f64 {
let sin = latitude.sin();
self.semi_major_axis / (1.0 - self.eccentricity_squared() * sin * sin).sqrt()
}
/// Returns the radius of curvature in the meridian at the given
/// geodetic latitude, in meters.
///
/// The latitude is in radians.
pub fn meridian_radius(self, latitude: f64) -> f64 {
let sin = latitude.sin();
let squared = self.eccentricity_squared();
let denominator = 1.0 - squared * sin * sin;
self.semi_major_axis * (1.0 - squared) / denominator.powf(1.5)
}
/// Returns the geocentric latitude corresponding to a geodetic
/// latitude, in radians.
///
/// The geocentric latitude is measured from the equatorial plane to the
/// radius vector, while the geodetic latitude is measured to the
/// ellipsoid normal. The geocentric value is the smaller of the two
/// away from the equator and the poles.
pub fn geocentric_latitude(self, geodetic_latitude: f64) -> f64 {
let squared = self.eccentricity_squared();
let tangent = (1.0 - squared) * geodetic_latitude.tan();
tangent.atan()
}
}
/// The WGS 84 reference ellipsoid.
///
/// The two defining parameters are the semi-major axis `a = 6 378 137 m` and
/// the inverse flattening `1 / f = 298.257 223 563`.
///
/// Source: NIMA TR8350.2, third edition, 2000.
pub const WGS84: Ellipsoid = Ellipsoid::new(6_378_137.0, 298.257_223_563);
/// The Earth gravitational parameter for WGS 84, in cubic meters per second
/// squared.
///
/// This is held here rather than in `apsis-core` so that a caller working
/// with the ellipsoid can use the matching gravity field. The value is
/// `3.986 004 418e14 m^3/s^2`.
pub const WGS84_GRAVITATIONAL_PARAMETER: f64 = 3.986_004_418e14;

182
crates/physics/apsis-geodesy/src/geodetic.rs Normal file
View File

@ -0,0 +1,182 @@
//! Geodetic positions and their conversion to Earth-centered,
//! Earth-fixed Cartesian coordinates.
use crate::ellipsoid::Ellipsoid;
use apsis_math::Vec3;
/// A geodetic position: latitude, longitude, and height above an ellipsoid.
///
/// Latitude and longitude are in radians and height is in meters. Latitude
/// is geodetic, measured between the ellipsoid normal and the equatorial
/// plane, and lies in `[-pi/2, pi/2]`. Longitude is measured east from the
/// prime meridian. Height is the distance along the ellipsoid normal, taken
/// as positive outside the ellipsoid.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Geodetic {
/// Geodetic latitude, in radians.
pub latitude: f64,
/// Longitude, in radians, positive east.
pub longitude: f64,
/// Height above the ellipsoid, in meters.
pub height: f64,
}
impl Geodetic {
/// Creates a geodetic position from latitude, longitude, and height.
pub const fn new(latitude: f64, longitude: f64, height: f64) -> Self {
Self {
latitude,
longitude,
height,
}
}
/// Converts this geodetic position to Earth-centered, Earth-fixed
/// Cartesian coordinates on the given ellipsoid.
///
/// The frame has its third axis along the ellipsoid's minor axis
/// through the north pole, its first axis through the intersection of
/// the equator and the prime meridian, and its second axis completing
/// a right-handed set. The result is in meters.
///
/// The closed form is
///
/// ```text
/// N = a / sqrt(1 - e^2 sin^2(lat))
/// x = (N + h) cos(lat) cos(lon)
/// y = (N + h) cos(lat) sin(lon)
/// z = (N (1 - e^2) + h) sin(lat)
/// ```
///
/// where `N` is the prime vertical radius of curvature. This form is
/// exact everywhere, including at the poles, where the cosine factors
/// vanish.
pub fn to_ecef(self, ellipsoid: Ellipsoid) -> Ecef {
ecef_from_geodetic(self, ellipsoid)
}
/// Returns `true` when every field is finite and the latitude and
/// longitude are inside their ranges.
pub fn is_valid(self) -> bool {
let half_pi = core::f64::consts::FRAC_PI_2;
self.latitude.is_finite()
&& self.longitude.is_finite()
&& self.height.is_finite()
&& self.latitude >= -half_pi
&& self.latitude <= half_pi
&& self.longitude >= -core::f64::consts::PI
&& self.longitude <= core::f64::consts::PI
}
}
/// An Earth-centered, Earth-fixed Cartesian position.
///
/// The frame is the one described by [`Geodetic::to_ecef`]. The wrapped
/// [`Vec3`] holds the position in meters.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Ecef(pub Vec3);
impl Ecef {
/// Creates an Earth-centered, Earth-fixed position from a vector in
/// meters.
pub const fn new(position: Vec3) -> Self {
Self(position)
}
/// Returns the position in meters.
pub const fn position(self) -> Vec3 {
self.0
}
/// Converts this position to a geodetic position on the given
/// ellipsoid.
///
/// Returns `None` when the position is not finite, or when it lies on
/// the polar axis so that no longitude is defined.
pub fn to_geodetic(self, ellipsoid: Ellipsoid) -> Option<Geodetic> {
geodetic_from_ecef(self, ellipsoid)
}
}
/// Converts a geodetic position to Earth-centered, Earth-fixed Cartesian
/// coordinates.
///
/// See [`Geodetic::to_ecef`] for the frame definition and the closed form.
pub fn ecef_from_geodetic(geodetic: Geodetic, ellipsoid: Ellipsoid) -> Ecef {
let Geodetic {
latitude,
longitude,
height,
} = geodetic;
let (sin_latitude, cos_latitude) = latitude.sin_cos();
let (sin_longitude, cos_longitude) = longitude.sin_cos();
let prime_vertical = ellipsoid.prime_vertical_radius(latitude);
let eccentricity_squared = ellipsoid.eccentricity_squared();
let horizontal = prime_vertical + height;
let x = horizontal * cos_latitude * cos_longitude;
let y = horizontal * cos_latitude * sin_longitude;
let z = (prime_vertical * (1.0 - eccentricity_squared) + height) * sin_latitude;
Ecef(Vec3::new(x, y, z))
}
/// Converts Earth-centered, Earth-fixed Cartesian coordinates to a geodetic
/// position.
///
/// The latitude and height are recovered by Bowring's method, which solves
/// the meridian equation in closed form and then refines the result with a
/// few fixed point steps. The iteration converges to well below a
/// micrometer for positions from below the ellipsoid out to geostationary
/// radius.
///
/// Returns `None` when the position is not finite, or when it lies on the
/// polar axis so that no longitude is defined.
pub fn geodetic_from_ecef(ecef: Ecef, ellipsoid: Ellipsoid) -> Option<Geodetic> {
let Vec3 { x, y, z } = ecef.0;
if !ecef.0.is_finite() {
return None;
}
let squared = ellipsoid.eccentricity_squared();
let second_squared = ellipsoid.second_eccentricity_squared();
let semi_major = ellipsoid.semi_major_axis();
let semi_minor = ellipsoid.semi_minor_axis();
let horizontal_squared = x * x + y * y;
if horizontal_squared == 0.0 {
// On the polar axis the longitude is undefined.
return None;
}
let horizontal = horizontal_squared.sqrt();
let longitude = y.atan2(x);
// Bowring's initial latitude estimate, obtained by treating the point
// as if it lay on a locally spherical surface.
let theta = (z * semi_major).atan2(horizontal * semi_minor);
let (sin_theta, cos_theta) = theta.sin_cos();
let mut latitude = (z + second_squared * semi_minor * sin_theta * sin_theta * sin_theta)
.atan2(horizontal - squared * semi_major * cos_theta * cos_theta * cos_theta);
// Refine with fixed point steps on the ellipsoid equation. Three steps
// reduce the error to the floating point noise floor for every
// terrestrial altitude.
for _ in 0..3 {
let (sin_latitude, _) = latitude.sin_cos();
latitude = (z + squared * ellipsoid.prime_vertical_radius(latitude) * sin_latitude)
.atan2(horizontal);
}
let (sin_latitude, cos_latitude) = latitude.sin_cos();
// Guard against a latitude that has converged exactly to a pole, where
// the division below would be singular.
if cos_latitude == 0.0 {
return None;
}
let height = if cos_latitude.abs() > 0.5 {
horizontal / cos_latitude - ellipsoid.prime_vertical_radius(latitude)
} else {
z / sin_latitude - ellipsoid.prime_vertical_radius(latitude) * (1.0 - squared)
};
Some(Geodetic::new(latitude, longitude, height))
}

33
crates/physics/apsis-geodesy/src/lib.rs
View File

@ -1 +1,34 @@
//! Reference ellipsoids and geodetic computations.
//!
//! A geodetic position is written as latitude, longitude, and height above
//! an ellipsoid. Latitude here is always geodetic, the angle between the
//! ellipsoid normal and the equatorial plane, not geocentric. The two
//! differ by up to about 0.19 degrees at mid latitudes, so the distinction
//! matters for pointing and ranging.
//!
//! [`Wgs84`] is the ellipsoid used by GPS and by almost all satellite work.
//! Its defining parameters are the semi-major axis and the inverse
//! flattening; every other quantity is derived from those two, so the
//! derived values agree with the ones published in the standard to the
//! precision of the defining parameters.
//!
//! Source: National Imagery and Mapping Agency, *Department of Defense World
//! Geodetic System 1984*, Technical Report TR8350.2, third edition, 2000.
//!
//! ```
//! use apsis_geodesy::{Geodetic, WGS84};
//!
//! // The equator has zero height and a radius equal to the semi-major axis.
//! let point = Geodetic::new(0.0, 0.0, 0.0);
//! let cartesian = point.to_ecef(WGS84);
//! assert!((cartesian.position().norm() - WGS84.semi_major_axis()).abs() < 1e-9);
//! ```
#![forbid(unsafe_code)]
#![deny(missing_docs)]
mod ellipsoid;
mod geodetic;
pub use ellipsoid::{Ellipsoid, WGS84, WGS84_GRAVITATIONAL_PARAMETER};
pub use geodetic::{Ecef, Geodetic, ecef_from_geodetic, geodetic_from_ecef};

84
crates/physics/apsis-geodesy/tests/ellipsoid.rs Normal file
View File

@ -0,0 +1,84 @@
//! Tests for the reference ellipsoid.
use apsis_geodesy::{Ellipsoid, WGS84};
#[test]
fn defining_parameters_match_the_standard() {
assert_eq!(WGS84.semi_major_axis(), 6_378_137.0);
assert_eq!(WGS84.inverse_flattening(), 298.257_223_563);
assert!((WGS84.flattening() - 1.0 / 298.257_223_563).abs() < 1e-15);
}
#[test]
fn derived_axes_match_the_published_values() {
// The WGS 84 semi-minor axis is 6 356 752.314 245 m.
assert!((WGS84.semi_minor_axis() - 6_356_752.314_245).abs() < 1e-6);
// The first eccentricity is 0.081 819 190 842 622.
assert!((WGS84.eccentricity() - 0.081_819_190_842_622).abs() < 1e-12);
}
#[test]
fn eccentricity_is_consistent_with_the_flattening() {
let flattening = WGS84.flattening();
let expected = 2.0 * flattening - flattening * flattening;
assert!((WGS84.eccentricity_squared() - expected).abs() < 1e-18);
}
#[test]
fn second_eccentricity_relates_to_the_first() {
let first = WGS84.eccentricity_squared();
let second = WGS84.second_eccentricity_squared();
assert!((second - first / (1.0 - first)).abs() < 1e-18);
// Derived from the defining parameters this is 0.006 739 496 742 276.
assert!((second - 0.006_739_496_742_276_435).abs() < 1e-15);
}
#[test]
fn prime_vertical_radius_spans_the_expected_range() {
// At the equator it equals the semi-major axis.
assert!((WGS84.prime_vertical_radius(0.0) - WGS84.semi_major_axis()).abs() < 1e-9);
// At the pole it equals the polar radius of curvature, a / sqrt(1 - e^2),
// which is 6 399 593.625 9 m.
let pole = WGS84.prime_vertical_radius(core::f64::consts::FRAC_PI_2);
assert!((pole - WGS84.polar_radius_of_curvature()).abs() < 1e-9);
assert!((pole - 6_399_593.625_9).abs() < 1e-3);
}
#[test]
fn meridian_radius_spans_the_expected_range() {
// At the equator it is a (1 - e^2), which is 6 335 439.327 m.
let equator = WGS84.meridian_radius(0.0);
let expected = WGS84.semi_major_axis() * (1.0 - WGS84.eccentricity_squared());
assert!((equator - expected).abs() < 1e-6);
assert!((equator - 6_335_439.327_1).abs() < 1e-3);
// At the pole it is a / sqrt(1 - e^2).
let pole = WGS84.meridian_radius(core::f64::consts::FRAC_PI_2);
assert!((pole - WGS84.polar_radius_of_curvature()).abs() < 1e-6);
}
#[test]
fn geocentric_latitude_is_zero_at_the_equator_and_pole() {
assert!(WGS84.geocentric_latitude(0.0).abs() < 1e-12);
let pole = WGS84.geocentric_latitude(core::f64::consts::FRAC_PI_2);
assert!((pole - core::f64::consts::FRAC_PI_2).abs() < 1e-12);
}
#[test]
fn geocentric_latitude_is_smaller_than_geodetic_away_from_the_axes() {
let geodetic = 45.0_f64.to_radians();
let geocentric = WGS84.geocentric_latitude(geodetic);
assert!(geocentric < geodetic);
// The maximum separation is about 0.19 degrees at 45 degrees.
let difference = (geodetic - geocentric).to_degrees();
assert!((difference - 0.1924).abs() < 0.001, "got {difference}");
}
#[test]
fn a_sphere_has_no_flattening_or_eccentricity() {
let sphere = Ellipsoid::new(1_000.0, f64::INFINITY);
assert_eq!(sphere.flattening(), 0.0);
assert_eq!(sphere.eccentricity_squared(), 0.0);
assert!((sphere.semi_minor_axis() - 1_000.0).abs() < 1e-12);
assert!((sphere.prime_vertical_radius(0.7) - 1_000.0).abs() < 1e-12);
assert!((sphere.meridian_radius(0.7) - 1_000.0).abs() < 1e-12);
}

150
crates/physics/apsis-geodesy/tests/geodetic.rs Normal file
View File

@ -0,0 +1,150 @@
//! Tests for geodetic and Earth-centered, Earth-fixed conversions.
use apsis_geodesy::{Ecef, Geodetic, WGS84};
use apsis_math::Vec3;
/// Tolerance for position comparisons, in meters.
const POSITION_TOL: f64 = 1e-4;
/// Tolerance for latitude and longitude comparisons, in radians.
///
/// One microdegree is about 1.1e-8 radians, or roughly a centimeter on the
/// Earth's surface.
const ANGLE_TOL: f64 = 1e-11;
#[test]
fn equatorial_point_lies_on_the_semi_major_axis() {
let point = Geodetic::new(0.0, 0.0, 0.0).to_ecef(WGS84);
assert!((point.position().x - WGS84.semi_major_axis()).abs() < POSITION_TOL);
assert!(point.position().y.abs() < POSITION_TOL);
assert!(point.position().z.abs() < POSITION_TOL);
}
#[test]
fn pole_lies_on_the_semi_minor_axis() {
let point = Geodetic::new(core::f64::consts::FRAC_PI_2, 0.0, 0.0).to_ecef(WGS84);
assert!(point.position().x.abs() < POSITION_TOL);
assert!(point.position().y.abs() < POSITION_TOL);
assert!((point.position().z - WGS84.semi_minor_axis()).abs() < POSITION_TOL);
}
#[test]
fn longitude_quarter_turn_places_the_point_on_the_second_axis() {
let point = Geodetic::new(0.0, core::f64::consts::FRAC_PI_2, 0.0).to_ecef(WGS84);
assert!(point.position().x.abs() < POSITION_TOL);
assert!((point.position().y - WGS84.semi_major_axis()).abs() < POSITION_TOL);
}
#[test]
fn height_moves_the_point_along_the_ellipsoid_normal() {
let surface = Geodetic::new(0.6, 0.3, 0.0).to_ecef(WGS84);
let elevated = Geodetic::new(0.6, 0.3, 500.0).to_ecef(WGS84);
assert!((surface.position().distance(elevated.position()) - 500.0).abs() < POSITION_TOL);
// The normal at a non-polar latitude is not vertical, so the height
// changes the horizontal components as well as the third one.
assert!(elevated.position().x > surface.position().x);
}
#[test]
fn conversion_round_trips_over_a_global_grid() {
for latitude_degrees in [-89.9_f64, -60.0, -45.0, 0.0, 30.0, 45.0, 60.0, 89.9] {
for longitude_degrees in [-179.0_f64, -90.0, 0.0, 45.0, 90.0, 179.0] {
for height in [-1_000.0_f64, 0.0, 1_000.0, 35_786_000.0] {
let original = Geodetic::new(
latitude_degrees.to_radians(),
longitude_degrees.to_radians(),
height,
);
let ecef = original.to_ecef(WGS84);
let recovered = ecef
.to_geodetic(WGS84)
.expect("off-axis points have a longitude");
assert!(
(recovered.latitude - original.latitude).abs() < ANGLE_TOL,
"latitude mismatch at {latitude_degrees}, {longitude_degrees}, {height}"
);
assert!(
(recovered.longitude - original.longitude).abs() < ANGLE_TOL,
"longitude mismatch at {latitude_degrees}, {longitude_degrees}, {height}"
);
assert!(
(recovered.height - original.height).abs() < POSITION_TOL,
"height mismatch at {latitude_degrees}, {longitude_degrees}, {height}: {} vs {}",
recovered.height,
original.height
);
}
}
}
}
#[test]
fn round_trip_preserves_the_cartesian_position() {
let original = Geodetic::new(0.921, -1.234, 12_345.0).to_ecef(WGS84);
let recovered = original
.to_geodetic(WGS84)
.expect("off-axis points have a longitude")
.to_ecef(WGS84);
assert!(original.position().distance(recovered.position()) < POSITION_TOL);
}
#[test]
fn polar_axis_has_no_defined_longitude() {
let on_axis = Ecef::new(Vec3::new(0.0, 0.0, WGS84.semi_minor_axis()));
assert_eq!(on_axis.to_geodetic(WGS84), None);
// The origin is also on the axis.
assert_eq!(Ecef::new(Vec3::ZERO).to_geodetic(WGS84), None);
}
#[test]
fn non_finite_positions_are_rejected() {
let invalid = Ecef::new(Vec3::new(f64::NAN, 0.0, 6_000_000.0));
assert_eq!(invalid.to_geodetic(WGS84), None);
let infinite = Ecef::new(Vec3::new(f64::INFINITY, 0.0, 0.0));
assert_eq!(infinite.to_geodetic(WGS84), None);
}
#[test]
fn known_point_matches_an_independent_computation() {
// Latitude 37.5 degrees north, longitude 121.333333... degrees west, at
// zero height, evaluated with the defining WGS 84 parameters in
// extended precision.
let point = Geodetic::new(
37.5_f64.to_radians(),
-121.333_333_333_333_33_f64.to_radians(),
0.0,
)
.to_ecef(WGS84);
assert!(
(point.position().x + 2_634_612.181_141_605_6).abs() < 1e-6,
"x = {}",
point.position().x
);
assert!(
(point.position().y + 4_327_502.253_728_065).abs() < 1e-6,
"y = {}",
point.position().y
);
assert!(
(point.position().z - 3_861_564.098_809_606_4).abs() < 1e-6,
"z = {}",
point.position().z
);
}
#[test]
fn validity_is_reported() {
assert!(Geodetic::new(0.5, 0.5, 100.0).is_valid());
assert!(Geodetic::new(core::f64::consts::FRAC_PI_2, 0.0, 0.0).is_valid());
assert!(!Geodetic::new(2.0, 0.0, 0.0).is_valid());
assert!(!Geodetic::new(0.0, 4.0, 0.0).is_valid());
assert!(!Geodetic::new(0.0, 0.0, f64::NAN).is_valid());
}
#[test]
fn ecef_wrapper_exposes_its_vector() {
let vector = Vec3::new(1.0, 2.0, 3.0);
let ecef = Ecef::new(vector);
assert_eq!(ecef.position(), vector);
assert_eq!(ecef, Ecef(vector));
}