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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
import pytest
import numpy as np
from ase.quaternions import Quaternion
TEST_N = 200
def axang_rotm(u, theta):
u = np.array(u, float)
u /= np.linalg.norm(u)
# Cross product matrix for u
ucpm = np.array([[0, -u[2], u[1]], [u[2], 0, -u[0]], [-u[1], u[0], 0]])
# Rotation matrix
rotm = (np.cos(theta) * np.identity(3) + np.sin(theta) * ucpm +
(1 - np.cos(theta)) * np.kron(u[:, None], u[None, :]))
return rotm
def rand_rotm(rng=np.random.RandomState(0)):
"""Axis & angle rotations."""
u = rng.rand(3)
theta = rng.rand() * np.pi * 2
return axang_rotm(u, theta)
def eulang_rotm(a, b, c, mode='zyz'):
rota = axang_rotm([0, 0, 1], a)
rotc = axang_rotm([0, 0, 1], c)
if mode == 'zyz':
rotb = axang_rotm([0, 1, 0], b)
elif mode == 'zxz':
rotb = axang_rotm([1, 0, 0], b)
return np.dot(rotc, np.dot(rotb, rota))
@pytest.fixture
def rng():
return np.random.RandomState(0)
def test_quaternions_rotations(rng):
# First: test that rotations DO work
for i in range(TEST_N):
# n random tests
rotm = rand_rotm(rng)
q = Quaternion.from_matrix(rotm)
assert np.allclose(rotm, q.rotation_matrix())
# Now test this with a vector
v = rng.rand(3)
vrotM = np.dot(rotm, v)
vrotQ = q.rotate(v)
assert np.allclose(vrotM, vrotQ)
def test_quaternions_gimbal(rng):
# Second: test the special case of a PI rotation
rotm = np.identity(3)
rotm[:2, :2] *= -1 # Rotate PI around z axis
q = Quaternion.from_matrix(rotm)
assert not np.isnan(q.q).any()
def test_quaternions_overload(rng):
# Third: test compound rotations and operator overload
for i in range(TEST_N):
rotm1 = rand_rotm(rng)
rotm2 = rand_rotm(rng)
q1 = Quaternion.from_matrix(rotm1)
q2 = Quaternion.from_matrix(rotm2)
assert np.allclose(np.dot(rotm2, rotm1),
(q2 * q1).rotation_matrix())
# Now test this with a vector
v = rng.rand(3)
vrotM = np.dot(rotm2, np.dot(rotm1, v))
vrotQ = (q2 * q1).rotate(v)
assert np.allclose(vrotM, vrotQ)
def test_quaternions_euler(rng):
# Fourth: test Euler angles
for mode in ['zyz', 'zxz']:
for i in range(TEST_N):
abc = rng.rand(3) * 2 * np.pi
q_eul = Quaternion.from_euler_angles(*abc, mode=mode)
rot_eul = eulang_rotm(*abc, mode=mode)
assert(np.allclose(rot_eul, q_eul.rotation_matrix()))
# Test conversion back and forth
abc_2 = q_eul.euler_angles(mode=mode)
q_eul_2 = Quaternion.from_euler_angles(*abc_2, mode=mode)
assert(np.allclose(q_eul_2.q, q_eul.q))
def test_quaternions_rotm(rng):
# Fifth: test that conversion back to rotation matrices works properly
for i in range(TEST_N):
rotm1 = rand_rotm(rng)
rotm2 = rand_rotm(rng)
q1 = Quaternion.from_matrix(rotm1)
q2 = Quaternion.from_matrix(rotm2)
assert(np.allclose(q1.rotation_matrix(), rotm1))
assert(np.allclose(q2.rotation_matrix(), rotm2))
assert(np.allclose((q1 * q2).rotation_matrix(), np.dot(rotm1, rotm2)))
assert(np.allclose((q1 * q2).rotation_matrix(), np.dot(rotm1, rotm2)))
def test_quaternions_axang(rng):
# Sixth: test conversion to axis + angle
q = Quaternion()
n, theta = q.axis_angle()
assert(theta == 0)
u = np.array([1, 0.5, 1])
u /= np.linalg.norm(u)
alpha = 1.25
q = Quaternion.from_matrix(axang_rotm(u, alpha))
n, theta = q.axis_angle()
assert(np.isclose(theta, alpha))
assert(np.allclose(u, n))
|
