use crate::centers::ReferenceCenter;
use crate::ellipsoid::{Ellipsoid, HasEllipsoid};
use crate::frames::ReferenceFrame;
use qtty::angular::Degrees;
use qtty::length::LengthUnit;
use qtty::length::Meters;
use qtty::units::{Meter, Radian};
use qtty::Quantity;
use std::marker::PhantomData;
#[cfg(feature = "serde")]
#[path = "position_serde.rs"]
mod position_serde;
#[derive(Debug, Clone, PartialEq)]
pub struct GeodeticConvergenceError {
pub iterations: u32,
pub last_residual: f64,
}
impl std::fmt::Display for GeodeticConvergenceError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(
f,
"geodetic Bowring iteration failed to converge after {} iterations \
(last latitude residual: {:e} rad)",
self.iterations, self.last_residual
)
}
}
impl std::error::Error for GeodeticConvergenceError {}
#[derive(Debug, Clone, Copy)]
pub struct Position<C: ReferenceCenter, F: ReferenceFrame, U: LengthUnit = Meter> {
pub lon: Degrees,
pub lat: Degrees,
pub height: Quantity<U>,
center_params: C::Params,
_frame: PhantomData<F>,
}
impl<C, F, U> PartialEq for Position<C, F, U>
where
C: ReferenceCenter,
C::Params: PartialEq,
F: ReferenceFrame,
U: LengthUnit,
{
fn eq(&self, other: &Self) -> bool {
self.lon == other.lon
&& self.lat == other.lat
&& self.height == other.height
&& self.center_params == other.center_params
}
}
impl<C, F, U> Position<C, F, U>
where
C: ReferenceCenter,
F: ReferenceFrame,
U: LengthUnit,
{
pub const fn new_raw_with_params(
center_params: C::Params,
lon: Degrees,
lat: Degrees,
height: Quantity<U>,
) -> Self {
Self {
lon,
lat,
height,
center_params,
_frame: PhantomData,
}
}
pub fn new_with_params(
center_params: C::Params,
lon: Degrees,
lat: Degrees,
height: Quantity<U>,
) -> Self {
let (lon_n, lat_n) = super::normalize_lon_lat(lon.value(), lat.value());
Self::new_raw_with_params(
center_params,
Degrees::new(lon_n),
Degrees::new(lat_n),
height,
)
}
#[inline]
pub fn center_params(&self) -> &C::Params {
&self.center_params
}
}
impl<C, F, U> Position<C, F, U>
where
C: ReferenceCenter<Params = ()>,
F: ReferenceFrame,
U: LengthUnit,
{
pub const fn new_raw(lon: Degrees, lat: Degrees, height: Quantity<U>) -> Self {
Self::new_raw_with_params((), lon, lat, height)
}
pub fn new(lon: Degrees, lat: Degrees, height: Quantity<U>) -> Self {
Self::new_with_params((), lon, lat, height)
}
pub fn try_new(
lon: Degrees,
lat: Degrees,
height: Quantity<U>,
) -> Result<Self, core::convert::Infallible> {
Ok(Self::new(lon, lat, height))
}
pub const ORIGIN: Self = Self::new_raw(
Degrees::new(0.0),
Degrees::new(0.0),
Quantity::<U>::new(0.0),
);
}
impl<C, F, U> Position<C, F, U>
where
C: ReferenceCenter,
F: ReferenceFrame + HasEllipsoid,
U: LengthUnit,
{
pub fn to_cartesian<TargetU: LengthUnit>(&self) -> crate::cartesian::Position<C, F, TargetU>
where
C::Params: Clone,
{
let a = <F::Ellipsoid as Ellipsoid>::A;
let e2 = <F::Ellipsoid>::e2();
let lat_rad = self.lat.to::<Radian>();
let lon_rad = self.lon.to::<Radian>();
let h: Meters = self.height.to::<Meter>();
let (sin_lat, cos_lat) = lat_rad.sin_cos();
let (sin_lon, cos_lon) = lon_rad.sin_cos();
let n = Meters::new(a / (1.0 - e2 * sin_lat * sin_lat).sqrt());
let x_m = (n + h) * cos_lat * cos_lon;
let y_m = (n + h) * cos_lat * sin_lon;
let z_m = (n * (1.0 - e2) + h) * sin_lat;
crate::cartesian::Position::<C, F, TargetU>::new_with_params(
self.center_params.clone(),
x_m.to::<TargetU>(),
y_m.to::<TargetU>(),
z_m.to::<TargetU>(),
)
}
pub fn from_cartesian<SourceU: LengthUnit>(
pos: &crate::cartesian::Position<C, F, SourceU>,
) -> Self
where
C::Params: Clone,
{
Self::try_from_cartesian(pos).unwrap_or_else(|_| Self::from_cartesian_best_effort(pos))
}
pub fn try_from_cartesian<SourceU: LengthUnit>(
pos: &crate::cartesian::Position<C, F, SourceU>,
) -> Result<Self, GeodeticConvergenceError>
where
C::Params: Clone,
{
const MAX_ITERATIONS: u32 = 5;
const CONVERGENCE_THRESHOLD: f64 = 1e-14;
let a = <F::Ellipsoid as Ellipsoid>::A;
let e2 = <F::Ellipsoid>::e2();
let b = <F::Ellipsoid>::b();
let x = pos.x().to::<Meter>().value();
let y = pos.y().to::<Meter>().value();
let z = pos.z().to::<Meter>().value();
let lon_rad = y.atan2(x);
let p = (x * x + y * y).sqrt();
let ep2 = (a * a - b * b) / (b * b);
let theta = (z * a).atan2(p * b);
let sin_theta = theta.sin();
let cos_theta = theta.cos();
let mut lat = (z + ep2 * b * sin_theta * sin_theta * sin_theta)
.atan2(p - e2 * a * cos_theta * cos_theta * cos_theta);
let mut last_residual = f64::INFINITY;
let mut iterations: u32 = 0;
for _ in 0..MAX_ITERATIONS {
iterations += 1;
let sin_lat = lat.sin();
let new_lat = {
let n = a / (1.0 - e2 * sin_lat * sin_lat).sqrt();
(z + e2 * n * sin_lat).atan2(p)
};
last_residual = (new_lat - lat).abs();
lat = new_lat;
if last_residual < CONVERGENCE_THRESHOLD {
break;
}
}
if last_residual >= CONVERGENCE_THRESHOLD {
return Err(GeodeticConvergenceError {
iterations,
last_residual,
});
}
let sin_lat = lat.sin();
let cos_lat = lat.cos();
let n = a / (1.0 - e2 * sin_lat * sin_lat).sqrt();
let h = if cos_lat.abs() > 1e-10 {
p / cos_lat - n
} else {
z / sin_lat - n * (1.0 - e2)
};
Ok(Self::new_with_params(
pos.center_params().clone(),
Degrees::new(lon_rad.to_degrees()),
Degrees::new(lat.to_degrees()),
Meters::new(h).to::<U>(),
))
}
fn from_cartesian_best_effort<SourceU: LengthUnit>(
pos: &crate::cartesian::Position<C, F, SourceU>,
) -> Self
where
C::Params: Clone,
{
let a = <F::Ellipsoid as Ellipsoid>::A;
let e2 = <F::Ellipsoid>::e2();
let b = <F::Ellipsoid>::b();
let x = pos.x().to::<Meter>().value();
let y = pos.y().to::<Meter>().value();
let z = pos.z().to::<Meter>().value();
let lon_rad = y.atan2(x);
let p = (x * x + y * y).sqrt();
let ep2 = (a * a - b * b) / (b * b);
let theta = (z * a).atan2(p * b);
let sin_theta = theta.sin();
let cos_theta = theta.cos();
let mut lat = (z + ep2 * b * sin_theta * sin_theta * sin_theta)
.atan2(p - e2 * a * cos_theta * cos_theta * cos_theta);
for _ in 0..5 {
let sin_lat = lat.sin();
let n = a / (1.0 - e2 * sin_lat * sin_lat).sqrt();
lat = (z + e2 * n * sin_lat).atan2(p);
}
let sin_lat = lat.sin();
let cos_lat = lat.cos();
let n = a / (1.0 - e2 * sin_lat * sin_lat).sqrt();
let h = if cos_lat.abs() > 1e-10 {
p / cos_lat - n
} else {
z / sin_lat - n * (1.0 - e2)
};
Self::new_with_params(
pos.center_params().clone(),
Degrees::new(lon_rad.to_degrees()),
Degrees::new(lat.to_degrees()),
Meters::new(h).to::<U>(),
)
}
}
impl<C, F> Default for Position<C, F, Meter>
where
C: ReferenceCenter<Params = ()>,
F: ReferenceFrame,
{
fn default() -> Self {
Self::new_raw(
Degrees::new(0.0),
Degrees::new(0.0),
Quantity::<Meter>::new(0.0),
)
}
}
impl_quantity_fmt_triplet! {
impl[C, F, U] for Position<C, F, U>
where {
C: ReferenceCenter,
F: ReferenceFrame,
U: LengthUnit,
},
fmt_each: { Quantity<U>, },
|this, f, FmtOne| {
write!(
f,
"Center: {}, Frame: {}, lon: ",
C::center_name(),
F::frame_name()
)?;
FmtOne::fmt(&this.lon, f)?;
write!(f, ", lat: ")?;
FmtOne::fmt(&this.lat, f)?;
write!(f, ", h: ")?;
FmtOne::fmt(&this.height, f)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::{DeriveReferenceCenter as ReferenceCenter, DeriveReferenceFrame as ReferenceFrame};
#[allow(unused_imports)]
use qtty::angular::{Degrees, Radians};
#[allow(unused_imports)]
use qtty::length::{Kilometers, Meters};
use qtty::units::{Kilometer, Meter};
use qtty::{DEG, M};
#[derive(Debug, Copy, Clone, ReferenceFrame)]
struct TestFrame;
#[derive(Debug, Copy, Clone, ReferenceCenter)]
struct TestCenter;
#[derive(Clone, Debug, Default, PartialEq)]
struct TestParams {
id: i32,
}
#[derive(Debug, Copy, Clone, ReferenceCenter)]
#[center(params = TestParams)]
struct ParamCenter;
#[test]
fn new_stores_values() {
let p = Position::<TestCenter, TestFrame, Meter>::new(10.0 * DEG, 20.0 * DEG, 100.0 * M);
assert_eq!(p.lon, 10.0 * DEG);
assert_eq!(p.lat, 20.0 * DEG);
assert_eq!(p.height, 100.0 * M);
}
#[test]
fn new_normalises_angles() {
let p = Position::<TestCenter, TestFrame, Meter>::new(540.0 * DEG, 100.0 * DEG, 1.0 * M);
assert_eq!(p.lon, 0.0 * DEG);
assert_eq!(p.lat, 80.0 * DEG);
}
#[test]
fn new_raw_does_not_normalise() {
let p =
Position::<TestCenter, TestFrame, Meter>::new_raw(540.0 * DEG, 100.0 * DEG, 1.0 * M);
assert_eq!(p.lon, 540.0 * DEG);
assert_eq!(p.lat, 100.0 * DEG);
}
#[test]
fn default_is_origin() {
let d = Position::<TestCenter, TestFrame, Meter>::default();
assert_eq!(d.lon, 0.0 * DEG);
assert_eq!(d.lat, 0.0 * DEG);
assert_eq!(d.height, 0.0 * M);
}
#[test]
fn equality_and_inequality() {
let a = Position::<TestCenter, TestFrame, Meter>::new(1.0 * DEG, 2.0 * DEG, 3.0 * M);
let b = Position::<TestCenter, TestFrame, Meter>::new(1.0 * DEG, 2.0 * DEG, 3.0 * M);
let c = Position::<TestCenter, TestFrame, Meter>::new(1.0 * DEG, 2.0 * DEG, 4.0 * M);
assert_eq!(a, b);
assert_ne!(a, c);
}
#[test]
fn generic_height_unit() {
let p = Position::<TestCenter, TestFrame, Kilometer>::new(
0.0 * DEG,
0.0 * DEG,
Kilometers::new(10.0),
);
assert_eq!(p.height, Kilometers::new(10.0));
}
#[test]
fn with_center_params() {
let params = TestParams { id: 42 };
let p = Position::<ParamCenter, TestFrame, Meter>::new_raw_with_params(
params.clone(),
5.0 * DEG,
10.0 * DEG,
2.0 * M,
);
assert_eq!(p.center_params(), ¶ms);
}
#[test]
fn try_new_is_infallible() {
let p = Position::<TestCenter, TestFrame, Meter>::try_new(0.0 * DEG, 271.0 * DEG, 0.0 * M)
.unwrap();
assert_eq!(p.lat, -89.0 * DEG);
}
#[test]
fn display() {
let p = Position::<TestCenter, TestFrame, Meter>::new(10.0 * DEG, 20.0 * DEG, 100.0 * M);
let s = p.to_string();
assert!(s.contains("TestCenter"));
assert!(s.contains("TestFrame"));
let s_prec = format!("{:.2}", p);
let expected_h_prec = format!("{:.2}", p.height);
assert!(s_prec.contains(&format!("h: {expected_h_prec}")));
let s_exp = format!("{:.3e}", p);
let expected_lon_exp = format!("{:.3e}", p.lon);
assert!(s_exp.contains(&format!("lon: {expected_lon_exp}")));
}
#[derive(Debug, Copy, Clone, ReferenceFrame)]
struct TestEllipsoidFrame;
impl crate::ellipsoid::HasEllipsoid for TestEllipsoidFrame {
type Ellipsoid = crate::ellipsoid::Wgs84;
}
#[test]
fn try_from_cartesian_round_trip_converges() {
let original = Position::<TestCenter, TestEllipsoidFrame, Meter>::new(
-17.89 * DEG,
28.76 * DEG,
2200.0 * M,
);
let cart = original.to_cartesian::<Meter>();
let back = Position::<TestCenter, TestEllipsoidFrame, Meter>::try_from_cartesian(&cart)
.expect("Bowring iteration should converge for a well-defined point");
let max_angle_err_deg = 1.0e-10;
assert!(
(back.lon.value() - original.lon.value()).abs() < max_angle_err_deg,
"longitude residual {} exceeds tolerance",
(back.lon.value() - original.lon.value()).abs()
);
assert!(
(back.lat.value() - original.lat.value()).abs() < max_angle_err_deg,
"latitude residual {} exceeds tolerance",
(back.lat.value() - original.lat.value()).abs()
);
assert!(
(back.height.value() - original.height.value()).abs() < 1.0e-6,
"height residual {} m exceeds tolerance",
(back.height.value() - original.height.value()).abs()
);
}
#[test]
fn geodetic_convergence_error_display_mentions_residual() {
let err = GeodeticConvergenceError {
iterations: 5,
last_residual: 1.5e-10,
};
let msg = format!("{err}");
assert!(msg.contains("5"));
assert!(msg.contains("1.5e-10") || msg.contains("1.5e-10"));
}
}