def testQuaternionMatrix(self): q = matmath.axisAngleToQuaternion([1, 0, 0], np.radians(90)) qmat = matmath.quaternionToRotationMatrix(q) rmat = matmath.xRotationMatrix(math.radians(90)) np.testing.assert_almost_equal(qmat, rmat) q = matmath.axisAngleToQuaternion([0, 1, 0], np.radians(90)) qmat = matmath.quaternionToRotationMatrix(q) rmat = matmath.yRotationMatrix(math.radians(90)) np.testing.assert_almost_equal(qmat, rmat) q = matmath.axisAngleToQuaternion([0, 0, 1], np.radians(90)) qmat = matmath.quaternionToRotationMatrix(q) rmat = matmath.zRotationMatrix(math.radians(90)) np.testing.assert_almost_equal(qmat, rmat)
def testRotY(self): pt = np.array([0, 0, 1, 1]) mat = matmath.yRotationMatrix(math.radians(90)) npt = pt.dot(mat) np.testing.assert_almost_equal(npt, [1, 0, 0, 1])