-
Notifications
You must be signed in to change notification settings - Fork 1
/
anglefunc.py
55 lines (46 loc) · 1.41 KB
/
anglefunc.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
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on 2018/3/6 15:19
@author: Pete
@email: [email protected]
@file: anglefunc.py
@software: PyCharm Community Edition
"""
import math
"""
General functions to deal with angles
"""
def angleNormalize(angle):
"""
Keep angle in {0, 2pi)
:param angle:
:return: Normalized angle value
"""
return (angle % (2.0 * math.pi) + 2.0 * math.pi) % (2.0 * math.pi)
def meanAngle(angleList, normalize = True):
"""
Calculate the average of a set of circular data in radian
:param angleList: a set of circular data
:return: average value in radian
"""
sinSum = 0
cosSum = 0
for angle in angleList:
sinSum += math.sin(angle)
cosSum += math.cos(angle)
meanValue = math.atan2(sinSum, cosSum)
return angleNormalize(meanValue) if normalize else meanValue
"""
General functions to deal with walking directions
"""
DHT = (50.0 * math.pi) / 180.0 # Threshold for the delta of direction
def isHeadingMatch(headingDir, baseDirection):
headingDir = angleNormalize(headingDir) # [0, 2pi)
baseDirection = angleNormalize(baseDirection) # [0, 2pi)
diffAngle = headingDir - baseDirection if headingDir >= baseDirection else baseDirection - headingDir
if diffAngle > math.pi:
diffAngle -= 2.0 * math.pi
return True if math.fabs(diffAngle) < DHT else False
if __name__ == "__main__":
print("Done.")