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)