forked from ET-NCMP/MarineQC
-
Notifications
You must be signed in to change notification settings - Fork 0
/
spherical_geometry.py
215 lines (175 loc) · 7.17 KB
/
spherical_geometry.py
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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
import numpy as np
import math
"""
The spherical geometry module is a simple collection of calculations on a sphere
Sourced from http://williams.best.vwh.net/avform.htm
"""
earths_radius = 6371.0088
radians_per_degree = np.pi / 180.
def sphere_distance(lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points on the sphere designated
by their latitude and longitude
:param lat1: latitude of first point
:param lon1: longitude of first point
:param lat2: latitude of second point
:param lon2: longitude of second point
:type lat1: float
:type lon1: float
:type lat2: float
:type lon2: float
:return: return the great circle distance in kilometres between the two points
:rtype: float
The great circle distance is the shortest distance between any two points on the Earths surface.
The calculation is done by first calulating the angular distance between the points and then
multiplying that by the radius of the Earth. The angular distance calculation is handled by
another function.
"""
delta = angular_distance(lat1, lon1, lat2, lon2) * earths_radius
return delta
def angular_distance(lat1, lon1, lat2, lon2):
"""
calculate distance between two points on a sphere
input latitudes and longitudes should be in degrees
output is in radians
:param lat1: latitude of first point
:param lon1: longitude of first point
:param lat2: latitude of second point
:param lon2: longitude of second point
:type lat1: float
:type lon1: float
:type lat2: float
:type lon2: float
:return: return the angular great circle distance between the two points in radians
:rtype: float
"""
assert lat1 is not None and not (math.isnan(lat1))
assert lon1 is not None and not (math.isnan(lon1))
assert lat2 is not None and not (math.isnan(lat2))
assert lon2 is not None and not (math.isnan(lon2))
# convert degrees to radians
lat1 = lat1 * radians_per_degree
lon1 = lon1 * radians_per_degree
lat2 = lat2 * radians_per_degree
lon2 = lon2 * radians_per_degree
delta_lambda = abs(lon1 - lon2)
bit1 = (np.cos(lat2) * np.sin(delta_lambda))
bit1 = bit1 * bit1
bit2 = (np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(lat2) * np.cos(delta_lambda))
bit2 = bit2 * bit2
topbit = bit1 + bit2
topbit = np.sqrt(topbit)
bottombit = np.sin(lat1) * np.sin(lat2) + np.cos(lat1) * np.cos(lat2) * np.cos(delta_lambda)
delta = np.arctan2(topbit, bottombit)
return delta
def lat_lon_from_course_and_distance(lat1, lon1, tc, d):
"""
calculate a latitude and longitude given a starting point, course (in radian) and
angular distance (also in radians) #http://williams.best.vwh.net/avform.htm#LL
:param lat1: latitude of first point in degrees
:param lon1: longitude of first point in degrees
:param tc: true course measured clockwise from north in degrees
:param d: distance travelled in kilometres
:type lat1: float
:type lon1: float
:return: return the new latitude and longitude
:rtype: float
"""
lat1 = lat1 * radians_per_degree
lon1 = lon1 * radians_per_degree
tcr = tc * radians_per_degree
dr = d / earths_radius
lat = np.arcsin(np.sin(lat1) * np.cos(dr) + np.cos(lat1) * np.sin(dr) * np.cos(tcr))
dlon = np.arctan2(np.sin(tcr) * np.sin(dr) * np.cos(lat1), np.cos(dr) - np.sin(lat1) * np.sin(lat))
lon = math.fmod(lon1 + dlon + np.pi, 2. * np.pi) - np.pi
lat = lat / radians_per_degree
lon = lon / radians_per_degree
return lat, lon
def course_between_points(lat1, lon1, lat2, lon2):
"""
given two points find the initial true course at point1
inputs are in degrees and output is in degrees
:param lat1: latitude of first point in degrees
:param lon1: longitude of first point in degrees
:param lat2: latitude of second point in degrees
:param lon2: longitude of second point in degrees
:type lat1: float
:type lon1: float
:type lat2: float
:type lon2: float
:return: return the initial true course in degrees at point one along the great circle between point
one and point two
:rtype: float
"""
d = angular_distance(lat1, lon1, lat2, lon2)
if d != 0:
lat1 = lat1 * radians_per_degree
lon1 = lon1 * radians_per_degree
lat2 = lat2 * radians_per_degree
lon2 = lon2 * radians_per_degree
if np.cos(lat1) < 0.0000001:
if lat1 > 0:
tc1 = np.pi
else:
tc1 = 2. * np.pi
else:
if np.sin(lon2 - lon1) > 0:
if 1.0 >= (np.sin(lat2) - np.sin(lat1) * np.cos(d)) / (np.sin(d) * np.cos(lat1)) >= -1.0:
tc1 = np.arccos(
(np.sin(lat2) - np.sin(lat1) * np.cos(d)) /
(np.sin(d) * np.cos(lat1))
)
else:
tc1 = float('nan')
else:
if 1.0 >= (np.sin(lat2) - np.sin(lat1) * np.cos(d)) / (np.sin(d) * np.cos(lat1)) >= -1.0:
tc1 = 2. * np.pi - np.arccos(
(np.sin(lat2) - np.sin(lat1) * np.cos(d)) /
(np.sin(d) * np.cos(lat1))
)
else:
tc1 = float('nan')
if math.isnan(tc1):
tc1 = math.fmod(np.arctan2(np.sin(lon1 - lon2) * np.cos(lat2),
np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(lat2) * np.cos(lon1 - lon2)),
2 * np.pi)
else:
tc1 = 0.0
return tc1 / radians_per_degree
def intermediate_point(lat1, lon1, lat2, lon2, f):
"""
given two lat,lon point find the latitude and longitude that are a fraction f
of the great circle distance between them http://williams.best.vwh.net/avform.htm#Intermediate
:param lat1: latitude of first point in degrees
:param lon1: longitude of first point in degrees
:param lat2: latitude of second point in degrees
:param lon2: longitude of second point in degrees
:param f: fraction of distance between the two points
:type lat1: float
:type lon1: float
:type lat2: float
:type lon2: float
:type f: float
:return: return the latitude and longitude of the point a fraction f along the great circle between the
first and second points.
:rtype: float
"""
assert f <= 1.0, f
d = angular_distance(lat1, lon1, lat2, lon2)
if d != 0.0:
# convert degrees to radians
lat1 = lat1 * radians_per_degree
lon1 = lon1 * radians_per_degree
lat2 = lat2 * radians_per_degree
lon2 = lon2 * radians_per_degree
a = np.sin((1 - f) * d) / np.sin(d)
b = np.sin(f * d) / np.sin(d)
x = a * np.cos(lat1) * np.cos(lon1) + b * np.cos(lat2) * np.cos(lon2)
y = a * np.cos(lat1) * np.sin(lon1) + b * np.cos(lat2) * np.sin(lon2)
z = a * np.sin(lat1) + b * np.sin(lat2)
lat = np.arctan2(z, np.sqrt(x * x + y * y)) / radians_per_degree
lon = np.arctan2(y, x) / radians_per_degree
else:
lat = lat1
lon = lon1
return lat, lon