Last active
October 9, 2022 22:36
-
-
Save stephenHartzell/00e812ceef6901f30290203945e2cf8b to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
def los_to_earth(position, pointing): | |
"""Find the intersection of a pointing vector with the Earth | |
Finds the intersection of a pointing vector u and starting point s with the WGS-84 geoid | |
Args: | |
position (np.array): length 3 array defining the starting point location(s) in meters | |
pointing (np.array): length 3 array defining the pointing vector(s) (must be a unit vector) | |
Returns: | |
np.array: length 3 defining the point(s) of intersection with the surface of the Earth in meters | |
""" | |
a = 6378137.0 | |
b = 6378137.0 | |
c = 6356752.314245 | |
x = position[0] | |
y = position[1] | |
z = position[2] | |
u = pointing[0] | |
v = pointing[1] | |
w = pointing[2] | |
value = -a**2*b**2*w*z - a**2*c**2*v*y - b**2*c**2*u*x | |
radical = a**2*b**2*w**2 + a**2*c**2*v**2 - a**2*v**2*z**2 + 2*a**2*v*w*y*z - a**2*w**2*y**2 + b**2*c**2*u**2 - b**2*u**2*z**2 + 2*b**2*u*w*x*z - b**2*w**2*x**2 - c**2*u**2*y**2 + 2*c**2*u*v*x*y - c**2*v**2*x**2 | |
magnitude = a**2*b**2*w**2 + a**2*c**2*v**2 + b**2*c**2*u**2 | |
if radical < 0: | |
raise ValueError("The Line-of-Sight vector does not point toward the Earth") | |
d = (value - a*b*c*np.sqrt(radical)) / magnitude | |
if d < 0: | |
raise ValueError("The Line-of-Sight vector does not point toward the Earth") | |
return np.array([ | |
x + d * u, | |
y + d * v, | |
z + d * w, | |
]) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Fixed it! Thanks! I don't how the previous number got in there.