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

This commit is contained in:
Mark Qvist 2023-10-25 01:35:46 +02:00
parent 815f85adeb
commit 628c93e1f1

View File

@ -2,12 +2,19 @@ import time
from math import pi, sin, cos, acos, tan, atan, atan2 from math import pi, sin, cos, acos, tan, atan, atan2
from math import radians, degrees, sqrt from math import radians, degrees, sqrt
# WGS84 Parameters
# a = 6378137.0,
# f = 0.0033528106647474805,
# e2 = 0.0066943799901413165,
# b = 6356752.314245179,
# Default planetary metrics # Planetary metrics
equatorial_radius = 6378.137 *1e3 equatorial_radius = 6378.137 *1e3
polar_radius = 6356.7523142 *1e3 polar_radius = 6356.7523142 *1e3
ellipsoid_flattening = 1-(polar_radius/equatorial_radius) ellipsoid_flattening = 1-(polar_radius/equatorial_radius)
eccentricity_squared = 2*ellipsoid_flattening-pow(ellipsoid_flattening,2) eccentricity_squared = 2*ellipsoid_flattening-pow(ellipsoid_flattening,2)
###############################
mean_earth_radius = (1/3)*(2*equatorial_radius+polar_radius) mean_earth_radius = (1/3)*(2*equatorial_radius+polar_radius)
def central_angle(c1, c2): def central_angle(c1, c2):
@ -52,7 +59,7 @@ def euclidian_point(latitude, longtitude, altitude=0, ellipsoid=True):
# Calculate euclidian coordinates from longtitude # Calculate euclidian coordinates from longtitude
# and geocentric latitude. # and geocentric latitude.
gclat = radians(geocentric_latitude(latitude)) if ellipsoid else lat 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 y = cos(gclat)*sin(lon)*r
z = sin(gclat)*r z = sin(gclat)*r
@ -67,7 +74,7 @@ def euclidian_point(latitude, longtitude, altitude=0, ellipsoid=True):
y += altitude*normal_y y += altitude*normal_y
z += altitude*normal_z z += altitude*normal_z
return (x,y,z) return (x,y,z, normal_x, normal_y, normal_z)
def distance(p1, p2): def distance(p1, p2):
dx = p1[0]-p2[0] dx = p1[0]-p2[0]
@ -76,12 +83,14 @@ def distance(p1, p2):
return sqrt(dx*dx + dy*dy + dz*dz) return sqrt(dx*dx + dy*dy + dz*dz)
def euclidian_distance(c1, c2, ellipsoid=True): 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 and len(c2) >= 2:
if len(c1) == 2: c1 += (0,) if len(c1) == 2: c1 += (0,)
if len(c2) == 2: c2 += (0,) if len(c2) == 2: c2 += (0,)
return distance( return distance(
euclidian_point(c1[0], c1[1], c1[2], ellipsoid=ellipsoid), euclidian_point(lat1, lon1, alt1, ellipsoid=ellipsoid),
euclidian_point(c2[0], c2[1], c2[2], ellipsoid=ellipsoid) euclidian_point(lat2, lon2, alt2, ellipsoid=ellipsoid)
) )
else: else:
return None return None
@ -94,6 +103,9 @@ def ellipsoid_distance(c1, c2):
# TODO: Update this to the method described by Karney in 2013 # TODO: Update this to the method described by Karney in 2013
# instead of using Vincenty's algorithm. # instead of using Vincenty's algorithm.
try: try:
if c1[:2] == c2[:2]:
return 0
if c1[0] == 0.0: c1 = (1e-6, c1[1]) if c1[0] == 0.0: c1 = (1e-6, c1[1])
a = equatorial_radius a = equatorial_radius
f = ellipsoid_flattening f = ellipsoid_flattening
@ -151,40 +163,104 @@ def ellipsoid_distance(c1, c2):
except Exception as e: except Exception as e:
return None return None
def orthodromic_distance(c1, c2, spherical=False): def azalt(c1, c2, ellipsoid=True):
if spherical: c2rp = rotate_globe(c1, c2, ellipsoid=ellipsoid)
return spherical_distance(c1, c2) print(str(c2rp))
else:
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) return ellipsoid_distance(c1, c2)
else:
return spherical_distance(c1, c2)
# def tests(): # def tests():
# import RNS # import RNS
# import numpy as np
# from geographiclib.geodesic import Geodesic # from geographiclib.geodesic import Geodesic
# geod = Geodesic.WGS84 # geod = Geodesic.WGS84
# coords = [ # coords = [
# [(57.758793, 22.605194), (43.048838, -9.241343)], # [(51.2308, 4.38703, 0.0), (47.699437, 9.268651, 0.0)],
# [(0.0, 0.0), (0.0, 0.0)], # [(51.230800, 4.38703, 0.0), (51.230801, 4.38703, 0.0)],
# [(-90.0, 0.0), (90.0, 0.0)], # [(35.3524, 135.0302, 100), (35.3532,135.0305, 500)],
# [(-90.0, 0.0), (78.0, 0.0)], # [(57.758793, 22.605194, 0.0), (43.048838, -9.241343, 0.0)],
# [(0.0, 0.0), (0.5, 179.5)], # [(0.0, 0.0, 0.0), (0.0, 0.0, 0.0)],
# [(0.7, 0.0), (0.0, -180.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: # for cs in coords:
# c1 = cs[0]; c2 = cs[1] # c1 = cs[0]; c2 = cs[1]
# print("Testing: "+str(c1)+" -> "+str(c2)) # print("Testing: "+str(c1)+" -> "+str(c2))
# us = time.time() # 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") # print("Lib computed in "+str(round((time.time()-us)*1e6, 3))+"us")
# us = time.time() # us = time.time()
# eld = orthodromic_distance(c1,c2,spherical=False) # eld = orthodromic_distance(c1,c2,ellipsoid=True)
# if eld: # if eld:
# print("Own computed in "+str(round((time.time()-us)*1e6, 3))+"us") # print("Own computed in "+str(round((time.time()-us)*1e6, 3))+"us")
# else: # else:
# print("Own TIMED OUT in "+str(round((time.time()-us)*1e6, 3))+"us") # print("Own timed out in "+str(round((time.time()-us)*1e6, 3))+"us")
# ed_own = euclidian_distance(c1,c2,ellipsoid=True)
# print("Euclidian = "+RNS.prettydistance(euclidian_distance(c1,c2))) # sd_own = orthodromic_distance(c1,c2,ellipsoid=False)
# print("Spherical = "+RNS.prettydistance(orthodromic_distance(c1,c2))) # aa = azalt(c1,c2,ellipsoid=True)
# if eld: print("Ellipsoid = "+RNS.prettydistance(eld)) # fac = 1
# print("EllipLib = "+RNS.prettydistance(g['s12'])) # if eld: print("LibDiff = "+RNS.prettydistance(g['s12']-eld)+f" {fac*g['s12']-fac*eld}")
# if eld: print("Diff = "+RNS.prettydistance(g['s12']-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("") # print("")