def from_matrix(cls, matrix):
# type: (numpy.matrix) -> Transform
# Check matrix properties
if matrix.shape != (4, 4):
raise ValueError(
'Invalid matrix shape: Input must be a 4x4 matrix')
if not numpy.isclose(numpy.linalg.det(matrix), 1.0):
raise ValueError('Matrix determinant must be +1.0')
q = Quaternion.from_matrix(matrix[0:3, 0:3])
t = Vector3(matrix[0:3, 3].flatten())
return Transform(t, q)
def test_from_xy(self):
random_qt = Quaternion.from_euler_extrinsic(0.2, 0.3, 0.9)
x_vec = Vector3(1, 0, 0) * random_qt
y_vec = Vector3(0, 1, 0) * random_qt
z_vec = Vector3(0, 0, 1) * random_qt
m = numpy.identity(3)
m[:, 0] = x_vec.data()
m[:, 1] = y_vec.data()
m[:, 2] = z_vec.data()
m = numpy.matrix(m)
new_q = Quaternion.from_matrix(m)
self.assertTrue(
numpy.isclose(new_q.coeffs(), random_qt.coeffs()).all())