def testMatrix33(self): m = Matrix33([[1, 2, 3], [4, 5, 6], [7, 8, 9]]) np.testing.assert_array_equal(m, [[1, 2, 3], [4, 5, 6], [7, 8, 9]]) np.testing.assert_array_equal(m.transpose(), [[1, 4, 7], [2, 5, 8], [3, 6, 9]]) self.assertFalse(m.invertible) self.assertAlmostEqual(m.determinant, 0.0, 5) m = Matrix33.identity() np.testing.assert_array_almost_equal(m, np.eye(3), decimal=5) m = Matrix33.ones() np.testing.assert_array_almost_equal(m, np.ones((3, 3)), decimal=5) m.m11 = 5 np.testing.assert_array_almost_equal(m.r1, [5, 1, 1], decimal=5) m.c2 = [8, 9, -7] np.testing.assert_array_almost_equal(m.r3, [1, -7, 1], decimal=5) m = Matrix33.fromTranslation([2, 3]) expected = [[1, 0, 2], [0, 1, 3], [0, 0, 1]] np.testing.assert_array_almost_equal(m, expected, decimal=5) self.assertRaises(AttributeError, lambda: m.r4) with self.assertRaises(AttributeError): m.m14 = 50 self.assertNotEqual(repr(m), str(m))
def testXYZEulersFromMatrix(self): matrix = Matrix33.identity() expected = Vector3([0.0, 0.0, 0.0]) result = xyz_eulers_from_matrix(matrix) np.testing.assert_array_almost_equal(result, expected, decimal=5) matrix = Matrix33([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) expected = Vector3(np.radians([0.0, 0.0, 90.0])) result = xyz_eulers_from_matrix(matrix) np.testing.assert_array_almost_equal(result, expected, decimal=5) matrix = Matrix33([[0.4924039, -0.8528686, 0.1736482], [0.7934120, 0.3578208, -0.4924039], [0.3578208, 0.3802361, 0.8528686]]) expected = Vector3(np.radians([30.0, 10.0, 60.0])) result = xyz_eulers_from_matrix(matrix) np.testing.assert_array_almost_equal(result, expected, decimal=5) matrix = Matrix33([ [-0.0000000, -0.0000000, -1.0000000], [-0.3420202, 0.9396926, -0.0000000], [0.9396926, 0.3420202, -0.0000000], ]) expected = Vector3(np.radians([20.0, -90.0, 0.0])) result = xyz_eulers_from_matrix(matrix) np.testing.assert_array_almost_equal(result, expected, decimal=5) matrix = Matrix33([[0.0000000, 0.0000000, 1.0000000], [0.6427876, 0.7660444, -0.0000000], [-0.7660444, 0.6427876, 0.0000000]]) expected = Vector3(np.radians([40.0, 90, 0.0])) result = xyz_eulers_from_matrix(matrix) np.testing.assert_array_almost_equal(result, expected, decimal=5)
def testMatrixFromZYXEulers(self): eulers = Vector3([0.0, 0.0, 0.0]) expected = Matrix33.identity() result = matrix_from_zyx_eulers(eulers) np.testing.assert_array_almost_equal(result, expected, decimal=5) eulers = Vector3(np.radians([90.0, 0.0, 0.0])) expected = Matrix33([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) result = matrix_from_zyx_eulers(eulers) np.testing.assert_array_almost_equal(result, expected, decimal=5) eulers = Vector3(np.radians([60.0, 10.0, 30.0])) expected = Matrix33([[0.4924039, -0.7065880, 0.5082046], [0.8528686, 0.5082046, -0.1197639], [-0.1736482, 0.4924039, 0.8528686]]) result = matrix_from_zyx_eulers(eulers) np.testing.assert_array_almost_equal(result, expected, decimal=5)
def testMatrixFromXYZEulers(self): eulers = Vector3([0.0, 0.0, 0.0]) expected = Matrix33.identity() result = matrix_from_xyz_eulers(eulers) np.testing.assert_array_almost_equal(result, expected, decimal=5) eulers = Vector3(np.radians([0.0, 0.0, 90.0])) expected = Matrix33([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) result = matrix_from_xyz_eulers(eulers) np.testing.assert_array_almost_equal(result, expected, decimal=5) eulers = Vector3(np.radians([30.0, 10.0, 60.0])) expected = Matrix33([[0.4924039, -0.8528686, 0.1736482], [0.7934120, 0.3578208, -0.4924039], [0.3578208, 0.3802361, 0.8528686]]) result = matrix_from_xyz_eulers(eulers) np.testing.assert_array_almost_equal(result, expected, decimal=5)
def testAngleAxisToMatrix(self): axis = Vector3([1.0, 0.0, 0.0]) expected = Matrix33.identity() result = angle_axis_to_matrix(0.0, axis) np.testing.assert_array_almost_equal(result, expected, decimal=5) axis = Vector3([0.0, 1.0, 0.0]) expected = Matrix33([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) result = angle_axis_to_matrix(math.radians(90), axis) np.testing.assert_array_almost_equal(result, expected, decimal=5) axis = Vector3([0.0, 0.707107, 0.707107]) expected = Matrix33([[0.7071068, -0.5000000, 0.5000000], [0.5000000, 0.8535534, 0.1464466], [-0.5000000, 0.1464466, 0.8535534]]) result = angle_axis_to_matrix(math.radians(45), axis) np.testing.assert_array_almost_equal(result, expected, decimal=5)
def testMatrixToAngleAxis(self): matrix = Matrix33.identity() angle, axis = matrix_to_angle_axis(matrix) np.testing.assert_array_almost_equal(axis, [0.0, 0.0, 1.0], decimal=5) self.assertAlmostEqual(angle, 0.0, 5) matrix = Matrix33([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) angle, axis = matrix_to_angle_axis(matrix) np.testing.assert_array_almost_equal(axis, [0.0, -1.0, 0.0], decimal=5) self.assertAlmostEqual(angle, -np.pi / 2, 5) matrix = Matrix33([[0.7071068, -0.5000000, 0.5000000], [0.5000000, 0.8535534, 0.1464466], [-0.5000000, 0.1464466, 0.8535534]]) angle, axis = matrix_to_angle_axis(matrix) np.testing.assert_array_almost_equal(axis, [0.0, 0.70710678, 0.70710678], decimal=5) self.assertAlmostEqual(angle, np.pi / 4, 5)
def testCheckRotation(self): self.assertFalse( check_rotation( np.array([[1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0]]))) self.assertFalse(check_rotation(10 * np.identity(4))) self.assertTrue(check_rotation(np.identity(4))) self.assertTrue( check_rotation( Matrix33([ [0.09175158, -0.90824842, -0.40824842], [-0.90824842, 0.09175158, -0.40824842], [0.40824842, 0.40824842, -0.81649683], ])))
def testRotationBtwVectors(self): matrix = rotation_btw_vectors([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]) np.testing.assert_array_almost_equal(matrix, Matrix33.identity(), decimal=5) matrix = rotation_btw_vectors([1.0, 0.0, 0.0], [1.0, 0.0, 0.0]) np.testing.assert_array_almost_equal(matrix, Matrix33.identity(), decimal=5) expected = Matrix33([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 1.0]]) matrix = rotation_btw_vectors([0.0, -1.0, 0.0], [0.0, 1.0, 0.0]) np.testing.assert_array_almost_equal(matrix, expected, decimal=5) expected = Matrix33([ [0.09175158, -0.90824842, -0.40824842], [-0.90824842, 0.09175158, -0.40824842], [0.40824842, 0.40824842, -0.81649683], ]) matrix = rotation_btw_vectors([0.57735027, 0.57735027, 0.57735027], [-0.707107, -0.707107, 0.0]) np.testing.assert_array_almost_equal(matrix, expected, decimal=5)
def testMatrix(self): self.assertRaises(ValueError, Matrix, 2, -1) self.assertRaises(ValueError, Matrix, 0, 2) self.assertRaises(ValueError, Matrix, 2, 2, {"values": [[1, 2], [3]]}) m = Matrix(2, 2, dtype=int) self.assertEqual(m[0, 0], 0) m[1, 1] = 5 self.assertEqual(m[1, 1], 5) m = Matrix(2, 2, [[1.0, 2.0], [3.0, 4.0]]) np.testing.assert_array_almost_equal(m.inverse(), [[-2.0, 1.0], [1.5, -0.5]], decimal=5) m1 = m.inverse() @ m # matrix multiplication np.testing.assert_array_almost_equal(m1, [[1, 0], [0, 1]]) m1 = np.array(m.inverse()) @ m np.testing.assert_array_almost_equal(m1, [[1, 0], [0, 1]]) m1 = m.inverse() @ np.array(m) np.testing.assert_array_almost_equal(m1, [[1, 0], [0, 1]]) a = np.array([[1, 2], [3, 4]]) m1 = m * a # element wise multiplication np.testing.assert_array_almost_equal(m1, [[1, 4], [9, 16]], decimal=5) m1 = m * 2 np.testing.assert_array_almost_equal(m1, [[2.0, 4.0], [6.0, 8.0]], decimal=5) m2 = 2 * m np.testing.assert_array_almost_equal(m1, m2, decimal=5) m1 = m + 2 np.testing.assert_array_almost_equal(m1, [[3.0, 4.0], [5.0, 6.0]], decimal=5) m1 = m - 2 np.testing.assert_array_almost_equal(m1, [[-1.0, 0.0], [1.0, 2.0]], decimal=5) m = Matrix.create(2, 4, [[1, 2, 3, 4], [5, 6, 7, 8]]) v = Vector.create(4, [1, 2, 3, 4]) result = m @ v # matrix vector multiplication np.testing.assert_array_almost_equal(m.transpose(), [[1, 5], [2, 6], [3, 7], [4, 8]]) self.assertTrue(isinstance(result, Vector)) np.testing.assert_array_equal(result, [30, 70]) self.assertRaises(ValueError, lambda: Matrix33() + Matrix44()) self.assertRaises(ValueError, lambda: Matrix33() - Matrix44()) self.assertRaises(ValueError, lambda: Matrix33() * Matrix44()) self.assertRaises(ValueError, lambda: Matrix33() * Vector4()) self.assertRaises(ValueError, lambda: Matrix33() @ Matrix44()) self.assertRaises(ValueError, lambda: Matrix33() @ Vector4())
def testQuaternion(self): q = Quaternion.identity() q[1] = 1.0 self.assertAlmostEqual(q.x, 0.0, 5) self.assertAlmostEqual(q.y, 1.0, 5) self.assertAlmostEqual(q.z, 0.0, 5) self.assertAlmostEqual(q.w, 1.0, 5) q.axis = [1.0, 1.0, 1.0] np.testing.assert_array_almost_equal(q.axis, [1.0, 1.0, 1.0], decimal=5) q.x = 1.0 q.y = q[0] q.z = 1.0 q.w = 0.0 np.testing.assert_array_almost_equal(q, [1.0, 1.0, 1.0, 0.0], decimal=5) q = q.normalize() np.testing.assert_array_almost_equal(q, [0.57735, 0.57735, 0.57735, 0.0], decimal=5) matrix = Matrix33([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) q = Quaternion.fromMatrix(matrix) np.testing.assert_array_almost_equal([0.7071067, 0.0, 0.0], q.axis, decimal=5) np.testing.assert_array_almost_equal(matrix, q.toMatrix(), decimal=5) matrix = Matrix33([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) q = Quaternion.fromMatrix(matrix) np.testing.assert_array_almost_equal([0.0, 0.0, 1.0], q.axis, decimal=5) np.testing.assert_array_almost_equal(matrix, q.toMatrix(), decimal=5) matrix = Matrix33([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) q = Quaternion.fromMatrix(matrix) np.testing.assert_array_almost_equal([0.0, 0.0, -0.7071067], q.axis, decimal=5) np.testing.assert_array_almost_equal(matrix, q.toMatrix(), decimal=5) matrix = Matrix33([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) q = Quaternion.fromMatrix(matrix) np.testing.assert_array_almost_equal([0.5, 0.5, 0.5], q.axis, decimal=5) np.testing.assert_array_almost_equal(matrix, q.toMatrix(), decimal=5) data = [-1, -1, -1, 1] * np.array(q) np.testing.assert_array_almost_equal(q.conjugate(), data, decimal=5) axis, angle = q.toAxisAngle() self.assertAlmostEqual(angle, 2.0943951, 5) np.testing.assert_array_almost_equal(axis, [0.57735, 0.57735, 0.57735], decimal=5) axis, angle = Quaternion().toAxisAngle() self.assertAlmostEqual(angle, np.pi, 5) np.testing.assert_array_almost_equal(axis, [0.0, 0.0, 0.0], decimal=5) array = np.array(Quaternion().normalize()) np.testing.assert_array_almost_equal(array, [0.0, 0.0, 0.0, 0.0], decimal=5) qu = Quaternion.fromAxisAngle(Vector3([1.0, 0.0, 0.0]), 0.0) axis, angle = qu.toAxisAngle() self.assertAlmostEqual(angle, 0.0, 5) np.testing.assert_array_almost_equal(axis, [0.0, 0.0, 0.0], decimal=5) mm = Matrix33([[-0.2128074, 0.5013429, 0.8386706], [0.9463776, -0.1077663, 0.3045583], [0.2430686, 0.8585113, -0.4515262]]) qq = Quaternion.fromMatrix(mm) p1 = qq.rotate([1, 2, 3]) p2 = mm @ Vector3([1, 2, 3]) np.testing.assert_array_almost_equal(p1, p2, decimal=5) np.testing.assert_array_almost_equal(mm, qq.toMatrix(), decimal=5) # test dot product self.assertAlmostEqual(q | qq, 0.9544055, 5) q = Quaternion(0.0, 1.0, 1.0, 1.0) np.testing.assert_array_almost_equal( q.inverse(), [-0.57735, -0.57735, -0.57735, 0.0], decimal=5) self.assertNotEqual(repr(qq), str(qq))