def test_getRotationMatrix(self):
phi = 1.
theta = 0.5
psi = -0.5
q = Quaternion(toVector(phi, theta, psi))
mat = q.getRotationMatrix()
mat2 = matrix(
[[
cos(theta) * cos(psi),
-cos(phi) * sin(psi) + sin(phi) * sin(theta) * cos(psi),
sin(phi) * sin(psi) + cos(phi) * sin(theta) * cos(psi)
],
[
cos(theta) * sin(psi),
cos(phi) * cos(psi) + sin(phi) * sin(theta) * sin(psi),
-sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi)
], [-sin(theta),
sin(phi) * cos(theta),
cos(phi) * cos(theta)]])
self.assertAlmostEqual(mat[0, 0].item(),
mat2[0, 0].item(),
delta=0.001)
self.assertAlmostEqual(mat[0, 1].item(),
mat2[0, 1].item(),
delta=0.001)
self.assertAlmostEqual(mat[0, 2].item(),
mat2[0, 2].item(),
delta=0.001)
self.assertAlmostEqual(mat[1, 0].item(),
mat2[1, 0].item(),
delta=0.001)
self.assertAlmostEqual(mat[1, 1].item(),
mat2[1, 1].item(),
delta=0.001)
self.assertAlmostEqual(mat[1, 2].item(),
mat2[1, 2].item(),
delta=0.001)
self.assertAlmostEqual(mat[2, 0].item(),
mat2[2, 0].item(),
delta=0.001)
self.assertAlmostEqual(mat[2, 1].item(),
mat2[2, 1].item(),
delta=0.001)
self.assertAlmostEqual(mat[2, 2].item(),
mat2[2, 2].item(),
delta=0.001)
