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))