Added geodesy functions

sbapp/sideband/geo.py

@@ -0,0 +1,190 @@
+import RNS
+import time
+from math import pi, sin, cos, acos, tan, atan, atan2
+from math import radians, degrees, sqrt
+
+
+# Default planetary metrics
+equatorial_radius    = 6378.137     *1e3
+polar_radius         = 6356.7523142 *1e3
+ellipsoid_flattening = 1-(polar_radius/equatorial_radius)
+eccentricity_squared = 2*ellipsoid_flattening-pow(ellipsoid_flattening,2)
+mean_earth_radius    = (1/3)*(2*equatorial_radius+polar_radius)
+
+def central_angle(c1, c2):
+    lat1 = radians(c1[0]); lon1 = radians(c1[1])
+    lat2 = radians(c2[0]); lon2 = radians(c2[1])
+
+    d_lat = abs(lat1-lat2)
+    d_lon = abs(lon1-lon2)
+    ca    = acos(
+        sin(lat1) * sin(lat2) +
+        cos(lat1) * cos(lat2) * cos(d_lon)
+    )
+    return ca
+
+def geocentric_latitude(geodetic_latitude):
+    e2  = eccentricity_squared
+    lat = radians(geodetic_latitude)
+    return degrees(atan((1.0 - e2) * tan(lat)))
+
+def geodetic_latitude(geocentric_latitude):
+    e2  = eccentricity_squared
+    lat = radians(geocentric_latitude)
+    return degrees(atan( (1/(1.0 - e2)) * tan(lat)))
+
+def ellipsoid_radius_at(latitude):
+    lat = radians(latitude)
+    a = equatorial_radius; b = polar_radius;
+    a2 = pow(a,2); b2 = pow(b,2)
+    r   = sqrt(
+        ( pow(a2*cos(lat), 2) + pow(b2*sin(lat), 2) )
+                              /
+         ( pow(a*cos(lat), 2) + pow(b*sin(lat), 2) )
+    )
+    return r
+
+def euclidian_point(latitude, longtitude, altitude=0, ellipsoid=True):
+    # Convert latitude and longtitude to radians
+    # and get ellipsoid or sphere radius
+    lat   = radians(latitude); lon = radians(longtitude)
+    r     = ellipsoid_radius_at(latitude) if ellipsoid else mean_earth_radius
+
+    # Calculate euclidian coordinates from longtitude
+    # and geocentric latitude.
+    gclat = radians(geocentric_latitude(latitude)) if ellipsoid else lat
+    x = cos(lat)*cos(lon)*r
+    y = cos(gclat)*sin(lon)*r
+    z = sin(gclat)*r
+
+    # Calculate surface normal of ellipsoid at
+    # coordinates to add altitude to point
+    normal_x = cos(lat)*cos(lon)
+    normal_y = cos(lat)*sin(lon)
+    normal_z = sin(lat)
+
+    if altitude != 0:
+        x += altitude*normal_x
+        y += altitude*normal_y
+        z += altitude*normal_z
+
+    return (x,y,z)
+
+def distance(p1, p2):
+    dx = p1[0]-p2[0]
+    dy = p1[1]-p2[1]
+    dz = p1[2]-p2[2]
+    return sqrt(dx*dx+dy*dy+dz*dz)
+
+def euclidian_distance(c1, c2, ellipsoid=True):
+    if len(c1) >= 2 and len(c2) >= 2:
+        if len(c1) == 2: c1 += (0,)
+        if len(c2) == 2: c2 += (0,)
+        return distance(
+            euclidian_point(c1[0], c1[1], c1[2], ellipsoid=ellipsoid),
+            euclidian_point(c2[0], c2[1], c2[2], ellipsoid=ellipsoid)
+        )
+    else:
+        return None
+
+def spherical_distance(c1, c2, altitude=0, r=mean_earth_radius):
+    d = (r+altitude)*central_angle(c1, c2)
+    return d
+
+def ellipsoid_distance(c1, c2):
+    # TODO: Update this to the method described by Karney in 2013
+    # instead of using Vincenty's algorithm.
+    try:
+        if c1[0] == 0.0: c1 = (1e-6, c1[1])
+        a = equatorial_radius
+        f = ellipsoid_flattening
+        b = (1 - f)*a # polar radius
+        tolerance = 1e-9 # to stop iteration
+
+        phi1, phi2 = radians(c1[0]), radians(c2[0])
+        U1 = atan((1-f)*tan(phi1))
+        U2 = atan((1-f)*tan(phi2))
+        L1, L2 = radians(c1[1]), radians(c2[1])
+        L = L2 - L1
+
+        lambda_old = L + 0
+
+        max_iterations = 10000
+        iteration = 0
+        timeout = 1.0
+        st = time.time()
+        while True:
+            iteration += 1
+            t = (cos(U2)*sin(lambda_old))**2
+            t += (cos(U1)*sin(U2) - sin(U1)*cos(U2)*cos(lambda_old))**2
+            sin_sigma = t**0.5
+            cos_sigma = sin(U1)*sin(U2) + cos(U1)*cos(U2)*cos(lambda_old)
+            sigma = atan2(sin_sigma, cos_sigma)
+        
+            sin_alpha = cos(U1)*cos(U2)*sin(lambda_old) / sin_sigma
+            cos_sq_alpha = 1 - sin_alpha**2
+            cos_2sigma_m = cos_sigma - 2*sin(U1)*sin(U2)/cos_sq_alpha
+            C = f*cos_sq_alpha*(4 + f*(4-3*cos_sq_alpha))/16
+        
+            t = sigma + C*sin_sigma*(cos_2sigma_m + C*cos_sigma*(-1 + 2*cos_2sigma_m**2))
+            lambda_new = L + (1 - C)*f*sin_alpha*t
+            if abs(lambda_new - lambda_old) <= tolerance:
+                break
+            else:
+                lambda_old = lambda_new
+
+            if iteration%1000 == 0:
+                if iteration >= max_iterations:
+                    return None
+                
+                if time.time() > st+timeout:
+                    return None
+
+        u2 = cos_sq_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)))
+        t = cos_2sigma_m + 0.25*B*(cos_sigma*(-1 + 2*cos_2sigma_m**2))
+        t -= (B/6)*cos_2sigma_m*(-3 + 4*sin_sigma**2)*(-3 + 4*cos_2sigma_m**2)
+        delta_sigma = B * sin_sigma * t
+        s = b*A*(sigma - delta_sigma)
+        return s
+
+    except Exception as e:
+        return None
+
+def orthodromic_distance(c1, c2, spherical=False):
+    if spherical:
+        return spherical_distance(c1, c2)
+    else:
+        return ellipsoid_distance(c1, c2)
+
+# def tests():
+#     from geographiclib.geodesic import Geodesic
+#     geod = Geodesic.WGS84
+#     coords = [
+#         [(57.758793, 22.605194), (43.048838, -9.241343)],
+#         [(0.0, 0.0), (0.0, 0.0)],
+#         [(-90.0, 0.0), (90.0, 0.0)],
+#         [(-90.0, 0.0), (78.0, 0.0)],
+#         [(0.0, 0.0), (0.5, 179.5)],
+#         [(0.7, 0.0), (0.0, -180.0)],
+#     ]
+#     for cs in coords:
+#         c1 = cs[0]; c2 = cs[1]
+#         print("Testing: "+str(c1)+"  ->  "+str(c2))
+#         us = time.time()
+#         ld = c1+c2; g = geod.Inverse(*ld)
+#         print("Lib computed in "+str(round((time.time()-us)*1e6, 3))+"us")
+#         us = time.time()
+#         eld = orthodromic_distance(c1,c2,spherical=False)
+#         if eld:
+#             print("Own computed in "+str(round((time.time()-us)*1e6, 3))+"us")
+#         else:
+#             print("Own TIMED OUT in "+str(round((time.time()-us)*1e6, 3))+"us")
+
+#         print("Euclidian = "+RNS.prettydistance(euclidian_distance(c1,c2)))
+#         print("Spherical = "+RNS.prettydistance(orthodromic_distance(c1,c2)))    
+#         if eld: print("Ellipsoid = "+RNS.prettydistance(eld))
+#         print("EllipLib  = "+RNS.prettydistance(g['s12']))
+#         if eld: print("Diff      = "+RNS.prettydistance(g['s12']-eld))
+#         print("")