- Notifications
You must be signed in to change notification settings - Fork 6.8k
/
Copy pathangle.py
83 lines (63 loc) · 1.7 KB
/
angle.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
importnumpyasnp
fromscipy.spatial.transformimportRotationasRot
defrot_mat_2d(angle):
"""
Create 2D rotation matrix from an angle
Parameters
----------
angle :
Returns
-------
A 2D rotation matrix
Examples
--------
>>> angle_mod(-4.0)
"""
returnRot.from_euler('z', angle).as_matrix()[0:2, 0:2]
defangle_mod(x, zero_2_2pi=False, degree=False):
"""
Angle modulo operation
Default angle modulo range is [-pi, pi)
Parameters
----------
x : float or array_like
A angle or an array of angles. This array is flattened for
the calculation. When an angle is provided, a float angle is returned.
zero_2_2pi : bool, optional
Change angle modulo range to [0, 2pi)
Default is False.
degree : bool, optional
If True, then the given angles are assumed to be in degrees.
Default is False.
Returns
-------
ret : float or ndarray
an angle or an array of modulated angle.
Examples
--------
>>> angle_mod(-4.0)
2.28318531
>>> angle_mod([-4.0])
np.array(2.28318531)
>>> angle_mod([-150.0, 190.0, 350], degree=True)
array([-150., -170., -10.])
>>> angle_mod(-60.0, zero_2_2pi=True, degree=True)
array([300.])
"""
ifisinstance(x, float):
is_float=True
else:
is_float=False
x=np.asarray(x).flatten()
ifdegree:
x=np.deg2rad(x)
ifzero_2_2pi:
mod_angle=x% (2*np.pi)
else:
mod_angle= (x+np.pi) % (2*np.pi) -np.pi
ifdegree:
mod_angle=np.rad2deg(mod_angle)
ifis_float:
returnmod_angle.item()
else:
returnmod_angle
