diff --git a/src/pymap3d/__init__.py b/src/pymap3d/__init__.py index 3108384..be09caf 100644 --- a/src/pymap3d/__init__.py +++ b/src/pymap3d/__init__.py @@ -88,7 +88,17 @@ geoc2geod, ) -from .nvector import geodetic2nvector, nvector2geodetic, ecef2nvector, nvector2ecef +from .nvector import ( + geodetic2nvector, + nvector2geodetic, + ecef2nvector, + nvector2ecef, + nvector_distance, + nvector_interpolate, + nvector_mean, + nvector_cross_track_distance, + nvector_intersection, +) from .rcurve import parallel, meridian, transverse, geocentric_radius @@ -158,11 +168,23 @@ "dca2geodetic", "aer2dca", "dca2aer", + "nvector_distance", + "nvector_interpolate", + "nvector_mean", + "nvector_cross_track_distance", + "nvector_intersection", ] from .aer import aer2eci, eci2aer from .azelradec import azel2radec, radec2azel -from .eci import ecef2eci, eci2ecef - -__all__ += ["aer2eci", "eci2aer", "ecef2eci", "eci2ecef"] +from .eci import ecef2eci, ecef2eci_state, eci2ecef, eci2ecef_state + +__all__ += [ + "aer2eci", + "eci2aer", + "ecef2eci", + "eci2ecef", + "ecef2eci_state", + "eci2ecef_state", +] diff --git a/src/pymap3d/aer.py b/src/pymap3d/aer.py index 017b741..00f64ee 100644 --- a/src/pymap3d/aer.py +++ b/src/pymap3d/aer.py @@ -170,9 +170,23 @@ def aer2geodetic( return ecef2geodetic(x, y, z, ell=ell, deg=deg) -def eci2aer(x, y, z, lat0, lon0, h0, t: datetime, *, deg: bool = True) -> tuple: +def eci2aer( + x, + y, + z, + lat0, + lon0, + h0, + t: datetime, + *, + deg: bool = True, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: """ - takes Earth Centered Inertial x,y,z ECI coordinates of point and gives az, el, slant range from Observer + takes Earth Centered Inertial x,y,z ECI coordinates of point and gives az, el, + slant range from Observer Parameters ---------- @@ -193,6 +207,12 @@ def eci2aer(x, y, z, lat0, lon0, h0, t: datetime, *, deg: bool = True) -> tuple: Observation time deg : bool, optional true: degrees, false: radians + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. + xp : float, optional + Polar motion x coordinate in arcseconds for the pure-Python path. + yp : float, optional + Polar motion y coordinate in arcseconds for the pure-Python path. Returns ------- @@ -204,7 +224,7 @@ def eci2aer(x, y, z, lat0, lon0, h0, t: datetime, *, deg: bool = True) -> tuple: slant range [meters] """ - xe, ye, ze = eci2ecef(x, y, z, t) + xe, ye, ze = eci2ecef(x, y, z, t, delta_ut1=delta_ut1, xp=xp, yp=yp) return ecef2aer(xe, ye, ze, lat0, lon0, h0, deg=deg) @@ -220,6 +240,9 @@ def aer2eci( ell: Ellipsoid | None = None, *, deg: bool = True, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, ) -> tuple: """ gives ECI of a point from an observer at az, el, slant range @@ -244,6 +267,12 @@ def aer2eci( reference ellipsoid deg : bool, optional degrees input/output (False: radians in/out) + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. + xp : float, optional + Polar motion x coordinate in arcseconds for the pure-Python path. + yp : float, optional + Polar motion y coordinate in arcseconds for the pure-Python path. Returns ------- @@ -260,7 +289,7 @@ def aer2eci( x, y, z = aer2ecef(az, el, srange, lat0, lon0, h0, ell, deg=deg) - return ecef2eci(x, y, z, t) + return ecef2eci(x, y, z, t, delta_ut1=delta_ut1, xp=xp, yp=yp) def aer2ecef( diff --git a/src/pymap3d/azelradec.py b/src/pymap3d/azelradec.py index 7e961ac..d0f7d27 100644 --- a/src/pymap3d/azelradec.py +++ b/src/pymap3d/azelradec.py @@ -28,6 +28,8 @@ def azel2radec( lon_deg: float, time: datetime, force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, ) -> tuple[float, float]: """ viewing angle (az, el) to sky coordinates (ra, dec) @@ -56,7 +58,9 @@ def azel2radec( """ if force_non_astropy or "astropy" not in sys.modules: - return vazel2radec(az_deg, el_deg, lat_deg, lon_deg, time) + return vazel2radec( + az_deg, el_deg, lat_deg, lon_deg, time, delta_ut1=delta_ut1 + ) else: return azel2radec_astropy(az_deg, el_deg, lat_deg, lon_deg, time) @@ -85,6 +89,8 @@ def radec2azel( lon_deg: float, time: datetime, force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, ) -> tuple[float, float]: """ sky coordinates (ra, dec) to viewing angle (az, el) @@ -113,7 +119,9 @@ def radec2azel( """ if force_non_astropy or "astropy" not in sys.modules: - return vradec2azel(ra_deg, dec_deg, lat_deg, lon_deg, time) + return vradec2azel( + ra_deg, dec_deg, lat_deg, lon_deg, time, delta_ut1=delta_ut1 + ) else: return radec2azel_astropy(ra_deg, dec_deg, lat_deg, lon_deg, time) diff --git a/src/pymap3d/earth_orientation.py b/src/pymap3d/earth_orientation.py new file mode 100644 index 0000000..e649409 --- /dev/null +++ b/src/pymap3d/earth_orientation.py @@ -0,0 +1,335 @@ +"""Internal Earth orientation helpers for pure-Python inertial/terrestrial transforms.""" + +from __future__ import annotations + +from datetime import datetime, timedelta, timezone +from math import cos, pi, sin, tau + +ARCSEC2RAD = pi / 648000.0 + +# UTC effective dates and cumulative TAI-UTC leap seconds. +# Sources: IERS/ITU leap second history through 2017-01-01. +_LEAP_SECONDS = ( + (datetime(1972, 1, 1, tzinfo=timezone.utc), 10), + (datetime(1972, 7, 1, tzinfo=timezone.utc), 11), + (datetime(1973, 1, 1, tzinfo=timezone.utc), 12), + (datetime(1974, 1, 1, tzinfo=timezone.utc), 13), + (datetime(1975, 1, 1, tzinfo=timezone.utc), 14), + (datetime(1976, 1, 1, tzinfo=timezone.utc), 15), + (datetime(1977, 1, 1, tzinfo=timezone.utc), 16), + (datetime(1978, 1, 1, tzinfo=timezone.utc), 17), + (datetime(1979, 1, 1, tzinfo=timezone.utc), 18), + (datetime(1980, 1, 1, tzinfo=timezone.utc), 19), + (datetime(1981, 7, 1, tzinfo=timezone.utc), 20), + (datetime(1982, 7, 1, tzinfo=timezone.utc), 21), + (datetime(1983, 7, 1, tzinfo=timezone.utc), 22), + (datetime(1985, 7, 1, tzinfo=timezone.utc), 23), + (datetime(1988, 1, 1, tzinfo=timezone.utc), 24), + (datetime(1990, 1, 1, tzinfo=timezone.utc), 25), + (datetime(1991, 1, 1, tzinfo=timezone.utc), 26), + (datetime(1992, 7, 1, tzinfo=timezone.utc), 27), + (datetime(1993, 7, 1, tzinfo=timezone.utc), 28), + (datetime(1994, 7, 1, tzinfo=timezone.utc), 29), + (datetime(1996, 1, 1, tzinfo=timezone.utc), 30), + (datetime(1997, 7, 1, tzinfo=timezone.utc), 31), + (datetime(1999, 1, 1, tzinfo=timezone.utc), 32), + (datetime(2006, 1, 1, tzinfo=timezone.utc), 33), + (datetime(2009, 1, 1, tzinfo=timezone.utc), 34), + (datetime(2012, 7, 1, tzinfo=timezone.utc), 35), + (datetime(2015, 7, 1, tzinfo=timezone.utc), 36), + (datetime(2017, 1, 1, tzinfo=timezone.utc), 37), +) + + +def ensure_utc(time: datetime) -> datetime: + """Normalize datetimes to UTC while preserving naive UTC behavior.""" + + if time.tzinfo is None: + return time.replace(tzinfo=timezone.utc) + + return time.astimezone(timezone.utc) + + +def utc_with_offset(time: datetime, seconds: float) -> datetime: + """Apply a seconds offset to a UTC-normalized datetime.""" + + return ensure_utc(time) + timedelta(seconds=seconds) + + +def leap_seconds(time: datetime) -> int: + """TAI-UTC for the supplied UTC epoch.""" + + utc = ensure_utc(time) + + leaps = _LEAP_SECONDS[0][1] + for effective, total in _LEAP_SECONDS: + if utc >= effective: + leaps = total + else: + break + + return leaps + + +def utc_to_tt(time: datetime) -> datetime: + """Convert UTC to TT using the embedded leap-second table.""" + + return utc_with_offset(time, leap_seconds(time) + 32.184) + + +def juliandate(time: datetime) -> float: + """ + Python datetime to Julian date. + + Adapted from Vallado/Meeus using UTC-normalized inputs. + """ + + time = ensure_utc(time) + + if time.month < 3: + year = time.year - 1 + month = time.month + 12 + else: + year = time.year + month = time.month + + A = int(year / 100.0) + B = 2 - A + int(A / 4.0) + C = ( + ( + ( + ( + time.second + + time.microsecond / 1e6 + ) + / 60.0 + + time.minute + ) + / 60.0 + + time.hour + ) + / 24.0 + ) + + return ( + int(365.25 * (year + 4716)) + + int(30.6001 * (month + 1)) + + time.day + + B + - 1524.5 + + C + ) + + +def julian_centuries(jd: float) -> float: + """Julian centuries from J2000.""" + + return (jd - 2451545.0) / 36525.0 + + +def greenwich_mean_sidereal_time(jd_ut1: float) -> float: + """Greenwich mean sidereal time in radians.""" + + t_ut1 = julian_centuries(jd_ut1) + gmst_sec = ( + 67310.54841 + + (876600 * 3600 + 8640184.812866) * t_ut1 + + 0.093104 * t_ut1**2 + - 6.2e-6 * t_ut1**3 + ) + + return gmst_sec * tau / 86400.0 % tau + + +def earth_rotation_angle(jd_ut1: float) -> float: + """Earth rotation angle in radians, per IAU 2000 formulation.""" + + return tau * (0.7790572732640 + 1.00273781191135448 * (jd_ut1 - 2451545.0)) % tau + + +def mean_obliquity(ttt: float) -> float: + """Mean obliquity of the ecliptic in radians.""" + + eps_arcsec = 84381.448 - 46.8150 * ttt - 0.00059 * ttt**2 + 0.001813 * ttt**3 + return eps_arcsec * ARCSEC2RAD + + +def nutation_short(ttt: float) -> tuple[float, float]: + """ + Short-form nutation model in radians. + + Uses the dominant Meeus/Vallado terms to capture most of the nutation signal + without bringing in the full IAU 2000B coefficient set. + """ + + L = (280.4665 + 36000.7698 * ttt) % 360.0 + Lp = (218.3165 + 481267.8813 * ttt) % 360.0 + omega = ( + 125.04452 - 1934.136261 * ttt + 0.0020708 * ttt**2 + ttt**3 / 450000.0 + ) % 360.0 + + L = L * pi / 180.0 + Lp = Lp * pi / 180.0 + omega = omega * pi / 180.0 + + dpsi_arcsec = ( + -17.20 * sin(omega) + - 1.32 * sin(2.0 * L) + - 0.23 * sin(2.0 * Lp) + + 0.21 * sin(2.0 * omega) + ) + deps_arcsec = ( + 9.20 * cos(omega) + + 0.57 * cos(2.0 * L) + + 0.10 * cos(2.0 * Lp) + - 0.09 * cos(2.0 * omega) + ) + + return dpsi_arcsec * ARCSEC2RAD, deps_arcsec * ARCSEC2RAD + + +def equation_of_equinoxes(ttt: float) -> float: + """Approximate equation of the equinoxes in radians.""" + + dpsi, deps = nutation_short(ttt) + return dpsi * cos(mean_obliquity(ttt) + deps) + + +def greenwich_apparent_sidereal_time(jd_ut1: float, jd_tt: float) -> float: + """Greenwich apparent sidereal time in radians.""" + + ttt = julian_centuries(jd_tt) + gast = greenwich_mean_sidereal_time(jd_ut1) + equation_of_equinoxes(ttt) + return gast % tau + + +def rotation_x(angle: float) -> tuple[tuple[float, float, float], ...]: + c = cos(angle) + s = sin(angle) + return ((1.0, 0.0, 0.0), (0.0, c, s), (0.0, -s, c)) + + +def rotation_y(angle: float) -> tuple[tuple[float, float, float], ...]: + c = cos(angle) + s = sin(angle) + return ((c, 0.0, -s), (0.0, 1.0, 0.0), (s, 0.0, c)) + + +def rotation_z(angle: float) -> tuple[tuple[float, float, float], ...]: + c = cos(angle) + s = sin(angle) + return ((c, s, 0.0), (-s, c, 0.0), (0.0, 0.0, 1.0)) + + +def matmul3(a, b): + """3x3 matrix multiply.""" + + return tuple( + tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)) for i in range(3) + ) + + +def matsub3(a, b): + """3x3 matrix subtraction.""" + + return tuple(tuple(a[i][j] - b[i][j] for j in range(3)) for i in range(3)) + + +def matscale3(a, scale: float): + """3x3 matrix scalar multiply.""" + + return tuple(tuple(scale * a[i][j] for j in range(3)) for i in range(3)) + + +def matvec3(a, v): + """3x3 matrix by 3-vector.""" + + return tuple(sum(a[i][k] * v[k] for k in range(3)) for i in range(3)) + + +def transpose3(a): + """Transpose a 3x3 matrix.""" + + return tuple(tuple(a[j][i] for j in range(3)) for i in range(3)) + + +def precession_matrix(ttt: float): + """IAU 1976 precession matrix, J2000 mean equator/equinox to mean-of-date.""" + + zeta = ( + 2306.2181 * ttt + 0.30188 * ttt**2 + 0.017998 * ttt**3 + ) * ARCSEC2RAD + theta = ( + 2004.3109 * ttt - 0.42665 * ttt**2 - 0.041833 * ttt**3 + ) * ARCSEC2RAD + zed = (2306.2181 * ttt + 1.09468 * ttt**2 + 0.018203 * ttt**3) * ARCSEC2RAD + + return matmul3(rotation_z(-zed), matmul3(rotation_y(theta), rotation_z(-zeta))) + + +def nutation_matrix(ttt: float): + """Nutation matrix, mean-of-date to true-of-date.""" + + dpsi, deps = nutation_short(ttt) + eps0 = mean_obliquity(ttt) + eps = eps0 + deps + + return matmul3(rotation_x(-eps), matmul3(rotation_z(-dpsi), rotation_x(eps0))) + + +def polar_motion_matrix(xp_arcsec: float = 0.0, yp_arcsec: float = 0.0): + """Polar motion matrix, true Earth-fixed to ITRF-like ECEF.""" + + xp = xp_arcsec * ARCSEC2RAD + yp = yp_arcsec * ARCSEC2RAD + return matmul3(rotation_y(xp), rotation_x(yp)) + + +def eci_to_ecef_matrix( + time: datetime, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +): + """ + Rotation matrix for the pure-Python ECI->ECEF path. + + The model uses: + - UTC -> UT1 with user-supplied ``delta_ut1`` seconds + - UTC -> TT via embedded leap seconds + - IAU 1976 precession + - truncated nutation / apparent sidereal time + - optional polar motion ``xp`` / ``yp`` in arcseconds + """ + + jd_ut1 = juliandate(utc_with_offset(time, delta_ut1)) + jd_tt = juliandate(utc_to_tt(time)) + ttt = julian_centuries(jd_tt) + + p = precession_matrix(ttt) + n = nutation_matrix(ttt) + st = rotation_z(greenwich_apparent_sidereal_time(jd_ut1, jd_tt)) + pm = polar_motion_matrix(xp, yp) + + return matmul3(pm, matmul3(st, matmul3(n, p))) + + +def eci_to_ecef_matrix_rate( + time: datetime, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, + dt_seconds: float = 0.5, +): + """ + Time derivative of the ECI->ECEF rotation matrix. + + Uses a central difference over the same Earth-orientation model as + ``eci_to_ecef_matrix()``. + """ + + dt = timedelta(seconds=dt_seconds) + c_plus = eci_to_ecef_matrix(time + dt, delta_ut1=delta_ut1, xp=xp, yp=yp) + c_minus = eci_to_ecef_matrix(time - dt, delta_ut1=delta_ut1, xp=xp, yp=yp) + + return matscale3(matsub3(c_plus, c_minus), 1.0 / (2.0 * dt_seconds)) diff --git a/src/pymap3d/ecef.py b/src/pymap3d/ecef.py index 3afe66f..b343228 100644 --- a/src/pymap3d/ecef.py +++ b/src/pymap3d/ecef.py @@ -411,7 +411,16 @@ def uvw2enu(u, v, w, lat0, lon0, deg: bool = True) -> tuple: def eci2geodetic( - x, y, z, t: datetime, ell: Ellipsoid | None = None, *, deg: bool = True + x, + y, + z, + t: datetime, + ell: Ellipsoid | None = None, + *, + deg: bool = True, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, ) -> tuple: """ convert Earth Centered Internal ECI to geodetic coordinates @@ -432,6 +441,12 @@ def eci2geodetic( planet ellipsoid model deg : bool, optional if True, degrees. if False, radians + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. + xp : float, optional + Polar motion x coordinate in arcseconds for the pure-Python path. + yp : float, optional + Polar motion y coordinate in arcseconds for the pure-Python path. Results ------- @@ -445,13 +460,22 @@ def eci2geodetic( eci2geodetic() a.k.a. eci2lla() """ - xecef, yecef, zecef = eci2ecef(x, y, z, t) + xecef, yecef, zecef = eci2ecef(x, y, z, t, delta_ut1=delta_ut1, xp=xp, yp=yp) return ecef2geodetic(xecef, yecef, zecef, ell, deg) def geodetic2eci( - lat, lon, alt, t: datetime, ell: Ellipsoid | None = None, *, deg: bool = True + lat, + lon, + alt, + t: datetime, + ell: Ellipsoid | None = None, + *, + deg: bool = True, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, ) -> tuple: """ convert geodetic coordinates to Earth Centered Internal ECI @@ -472,6 +496,12 @@ def geodetic2eci( planet ellipsoid model deg : bool, optional if True, degrees. if False, radians + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. + xp : float, optional + Polar motion x coordinate in arcseconds for the pure-Python path. + yp : float, optional + Polar motion y coordinate in arcseconds for the pure-Python path. Results ------- @@ -487,7 +517,7 @@ def geodetic2eci( x, y, z = geodetic2ecef(lat, lon, alt, ell, deg) - return ecef2eci(x, y, z, t) + return ecef2eci(x, y, z, t, delta_ut1=delta_ut1, xp=xp, yp=yp) def enu2ecef( diff --git a/src/pymap3d/eci.py b/src/pymap3d/eci.py index 4637410..7c7b232 100644 --- a/src/pymap3d/eci.py +++ b/src/pymap3d/eci.py @@ -9,21 +9,38 @@ try: import numpy import astropy.units as u - from astropy.coordinates import GCRS, ITRS, CartesianRepresentation, EarthLocation + from astropy.coordinates import ( + GCRS, + ITRS, + CartesianDifferential, + CartesianRepresentation, + EarthLocation, + ) except ImportError: pass +from .earth_orientation import eci_to_ecef_matrix, eci_to_ecef_matrix_rate, matvec3, transpose3 -from .sidereal import greenwichsrt, juliandate - -__all__ = ["eci2ecef", "ecef2eci"] +__all__ = ["eci2ecef", "ecef2eci", "eci2ecef_state", "ecef2eci_state"] -def eci2ecef(x, y, z, time: datetime, force_non_astropy: bool = False) -> tuple: +def eci2ecef( + x, + y, + z, + time: datetime, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: """ Observer => Point ECI => ECEF - J2000 frame + For the Astropy path, this converts from GCRS to ITRS. + For the pure-Python path, this uses a J2000-like inertial frame with + precession, truncated nutation, sidereal rotation, and optional polar motion. Parameters ---------- @@ -37,6 +54,12 @@ def eci2ecef(x, y, z, time: datetime, force_non_astropy: bool = False) -> tuple: time of obsevation (UTC) force_non_astropy : bool if True, force use of less accurate Numpy implementation even if Astropy is available + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. + xp : float, optional + Polar motion x coordinate in arcseconds for the pure-Python path. + yp : float, optional + Polar motion y coordinate in arcseconds for the pure-Python path. Results ------- @@ -50,15 +73,13 @@ def eci2ecef(x, y, z, time: datetime, force_non_astropy: bool = False) -> tuple: if "astropy" in sys.modules and not force_non_astropy: xe, ye, ze = eci2ecef_astropy(x, y, z, time) - elif "numpy" in sys.modules: + else: logging.warning( - f"{__name__}: Numpy implementation has much less accuracy than Astropy" + f"{__name__}: using pure-Python ECI/ECEF fallback, less accurate than Astropy" ) - xe, ye, ze = eci2ecef_numpy(x, y, z, time) - else: - raise ImportError("eci2ecef requires either Numpy or Astropy") + xe, ye, ze = eci2ecef_numpy(x, y, z, time, delta_ut1=delta_ut1, xp=xp, yp=yp) - return xe.squeeze()[()], ye.squeeze()[()], ze.squeeze()[()] + return _squeeze_xyz(xe, ye, ze) def eci2ecef_astropy(x, y, z, t: datetime) -> tuple: @@ -78,38 +99,37 @@ def eci2ecef_astropy(x, y, z, t: datetime) -> tuple: return x_ecef, y_ecef, z_ecef -def eci2ecef_numpy(x, y, z, t: datetime) -> tuple: +def eci2ecef_numpy( + x, + y, + z, + t: datetime, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: """ - eci2ecef using Numpy + eci2ecef using the internal pure-Python Earth orientation model. see eci2ecef() for description """ - x = numpy.atleast_1d(x) - y = numpy.atleast_1d(y) - z = numpy.atleast_1d(z) - gst = numpy.atleast_1d(greenwichsrt(juliandate(t))) - assert x.shape == y.shape == z.shape, ( - f"shape mismatch: x: ${x.shape} y: {y.shape} z: {z.shape}" - ) - - if gst.size == 1 and x.size != 1: - gst = numpy.broadcast_to(gst, x.shape[0]) - assert x.size == gst.size, f"shape mismatch: x: {x.shape} gst: {gst.shape}" - - eci = numpy.column_stack((x.ravel(), y.ravel(), z.ravel())) - ecef = numpy.empty((x.size, 3)) - for i in range(eci.shape[0]): - ecef[i, :] = R3(gst[i]) @ eci[i, :].T - - x_ecef = ecef[:, 0].reshape(x.shape) - y_ecef = ecef[:, 1].reshape(y.shape) - z_ecef = ecef[:, 2].reshape(z.shape) - - return x_ecef.squeeze()[()], y_ecef.squeeze()[()], z_ecef.squeeze()[()] - - -def ecef2eci(x, y, z, time: datetime, force_non_astropy: bool = False) -> tuple: + rotation = eci_to_ecef_matrix(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + return _squeeze_xyz(*_apply_rotation(rotation, x, y, z)) + + +def ecef2eci( + x, + y, + z, + time: datetime, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: """ Point => Point ECEF => ECI @@ -128,6 +148,12 @@ def ecef2eci(x, y, z, time: datetime, force_non_astropy: bool = False) -> tuple: time of observation force_non_astropy : bool if True, force use of less accurate Numpy implementation even if Astropy is available + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. + xp : float, optional + Polar motion x coordinate in arcseconds for the pure-Python path. + yp : float, optional + Polar motion y coordinate in arcseconds for the pure-Python path. Results ------- @@ -141,15 +167,13 @@ def ecef2eci(x, y, z, time: datetime, force_non_astropy: bool = False) -> tuple: if "astropy" in sys.modules and not force_non_astropy: xe, ye, ze = ecef2eci_astropy(x, y, z, time) - elif "numpy" in sys.modules: + else: logging.warning( - f"{__name__}: Numpy implementation has much less accuracy than Astropy" + f"{__name__}: using pure-Python ECI/ECEF fallback, less accurate than Astropy" ) - xe, ye, ze = ecef2eci_numpy(x, y, z, time) - else: - raise ImportError("ecef2eci requires either Numpy or Astropy") + xe, ye, ze = ecef2eci_numpy(x, y, z, time, delta_ut1=delta_ut1, xp=xp, yp=yp) - return xe, ye, ze + return _squeeze_xyz(xe, ye, ze) def ecef2eci_astropy(x, y, z, t: datetime) -> tuple: @@ -163,32 +187,356 @@ def ecef2eci_astropy(x, y, z, t: datetime) -> tuple: return eci.x.value, eci.y.value, eci.z.value -def ecef2eci_numpy(x, y, z, t: datetime) -> tuple: +def ecef2eci_numpy( + x, + y, + z, + t: datetime, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: """ecef2eci using Numpy see ecef2eci() for description """ + rotation = transpose3(eci_to_ecef_matrix(t, delta_ut1=delta_ut1, xp=xp, yp=yp)) + return _squeeze_xyz(*_apply_rotation(rotation, x, y, z)) + + +def eci2ecef_state( + x, + y, + z, + vx, + vy, + vz, + time: datetime, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """ + Transform an ECI position/velocity state to ECEF. + + Velocity transforms depend on both position and velocity because ECEF rotates + with the Earth. + """ + + if "astropy" in sys.modules and not force_non_astropy: + state = eci2ecef_state_astropy(x, y, z, vx, vy, vz, time) + else: + logging.warning( + f"{__name__}: using pure-Python ECI/ECEF fallback, less accurate than Astropy" + ) + state = eci2ecef_state_numpy( + x, + y, + z, + vx, + vy, + vz, + time, + delta_ut1=delta_ut1, + xp=xp, + yp=yp, + ) + + return _squeeze_state(*state) + + +def eci2ecef_state_astropy(x, y, z, vx, vy, vz, t: datetime) -> tuple: + """eci2ecef_state using Astropy.""" + + gcrs = GCRS( + CartesianRepresentation( + x * u.m, + y * u.m, + z * u.m, + differentials=CartesianDifferential(vx * u.m / u.s, vy * u.m / u.s, vz * u.m / u.s), + ), + obstime=t, + ) + itrs = gcrs.transform_to(ITRS(obstime=t)) + rep = itrs.cartesian + diff = rep.differentials["s"] + + return ( + rep.x.to_value(u.m), + rep.y.to_value(u.m), + rep.z.to_value(u.m), + diff.d_x.to_value(u.m / u.s), + diff.d_y.to_value(u.m / u.s), + diff.d_z.to_value(u.m / u.s), + ) + + +def eci2ecef_state_numpy( + x, + y, + z, + vx, + vy, + vz, + t: datetime, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """eci2ecef_state using the internal pure-Python Earth orientation model.""" + + rotation = eci_to_ecef_matrix(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + rotation_rate = eci_to_ecef_matrix_rate(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + return _squeeze_state( + *_apply_state_rotation( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + inverse=False, + ) + ) + + +def ecef2eci_state( + x, + y, + z, + vx, + vy, + vz, + time: datetime, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """ + Transform an ECEF position/velocity state to ECI. + + Velocity transforms depend on both position and velocity because ECEF rotates + with the Earth. + """ + + if "astropy" in sys.modules and not force_non_astropy: + state = ecef2eci_state_astropy(x, y, z, vx, vy, vz, time) + else: + logging.warning( + f"{__name__}: using pure-Python ECI/ECEF fallback, less accurate than Astropy" + ) + state = ecef2eci_state_numpy( + x, + y, + z, + vx, + vy, + vz, + time, + delta_ut1=delta_ut1, + xp=xp, + yp=yp, + ) + + return _squeeze_state(*state) + + +def ecef2eci_state_astropy(x, y, z, vx, vy, vz, t: datetime) -> tuple: + """ecef2eci_state using Astropy.""" + + itrs = ITRS( + CartesianRepresentation( + x * u.m, + y * u.m, + z * u.m, + differentials=CartesianDifferential(vx * u.m / u.s, vy * u.m / u.s, vz * u.m / u.s), + ), + obstime=t, + ) + gcrs = itrs.transform_to(GCRS(obstime=t)) + rep = gcrs.cartesian + diff = rep.differentials["s"] + + return ( + rep.x.to_value(u.m), + rep.y.to_value(u.m), + rep.z.to_value(u.m), + diff.d_x.to_value(u.m / u.s), + diff.d_y.to_value(u.m / u.s), + diff.d_z.to_value(u.m / u.s), + ) + + +def ecef2eci_state_numpy( + x, + y, + z, + vx, + vy, + vz, + t: datetime, + *, + delta_ut1: float = 0.0, + xp: float = 0.0, + yp: float = 0.0, +) -> tuple: + """ecef2eci_state using the internal pure-Python Earth orientation model.""" + + rotation = eci_to_ecef_matrix(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + rotation_rate = eci_to_ecef_matrix_rate(t, delta_ut1=delta_ut1, xp=xp, yp=yp) + return _squeeze_state( + *_apply_state_rotation( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + inverse=True, + ) + ) + + +def _apply_rotation(rotation, x, y, z): + """Apply a 3x3 rotation to scalar or NumPy coordinate inputs.""" + + if "numpy" not in sys.modules: + return matvec3(rotation, (x, y, z)) + x = numpy.atleast_1d(x) y = numpy.atleast_1d(y) z = numpy.atleast_1d(z) - gst = numpy.atleast_1d(greenwichsrt(juliandate(t))) - assert x.shape == y.shape == z.shape - assert x.size == gst.size + assert x.shape == y.shape == z.shape, ( + f"shape mismatch: x: {x.shape} y: {y.shape} z: {z.shape}" + ) - ecef = numpy.column_stack((x.ravel(), y.ravel(), z.ravel())) - eci = numpy.empty((x.size, 3)) - for i in range(x.size): - eci[i, :] = R3(gst[i]).T @ ecef[i, :] + vin = numpy.column_stack((x.ravel(), y.ravel(), z.ravel())) + vout = numpy.empty((x.size, 3)) + for i in range(vin.shape[0]): + vout[i, :] = matvec3(rotation, tuple(vin[i, :])) return ( - eci[:, 0].reshape(x.shape).squeeze()[()], - eci[:, 1].reshape(y.shape).squeeze()[()], - eci[:, 2].reshape(z.shape).squeeze()[()], + vout[:, 0].reshape(x.shape), + vout[:, 1].reshape(y.shape), + vout[:, 2].reshape(z.shape), + ) + + +def _apply_state_rotation( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + *, + inverse: bool, +): + """Apply a position/velocity frame transform.""" + + if "numpy" not in sys.modules: + return _apply_state_rotation_scalar( + rotation, rotation_rate, x, y, z, vx, vy, vz, inverse=inverse + ) + + x = numpy.atleast_1d(x) + y = numpy.atleast_1d(y) + z = numpy.atleast_1d(z) + vx = numpy.atleast_1d(vx) + vy = numpy.atleast_1d(vy) + vz = numpy.atleast_1d(vz) + assert x.shape == y.shape == z.shape == vx.shape == vy.shape == vz.shape, ( + "shape mismatch between position and velocity inputs" ) + pos_in = numpy.column_stack((x.ravel(), y.ravel(), z.ravel())) + vel_in = numpy.column_stack((vx.ravel(), vy.ravel(), vz.ravel())) + pos_out = numpy.empty((x.size, 3)) + vel_out = numpy.empty((x.size, 3)) + + for i in range(pos_in.shape[0]): + pos_i, vel_i = _apply_state_rotation_scalar( + rotation, + rotation_rate, + *tuple(pos_in[i, :]), + *tuple(vel_in[i, :]), + inverse=inverse, + ) + pos_out[i, :] = pos_i + vel_out[i, :] = vel_i -def R3(x: float): - """Rotation matrix for ECI""" - return numpy.array( - [[numpy.cos(x), numpy.sin(x), 0], [-numpy.sin(x), numpy.cos(x), 0], [0, 0, 1]] + return ( + pos_out[:, 0].reshape(x.shape), + pos_out[:, 1].reshape(y.shape), + pos_out[:, 2].reshape(z.shape), + vel_out[:, 0].reshape(vx.shape), + vel_out[:, 1].reshape(vy.shape), + vel_out[:, 2].reshape(vz.shape), ) + + +def _apply_state_rotation_scalar( + rotation, + rotation_rate, + x, + y, + z, + vx, + vy, + vz, + *, + inverse: bool, +): + """Scalar state rotation helper.""" + + if inverse: + position = matvec3(transpose3(rotation), (x, y, z)) + coriolis = matvec3(rotation_rate, position) + velocity = matvec3( + transpose3(rotation), + (vx - coriolis[0], vy - coriolis[1], vz - coriolis[2]), + ) + else: + position = matvec3(rotation, (x, y, z)) + spin = matvec3(rotation_rate, (x, y, z)) + rotated_velocity = matvec3(rotation, (vx, vy, vz)) + velocity = tuple(rotated_velocity[i] + spin[i] for i in range(3)) + + return position, velocity + + +def _squeeze_xyz(x, y, z): + """Preserve scalar inputs while supporting array outputs.""" + + try: + return x.squeeze()[()], y.squeeze()[()], z.squeeze()[()] + except AttributeError: + return x, y, z + + +def _squeeze_state(x, y, z, vx, vy, vz): + """Preserve scalar state inputs while supporting array outputs.""" + + try: + return ( + x.squeeze()[()], + y.squeeze()[()], + z.squeeze()[()], + vx.squeeze()[()], + vy.squeeze()[()], + vz.squeeze()[()], + ) + except AttributeError: + return x, y, z, vx, vy, vz diff --git a/src/pymap3d/karney.py b/src/pymap3d/karney.py new file mode 100644 index 0000000..a221044 --- /dev/null +++ b/src/pymap3d/karney.py @@ -0,0 +1,1024 @@ +""" +Karney's geodesic algorithms for the ellipsoid. + +Pure Python implementation based on: + C.F.F. Karney, "Algorithms for geodesics", + J. Geodesy 87, 43-55 (2013) + https://doi.org/10.1007/s00190-012-0578-z + +Algorithm structure and coefficient tables adapted from geographiclib +(MIT license, Copyright (c) Charles Karney). + +More robust than Vincenty's method, particularly for near-antipodal +points and nearly-equatorial lines. +""" + +from __future__ import annotations + +import math as _math + +from .ellipsoid import Ellipsoid +from .mathfun import copysign, fmod, hypot, isnan, sqrt + +__all__ = ["geodesic_direct", "geodesic_inverse", "geodesic_line", "geodesic_area"] + +# --- Constants --- +_tiny = sqrt(2.220446049250313e-16) +_tol0 = 2.220446049250313e-16 +_tol1 = 200 * _tol0 +_tol2 = sqrt(_tol0) +_xthresh = 1000 * _tol2 +_maxit1 = 20 +_maxit2 = _maxit1 + 10 + 1 + +_nC1 = 6 +_nC1p = 6 +_nC2 = 6 +_nC3 = 6 +_nC4 = 6 + +# Outmask flags +_DISTANCE = 1 +_REDUCEDLENGTH = 1 << 10 +_GEODESICSCALE = 1 << 11 +_OUT = _DISTANCE | _REDUCEDLENGTH | _GEODESICSCALE + + +# --- Low-level helpers --- + +def _sum_error(u, v): + s = u + v + up = s - v + vpp = s - up + up -= u + vpp -= v + return s, -(up + vpp) + + +def _AngNormalize(x): + y = fmod(x, 360.0) + if y <= -180: + y += 360.0 + elif y > 180: + y -= 360.0 + return y + + +def _AngRound(x): + z = 1 / 16.0 + y = abs(x) + if y < z: + y = z - y + y = z - y + return copysign(y, x) + + +def _AngDiff(x, y): + d, t = _sum_error(-x, y) + d = _AngNormalize(d) + d, t2 = _sum_error(d if abs(d) != 180 else copysign(180.0, -t), t) + return d, t2 + + +def _sincosd(x): + r = fmod(x, 360.0) + q = 0 if isnan(r) else int(round(r / 90.0)) + r -= 90.0 * q + r = _math.radians(r) + s, c = _math.sin(r), _math.cos(r) + q = q % 4 + if q == 1: + s, c = c, -s + elif q == 2: + s, c = -s, -c + elif q == 3: + s, c = -c, s + if s == 0: + s = 0.0 + if c == 0: + c = 0.0 + return s, c + + +def _atan2d(y, x): + q = 0 + if abs(y) > abs(x): + x, y = y, x + q = 2 + if x < 0: + x = -x + q += 1 + ang = _math.degrees(_math.atan2(y, x)) + if q == 1: + ang = copysign(180.0, y) - ang + elif q == 2: + ang = 90.0 - ang + elif q == 3: + ang = -90.0 + ang + return ang + + +def _norm2(x, y): + r = hypot(x, y) + return x / r, y / r + + +def _polyval(N, p, s, x): + y = p[s] + for i in range(1, N + 1): + y = y * x + p[s + i] + return y + + +# --- Series evaluations --- +# Coefficient tables from geographiclib (MIT license). + +_A1coeff = (1, 4, 64, 0, 256) +_C1coeff = ( + -1, 6, -16, 32, + -9, 64, -128, 2048, + 9, -16, 768, + 3, -5, 512, + -7, 1280, + -7, 2048, +) +_C1pcoeff = ( + 205, -432, 768, 1536, + 4005, -4736, 3840, 12288, + -225, 116, 384, + -7173, 2695, 7680, + 3467, 7680, + 38081, 61440, +) +_A2coeff = (-11, -28, -192, 0, 256) +_C2coeff = ( + 1, 2, 16, 32, + 35, 64, 384, 2048, + 15, 80, 768, + 7, 35, 512, + 63, 1280, + 77, 2048, +) +_A3coeff = ( + -3, 128, + -2, -3, 64, + -1, -3, -1, 16, + 3, -1, -2, 8, + 1, -1, 2, + 1, 1, +) +_C3coeff = ( + 3, 128, + 2, 5, 128, + -1, 3, 3, 64, + -1, 0, 1, 8, + -1, 1, 4, + 5, 256, + 1, 3, 128, + -3, -2, 3, 64, + 1, -3, 2, 32, + 7, 512, + -10, 9, 384, + 5, -9, 5, 192, + 7, 512, + -14, 7, 512, + 21, 2560, +) +_C4coeff = ( + 97, 15015, + 1088, 156, 45045, + -224, -4784, 1573, 45045, + -10656, 14144, -4576, -858, 45045, + 64, 624, -4576, 6864, -3003, 15015, + 100, 208, 572, 3432, -12012, 30030, 45045, + 1, 9009, + -2944, 468, 135135, + 5792, 1040, -1287, 135135, + 5952, -11648, 9152, -2574, 135135, + -64, -624, 4576, -6864, 3003, 135135, + 8, 10725, + 1856, -936, 225225, + -8448, 4992, -1144, 225225, + -1440, 4160, -4576, 1716, 225225, + -136, 63063, + 1024, -208, 105105, + 3584, -3328, 1144, 315315, + -128, 135135, + -2560, 832, 405405, + 128, 99099, +) + + +def _A1m1f(eps): + eps2 = eps * eps + t = _polyval(3, _A1coeff, 0, eps2) / _A1coeff[4] + return (t + eps) / (1 - eps) + + +def _C1f(eps, c): + eps2 = eps * eps + d = eps + o = 0 + for l in range(1, _nC1 + 1): + m = (_nC1 - l) // 2 + c[l] = d * _polyval(m, _C1coeff, o, eps2) / _C1coeff[o + m + 1] + o += m + 2 + d *= eps + + +def _C1pf(eps, c): + eps2 = eps * eps + d = eps + o = 0 + for l in range(1, _nC1p + 1): + m = (_nC1p - l) // 2 + c[l] = d * _polyval(m, _C1pcoeff, o, eps2) / _C1pcoeff[o + m + 1] + o += m + 2 + d *= eps + + +def _A2m1f(eps): + eps2 = eps * eps + t = _polyval(3, _A2coeff, 0, eps2) / _A2coeff[4] + return (t - eps) / (1 + eps) + + +def _C2f(eps, c): + eps2 = eps * eps + d = eps + o = 0 + for l in range(1, _nC2 + 1): + m = (_nC2 - l) // 2 + c[l] = d * _polyval(m, _C2coeff, o, eps2) / _C2coeff[o + m + 1] + o += m + 2 + d *= eps + + +def _SinCosSeries(sinp, sinx, cosx, c, n): + ar = 2.0 * (cosx - sinx) * (cosx + sinx) + if n % 2 == 1: + y0 = c[n] + y1 = 0.0 + n -= 1 + else: + y0 = y1 = 0.0 + while n > 0: + y1 = ar * y0 - y1 + c[n] + n -= 1 + y0 = ar * y1 - y0 + c[n] + n -= 1 + if sinp: + return 2.0 * sinx * cosx * y0 + else: + return cosx * (y0 - y1) + + +def _Astroid(x, y): + p = x * x + q = y * y + r = (p + q - 1.0) / 6.0 + if not (q == 0 and r <= 0): + S = p * q / 4.0 + r2 = r * r + r3 = r * r2 + disc = S * (S + 2.0 * r3) + u = r + if disc >= 0: + T3 = S + r3 + T3 += -sqrt(disc) if T3 < 0 else sqrt(disc) + T = T3 ** (1.0 / 3.0) if T3 >= 0 else -((-T3) ** (1.0 / 3.0)) + u += T + (r2 / T if T != 0 else 0.0) + else: + ang = _math.atan2(sqrt(-disc), -(S + r3)) + u += 2.0 * r * _math.cos(ang / 3.0) + v = sqrt(u * u + q) + uv = u + v if u >= 0 else q / (v - u) + w = (uv - q) / (2.0 * v) + k = uv / (sqrt(uv + w * w) + w) + else: + k = 0.0 + return k + + +# --- Geodesic class --- + + +class _Geodesic: + + def __init__(self, ell: Ellipsoid | None = None): + if ell is None: + ell = Ellipsoid.from_name("wgs84") + self.a = ell.semimajor_axis + self.f = ell.flattening + self._f1 = 1.0 - self.f + self._e2 = self.f * (2.0 - self.f) + self._ep2 = self._e2 / (self._f1 ** 2) + self._n = self.f / (2.0 - self.f) + self._b = self.a * self._f1 + if self._e2 == 0: + self._c2 = self.a ** 2 + elif self._e2 > 0: + e = sqrt(self._e2) + self._c2 = (self.a ** 2 + self._b ** 2 * _math.atanh(e) / e) / 2.0 + else: + e = sqrt(-self._e2) + self._c2 = (self.a ** 2 + self._b ** 2 * _math.atan(e) / e) / 2.0 + self._etol2 = 0.1 * _tol2 / sqrt( + max(0.001, abs(self.f)) * min(1.0, 1.0 - self.f / 2.0) / 2.0) + + # Precompute A3, C3, C4 coefficient arrays (polynomials in n evaluated) + self._A3x = self._compute_A3x() + self._C3x = self._compute_C3x() + self._C4x = self._compute_C4x() + + def _compute_A3x(self): + _A3x = [0.0] * _nC3 + o = 0 + k = 0 + for j in range(_nC3 - 1, -1, -1): + m = min(_nC3 - j - 1, j) + _A3x[k] = _polyval(m, _A3coeff, o, self._n) / _A3coeff[o + m + 1] + o += m + 2 + k += 1 + return _A3x + + def _compute_C3x(self): + _C3x = [0.0] * ((_nC3 * (_nC3 - 1)) // 2) + o = 0 + k = 0 + for l in range(1, _nC3): + for j in range(_nC3 - 1, l - 1, -1): + m = min(_nC3 - j - 1, j) + _C3x[k] = _polyval(m, _C3coeff, o, self._n) / _C3coeff[o + m + 1] + o += m + 2 + k += 1 + return _C3x + + def _compute_C4x(self): + _C4x = [0.0] * ((_nC4 * (_nC4 + 1)) // 2) + o = 0 + k = 0 + for l in range(_nC4): + for j in range(_nC4 - 1, l - 1, -1): + m = _nC4 - j - 1 + _C4x[k] = _polyval(m, _C4coeff, o, self._n) / _C4coeff[o + m + 1] + o += m + 2 + k += 1 + return _C4x + + def _A3f(self, eps): + return _polyval(_nC3 - 1, self._A3x, 0, eps) + + def _C3f(self, eps, c): + mult = 1.0 + o = 0 + for l in range(1, _nC3): + mult *= eps + m = _nC3 - 1 - l + c[l] = mult * _polyval(m, self._C3x, o, eps) + o += m + 1 + + def _C4f(self, eps, c): + mult = 1.0 + o = 0 + for l in range(_nC4): + m = _nC4 - l - 1 + c[l] = mult * _polyval(m, self._C4x, o, eps) + o += m + 1 + mult *= eps + + def _Lengths(self, eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, + cbet1, cbet2, outmask, C1a, C2a): + s12b = m12b = m0 = M12 = M21 = _math.nan + A1 = _A1m1f(eps) + _C1f(eps, C1a) + if outmask & (_REDUCEDLENGTH | _GEODESICSCALE): + A2 = _A2m1f(eps) + _C2f(eps, C2a) + m0x = A1 - A2 + A2 = 1.0 + A2 + A1 = 1.0 + A1 + + if outmask & _DISTANCE: + B1 = (_SinCosSeries(True, ssig2, csig2, C1a, _nC1) - + _SinCosSeries(True, ssig1, csig1, C1a, _nC1)) + s12b = A1 * (sig12 + B1) + if outmask & (_REDUCEDLENGTH | _GEODESICSCALE): + B2 = (_SinCosSeries(True, ssig2, csig2, C2a, _nC2) - + _SinCosSeries(True, ssig1, csig1, C2a, _nC2)) + J12 = m0x * sig12 + (A1 * B1 - A2 * B2) + elif outmask & (_REDUCEDLENGTH | _GEODESICSCALE): + for l in range(1, _nC1 + 1): + C2a[l] = A1 * C1a[l] - A2 * C2a[l] + J12 = m0x * sig12 + ( + _SinCosSeries(True, ssig2, csig2, C2a, _nC1) - + _SinCosSeries(True, ssig1, csig1, C2a, _nC1)) + + if outmask & _REDUCEDLENGTH: + m0 = m0x + m12b = dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) - csig1 * csig2 * J12 + + if outmask & _GEODESICSCALE: + csig12 = csig1 * csig2 + ssig1 * ssig2 + t = self._ep2 * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2) + M12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1 + M21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2 + + return s12b, m12b, m0, M12, M21 + + def _InverseStart(self, sbet1, cbet1, dn1, sbet2, cbet2, dn2, + lam12, slam12, clam12, C1a, C2a): + sig12 = -1.0 + salp1 = 2.0 + calp1 = salp2 = calp2 = dnm = _math.nan + + sbet12 = sbet2 * cbet1 - cbet2 * sbet1 + cbet12 = cbet2 * cbet1 + sbet2 * sbet1 + sbet12a = sbet2 * cbet1 + cbet2 * sbet1 + + shortline = cbet12 >= 0 and sbet12 < 0.5 and cbet2 * lam12 < 0.5 + if shortline: + sbetm2 = (sbet1 + sbet2) ** 2 + sbetm2 /= sbetm2 + (cbet1 + cbet2) ** 2 + dnm = sqrt(1.0 + self._ep2 * sbetm2) + omg12 = lam12 / (self._f1 * dnm) + somg12 = _math.sin(omg12) + comg12 = _math.cos(omg12) + else: + somg12 = slam12 + comg12 = clam12 + + salp1 = cbet2 * somg12 + calp1 = ( + sbet12 + cbet2 * sbet1 * somg12 ** 2 / (1.0 + comg12) + if comg12 >= 0 + else sbet12a - cbet2 * sbet1 * somg12 ** 2 / (1.0 - comg12)) + + ssig12 = hypot(salp1, calp1) + csig12 = sbet1 * sbet2 + cbet1 * cbet2 * comg12 + + if shortline and ssig12 * self._ep2 / sqrt(1.0 + self._ep2) < 0.01: + # Short line case + pass + + if not (csig12 >= 0 or ssig12 >= 6 * abs(self.f) * _math.pi * cbet1 ** 2): + if self.f >= 0: + k2 = sbet1 ** 2 * self._ep2 + eps = k2 / (2.0 * (1.0 + sqrt(1.0 + k2)) + k2) + lamscale = self.f * cbet1 * self._A3f(eps) * _math.pi + betscale = lamscale * cbet1 + x = (lam12 - _math.pi) / lamscale + y = sbet12a / betscale + else: + cbet12a = cbet2 * cbet1 - sbet2 * sbet1 + bet12a = _math.atan2(sbet12a, cbet12a) + _, m12b, m0, _, _ = self._Lengths( + self._n, _math.pi + bet12a, + sbet1, -cbet1, dn1, sbet2, cbet2, dn2, + cbet1, cbet2, _REDUCEDLENGTH, C1a, C2a) + x = -1.0 + m12b / (cbet1 * cbet2 * m0 * _math.pi) + betscale = (sbet12a / x if x < -0.01 + else -self.f * cbet1 ** 2 * _math.pi) + lamscale = betscale / cbet1 + y = (lam12 - _math.pi) / lamscale + + if y < -_tol1 and x > -1.0 - _xthresh: + if self.f >= 0: + salp1 = min(1.0, -x) + calp1 = -sqrt(1.0 - salp1 ** 2) + else: + calp1 = max(-1.0 if x > -_tol1 else -1.0, x) + salp1 = sqrt(1.0 - calp1 ** 2) + else: + k = _Astroid(x, y) + omg12a = lamscale * ( + -x * k / (1.0 + k) if self.f >= 0 + else -y * (1.0 + k) / k) + somg12 = _math.sin(omg12a) + comg12 = -_math.cos(omg12a) + salp1 = cbet2 * somg12 + calp1 = sbet12a - cbet2 * sbet1 * somg12 ** 2 / (1.0 - comg12) + + if salp1 > 0 or isnan(salp1): + salp1, calp1 = _norm2(salp1, calp1) + else: + salp1 = 1.0 + calp1 = 0.0 + return sig12, salp1, calp1, salp2, calp2, dnm + + def _Lambda12(self, sbet1, cbet1, dn1, sbet2, cbet2, dn2, + salp1, calp1, slam120, clam120, diffp, C1a, C2a, C3a): + if sbet1 == 0 and calp1 == 0: + calp1 = -_tiny + + salp0 = salp1 * cbet1 + calp0 = hypot(calp1, salp1 * sbet1) + + ssig1 = sbet1 + somg1 = salp0 * sbet1 + csig1 = comg1 = calp1 * cbet1 + ssig1, csig1 = _norm2(ssig1, csig1) + + salp2 = salp0 / cbet2 if cbet2 != cbet1 else salp1 + calp2 = ( + sqrt((calp1 * cbet1) ** 2 + + ((cbet2 - cbet1) * (cbet1 + cbet2) if cbet2 != cbet1 + else (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 + if cbet2 != cbet1 or abs(sbet2) != -sbet1 + else abs(calp1)) + + ssig2 = sbet2 + somg2 = salp0 * sbet2 + csig2 = comg2 = calp2 * cbet2 + ssig2, csig2 = _norm2(ssig2, csig2) + + sig12 = _math.atan2( + max(0.0, csig1 * ssig2 - ssig1 * csig2), + csig1 * csig2 + ssig1 * ssig2) + + somg12 = max(0.0, comg1 * somg2 - somg1 * comg2) + comg12 = comg1 * comg2 + somg1 * somg2 + eta = _math.atan2( + somg12 * clam120 - comg12 * slam120, + comg12 * clam120 + somg12 * slam120) + + k2 = calp0 ** 2 * self._ep2 + eps = k2 / (2.0 * (1.0 + sqrt(1.0 + k2)) + k2) + self._C3f(eps, C3a) + B312 = (_SinCosSeries(True, ssig2, csig2, C3a, _nC3 - 1) - + _SinCosSeries(True, ssig1, csig1, C3a, _nC3 - 1)) + A3 = self._A3f(eps) + domg12 = -self.f * A3 * salp0 * (sig12 + B312) + lam12 = eta + domg12 + + if diffp: + if calp2 == 0: + dlam12 = -2.0 * self._f1 * dn1 / sbet1 + else: + _, dlam12, _, _, _ = self._Lengths( + eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, + cbet1, cbet2, _REDUCEDLENGTH, C1a, C2a) + dlam12 *= self._f1 / (calp2 * cbet2) + else: + dlam12 = _math.nan + + return (lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2, + eps, somg12, comg12, dlam12) + + def _Inverse(self, lat1, lon1, lat2, lon2): + lon12, lon12s = _AngDiff(lon1, lon2) + lonsign = 1.0 if lon12 >= 0 else -1.0 + lon12 = lonsign * _AngRound(lon12) + lon12s = _AngRound((180.0 - lon12) - lonsign * lon12s) + lam12 = _math.radians(lon12) + if lon12 > 90: + slam12, clam12 = _sincosd(lon12s) + clam12 = -clam12 + else: + slam12, clam12 = _sincosd(lon12) + + lat1 = _AngRound(_AngNormalize(lat1)) + lat2 = _AngRound(lat2) + + swapp = -1.0 if abs(lat1) < abs(lat2) else 1.0 + if swapp < 0: + lonsign *= -1.0 + lat1, lat2 = lat2, lat1 + + latsign = 1.0 if lat1 < 0 else -1.0 + lat1 *= latsign + lat2 *= latsign + + sbet1, cbet1 = _sincosd(lat1) + sbet1 *= self._f1 + sbet1, cbet1 = _norm2(sbet1, cbet1) + cbet1 = max(_tiny, cbet1) + + sbet2, cbet2 = _sincosd(lat2) + sbet2 *= self._f1 + sbet2, cbet2 = _norm2(sbet2, cbet2) + cbet2 = max(_tiny, cbet2) + + if cbet1 < -sbet1: + if cbet2 == cbet1: + sbet2 = sbet1 if sbet2 < 0 else -sbet1 + + dn1 = sqrt(1.0 + self._ep2 * sbet1 ** 2) + dn2 = sqrt(1.0 + self._ep2 * sbet2 ** 2) + + meridian = lat1 == -90 or slam12 == 0 + + C1a = [0.0] * (_nC1 + 1) + C2a = [0.0] * (_nC2 + 1) + C3a = [0.0] * _nC3 + + s12x = m12x = a12 = _math.nan + M12 = M21 = _math.nan + + if meridian: + calp1 = clam12 + salp1 = slam12 + calp2 = 1.0 + salp2 = 0.0 + + ssig1 = sbet1 + csig1 = calp1 * cbet1 + ssig2 = sbet2 + csig2 = calp2 * cbet2 + + sig12 = _math.atan2( + max(0.0, csig1 * ssig2 - ssig1 * csig2), + csig1 * csig2 + ssig1 * ssig2) + + s12x, m12x, _, M12, M21 = self._Lengths( + self._n, sig12, + ssig1, csig1, dn1, ssig2, csig2, dn2, + cbet1, cbet2, _OUT, C1a, C2a) + + if sig12 < 1 or m12x >= 0: + if sig12 < 3 * _tiny: + sig12 = m12x = 0.0 + m12x *= self._b + s12x *= self._b + a12 = _math.degrees(sig12) + else: + meridian = False + + # Equatorial case: both points on equator, geodesic runs along equator + if (not meridian and + sbet1 == 0 and + (self.f <= 0 or lon12s >= self.f * 180)): + calp1 = calp2 = 0.0 + salp1 = salp2 = 1.0 + s12x = self.a * lam12 + sig12 = lam12 / self._f1 + m12x = self._b * _math.sin(sig12) + M12 = M21 = _math.cos(sig12) + a12 = lon12 / self._f1 + + elif not meridian: + sig12, salp1, calp1, salp2, calp2, dnm = self._InverseStart( + sbet1, cbet1, dn1, sbet2, cbet2, dn2, + lam12, slam12, clam12, C1a, C2a) + + if sig12 >= 0: + B1 = _SinCosSeries(True, _math.sin(sig12), _math.cos(sig12), C1a, _nC1) + s12x = self._b * (1 + _A1m1f(self._n)) * (sig12 + B1) + m12x = 0.0 + M12 = M21 = 1.0 + a12 = _math.degrees(sig12) + else: + tripb = False + salp1a = _tiny + calp1a = 1.0 + salp1b = _tiny + calp1b = -1.0 + + for numit in range(_maxit2): + (v, salp2, calp2, sig12, + ssig1, csig1, ssig2, csig2, + eps, domg12, comg12, dlam12) = self._Lambda12( + sbet1, cbet1, dn1, sbet2, cbet2, dn2, + salp1, calp1, slam12, clam12, + numit < _maxit1, C1a, C2a, C3a) + + if tripb or not (abs(v) >= (8 if numit < _maxit1 else 1) * _tol0): + break + + if v > 0 and (numit < _maxit1 or calp1 / salp1 > calp1b / salp1b): + salp1b = salp1 + calp1b = calp1 + elif v < 0 and (numit < _maxit1 or calp1 / salp1 < calp1a / salp1a): + salp1a = salp1 + calp1a = calp1 + + if numit < _maxit1 and dlam12 != 0: + dalp1 = -v / dlam12 + sdalp1 = _math.sin(dalp1) + cdalp1 = _math.cos(dalp1) + nsalp1 = salp1 * cdalp1 + calp1 * sdalp1 + if nsalp1 > 0: + calp1 = calp1 * cdalp1 - salp1 * sdalp1 + salp1 = nsalp1 + salp1, calp1 = _norm2(salp1, calp1) + tripb = abs(v) <= 16 * _tol0 + continue + + salp1 = (salp1a + salp1b) / 2.0 + calp1 = (calp1a + calp1b) / 2.0 + salp1, calp1 = _norm2(salp1, calp1) + tripb = False + if abs(salp1a - salp1b) < _tol0 and abs(calp1a - calp1b) < _tol0: + break + + s12x, m12x, _, M12, M21 = self._Lengths( + eps, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, + cbet1, cbet2, _OUT, C1a, C2a) + m12x *= self._b + s12x *= self._b + a12 = _math.degrees(sig12) + + if swapp < 0: + salp1, salp2 = salp2, salp1 + calp1, calp2 = calp2, calp1 + M12, M21 = M21, M12 + + salp1 *= swapp * lonsign + calp1 *= swapp * latsign + salp2 *= swapp * lonsign + calp2 *= swapp * latsign + + azi1 = _atan2d(salp1, calp1) + azi2 = _atan2d(salp2, calp2) + return s12x, azi1, azi2 + + def _Direct(self, lat1, lon1, azi1, s12): + salp1, calp1 = _sincosd(_AngNormalize(azi1)) + sbet1, cbet1 = _sincosd(_AngRound(lat1)) + sbet1 *= self._f1 + sbet1, cbet1 = _norm2(sbet1, cbet1) + cbet1 = max(_tiny, cbet1) + + dn1 = sqrt(1.0 + self._ep2 * sbet1 ** 2) + salp0 = salp1 * cbet1 + calp0 = hypot(calp1, salp1 * sbet1) + + ssig1 = sbet1 + somg1 = salp0 * sbet1 + csig1 = comg1 = calp1 * cbet1 + # Handle equatorial case: avoid norm2(0,0) + if ssig1 != 0 or csig1 != 0: + ssig1, csig1 = _norm2(ssig1, csig1) + else: + ssig1 = 0.0 + csig1 = 1.0 + + k2 = calp0 ** 2 * self._ep2 + eps = k2 / (2.0 * (1.0 + sqrt(1.0 + k2)) + k2) + + A1 = _A1m1f(eps) + C1a = [0.0] * (_nC1 + 1) + _C1f(eps, C1a) + C1pa = [0.0] * (_nC1p + 1) + _C1pf(eps, C1pa) + + A1p1 = 1.0 + A1 + B11 = _SinCosSeries(True, ssig1, csig1, C1a, _nC1) + s_B11 = _math.sin(B11) + c_B11 = _math.cos(B11) + stau1 = ssig1 * c_B11 + csig1 * s_B11 + ctau1 = csig1 * c_B11 - ssig1 * s_B11 + + tau12 = s12 / (self._b * A1p1) + s_tau12 = _math.sin(tau12) + c_tau12 = _math.cos(tau12) + + stau2 = stau1 * c_tau12 + ctau1 * s_tau12 + ctau2 = ctau1 * c_tau12 - stau1 * s_tau12 + + B12 = -_SinCosSeries(True, stau2, ctau2, C1pa, _nC1p) + sig12 = tau12 - (B12 - B11) + ssig12 = _math.sin(sig12) + csig12 = _math.cos(sig12) + + if abs(self.f) > 0.01: + ssig2 = ssig1 * csig12 + csig1 * ssig12 + csig2 = csig1 * csig12 - ssig1 * ssig12 + B12 = _SinCosSeries(True, ssig2, csig2, C1a, _nC1) + serr = (1.0 + A1) * (sig12 + (B12 - B11)) - s12 / self._b + sig12 -= serr / sqrt(1.0 + k2 * ssig2 ** 2) + ssig12 = _math.sin(sig12) + csig12 = _math.cos(sig12) + + ssig2 = ssig1 * csig12 + csig1 * ssig12 + csig2 = csig1 * csig12 - ssig1 * ssig12 + dn2 = sqrt(1.0 + k2 * ssig2 ** 2) + + sbet2 = calp0 * ssig2 + cbet2 = hypot(salp0, calp0 * csig2) + if cbet2 == 0: + cbet2 = csig2 = _tiny + + salp2 = salp0 + calp2 = calp0 * csig2 + + somg2 = salp0 * ssig2 + comg2 = csig2 + + C3a = [0.0] * _nC3 + self._C3f(eps, C3a) + A3c = self._A3f(eps) + B31 = _SinCosSeries(True, ssig1, csig1, C3a, _nC3 - 1) + B32 = _SinCosSeries(True, ssig2, csig2, C3a, _nC3 - 1) + + # Use LONG_UNROLL method for robustness (handles equatorial case) + E = copysign(1.0, salp0) + omg12 = E * (sig12 + - (_math.atan2(ssig2, csig2) - _math.atan2(ssig1, csig1)) + + (_math.atan2(E * somg2, comg2) - _math.atan2(E * somg1, comg1))) + lam12 = omg12 - self.f * A3c * salp0 * (sig12 + (B32 - B31)) + + lon12 = _math.degrees(lam12) + lat2 = _atan2d(sbet2, self._f1 * cbet2) + lon2 = _AngNormalize(lon1 + lon12) + azi2 = _atan2d(salp2, calp2) + return lat2, lon2, azi2 + + +# --- Cache --- + +_cache = {} + + +def _get_geodesic(ell: Ellipsoid | None) -> _Geodesic: + key = None if ell is None else (ell.semimajor_axis, ell.flattening) + if key not in _cache: + _cache[key] = _Geodesic(ell) + return _cache[key] + + +# --- Public API --- + + +def geodesic_inverse( + lat1: float, lon1: float, lat2: float, lon2: float, + ell: Ellipsoid | None = None, deg: bool = True, +) -> tuple: + """ + Solve the inverse geodesic problem using Karney's algorithm. + + Given two points, find the distance and azimuths of the geodesic + connecting them. + + Parameters + ---------- + lat1 : float + Geodetic latitude of first point. + lon1 : float + Geodetic longitude of first point. + lat2 : float + Geodetic latitude of second point. + lon2 : float + Geodetic longitude of second point. + ell : Ellipsoid, optional + Reference ellipsoid (default WGS84). + deg : bool, optional + If True (default), angles in degrees; if False, radians. + + Returns + ------- + dist : float + Geodesic distance in meters. + azi1 : float + Forward azimuth at first point (clockwise from north). + azi2 : float + Back-azimuth at second point (clockwise from north). + """ + g = _get_geodesic(ell) + if not deg: + lat1, lon1 = _math.degrees(lat1), _math.degrees(lon1) + lat2, lon2 = _math.degrees(lat2), _math.degrees(lon2) + dist, azi1, azi2 = g._Inverse(lat1, lon1, lat2, lon2) + if not deg: + azi1, azi2 = _math.radians(azi1), _math.radians(azi2) + return dist, azi1, azi2 + + +def geodesic_direct( + lat1: float, lon1: float, azi1: float, dist: float, + ell: Ellipsoid | None = None, deg: bool = True, +) -> tuple: + """ + Solve the direct geodesic problem using Karney's algorithm. + + Given a starting point, azimuth, and distance, find the endpoint + and back-azimuth. + + Parameters + ---------- + lat1 : float + Geodetic latitude of starting point. + lon1 : float + Geodetic longitude of starting point. + azi1 : float + Forward azimuth at starting point (clockwise from north). + dist : float + Distance along geodesic in meters. + ell : Ellipsoid, optional + Reference ellipsoid (default WGS84). + deg : bool, optional + If True (default), angles in degrees; if False, radians. + + Returns + ------- + lat2 : float + Geodetic latitude of endpoint. + lon2 : float + Geodetic longitude of endpoint. + azi2 : float + Back-azimuth at endpoint (clockwise from north). + """ + g = _get_geodesic(ell) + if not deg: + lat1, lon1 = _math.degrees(lat1), _math.degrees(lon1) + azi1 = _math.degrees(azi1) + lat2, lon2, azi2 = g._Direct(lat1, lon1, azi1, dist) + if not deg: + lat2, lon2, azi2 = _math.radians(lat2), _math.radians(lon2), _math.radians(azi2) + return lat2, lon2, azi2 + + +def geodesic_line( + lat1: float, lon1: float, lat2: float, lon2: float, + ell: Ellipsoid | None = None, npts: int = 100, deg: bool = True, +) -> tuple: + """ + Compute intermediate points along a geodesic. + + Parameters + ---------- + lat1, lon1 : float + First endpoint. + lat2, lon2 : float + Second endpoint. + ell : Ellipsoid, optional + Reference ellipsoid (default WGS84). + npts : int, optional + Number of points (default 100). + deg : bool, optional + If True (default), angles in degrees; if False, radians. + + Returns + ------- + lats : list of float + Latitudes of intermediate points. + lons : list of float + Longitudes of intermediate points. + """ + dist, azi, _ = geodesic_inverse(lat1, lon1, lat2, lon2, ell=ell, deg=deg) + lats = [] + lons = [] + for i in range(npts): + frac = i / (npts - 1) if npts > 1 else 0.0 + lat, lon, _ = geodesic_direct(lat1, lon1, azi, dist * frac, ell=ell, deg=deg) + lats.append(lat) + lons.append(lon) + return lats, lons + + +def geodesic_area( + lats, lons, ell: Ellipsoid | None = None, deg: bool = True, +) -> tuple: + """ + Compute the area and perimeter of a geodesic polygon. + + Counterclockwise vertex ordering gives positive area. + + Parameters + ---------- + lats : array-like + Latitudes of polygon vertices. + lons : array-like + Longitudes of polygon vertices. + ell : Ellipsoid, optional + Reference ellipsoid (default WGS84). + deg : bool, optional + If True (default), angles in degrees; if False, radians. + + Returns + ------- + area : float + Area in square meters (signed: CCW = positive). + perimeter : float + Perimeter in meters. + """ + n = len(lats) + if n < 3: + return 0.0, 0.0 + + g = _get_geodesic(ell) + perimeter = 0.0 + area_sum = 0.0 + azi2_prev = None + azi1_first = None + + for i in range(n): + j = (i + 1) % n + dist_ij, azi1_ij, azi2_ij = geodesic_inverse( + lats[i], lons[i], lats[j], lons[j], ell=ell, deg=deg) + perimeter += dist_ij + if i == 0: + azi1_first = azi1_ij + else: + diff, _ = _AngDiff(azi2_prev, azi1_ij) + area_sum += diff + azi2_prev = azi2_ij + + diff, _ = _AngDiff(azi2_prev, azi1_first) + area_sum += diff + + area = g._c2 * _math.radians(area_sum) + area0 = 4.0 * _math.pi * g._c2 + if abs(area) > area0 / 2: + area -= copysign(area0, area) + return area, perimeter diff --git a/src/pymap3d/mathfun.py b/src/pymap3d/mathfun.py index a21cb9f..f647490 100644 --- a/src/pymap3d/mathfun.py +++ b/src/pymap3d/mathfun.py @@ -5,6 +5,7 @@ from __future__ import annotations try: + from numpy import arccos as acos from numpy import arcsin as asin from numpy import arcsinh as asinh from numpy import arctan as atan @@ -12,9 +13,11 @@ from numpy import arctanh as atanh from numpy import ( cbrt, + copysign, cos, degrees, exp, + fmod, hypot, inf, isclose, @@ -30,14 +33,17 @@ ) except ImportError: from math import ( # type: ignore + acos, asin, asinh, atan, atan2, atanh, + copysign, cos, degrees, exp, + fmod, hypot, inf, isclose, @@ -80,15 +86,18 @@ def cbrt(x) -> float: # type: ignore __all__ = [ + "acos", "asin", "asinh", "atan", "atan2", "atanh", "cbrt", + "copysign", "cos", "degrees", "exp", + "fmod", "hypot", "inf", "isclose", diff --git a/src/pymap3d/nvector.py b/src/pymap3d/nvector.py index 4aa1234..4806071 100644 --- a/src/pymap3d/nvector.py +++ b/src/pymap3d/nvector.py @@ -1,6 +1,16 @@ +""" +N-vector based geodetic operations. + +Reference +--------- +Gade, K. (2010). A Nonsingular Horizontal Position Representation, +The Journal of Navigation, Volume 63, Issue 03, pp 395-417, July 2010. +(www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf) +""" + from __future__ import annotations -from .mathfun import degrees, radians, sin, cos, atan2, asin +from .mathfun import acos, asin, atan2, cos, degrees, radians, sin, sqrt from .ecef import geodetic2ecef, ecef2geodetic from .ellipsoid import Ellipsoid @@ -105,3 +115,212 @@ def nvector2ecef(n1, n2, n3, alt=0, ell: Ellipsoid | None = None, deg: bool = Tr """ lat, lon = nvector2geodetic(n1, n2, n3, deg=deg) return geodetic2ecef(lat, lon, alt, ell=ell, deg=deg) + + +# --- Private helpers for 3-component vector math --- + + +def _dot3(a1, a2, a3, b1, b2, b3): + """Dot product of two 3-vectors given as components.""" + return a1 * b1 + a2 * b2 + a3 * b3 + + +def _cross3(a1, a2, a3, b1, b2, b3): + """Cross product of two 3-vectors given as components.""" + return (a2 * b3 - a3 * b2, a3 * b1 - a1 * b3, a1 * b2 - a2 * b1) + + +def _norm3(a1, a2, a3): + """Normalize a 3-vector to unit length.""" + r = sqrt(a1 * a1 + a2 * a2 + a3 * a3) + return (a1 / r, a2 / r, a3 / r) + + +# --- N-vector spherical geometry operations --- + + +def _default_radius(): + """Mean Earth radius (equal-volume sphere) in meters.""" + from .rsphere import eqavol + + return eqavol() + + +def nvector_distance(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3, radius: float | None = None) -> float: + """ + Great-circle distance between two n-vectors. + + Uses atan2 formulation for numerical stability at all angular separations. + + Parameters + ---------- + n1_1, n1_2, n1_3 : float or array-like + Components of first n-vector. + n2_1, n2_2, n2_3 : float or array-like + Components of second n-vector. + radius : float, optional + Sphere radius in meters (default: WGS84 equal-volume sphere, ~6371 km). + + Returns + ------- + dist : float + Great-circle distance in meters. + """ + if radius is None: + radius = _default_radius() + + cx, cy, cz = _cross3(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3) + cross_mag = sqrt(cx * cx + cy * cy + cz * cz) + dot = _dot3(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3) + + return radius * atan2(cross_mag, dot) + + +def nvector_interpolate(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3, fraction: float) -> tuple: + """ + Spherical linear interpolation (SLERP) between two n-vectors. + + Parameters + ---------- + n1_1, n1_2, n1_3 : float + Components of first n-vector. + n2_1, n2_2, n2_3 : float + Components of second n-vector. + fraction : float + Interpolation fraction. 0.0 returns n1, 1.0 returns n2. + + Returns + ------- + ni_1, ni_2, ni_3 : float + Components of interpolated n-vector. + """ + dot = _dot3(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3) + # clamp to [-1, 1] for safety + dot = max(-1.0, min(1.0, dot)) + + omega = acos(dot) + + # For very small angles, fall back to normalized linear interpolation + if abs(omega) < 1e-10: + i1 = (1.0 - fraction) * n1_1 + fraction * n2_1 + i2 = (1.0 - fraction) * n1_2 + fraction * n2_2 + i3 = (1.0 - fraction) * n1_3 + fraction * n2_3 + return _norm3(i1, i2, i3) + + s = sin(omega) + a = sin((1.0 - fraction) * omega) / s + b = sin(fraction * omega) / s + + i1 = a * n1_1 + b * n2_1 + i2 = a * n1_2 + b * n2_2 + i3 = a * n1_3 + b * n2_3 + + return _norm3(i1, i2, i3) + + +def nvector_mean(n1s, n2s, n3s) -> tuple: + """ + Mean geographic position of multiple n-vectors. + + Computes the normalized centroid of the input n-vectors on the unit sphere. + + Parameters + ---------- + n1s, n2s, n3s : array-like + Sequences of n-vector components. + + Returns + ------- + n1, n2, n3 : float + Components of the mean n-vector (normalized to unit length). + """ + s1 = sum(n1s) + s2 = sum(n2s) + s3 = sum(n3s) + + return _norm3(s1, s2, s3) + + +def nvector_cross_track_distance( + n1_1, n1_2, n1_3, + n2_1, n2_2, n2_3, + np_1, np_2, np_3, + radius: float | None = None, +) -> float: + """ + Cross-track distance from a point to a great circle path. + + Parameters + ---------- + n1_1, n1_2, n1_3 : float + Components of n-vector for first point on the path. + n2_1, n2_2, n2_3 : float + Components of n-vector for second point on the path. + np_1, np_2, np_3 : float + Components of n-vector for the query point. + radius : float, optional + Sphere radius in meters (default: WGS84 equal-volume sphere). + + Returns + ------- + dist : float + Signed cross-track distance in meters. Positive means the point + is to the left of the path (from n1 toward n2). + """ + if radius is None: + radius = _default_radius() + + # Great circle pole: normal to the plane containing n1 and n2 + cx, cy, cz = _cross3(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3) + cx, cy, cz = _norm3(cx, cy, cz) + + # Signed angular distance from the great circle + return radius * asin(_dot3(cx, cy, cz, np_1, np_2, np_3)) + + +def nvector_intersection( + n1_1, n1_2, n1_3, + n2_1, n2_2, n2_3, + n3_1, n3_2, n3_3, + n4_1, n4_2, n4_3, +) -> tuple: + """ + Intersection of two great circle paths. + + Path 1 passes through (n1, n2), path 2 passes through (n3, n4). + Returns one of the two antipodal intersection points, chosen as the + one closest to the centroid of all four input points. + + Parameters + ---------- + n1_1, n1_2, n1_3 : float + Components of first n-vector on path 1. + n2_1, n2_2, n2_3 : float + Components of second n-vector on path 1. + n3_1, n3_2, n3_3 : float + Components of first n-vector on path 2. + n4_1, n4_2, n4_3 : float + Components of second n-vector on path 2. + + Returns + ------- + ni_1, ni_2, ni_3 : float + Components of the intersection n-vector. + """ + # Normals to each great circle plane + c1_1, c1_2, c1_3 = _cross3(n1_1, n1_2, n1_3, n2_1, n2_2, n2_3) + c2_1, c2_2, c2_3 = _cross3(n3_1, n3_2, n3_3, n4_1, n4_2, n4_3) + + # Intersection is perpendicular to both normals + i1, i2, i3 = _cross3(c1_1, c1_2, c1_3, c2_1, c2_2, c2_3) + i1, i2, i3 = _norm3(i1, i2, i3) + + # Pick the solution closest to the centroid of all four points + m1 = n1_1 + n2_1 + n3_1 + n4_1 + m2 = n1_2 + n2_2 + n3_2 + n4_2 + m3 = n1_3 + n2_3 + n3_3 + n4_3 + + if _dot3(i1, i2, i3, m1, m2, m3) < 0: + i1, i2, i3 = -i1, -i2, -i3 + + return i1, i2, i3 diff --git a/src/pymap3d/sidereal.py b/src/pymap3d/sidereal.py index 5ecf984..fa66ced 100644 --- a/src/pymap3d/sidereal.py +++ b/src/pymap3d/sidereal.py @@ -6,6 +6,13 @@ import logging from .timeconv import str2dt +from .earth_orientation import ( + greenwich_apparent_sidereal_time, + greenwich_mean_sidereal_time, + juliandate as _juliandate, + utc_to_tt, + utc_with_offset, +) try: import astropy.units as u @@ -19,7 +26,11 @@ def datetime2sidereal( - time: datetime, lon_radians: float, force_non_astropy: bool = False + time: datetime, + lon_radians: float, + force_non_astropy: bool = False, + *, + delta_ut1: float = 0.0, ): """ Convert ``datetime`` to local sidereal time @@ -33,6 +44,8 @@ def datetime2sidereal( longitude (radians) force_non_astropy : bool if True, force use of less accurate Numpy implementation even if Astropy is available + delta_ut1 : float, optional + UT1-UTC in seconds for the pure-Python path. Defaults to ``0.0``. Results ------- @@ -42,13 +55,21 @@ def datetime2sidereal( """ if isinstance(time, (tuple, list)): - return [datetime2sidereal(t, lon_radians) for t in time] + return [ + datetime2sidereal( + t, + lon_radians, + force_non_astropy=force_non_astropy, + delta_ut1=delta_ut1, + ) + for t in time + ] if "astropy" in sys.modules and not force_non_astropy: return datetime2sidereal_astropy(time, lon_radians) else: - logging.debug(f"{__name__}: Vallado implementation") - return datetime2sidereal_vallado(time, lon_radians) + logging.debug(f"{__name__}: pure-Python apparent sidereal implementation") + return datetime2sidereal_vallado(time, lon_radians, delta_ut1=delta_ut1) def datetime2sidereal_astropy(t: datetime, lon_radians: float): @@ -63,16 +84,17 @@ def datetime2sidereal_astropy(t: datetime, lon_radians: float): return tsr.radian -def datetime2sidereal_vallado(t: datetime, lon_radians: float): +def datetime2sidereal_vallado( + t: datetime, lon_radians: float, *, delta_ut1: float = 0.0 +): """datetime to sidereal time using Vallado methods see datetime2sidereal() for description """ - jd = juliandate(str2dt(t)) - # Greenwich Sidereal time RADIANS - gst = greenwichsrt(jd) - # Algorithm 15 p. 188 rotate GST to LOCAL SIDEREAL TIME - return gst + lon_radians + jd_ut1 = juliandate(utc_with_offset(str2dt(t), delta_ut1)) + jd_tt = juliandate(utc_to_tt(str2dt(t))) + gst = greenwich_apparent_sidereal_time(jd_ut1, jd_tt) + return (gst + lon_radians) % tau def juliandate(time: datetime): @@ -97,27 +119,7 @@ def juliandate(time: datetime): if isinstance(time, (tuple, list)): return list(map(juliandate, time)) - if time.month < 3: - year = time.year - 1 - month = time.month + 12 - else: - year = time.year - month = time.month - - A = int(year / 100.0) - B = 2 - A + int(A / 4.0) - C = ( - ((time.second + time.microsecond / 1e6) / 60.0 + time.minute) / 60.0 + time.hour - ) / 24.0 - - return ( - int(365.25 * (year + 4716)) - + int(30.6001 * (month + 1)) - + time.day - + B - - 1524.5 - + C - ) + return _juliandate(time) def greenwichsrt(Jdate: float): @@ -141,16 +143,4 @@ def greenwichsrt(Jdate: float): if isinstance(Jdate, (tuple, list)): return list(map(greenwichsrt, Jdate)) - # %% Vallado Eq. 3-42 p. 184, Seidelmann 3.311-1 - tUT1 = (Jdate - 2451545.0) / 36525.0 - - # Eqn. 3-47 p. 188 - gmst_sec = ( - 67310.54841 - + (876600 * 3600 + 8640184.812866) * tUT1 - + 0.093104 * tUT1**2 - - 6.2e-6 * tUT1**3 - ) - - # 1/86400 and %(2*pi) implied by units of radians - return gmst_sec * tau / 86400.0 % tau + return greenwich_mean_sidereal_time(Jdate) diff --git a/src/pymap3d/tests/test_eci.py b/src/pymap3d/tests/test_eci.py index e32f532..237ffbd 100644 --- a/src/pymap3d/tests/test_eci.py +++ b/src/pymap3d/tests/test_eci.py @@ -11,6 +11,7 @@ ECI = (-2981784, 5207055, 3161595) ECEF = [-5762640, -1682738, 3156028] +ECI_STATE = (-2981784, 5207055, 3161595, 100.0, -250.0, 50.0) UTC = datetime.datetime(2019, 1, 4, 12, tzinfo=datetime.timezone.utc) @@ -25,13 +26,9 @@ def test_eci2ecef(): def test_eci2ecef_numpy(): - pytest.importorskip("numpy") - ecef = pm.eci2ecef(*ECI, UTC, force_non_astropy=True) - rel = 0.025 - - assert ecef == approx(ECEF, rel=rel) + assert ecef == approx(ECEF, abs=60) assert isinstance(ecef[0], float) assert isinstance(ecef[1], float) assert isinstance(ecef[2], float) @@ -62,13 +59,9 @@ def test_ecef2eci(force_non_astropy): def test_ecef2eci_numpy(): - pytest.importorskip("numpy") - eci = pm.eci.ecef2eci_numpy(*ECEF, UTC) - rel = 0.025 - - assert eci == approx(ECI, rel=rel) + assert eci == approx(ECI, abs=60) assert isinstance(eci[0], float) assert isinstance(eci[1], float) assert isinstance(eci[2], float) @@ -87,28 +80,90 @@ def test_ecef2eci_astropy(): assert isinstance(eci[2], float) -def test_eci2geodetic(): +def test_eci2ecef_state(): + state = pm.eci2ecef_state(*ECI_STATE, UTC) + + assert all(isinstance(component, float) for component in state) + + +def test_ecef2eci_state(): pytest.importorskip("numpy") + state = pm.ecef2eci_state(*ECEF, 100.0, -250.0, 50.0, UTC) + + assert all(isinstance(component, float) for component in state) + + +def test_eci2ecef_state_numpy(): + pytest.importorskip("astropy") + + state = pm.eci2ecef_state(*ECI_STATE, UTC, force_non_astropy=True) + ref = pm.eci2ecef_state(*ECI_STATE, UTC) + + assert state[:3] == approx(ref[:3], abs=60) + assert state[3:] == approx(ref[3:], abs=0.05) + + +def test_ecef2eci_state_numpy(): + pytest.importorskip("astropy") + + ecef_state = pm.eci2ecef_state(*ECI_STATE, UTC) + state = pm.ecef2eci_state(*ecef_state, UTC, force_non_astropy=True) + ref = pm.ecef2eci_state(*ecef_state, UTC) + + assert state[:3] == approx(ref[:3], abs=60) + assert state[3:] == approx(ref[3:], abs=0.05) + + +def test_eci2geodetic(): lla = pm.eci2geodetic(*ECI, UTC) - rel = 0.01 if astropy is None else 0.0001 + rel = 2e-6 if astropy is None else 0.0001 assert lla == approx([27.880801, -163.722058, 408850.646], rel=rel) def test_geodetic2eci(): - pytest.importorskip("numpy") - lla = [27.880801, -163.722058, 408850.646] eci = pm.geodetic2eci(*lla, UTC) - rel = 0.01 if astropy is None else 0.0001 + rel = 2e-5 if astropy is None else 0.0001 assert eci == approx([-2981784, 5207055, 3161595], rel=rel) +def test_eci_ecef_eop_roundtrip(): + eop = {"delta_ut1": -0.139198, "xp": 0.094, "yp": 0.324} + + ecef = pm.eci2ecef(*ECI, UTC, force_non_astropy=True, **eop) + eci = pm.ecef2eci(*ecef, UTC, force_non_astropy=True, **eop) + + assert eci == approx(ECI, abs=1e-6) + + +def test_eci_ecef_state_eop_roundtrip(): + eop = {"delta_ut1": -0.139198, "xp": 0.094, "yp": 0.324} + + ecef_state = pm.eci2ecef_state(*ECI_STATE, UTC, force_non_astropy=True, **eop) + eci_state = pm.ecef2eci_state(*ecef_state, UTC, force_non_astropy=True, **eop) + + assert eci_state == approx(ECI_STATE, abs=1e-6) + + +def test_stationary_ecef_point_has_nonzero_eci_velocity(): + xyz = pm.geodetic2ecef(28, -80, 100) + + eci_state = pm.ecef2eci_state(*xyz, 0.0, 0.0, 0.0, UTC, force_non_astropy=True) + + assert abs(eci_state[3]) > 100 + assert abs(eci_state[4]) > 100 + + ecef_state = pm.eci2ecef_state(*eci_state, UTC, force_non_astropy=True) + + assert ecef_state == approx((*xyz, 0.0, 0.0, 0.0), abs=1e-6) + + def test_eci_aer(): # test coords from Matlab eci2aer pytest.importorskip("numpy") diff --git a/src/pymap3d/tests/test_karney.py b/src/pymap3d/tests/test_karney.py new file mode 100644 index 0000000..2db0761 --- /dev/null +++ b/src/pymap3d/tests/test_karney.py @@ -0,0 +1,118 @@ +"""Tests for Karney's geodesic algorithms.""" + +import math + +import pymap3d.karney as karney +import pytest +from pytest import approx + + +class TestGeodesicInverse: + def test_coincident_points(self): + dist, azi1, azi2 = karney.geodesic_inverse(0, 0, 0, 0) + assert dist == approx(0, abs=1e-6) + + def test_equatorial(self): + """Quarter circumference along equator.""" + dist, azi1, azi2 = karney.geodesic_inverse(0, 0, 0, 90) + assert dist == approx(1.001875e7, rel=0.001) + assert azi1 == approx(90, abs=0.01) + + def test_meridional(self): + """Pole to equator along meridian.""" + dist, azi1, azi2 = karney.geodesic_inverse(90, 0, 0, 0) + assert dist == approx(1.00019657e7, rel=0.001) + assert azi1 == approx(180, abs=0.01) + + def test_pole_to_pole(self): + """North pole to south pole.""" + dist, azi1, azi2 = karney.geodesic_inverse(90, 0, -90, 0) + assert dist == approx(2.000393145e7, rel=0.001) + + def test_near_antipodal(self): + """Near-antipodal case where Vincenty struggles.""" + dist, azi1, azi2 = karney.geodesic_inverse(0, 0, 0.5, 179.5) + assert dist == approx(1.9936e7, rel=0.01) + + def test_short_distance(self): + """Short distance (~3km).""" + dist, azi1, azi2 = karney.geodesic_inverse(10, 20, 10.02137267, 20.0168471) + assert dist == approx(3e3, rel=0.01) + + def test_radians(self): + dist, azi1, azi2 = karney.geodesic_inverse(0, 0, 0, math.pi / 2, deg=False) + assert dist == approx(1.001875e7, rel=0.001) + + def test_agreement_with_vincenty(self): + """For non-pathological cases, should agree with Vincenty.""" + pytest.importorskip("pymap3d.vincenty") + import pymap3d.vincenty as vincenty + + lat1, lon1 = 40.0, -74.0 + lat2, lon2 = 48.8, 2.35 + + dist_k, azi1_k, _ = karney.geodesic_inverse(lat1, lon1, lat2, lon2) + dist_v, azi1_v = vincenty.vdist(lat1, lon1, lat2, lon2) + + assert dist_k == approx(dist_v, rel=1e-5) + assert azi1_k == approx(azi1_v, rel=1e-4) + + +class TestGeodesicDirect: + def test_zero_distance(self): + lat2, lon2, azi2 = karney.geodesic_direct(0, 0, 90, 0) + assert lat2 == approx(0, abs=1e-10) + assert lon2 == approx(0, abs=1e-10) + + def test_equatorial_east(self): + """Travel east along equator.""" + lat2, lon2, azi2 = karney.geodesic_direct(0, 0, 90, 1.001875e7) + assert lat2 == approx(0, abs=0.01) + assert lon2 == approx(90, abs=0.1) + + def test_north_from_equator(self): + """Travel north from equator.""" + lat2, lon2, azi2 = karney.geodesic_direct(0, 0, 0, 1.00019657e7) + assert lat2 == approx(90, abs=0.1) + + def test_roundtrip(self): + """Direct then inverse should recover distance and azimuth.""" + lat1, lon1, azi1 = 40.0, -74.0, 45.0 + d = 1e6 + + lat2, lon2, azi2 = karney.geodesic_direct(lat1, lon1, azi1, d) + dist_back, azi1_back, _ = karney.geodesic_inverse(lat1, lon1, lat2, lon2) + + assert dist_back == approx(d, rel=1e-6) + assert azi1_back == approx(azi1, abs=0.01) + + +class TestGeodesicLine: + def test_endpoints(self): + """First and last points should match the given endpoints.""" + lats, lons = karney.geodesic_line(0, 0, 0, 90, npts=10) + assert lats[0] == approx(0, abs=1e-6) + assert lons[0] == approx(0, abs=1e-6) + assert lats[-1] == approx(0, abs=0.1) + assert lons[-1] == approx(90, abs=0.1) + + def test_npts(self): + lats, lons = karney.geodesic_line(0, 0, 45, 45, npts=50) + assert len(lats) == 50 + assert len(lons) == 50 + + +class TestGeodesicArea: + def test_triangle(self): + """Small equilateral-ish triangle.""" + lats = [0, 1, 0.5] + lons = [0, 0, 0.866] + area, perim = karney.geodesic_area(lats, lons) + # Should be positive (CCW) and roughly match planar area + assert area > 0 + assert perim > 0 + + def test_too_few_points(self): + area, perim = karney.geodesic_area([0, 1], [0, 1]) + assert area == 0.0 + assert perim == 0.0 diff --git a/src/pymap3d/tests/test_nvector_ops.py b/src/pymap3d/tests/test_nvector_ops.py new file mode 100644 index 0000000..edcf8c1 --- /dev/null +++ b/src/pymap3d/tests/test_nvector_ops.py @@ -0,0 +1,181 @@ +"""Tests for n-vector spherical geometry operations.""" + +import math + +import pymap3d as pm +from pymap3d.nvector import ( + nvector_distance, + nvector_interpolate, + nvector_mean, + nvector_cross_track_distance, + nvector_intersection, + geodetic2nvector, +) +import pytest +from pytest import approx + + +# Mean Earth radius (equal-volume sphere) +R = 6371000.8 + + +def _g2n(lat, lon): + """Shorthand: geodetic degrees to n-vector.""" + return geodetic2nvector(lat, lon, deg=True) + + +class TestNvectorDistance: + def test_same_point(self): + n = _g2n(45, 90) + assert nvector_distance(*n, *n) == approx(0, abs=1e-10) + + def test_antipodal(self): + n1 = _g2n(0, 0) + n2 = _g2n(0, 180) + assert nvector_distance(*n1, *n2) == approx(math.pi * R, rel=1e-6) + + def test_pole_to_equator(self): + n1 = _g2n(90, 0) + n2 = _g2n(0, 0) + assert nvector_distance(*n1, *n2) == approx(math.pi / 2 * R, rel=1e-6) + + def test_known_distance(self): + # London (51.5, -0.1) to Paris (48.9, 2.35) ~338 km (spherical) + n1 = _g2n(51.5, -0.1) + n2 = _g2n(48.9, 2.35) + dist = nvector_distance(*n1, *n2) + assert dist == approx(337582, rel=0.001) + + def test_custom_radius(self): + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + dist = nvector_distance(*n1, *n2, radius=1.0) + assert dist == approx(math.pi / 2, rel=1e-10) + + +class TestNvectorInterpolate: + def test_fraction_zero(self): + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + result = nvector_interpolate(*n1, *n2, 0.0) + assert result[0] == approx(n1[0], abs=1e-10) + assert result[1] == approx(n1[1], abs=1e-10) + assert result[2] == approx(n1[2], abs=1e-10) + + def test_fraction_one(self): + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + result = nvector_interpolate(*n1, *n2, 1.0) + assert result[0] == approx(n2[0], abs=1e-10) + assert result[1] == approx(n2[1], abs=1e-10) + assert result[2] == approx(n2[2], abs=1e-10) + + def test_midpoint_equator(self): + # Midpoint between (0,0) and (0,90) should be (0,45) + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + result = nvector_interpolate(*n1, *n2, 0.5) + expected = _g2n(0, 45) + assert result[0] == approx(expected[0], abs=1e-10) + assert result[1] == approx(expected[1], abs=1e-10) + assert result[2] == approx(expected[2], abs=1e-10) + + def test_midpoint_meridian(self): + # Midpoint between (0,0) and (90,0) should be (45,0) + n1 = _g2n(0, 0) + n2 = _g2n(90, 0) + result = nvector_interpolate(*n1, *n2, 0.5) + expected = _g2n(45, 0) + assert result[0] == approx(expected[0], abs=1e-10) + assert result[1] == approx(expected[1], abs=1e-10) + assert result[2] == approx(expected[2], abs=1e-10) + + def test_same_point(self): + n = _g2n(30, 60) + result = nvector_interpolate(*n, *n, 0.5) + assert result[0] == approx(n[0], abs=1e-10) + assert result[1] == approx(n[1], abs=1e-10) + assert result[2] == approx(n[2], abs=1e-10) + + +class TestNvectorMean: + def test_single_point(self): + n = _g2n(45, 90) + result = nvector_mean([n[0]], [n[1]], [n[2]]) + assert result[0] == approx(n[0], abs=1e-10) + assert result[1] == approx(n[1], abs=1e-10) + assert result[2] == approx(n[2], abs=1e-10) + + def test_symmetric_points(self): + # Points symmetric about the equator at lon=0 should average to (0, 0) + n1 = _g2n(30, 0) + n2 = _g2n(-30, 0) + result = nvector_mean([n1[0], n2[0]], [n1[1], n2[1]], [n1[2], n2[2]]) + expected = _g2n(0, 0) + assert result[0] == approx(expected[0], abs=1e-10) + assert result[1] == approx(expected[1], abs=1e-10) + assert result[2] == approx(expected[2], abs=1e-10) + + def test_four_symmetric_points(self): + # Four points at (45,0), (45,90), (45,180), (45,270) -> mean at north pole + pts = [_g2n(45, lon) for lon in [0, 90, 180, 270]] + n1s = [p[0] for p in pts] + n2s = [p[1] for p in pts] + n3s = [p[2] for p in pts] + result = nvector_mean(n1s, n2s, n3s) + expected = _g2n(90, 0) + assert result[2] == approx(expected[2], abs=1e-6) + + +class TestNvectorCrossTrackDistance: + def test_point_on_path(self): + # Point on the equator, path along the equator -> distance = 0 + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + np_ = _g2n(0, 45) + assert nvector_cross_track_distance(*n1, *n2, *np_) == approx(0, abs=1) + + def test_pole_to_equator_path(self): + # North pole, path along the equator -> distance = R * pi/2 + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + np_ = _g2n(90, 0) + dist = nvector_cross_track_distance(*n1, *n2, *np_) + assert abs(dist) == approx(math.pi / 2 * R, rel=1e-6) + + def test_sign(self): + # Point north of eastward equatorial path should be positive (left) + n1 = _g2n(0, 0) + n2 = _g2n(0, 90) + np_north = _g2n(10, 45) + np_south = _g2n(-10, 45) + d_north = nvector_cross_track_distance(*n1, *n2, *np_north) + d_south = nvector_cross_track_distance(*n1, *n2, *np_south) + assert d_north > 0 + assert d_south < 0 + + +class TestNvectorIntersection: + def test_equator_and_meridian(self): + # Equator path (0,0)-(0,90) intersects meridian (90,0)-(-90,0) + # Intersection should be at (0,0) or (0,180) + n1 = _g2n(0, -10) + n2 = _g2n(0, 10) + n3 = _g2n(10, 0) + n4 = _g2n(-10, 0) + result = nvector_intersection(*n1, *n2, *n3, *n4) + # Should be near (0, 0) + expected = _g2n(0, 0) + assert result[0] == approx(expected[0], abs=1e-10) + assert result[1] == approx(expected[1], abs=1e-10) + assert result[2] == approx(expected[2], abs=1e-10) + + def test_perpendicular_meridians(self): + # Meridian at lon=0 and meridian at lon=90 intersect at the poles + n1 = _g2n(10, 0) + n2 = _g2n(20, 0) + n3 = _g2n(10, 90) + n4 = _g2n(20, 90) + result = nvector_intersection(*n1, *n2, *n3, *n4) + # Should be near north pole (n3 component ~1) + assert abs(result[2]) == approx(1.0, abs=1e-10) diff --git a/src/pymap3d/vallado.py b/src/pymap3d/vallado.py index b21fb24..586f4e1 100644 --- a/src/pymap3d/vallado.py +++ b/src/pymap3d/vallado.py @@ -22,6 +22,8 @@ def azel2radec( lat_deg: float, lon_deg: float, time: datetime, + *, + delta_ut1: float = 0.0, ) -> tuple[float, float]: """ converts azimuth, elevation to right ascension, declination @@ -68,7 +70,7 @@ def azel2radec( (sin(el) - sin(lat) * sin(dec)) / (cos(dec) * cos(lat)), ) - lst = datetime2sidereal(time, lon) # lon, ra in RADIANS + lst = datetime2sidereal(time, lon, delta_ut1=delta_ut1) # lon, ra in RADIANS """ by definition right ascension [0, 360) degrees """ return degrees(lst - lha) % 360, degrees(dec) @@ -80,6 +82,8 @@ def radec2azel( lat_deg: float, lon_deg: float, time: datetime, + *, + delta_ut1: float = 0.0, ) -> tuple[float, float]: """ converts right ascension, declination to azimuth, elevation @@ -118,7 +122,7 @@ def radec2azel( lat = radians(lat_deg) lon = radians(lon_deg) - lst = datetime2sidereal(time, lon) # RADIANS + lst = datetime2sidereal(time, lon, delta_ut1=delta_ut1) # RADIANS # %% Eq. 4-11 p. 267 LOCAL HOUR ANGLE lha = lst - ra # %% #Eq. 4-12 p. 267