Added az/alt calculator to geodesy functions, fixed error in euclidian distance calculation

sbapp/sideband/geo.py

@@ -2,12 +2,19 @@ import time
 from math import pi, sin, cos, acos, tan, atan, atan2
 from math import radians, degrees, sqrt
 
+# WGS84 Parameters
+# a  = 6378137.0,
+# f  = 0.0033528106647474805,
+# e2 = 0.0066943799901413165,
+# b  = 6356752.314245179,
 
-# Default planetary metrics
-equatorial_radius    = 6378.137     *1e3
+# 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):
@@ -52,7 +59,7 @@ def euclidian_point(latitude, longtitude, altitude=0, ellipsoid=True):
     # Calculate euclidian coordinates from longtitude
     # and geocentric latitude.
     gclat = radians(geocentric_latitude(latitude)) if ellipsoid else lat
-    x = cos(lat)*cos(lon)*r
+    x = cos(lon)*cos(gclat)*r
     y = cos(gclat)*sin(lon)*r
     z = sin(gclat)*r
 
@@ -67,21 +74,23 @@ def euclidian_point(latitude, longtitude, altitude=0, ellipsoid=True):
         y += altitude*normal_y
         z += altitude*normal_z
 
-    return (x,y,z)
+    return (x,y,z, normal_x, normal_y, normal_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)
+    return sqrt(dx*dx + dy*dy + dz*dz)
 
 def euclidian_distance(c1, c2, ellipsoid=True):
+    lat1 = c1[0]; lon1 = c1[1]; alt1 = c1[2]
+    lat2 = c2[0]; lon2 = c2[1]; alt2 = c2[2]
     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)
+            euclidian_point(lat1, lon1, alt1, ellipsoid=ellipsoid),
+            euclidian_point(lat2, lon2, alt2, ellipsoid=ellipsoid)
         )
     else:
         return None
@@ -94,6 +103,9 @@ 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[:2] == c2[:2]:
+            return 0
+
         if c1[0] == 0.0: c1 = (1e-6, c1[1])
         a = equatorial_radius
         f = ellipsoid_flattening
@@ -151,40 +163,104 @@ def ellipsoid_distance(c1, c2):
     except Exception as e:
         return None
 
-def orthodromic_distance(c1, c2, spherical=False):
-    if spherical:
-        return spherical_distance(c1, c2)
-    else:
+def azalt(c1, c2, ellipsoid=True):              
+    c2rp = rotate_globe(c1, c2, ellipsoid=ellipsoid)
+    print(str(c2rp))
+
+    altitude = None
+    azimuth = None
+    if (c2rp[2]*c2rp[2]) + (c2rp[1]*c2rp[1]) > 1e-6:
+        theta = degrees(atan2(c2rp[2], c2rp[1]))
+        azimuth = 90.0 - theta
+        if azimuth < 0: azimuth += 360
+        if azimuth > 360: azimuth -= 360
+        azimuth = round(azimuth,4)
+
+    c1p = euclidian_point(c1[0], c1[1], c1[2], ellipsoid=ellipsoid)
+    c2p = euclidian_point(c2[0], c2[1], c2[2], ellipsoid=ellipsoid)
+    nvd = normalised_vector_diff(c2p, c1p)
+    if nvd != None:
+        cax = nvd[0]; cay = nvd[1]; caz = nvd[2]
+        cnx = c1p[3]; cny = c1p[4]; cnz = c1p[5]
+        a = acos(cax*cnx + cay*cny + caz*cnz)
+        altitude = round(90 - degrees(a),4)
+
+    return (azimuth, altitude,4)
+
+def normalised_vector_diff(b, a):
+    dx = b[0] - a[0]
+    dy = b[1] - a[1]
+    dz = b[2] - a[2]
+    d_squared = dx*dx + dy*dy + dz*dz
+    if d_squared == 0:
+        return None
+
+    d = sqrt(d_squared)
+    return (dx/d, dy/d, dz/d)
+
+def rotate_globe(c1, c2, ellipsoid=True):
+    if len(c1) >= 2 and len(c2) >= 2:
+        if len(c1) == 2: c1 += (0,)
+        if len(c2) == 2: c2 += (0,)
+
+        c2r  = (c2[0], c2[1]-c1[1], c2[2])
+        c2rp = euclidian_point(c2r[0], c2r[1], c2r[2], ellipsoid=ellipsoid)
+
+        lat1 = -1*radians(c1[0])
+        if ellipsoid:
+            lat1 = radians(geocentric_latitude(degrees(lat1)))
+
+        lat1cos = cos(lat1)
+        lat1sin = sin(lat1)
+
+        c2x = (c2rp[0] * lat1cos) - (c2rp[2] * lat1sin)
+        c2y = c2rp[1]
+        c2z = (c2rp[0] * lat1sin) + (c2rp[2] * lat1cos)
+
+        return (c2x, c2y, c2z)
+
+def orthodromic_distance(c1, c2, ellipsoid=True):
+    if ellipsoid:
         return ellipsoid_distance(c1, c2)
+    else:
+        return spherical_distance(c1, c2)
 
 # def tests():
 #     import RNS
+#     import numpy as np
 #     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)],
+#         [(51.2308, 4.38703, 0.0), (47.699437, 9.268651, 0.0)],
+#         [(51.230800, 4.38703, 0.0), (51.230801, 4.38703, 0.0)],
+#         [(35.3524, 135.0302, 100), (35.3532,135.0305, 500)],
+#         [(57.758793, 22.605194, 0.0), (43.048838, -9.241343, 0.0)],
+#         [(0.0, 0.0, 0.0), (0.0, 0.0, 0.0)],
+#         [(-90.0, 0.0, 0.0), (90.0, 0.0, 0.0)],
+#         [(-90.0, 0.0, 0.0), (78.0, 0.0, 0.0)],
+#         [(0.0, 0.0, 0.0), (0.5, 179.5, 0.0)],
+#         [(0.7, 0.0, 0.0), (0.0, -180.0, 0.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)
+#         ld = c1+c2; g = geod.Inverse(c1[0], c1[1], c2[0], c2[1])
 #         print("Lib computed in "+str(round((time.time()-us)*1e6, 3))+"us")
 #         us = time.time()        
-#         eld = orthodromic_distance(c1,c2,spherical=False)
+#         eld = orthodromic_distance(c1,c2,ellipsoid=True)
 #         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("Own timed out in "+str(round((time.time()-us)*1e6, 3))+"us")
+#         ed_own = euclidian_distance(c1,c2,ellipsoid=True)
+#         sd_own = orthodromic_distance(c1,c2,ellipsoid=False)
+#         aa = azalt(c1,c2,ellipsoid=True)
+#         fac = 1
+#         if eld: print("LibDiff   = "+RNS.prettydistance(g['s12']-eld)+f"  {fac*g['s12']-fac*eld}")
+#         print("Spherical = "+RNS.prettydistance(sd_own)+f" {fac*sd_own}")
+#         # print("EllipLib  = "+RNS.prettydistance(g['s12'])+f" {fac*g['s12']}")
+#         if eld: print("Ellipsoid = "+RNS.prettydistance(eld)+f" {fac*eld}")
+#         print("Euclidian = "+RNS.prettydistance(ed_own)+f" {fac*ed_own}")
+#         print("AzAlt     = "+f" {aa[0]} / {aa[1]}")
 #         print("")