def test_rotation_matrix():
q = Quaternion(1,0,0,0)
r = q.to_rot()
for i, row in enumerate(((1, 0, 0), (0, 1, 0), (0, 0, 1))):
for j, col in enumerate(row):
assert r[i][j] == col
with pytest.raises(NotImplementedError):
Quaternion.from_rot(r)
def test_q2r():
q = Quaternion(1,0,0,0)
r = q.to_rot()
r = np.array(r)
assert np.eye(3).all() == r.all(), f"{r} != np.eye(3)"
q = Quaternion(0,0,0,1)
r = q.to_rot()
r = np.array(r)
assert np.diag([-1.0,-1.0,1.0]).all() == r.all(), f"{r} != np.diag([-1,-1,1])"
q = Quaternion(0.,0.5773503, 0.5773503, 0.5773503)
r = q.to_rot()
r = np.array(r)
a = np.array([
[-0.3333333, 0.6666667, 0.6666667],
[0.6666667, -0.3333333, 0.6666667],
[0.6666667, 0.6666667, -0.3333333]
])
# print(r,a)
assert r.all() == a.all(), f"{r} != {a}"
