-
Notifications
You must be signed in to change notification settings - Fork 20
/
rotation_math.py
57 lines (41 loc) · 1.25 KB
/
rotation_math.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
# Created on 11/3/2015
# Author: Daniel Chen ([email protected])
"""
Math functions to transform Raven rotation data.
"""
from math import cos, sin, sqrt, acos, asin, atan2, pow as pow_f
def r_to_tsp(R_str):
""" Convert rotation matrix to angle representation.
Returns a tuple of three angles
"""
param = R_str.split(',')
R00 = float(param[0])
R02 = float(param[2])
R10 = float(param[3])
R20 = float(param[6])
R21 = float(param[7])
R22 = float(param[8])
theta = -asin(R20)
sai = atan2(R21/cos(theta), R22/cos(theta))
phi = atan2(R10/cos(theta), R00/cos(theta))
tsp = (theta,sai,phi)
return tsp
def tsp_to_r(tsp):
""" Convert angle to rotation matrix representation.
"""
t = tsp[0]
s = tsp[1]
p = tsp[2]
R00 = cos(t)*cos(p)
R01 = sin(s)*sin(t)*cos(p)-cos(s)*sin(p)
R02 = cos(s)*sin(t)*cos(p)+sin(s)*sin(p)
R10 = cos(t)*sin(p)
R11 = sin(s)*sin(t)*sin(p)+cos(s)*cos(p)
R12 = cos(s)*sin(t)*sin(p)-sin(s)*cos(p)
R20 = -sin(t)
R21 = sin(s)*cos(t)
R22 = cos(s)*cos(t)
#R = '%5f;%5f;%5f;%5f;%5f;%5f;%5f;%5f;%5f' % \
# (R00, R01, R02, R10, R11, R12, R20, R21, R22)
R = (R00, R01, R02, R10, R11, R12, R20, R21, R22)
return R