mirror of
https://github.com/liberatedsystems/Sideband_CE.git
synced 2024-09-03 04:13:27 +02:00
Added az/alt calculator to geodesy functions, fixed error in euclidian distance calculation
This commit is contained in:
parent
815f85adeb
commit
628c93e1f1
@ -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("")
|
||||||
|
Loading…
Reference in New Issue
Block a user