"""
Geodetic calculations for navigation and tracking.
This module provides geodetic utilities including:
- Geodetic to ECEF conversions
- Direct and inverse geodetic problems
- Local tangent plane (ENU/NED) conversions
- Earth ellipsoid parameters
"""
import logging
from functools import lru_cache
from typing import Any, NamedTuple, Tuple
import numpy as np
from numpy.typing import ArrayLike, NDArray
from pytcl.coordinate_systems.conversions.geodetic import (
ecef2enu,
ecef2geodetic,
enu2ecef,
geodetic2ecef,
)
from pytcl.core.exceptions import ConvergenceError
# Module logger
_logger = logging.getLogger("pytcl.navigation.geodesy")
# Cache configuration for Vincenty geodetic calculations
_VINCENTY_CACHE_DECIMALS = 10 # 1e-10 rad ~ 0.64 mm on Earth (not 0.01 mm)
_VINCENTY_CACHE_MAXSIZE = 128 # Max cached coordinate pairs
[docs]
class Ellipsoid(NamedTuple):
"""
Earth ellipsoid parameters.
Attributes
----------
a : float
Semi-major axis (equatorial radius) in meters.
f : float
Flattening (a-b)/a.
"""
a: float
f: float
@property
def b(self) -> float:
"""Semi-minor axis (polar radius) in meters."""
return self.a * (1 - self.f)
@property
def e2(self) -> float:
"""First eccentricity squared."""
return self.f * (2 - self.f)
@property
def ep2(self) -> float:
"""Second eccentricity squared."""
return self.e2 / (1 - self.e2)
# Standard ellipsoids
WGS84 = Ellipsoid(a=6378137.0, f=1.0 / 298.257223563)
GRS80 = Ellipsoid(a=6378137.0, f=1.0 / 298.257222101)
SPHERE = Ellipsoid(a=6371000.0, f=0.0)
def _quantize_geodetic(val: float) -> float:
"""Quantize geodetic coordinate for cache key compatibility."""
return round(val, _VINCENTY_CACHE_DECIMALS)
@lru_cache(maxsize=_VINCENTY_CACHE_MAXSIZE)
def _inverse_geodetic_cached(
lat1_q: float,
lon1_q: float,
lat2_q: float,
lon2_q: float,
a: float,
f: float,
) -> Tuple[float, float, float]:
"""Cached Vincenty inverse geodetic computation (internal).
Returns (distance, azimuth1, azimuth2).
"""
b = a * (1 - f)
# Reduced latitudes
U1 = np.arctan((1 - f) * np.tan(lat1_q))
U2 = np.arctan((1 - f) * np.tan(lat2_q))
sin_U1, cos_U1 = np.sin(U1), np.cos(U1)
sin_U2, cos_U2 = np.sin(U2), np.cos(U2)
L = lon2_q - lon1_q
lam = L
converged = False
for _ in range(100):
sin_lam = np.sin(lam)
cos_lam = np.cos(lam)
sin_sigma = np.sqrt(
(cos_U2 * sin_lam) ** 2 + (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lam) ** 2
)
if sin_sigma == 0:
# Coincident points
return 0.0, 0.0, 0.0
cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lam
sigma = np.arctan2(sin_sigma, cos_sigma)
sin_alpha = cos_U1 * cos_U2 * sin_lam / sin_sigma
cos2_alpha = 1 - sin_alpha**2
if cos2_alpha == 0:
cos_2sigma_m = 0
else:
cos_2sigma_m = cos_sigma - 2 * sin_U1 * sin_U2 / cos2_alpha
C = f / 16 * cos2_alpha * (4 + f * (4 - 3 * cos2_alpha))
lam_new = L + (1 - C) * f * sin_alpha * (
sigma
+ C
* sin_sigma
* (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
)
if abs(lam_new - lam) < 1e-12:
converged = True
break
lam = lam_new
if not converged:
# Vincenty's lambda iteration fails to converge for nearly
# antipodal points -- and the partial result is silently wrong
# (measured 3.7 km / 22 degrees at (0, 0) -> (0.5, 179.7) deg).
# Karney's algorithm (geographiclib, the `geodesy` extra)
# converges everywhere; use it when available, fail loudly
# otherwise.
try:
from geographiclib.geodesic import Geodesic
except ImportError:
raise ConvergenceError(
"Vincenty's inverse iteration did not converge (nearly "
"antipodal points). Install the `geodesy` extra for the "
"geographiclib fallback, which converges for all point "
"pairs."
) from None
g = Geodesic(a, f).Inverse(
np.degrees(lat1_q),
np.degrees(lon1_q),
np.degrees(lat2_q),
np.degrees(lon2_q),
)
return (
float(g["s12"]),
float(np.radians(g["azi1"])),
float(np.radians(g["azi2"])),
)
u2 = cos2_alpha * (a**2 - b**2) / b**2
A = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))
B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)))
delta_sigma = (
B
* sin_sigma
* (
cos_2sigma_m
+ B
/ 4
* (
cos_sigma * (-1 + 2 * cos_2sigma_m**2)
- B
/ 6
* cos_2sigma_m
* (-3 + 4 * sin_sigma**2)
* (-3 + 4 * cos_2sigma_m**2)
)
)
)
distance = b * A * (sigma - delta_sigma)
# Azimuths
azimuth1 = np.arctan2(cos_U2 * sin_lam, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lam)
azimuth2 = np.arctan2(
cos_U1 * sin_lam, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lam
)
return float(distance), float(azimuth1), float(azimuth2)
@lru_cache(maxsize=_VINCENTY_CACHE_MAXSIZE)
def _direct_geodetic_cached(
lat1_q: float,
lon1_q: float,
azimuth_q: float,
distance_q: float,
a: float,
f: float,
) -> Tuple[float, float, float]:
"""Cached Vincenty direct geodetic computation (internal).
Returns (lat2, lon2, azimuth2).
"""
b = a * (1 - f)
sin_alpha1 = np.sin(azimuth_q)
cos_alpha1 = np.cos(azimuth_q)
# Reduced latitude
tan_U1 = (1 - f) * np.tan(lat1_q)
cos_U1 = 1.0 / np.sqrt(1 + tan_U1**2)
sin_U1 = tan_U1 * cos_U1
sigma1 = np.arctan2(tan_U1, cos_alpha1)
sin_alpha = cos_U1 * sin_alpha1
cos2_alpha = 1 - sin_alpha**2
u2 = cos2_alpha * (a**2 - b**2) / b**2
A = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))
B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)))
sigma = distance_q / (b * A)
for _ in range(100):
cos_2sigma_m = np.cos(2 * sigma1 + sigma)
sin_sigma = np.sin(sigma)
cos_sigma = np.cos(sigma)
delta_sigma = (
B
* sin_sigma
* (
cos_2sigma_m
+ B
/ 4
* (
cos_sigma * (-1 + 2 * cos_2sigma_m**2)
- B
/ 6
* cos_2sigma_m
* (-3 + 4 * sin_sigma**2)
* (-3 + 4 * cos_2sigma_m**2)
)
)
)
sigma_new = distance_q / (b * A) + delta_sigma
if abs(sigma_new - sigma) < 1e-12:
break
sigma = sigma_new
cos_2sigma_m = np.cos(2 * sigma1 + sigma)
sin_sigma = np.sin(sigma)
cos_sigma = np.cos(sigma)
sin_U2 = sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1
lat2 = np.arctan2(
sin_U2,
(1 - f)
* np.sqrt(
sin_alpha**2 + (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1) ** 2
),
)
lam = np.arctan2(
sin_sigma * sin_alpha1, cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
)
C = f / 16 * cos2_alpha * (4 + f * (4 - 3 * cos2_alpha))
L = lam - (1 - C) * f * sin_alpha * (
sigma
+ C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
)
lon2 = lon1_q + L
# Back azimuth
azimuth2 = np.arctan2(
sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
)
return float(lat2), float(lon2), float(azimuth2)
def _shaped(component: ArrayLike, shape: tuple) -> NDArray[np.float64]:
"""Reshape a delegated result component to the caller's input shape,
collapsing to a numpy scalar for scalar input (the historical return
type of these wrappers)."""
arr = np.asarray(component, dtype=np.float64).reshape(shape)
return arr[()] if shape == () else arr
[docs]
def geodetic_to_ecef(
lat: ArrayLike,
lon: ArrayLike,
alt: ArrayLike,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
"""
Convert geodetic coordinates to ECEF (Earth-Centered, Earth-Fixed).
Parameters
----------
lat : array_like
Geodetic latitude in radians.
lon : array_like
Geodetic longitude in radians.
alt : array_like
Altitude above ellipsoid in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
x, y, z : ndarray
ECEF coordinates in meters.
Examples
--------
>>> import numpy as np
>>> # Philadelphia (40°N, 75°W) at 100m altitude
>>> lat, lon, alt = np.radians(40.0), np.radians(-75.0), 100.0
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
>>> round(x / 1e6, 4) # ~1.27 million meters
1.2663
>>> # Equator at prime meridian
>>> x, y, z = geodetic_to_ecef(0.0, 0.0, 0.0)
>>> x # Semi-major axis (equatorial radius)
6378137.0
>>> y, z
(0.0, 0.0)
"""
lat = np.asarray(lat, dtype=np.float64)
lon = np.asarray(lon, dtype=np.float64)
alt = np.asarray(alt, dtype=np.float64)
shape = np.broadcast(lat, lon, alt).shape
lat_b, lon_b, alt_b = np.broadcast_arrays(lat, lon, alt)
ecef = geodetic2ecef(lat_b, lon_b, alt_b, ellipsoid.a, ellipsoid.f)
return _shaped(ecef[0], shape), _shaped(ecef[1], shape), _shaped(ecef[2], shape)
[docs]
class OsculatingSphere(NamedTuple):
"""
A local spherical approximation to the reference ellipsoid.
Attributes
----------
radius : float
Radius of the osculating sphere in meters.
center : ndarray
Offset of the sphere's center from the ellipsoid's center in ECEF
coordinates, shape (3,). Subtract this from ECEF positions to work
in the sphere-centered frame.
"""
radius: float
center: NDArray[np.float64]
[docs]
def osculating_sphere(
lat: float,
lon: float,
ellipsoid: Ellipsoid = WGS84,
) -> OsculatingSphere:
"""
Osculating sphere of the reference ellipsoid at a surface point.
The sphere has the Gaussian radius of curvature ``sqrt(M * N)`` (the
geometric mean of the meridian and prime-vertical radii of curvature)
and is positioned to touch the ellipsoid at the given latitude and
longitude. Used as the local spherical-Earth approximation in the
standard-exponential-model refraction routines.
Port of ``osculatingSpher4LatLon.m`` (the default 3D, no-geoid path).
Parameters
----------
lat : float
Geodetic latitude in radians.
lon : float
Longitude in radians.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
sphere : OsculatingSphere
The radius and the ECEF offset of the sphere's center.
Examples
--------
>>> sph = osculating_sphere(0.7, 0.1)
>>> print(f"{sph.radius:.3f}")
6374462.341
"""
a = ellipsoid.a
e2 = ellipsoid.e2
denom = np.sqrt(1 - e2 * np.sin(lat) ** 2)
# Radii of curvature in the meridian and the prime vertical.
m0 = a * (1 - e2) / denom**3
n0 = a / denom
radius = float(np.sqrt(m0 * n0))
x, y, z = geodetic_to_ecef(lat, lon, 0.0, ellipsoid)
on_ellipsoid = np.array([float(x), float(y), float(z)])
on_sphere = radius * np.array(
[
np.cos(lat) * np.cos(lon),
np.cos(lat) * np.sin(lon),
np.sin(lat),
]
)
return OsculatingSphere(radius, on_ellipsoid - on_sphere)
[docs]
def ecef_to_geodetic(
x: ArrayLike,
y: ArrayLike,
z: ArrayLike,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
"""
Convert ECEF coordinates to geodetic.
Parameters
----------
x, y, z : array_like
ECEF coordinates in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
lat : ndarray
Geodetic latitude in radians.
lon : ndarray
Geodetic longitude in radians.
alt : ndarray
Altitude above ellipsoid in meters.
Examples
--------
>>> import numpy as np
>>> # Point on equator at prime meridian
>>> lat, lon, alt = ecef_to_geodetic(6378137.0, 0.0, 0.0)
>>> np.degrees(lat), np.degrees(lon), float(alt)
(0.0, 0.0, 0.0)
>>> # Round-trip conversion
>>> x, y, z = geodetic_to_ecef(np.radians(45.0), np.radians(90.0), 1000.0)
>>> lat2, lon2, alt2 = ecef_to_geodetic(x, y, z)
>>> round(np.degrees(lat2), 6), round(np.degrees(lon2), 6), round(float(alt2), 3)
(45.0, 90.0, 1000.0)
Notes
-----
Uses Bowring's iterative algorithm for robust conversion.
References
----------
- Bowring, B.R., "Transformation from spatial to geographical
coordinates", Survey Review, 1976.
"""
x = np.asarray(x, dtype=np.float64)
y = np.asarray(y, dtype=np.float64)
z = np.asarray(z, dtype=np.float64)
shape = np.broadcast(x, y, z).shape
x_b, y_b, z_b = np.broadcast_arrays(x, y, z)
ecef = np.array([x_b.ravel(), y_b.ravel(), z_b.ravel()])
lat, lon, alt = ecef2geodetic(ecef, ellipsoid.a, ellipsoid.f)
return _shaped(lat, shape), _shaped(lon, shape), _shaped(alt, shape)
[docs]
def ecef_to_enu(
x: ArrayLike,
y: ArrayLike,
z: ArrayLike,
lat_ref: float,
lon_ref: float,
alt_ref: float,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
"""
Convert ECEF to local ENU (East-North-Up) coordinates.
Parameters
----------
x, y, z : array_like
ECEF coordinates in meters.
lat_ref : float
Reference latitude in radians.
lon_ref : float
Reference longitude in radians.
alt_ref : float
Reference altitude in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
east, north, up : ndarray
ENU coordinates in meters relative to reference point.
Examples
--------
>>> import numpy as np
>>> # Reference point: Philadelphia
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # Target point slightly east
>>> lat, lon, alt = np.radians(40.0), np.radians(-74.99), 0.0
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
>>> round(e, 1) # East displacement in meters
853.9
>>> abs(n) < 10 # North displacement should be ~0
True
>>> abs(u) < 10 # Up displacement should be ~0
True
"""
x = np.asarray(x, dtype=np.float64)
y = np.asarray(y, dtype=np.float64)
z = np.asarray(z, dtype=np.float64)
shape = np.broadcast(x, y, z).shape
ecef_ref = geodetic2ecef(lat_ref, lon_ref, alt_ref, ellipsoid.a, ellipsoid.f)
x_b, y_b, z_b = np.broadcast_arrays(x, y, z)
ecef = np.array([x_b.ravel(), y_b.ravel(), z_b.ravel()])
enu = ecef2enu(ecef, lat_ref, lon_ref, ecef_ref=ecef_ref)
return _shaped(enu[0], shape), _shaped(enu[1], shape), _shaped(enu[2], shape)
[docs]
def enu_to_ecef(
east: ArrayLike,
north: ArrayLike,
up: ArrayLike,
lat_ref: float,
lon_ref: float,
alt_ref: float,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
"""
Convert local ENU to ECEF coordinates.
Parameters
----------
east, north, up : array_like
ENU coordinates in meters relative to reference point.
lat_ref : float
Reference latitude in radians.
lon_ref : float
Reference longitude in radians.
alt_ref : float
Reference altitude in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
x, y, z : ndarray
ECEF coordinates in meters.
Examples
--------
>>> import numpy as np
>>> # Reference point
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # 1 km east, 500 m north, 100 m up
>>> x, y, z = enu_to_ecef(1000.0, 500.0, 100.0, lat_ref, lon_ref, alt_ref)
>>> # Convert back to verify
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
>>> round(e, 3), round(n, 3), round(u, 3)
(1000.0, 500.0, 100.0)
"""
east = np.asarray(east, dtype=np.float64)
north = np.asarray(north, dtype=np.float64)
up = np.asarray(up, dtype=np.float64)
shape = np.broadcast(east, north, up).shape
ecef_ref = geodetic2ecef(lat_ref, lon_ref, alt_ref, ellipsoid.a, ellipsoid.f)
e_b, n_b, u_b = np.broadcast_arrays(east, north, up)
enu = np.array([e_b.ravel(), n_b.ravel(), u_b.ravel()])
ecef = enu2ecef(enu, lat_ref, lon_ref, ecef_ref=ecef_ref)
return _shaped(ecef[0], shape), _shaped(ecef[1], shape), _shaped(ecef[2], shape)
[docs]
def ecef_to_ned(
x: ArrayLike,
y: ArrayLike,
z: ArrayLike,
lat_ref: float,
lon_ref: float,
alt_ref: float,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
"""
Convert ECEF to local NED (North-East-Down) coordinates.
Parameters
----------
x, y, z : array_like
ECEF coordinates in meters.
lat_ref : float
Reference latitude in radians.
lon_ref : float
Reference longitude in radians.
alt_ref : float
Reference altitude in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
north, east, down : ndarray
NED coordinates in meters relative to reference point.
Examples
--------
>>> import numpy as np
>>> # Reference point
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # Target above reference
>>> x, y, z = geodetic_to_ecef(lat_ref, lon_ref, 1000.0) # 1km above
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
>>> abs(n) < 1, abs(e) < 1, d # Should be ~0, ~0, -1000
(True, True, -1000.0...)
"""
east, north, up = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref, ellipsoid)
return north, east, -up
[docs]
def ned_to_ecef(
north: ArrayLike,
east: ArrayLike,
down: ArrayLike,
lat_ref: float,
lon_ref: float,
alt_ref: float,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
"""
Convert local NED to ECEF coordinates.
Parameters
----------
north, east, down : array_like
NED coordinates in meters relative to reference point.
lat_ref : float
Reference latitude in radians.
lon_ref : float
Reference longitude in radians.
alt_ref : float
Reference altitude in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
x, y, z : ndarray
ECEF coordinates in meters.
Examples
--------
>>> import numpy as np
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
>>> # 100m north, 50m east, 10m down
>>> x, y, z = ned_to_ecef(100.0, 50.0, 10.0, lat_ref, lon_ref, alt_ref)
>>> # Verify round-trip
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
>>> n, e, d
(100.0..., 50.0..., 10.0...)
"""
return enu_to_ecef(
east, north, -np.asarray(down), lat_ref, lon_ref, alt_ref, ellipsoid
)
[docs]
def direct_geodetic(
lat1: float,
lon1: float,
azimuth: float,
distance: float,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[float, float, float]:
"""
Solve the direct geodetic problem (Vincenty).
Given a starting point, azimuth, and distance, find the destination point.
Results are cached for repeated queries with the same parameters.
Parameters
----------
lat1 : float
Starting latitude in radians.
lon1 : float
Starting longitude in radians.
azimuth : float
Forward azimuth in radians (from north, clockwise).
distance : float
Distance in meters.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
lat2 : float
Destination latitude in radians.
lon2 : float
Destination longitude in radians.
azimuth2 : float
Back azimuth at destination in radians.
Examples
--------
>>> import numpy as np
>>> # From New York, travel 1000 km northeast
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
>>> azimuth = np.radians(45) # Northeast
>>> distance = 1_000_000 # 1000 km
>>> lat2, lon2, az2 = direct_geodetic(lat1, lon1, azimuth, distance)
>>> round(np.degrees(lat2), 4), round(np.degrees(lon2), 4) # Destination
(46.7148, -64.7496)
References
----------
- Vincenty, T., "Direct and Inverse Solutions of Geodesics on the
Ellipsoid with Application of Nested Equations", Survey Review, 1975.
"""
return _direct_geodetic_cached(
_quantize_geodetic(lat1),
_quantize_geodetic(lon1),
_quantize_geodetic(azimuth),
round(distance, 3), # 1mm precision for distance
ellipsoid.a,
ellipsoid.f,
)
[docs]
def inverse_geodetic(
lat1: float,
lon1: float,
lat2: float,
lon2: float,
ellipsoid: Ellipsoid = WGS84,
) -> Tuple[float, float, float]:
"""
Solve the inverse geodetic problem (Vincenty).
Given two points, find the distance and azimuths between them.
Results are cached for repeated queries with the same coordinates.
Parameters
----------
lat1 : float
Starting latitude in radians.
lon1 : float
Starting longitude in radians.
lat2 : float
Destination latitude in radians.
lon2 : float
Destination longitude in radians.
ellipsoid : Ellipsoid, optional
Reference ellipsoid (default: WGS84).
Returns
-------
distance : float
Geodesic distance in meters.
azimuth1 : float
Forward azimuth at start in radians.
azimuth2 : float
Back azimuth at destination in radians.
Examples
--------
>>> import numpy as np
>>> # Distance from New York to London
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060) # NYC
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278) # London
>>> dist, az1, az2 = inverse_geodetic(lat1, lon1, lat2, lon2)
>>> round(dist / 1000, 1) # Geodesic distance in km
5585.2
>>> np.degrees(az1) # Initial heading from NYC
51.2...
Notes
-----
For nearly antipodal points, where Vincenty's iteration does not
converge, the computation falls back to geographiclib's Karney
algorithm (the ``geodesy`` extra) or raises ``ConvergenceError``
when it is not installed -- it never returns a silently wrong
result (the unconverged partial answer was measured 3.7 km / 22
degrees off).
References
----------
- Vincenty, T., "Direct and Inverse Solutions of Geodesics on the
Ellipsoid with Application of Nested Equations", Survey Review, 1975.
"""
return _inverse_geodetic_cached(
_quantize_geodetic(lat1),
_quantize_geodetic(lon1),
_quantize_geodetic(lat2),
_quantize_geodetic(lon2),
ellipsoid.a,
ellipsoid.f,
)
[docs]
def haversine_distance(
lat1: float,
lon1: float,
lat2: float,
lon2: float,
radius: float = 6371000.0,
) -> float:
"""
Compute great-circle distance using the haversine formula.
Parameters
----------
lat1, lon1 : float
First point coordinates in radians.
lat2, lon2 : float
Second point coordinates in radians.
radius : float, optional
Earth radius in meters (default: 6371 km).
Returns
-------
float
Great-circle distance in meters.
Examples
--------
>>> import numpy as np
>>> # Distance from equator to 45°N along prime meridian
>>> lat1, lon1 = 0.0, 0.0
>>> lat2, lon2 = np.radians(45.0), 0.0
>>> dist = haversine_distance(lat1, lon1, lat2, lon2)
>>> dist / 1000 # ~5000 km
5003...
>>> # Same point -> 0 distance
>>> haversine_distance(0.0, 0.0, 0.0, 0.0)
0.0
Notes
-----
This is a spherical approximation. For higher accuracy on an ellipsoid,
use inverse_geodetic().
"""
dlat = lat2 - lat1
dlon = lon2 - lon1
a = np.sin(dlat / 2) ** 2 + np.cos(lat1) * np.cos(lat2) * np.sin(dlon / 2) ** 2
c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
return radius * c
[docs]
def clear_geodesy_cache() -> None:
"""Clear all geodesy computation caches.
This can be useful to free memory after processing large datasets
or when cache statistics are being monitored.
"""
_inverse_geodetic_cached.cache_clear()
_direct_geodetic_cached.cache_clear()
_logger.debug("Geodesy caches cleared")
[docs]
def get_geodesy_cache_info() -> dict[str, Any]:
"""Get cache statistics for geodesy computations.
Returns
-------
dict[str, Any]
Dictionary with cache statistics for inverse and direct geodetic caches.
"""
return {
"inverse_geodetic": _inverse_geodetic_cached.cache_info()._asdict(),
"direct_geodetic": _direct_geodetic_cached.cache_info()._asdict(),
}
__all__ = [
# Ellipsoids
"Ellipsoid",
"WGS84",
"GRS80",
"SPHERE",
# Coordinate conversions
"geodetic_to_ecef",
"ecef_to_geodetic",
"ecef_to_enu",
"enu_to_ecef",
"ecef_to_ned",
"ned_to_ecef",
# Geodetic problems
"direct_geodetic",
"inverse_geodetic",
"haversine_distance",
"OsculatingSphere",
"osculating_sphere",
# Cache management
"clear_geodesy_cache",
"get_geodesy_cache_info",
]