hierarchy_roach/math_utils.py at master · siminliu99/hierarchy_roach · GitHub

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
import math

def quat_to_eulerDegrees(orientation):
	x=orientation.x
	y=orientation.y
	z=orientation.z
	w=orientation.w

	ysqr = y*y

	t0 = +2.0 * (w * x + y*z)
	t1 = +1.0 - 2.0 * (x*x + ysqr)
	X = math.degrees(math.atan2(t0, t1))

	t2 = +2.0 * (w*y - z*x)
	t2 =  1 if t2 > 1 else t2
	t2 = -1 if t2 < -1 else t2
	Y = math.degrees(math.asin(t2))

	t3 = +2.0 * (w * z + x*y)
	t4 = +1.0 - 2.0 * (ysqr + z*z)
	Z = math.degrees(math.atan2(t3, t4))

	return [X,Y,Z]

# def quat_to_eulerDegrees2(vec):
# 	x=vec[0]
# 	y=vec[1]
# 	z=vec[2]
# 	w=vec[3]

# 	ysqr = y*y

# 	t0 = +2.0 * (w * x + y*z)
# 	t1 = +1.0 - 2.0 * (x*x + ysqr)
# 	X = math.degrees(math.atan2(t0, t1))

# 	t2 = +2.0 * (w*y - z*x)
# 	t2 =  1 if t2 > 1 else t2
# 	t2 = -1 if t2 < -1 else t2
# 	Y = math.degrees(math.asin(t2))

# 	t3 = +2.0 * (w * z + x*y)
# 	t4 = +1.0 - 2.0 * (ysqr + z*z)
# 	Z = math.degrees(math.atan2(t3, t4))

# 	return [X,Y,Z]

# print(quat_to_eulerDegrees2([-0.00123308494221, 0.00461236946285, -0.514774501324, -0.857312321663]))

# print(quat_to_eulerDegrees2([-0.0221652947366, 0.00243791984394, 0.451251357794, 0.89211833477]))
#

# [-0.15094553404043096, -0.5258682603658319, 61.966243954756706]
# [-2.1410102941504587, 1.3955230952483706, 53.63633304666553]