loxodrome_direct: stability near 90,-90,270,-270 azimuth. Fixes #42

geospace-code · Oct 17, 2021 · 5b8cf68 · 5b8cf68
1 parent 94c2ebb
commit 5b8cf68
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 16 deletions.
diff --git a/Examples/lox_stability.py b/Examples/lox_stability.py
@@ -28,14 +28,15 @@ def matlab_func(lat1: float, lon1: float, rng: float, az: float) -> tuple[float,
 clat, clon, rng = 40.0, -80.0, 10.0  # arbitrary
 
 for i in range(20):
-    azi = 90.0 + 10.0 ** (-i)
-
-    lat, lon = loxodrome_direct(clat, clon, rng, azi)
-
-    assert lat != lat_last
-    assert lon != lon_last
-
-    lat_matlab, lon_matlab = matlab_func(clat, clon, rng, azi)
-    rstr = f"azimuth: {azi} lat,lon: Python: {lat}, {lon}  Matlab: {lat_matlab}, {lon_matlab}"
-    if not (isclose(lat_matlab, lat, rel_tol=0.005) and isclose(lon_matlab, lon, rel_tol=0.001)):
-        logging.error(rstr)
+    for azi in (90 + 10.0 ** (-i), -90 + 10.0 ** (-i), 270 + 10.0 ** (-i), -270 + 10.0 ** (-i)):
+        lat, lon = loxodrome_direct(clat, clon, rng, azi)
+
+        assert lat != lat_last
+        assert lon != lon_last
+
+        lat_matlab, lon_matlab = matlab_func(clat, clon, rng, azi)
+        rstr = f"azimuth: {azi} lat,lon: Python: {lat}, {lon}  Matlab: {lat_matlab}, {lon_matlab}"
+        if not (
+            isclose(lat_matlab, lat, rel_tol=0.005) and isclose(lon_matlab, lon, rel_tol=0.001)
+        ):
+            logging.error(rstr)
diff --git a/src/pymap3d/enu.py b/src/pymap3d/enu.py
@@ -47,7 +47,7 @@ def enu2aer(
         slant range [meters]
     """
 
-    # 1 millimeter precision for singularity
+    # 1 millimeter precision for singularity stability
 
     try:
         e[abs(e) < 1e-3] = 0.0

diff --git a/src/pymap3d/lox.py b/src/pymap3d/lox.py
@@ -167,10 +167,10 @@ def loxodrome_direct(
     lat1: float | ndarray,
     lon1: float | ndarray,
     rng: float | ndarray,
-    a12: float | float,
+    a12: float,
     ell: Ellipsoid = None,
     deg: bool = True,
-) -> tuple[ndarray, ndarray]:
+) -> tuple[float | ndarray, float | ndarray]:
     """
     Given starting lat, lon with arclength and azimuth, compute final lat, lon
 
@@ -226,14 +226,24 @@ def loxodrome_direct(
     newiso = geodetic2isometric(lat2, ell, deg=False)
     iso = geodetic2isometric(lat1, ell, deg=False)
 
-    dlon = tan(a12) * (newiso - iso)
+    # stability near singularities
+    if (
+        abs(a12 - pi / 2) <= 1e-9
+        or abs(-a12 - pi / 2) <= 1e-9
+        or abs(a12 - 3 * pi / 2) <= 1e-9
+        or abs(-a12 - 3 * pi / 2) <= 1e-9
+    ):
+        dlon = newiso - iso
+    else:
+        dlon = tan(a12) * (newiso - iso)
+
     lon2 = lon1 + dlon
 
     if deg:
         lat2, lon2 = degrees(lat2), degrees(lon2)
 
     try:
-        return lat2.squeeze()[()], lon2.squeeze()[()]
+        return lat2.squeeze()[()], lon2.squeeze()[()]  # type: ignore
     except AttributeError:
         return lat2, lon2