-
Notifications
You must be signed in to change notification settings - Fork 11
/
angel_tool.py
74 lines (63 loc) · 4.32 KB
/
angel_tool.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
from math import radians, sin, cos, degrees, atan2, fabs, pi, atan
def get_angle_to_north(lon1, lat1, lon2, lat2):
"""
Args:
point p1(latA, lonA)
point p2(latB, lonB)
Returns:
bearing between the two GPS points,
default: the basis of heading direction is north
"""
lat1_radians = radians(lat1)
lon1_radians = radians(lon1)
lat2_radians = radians(lat2)
lon2_radians = radians(lon2)
lon_diff = lon2_radians - lon1_radians
y = sin(lon_diff) * cos(lat2_radians)
x = cos(lat1_radians) * sin(lat2_radians) - sin(lat1_radians) * cos(lat2_radians) * cos(lon_difference)
return (degrees(atan2(y, x)) + 360) % 360
def calculate_angle_diff(angle1, angle2):
diff = fabs(angle2 - angle1)
if diff > 180:
return 360.0 - diff
else:
return diff
def is_turn(shape):
start_angle = get_angle_to_north(shape[0][0], shape[0][1], shape[1][0], shape[1][1])
end_angle = get_angle_to_north(shape[-2][0], shape[-2][1], shape[-1][0], shape[-1][1])
diff = calculate_angle_diff(start_angle, end_angle)
print diff
if diff > 130:
return True
return False
def angle_avg(angles):
new_angles = []
for elem in angles:
if elem > 90:
elem = 450 - elem
else:
elem = 90 - elem
new_angles.append(elem)
sin_num = 0
cos_num = 0
for i, item in enumerate(new_angles):
sin_num += sin(radians(item))
cos_num += cos(radians(item))
sin_num /= len(new_angles)
cos_num /= len(new_angles)
result = atan(sin_num/cos_num)*(180/pi)
print result
if cos_num >= 0:
result = result
elif cos_num < 0:
result = result + 180
if result < 0:
result = 360 + result
if result < 90:
result = 90 - result
else:
result = 450 - result
return result
#x = [[114.08454, 22.54832], [114.08454, 22.54831], [114.08454, 22.54838], [114.08452, 22.54851], [114.08452, 22.54865], [114.08473, 22.54884], [114.08496, 22.54884], [114.0852, 22.54884], [114.08543, 22.54885], [114.08559, 22.54885], [114.08567, 22.54886], [114.08582, 22.54886], [114.08586, 22.54886], [114.08595, 22.54887], [114.08586, 22.54886], [114.08599, 22.54887], [114.08613, 22.54888], [114.08631, 22.54888], [114.08644, 22.54889], [114.08663, 22.5489], [114.08707, 22.54889], [114.08738, 22.54888], [114.08732, 22.54895], [114.0871, 22.54895], [114.08698, 22.54897], [114.08681, 22.54896], [114.08652, 22.54897], [114.0861, 22.54895], [114.08568, 22.54894], [114.08525, 22.54893], [114.0848, 22.54894], [114.08436, 22.54896], [114.08392, 22.54895], [114.08352, 22.54894], [114.08313, 22.54892], [114.08279, 22.5489], [114.08236, 22.54883], [114.08205, 22.54883], [114.08187, 22.54883], [114.08178, 22.54882], [114.08177, 22.54882], [114.08175, 22.54882], [114.08174, 22.54886], [114.08174, 22.54888], [114.08175, 22.54888], [114.08174, 22.54888], [114.08173, 22.54887], [114.08167, 22.54887], [114.0815, 22.54887], [114.0813, 22.54887], [114.08114, 22.54877], [114.08112, 22.5484], [114.08113, 22.54804], [114.08109, 22.54765], [114.08107, 22.54721], [114.08109, 22.54675], [114.0811, 22.54633], [114.0811, 22.54594], [114.08111, 22.54569], [114.08111, 22.54553], [114.08112, 22.54541], [114.08112, 22.5452], [114.08111, 22.54502], [114.08111, 22.54483], [114.08107, 22.54462], [114.08092, 22.54451], [114.08065, 22.54454], [114.08038, 22.5446], [114.08008, 22.54465], [114.07972, 22.54475], [114.07927, 22.54486], [114.07878, 22.54498], [114.07826, 22.54511], [114.07776, 22.54522], [114.07731, 22.54531], [114.07684, 22.54536], [114.0764, 22.54539], [114.07597, 22.54538], [114.07555, 22.54534], [114.07523, 22.54529], [114.07499, 22.54524], [114.07479, 22.54521], [114.07456, 22.54518], [114.07435, 22.54515], [114.07417, 22.54514], [114.07407, 22.54512], [114.07406, 22.54511], [114.07407, 22.54511], [114.07406, 22.54511], [114.07407, 22.5451], [114.07406, 22.5451], [114.07407, 22.5451], [114.07406, 22.5451], [114.07407, 22.5451], [114.07406, 22.5451], [114.07407, 22.5451], [114.07406, 22.5451], [114.07386, 22.54507], [114.07363, 22.54502], [114.07346, 22.54499], [114.07325, 22.54495], [114.07316, 22.54494], [114.07294, 22.54473], [114.07293, 22.54446], [114.07295, 22.54412], [114.07296, 22.54375], [114.07298, 22.54342], [114.07301, 22.5432]]
x = [360, 0,1]
print angle_avg(x)