MH370BackTrack/Geo.py at master · seanmcleod/MH370BackTrack · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
import math

earthRadius = 6371.0

def sphericalToECEF(latlon):
    lat, lon = latlon

    theta = lon
    phi = 90 - lat

    x = earthRadius * math.sin(math.radians(phi)) * math.cos(math.radians(theta))
    y = earthRadius * math.sin(math.radians(theta)) * math.sin(math.radians(phi))
    z = earthRadius * math.cos(math.radians(phi))

    return (x,y,z)

def sphericalToECEFAlt(latlon, alt):
    lat, lon = latlon

    theta = lon
    phi = 90 - lat

    x = (earthRadius + alt) * math.sin(math.radians(phi)) * math.cos(math.radians(theta))
    y = (earthRadius + alt) * math.sin(math.radians(theta)) * math.sin(math.radians(phi))
    z = (earthRadius + alt) * math.cos(math.radians(phi))

    return (x,y,z)


# Haversine great circle distance calculation for spherical earth
def greatCircleDistance(origin, destination):
    lat1, lon1 = origin
    lat2, lon2 = destination
    radius = earthRadius

    dlat = math.radians(lat2-lat1)
    dlon = math.radians(lon2-lon1)
    a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    d = radius * c

    return d

def greatCircleDestination(latlon, bearing, dist):
    R = earthRadius
    lat1, lon1 = latlon
    lat1 = math.radians(lat1)
    lon1 = math.radians(lon1)
    bearing = math.radians(bearing)
    lat2 = math.asin(math.sin(lat1)*math.cos(dist/R) + math.cos(lat1)*math.sin(dist/R)*math.cos(bearing))
    lon2 = lon1 + math.atan2(math.sin(bearing)*math.sin(dist/R)*math.cos(lat1), math.cos(dist/R)-math.sin(lat1)*math.sin(lat2))

    return math.degrees(lat2), math.degrees(lon2)

def bearing(latlon1, latlon2):
	#http://www.movable-type.co.uk/scripts/LatLong.html
    lat1, lon1 = latlon1
    lat1 = math.radians(lat1)
    lon1 = math.radians(lon1)
    lat2, lon2 = latlon2
    lat2 = math.radians(lat2)
    lon2 = math.radians(lon2)
    dlat = lat2-lat1
    dlon = lon2-lon1
    bearing = math.atan2( (math.sin(dlon)*math.cos(lat2)) , (math.cos(lat1)*math.sin(lat2) - math.sin(lat1)*math.cos(lat2)*math.cos(dlon)) )
    bearing = math.degrees(bearing)
    if bearing < 0.0:
        bearing += 360.0

    return bearing

def LOSSpeed(pos1, vel1, pos2, vel2):
    P = (pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2])
    V = (vel1[0]-vel2[0], vel1[1]-vel2[1], vel1[2]-vel2[2])

    # LOS = (V dot P)/|P|

    VdotP = V[0]*P[0] + V[1]*P[1] + V[2]*P[2]
    Pbar = math.sqrt(P[0]*P[0] + P[1]*P[1] + P[2]*P[2])

    return VdotP / Pbar

def ecefVelocities(latlon, speed, bearing):
    x,y,z = sphericalToECEF(latlon)

    latCircum = earthRadius * math.cos(math.radians(latlon[0])) * math.pi * 2.0
    equCircum = earthRadius * math.pi * 2.0

    lonDistance = speed * math.sin(math.radians(bearing))
    latDistance = speed * math.cos(math.radians(bearing))

    thetaDot = (lonDistance / latCircum) * 2 * math.pi
    phiDot = (latDistance / equCircum) * 2 * math.pi

    x2,y2,z2 = sphericalToECEF((latlon[0]+math.degrees(phiDot), latlon[1]+math.degrees(thetaDot)))

    return (x2-x, y2-y, z2-z)

def knotsToKms(knots):
    return (knots * 1.852)/(3600.0)

def nmToKm(nm):
    return nm * 1.852

def kmToNm(km):
    return km / 1.852

def KmsToKmh(kms):
    return kms * 3600.0

def KmsToKnots(kms):
    return KmsToKmh(kms) / 1.852