From 628c93e1f1f1a86d35e8270bdc915d384339cb6a Mon Sep 17 00:00:00 2001 From: Mark Qvist Date: Wed, 25 Oct 2023 01:35:46 +0200 Subject: [PATCH] Added az/alt calculator to geodesy functions, fixed error in euclidian distance calculation --- sbapp/sideband/geo.py | 130 +++++++++++++++++++++++++++++++++--------- 1 file changed, 103 insertions(+), 27 deletions(-) diff --git a/sbapp/sideband/geo.py b/sbapp/sideband/geo.py index 3aea02b..0900529 100644 --- a/sbapp/sideband/geo.py +++ b/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) +# us = time.time() +# 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("")