def orthonormalize_axes(xaxis, yaxis): """Corrects xaxis and yaxis to be unit vectors and orthonormal. Parameters ---------- xaxis: :class:`Vector` or list of float yaxis: :class:`Vector` or list of float Returns ------- tuple: (xaxis, yaxis) The corrected axes. Raises ------ ValueError: If xaxis and yaxis cannot span a plane. Examples -------- >>> xaxis = [1, 4, 5] >>> yaxis = [1, 0, -2] >>> xaxis, yaxis = orthonormalize_axes(xaxis, yaxis) >>> allclose(xaxis, [0.1543, 0.6172, 0.7715], tol=0.001) True >>> allclose(yaxis, [0.6929, 0.4891, -0.5298], tol=0.001) True """ xaxis = normalize_vector(xaxis) yaxis = normalize_vector(yaxis) zaxis = cross_vectors(xaxis, yaxis) if not norm_vector(zaxis): raise ValueError("Xaxis and yaxis cannot span a plane.") yaxis = cross_vectors(normalize_vector(zaxis), xaxis) return xaxis, yaxis
def decompose_matrix(M): """Calculates the components of rotation, translation, scale, shear, and perspective of a given transformation matrix M. Parameters ---------- M : :obj:`list` of :obj:`list` of :obj:`float` The square matrix of any dimension. Raises ------ ValueError If matrix is singular or degenerative. Returns ------- scale : :obj:`list` of :obj:`float` The 3 scale factors in x-, y-, and z-direction. shear : :obj:`list` of :obj:`float` The 3 shear factors for x-y, x-z, and y-z axes. angles : :obj:`list` of :obj:`float` The rotation specified through the 3 Euler angles about static x, y, z axes. translation : :obj:`list` of :obj:`float` The 3 values of translation. perspective : :obj:`list` of :obj:`float` The 4 perspective entries of the matrix. Examples -------- >>> trans1 = [1, 2, 3] >>> angle1 = [-2.142, 1.141, -0.142] >>> scale1 = [0.123, 2, 0.5] >>> T = matrix_from_translation(trans1) >>> R = matrix_from_euler_angles(angle1) >>> S = matrix_from_scale_factors(scale1) >>> M = multiply_matrices(multiply_matrices(T, R), S) >>> # M = compose_matrix(scale1, None, angle1, trans1, None) >>> scale2, shear2, angle2, trans2, persp2 = decompose_matrix(M) >>> allclose(scale1, scale2) True >>> allclose(angle1, angle2) True >>> allclose(trans1, trans2) True References ---------- .. [1] Slabaugh, 1999. *Computing Euler angles from a rotation matrix*. Available at: http://www.gregslabaugh.net/publications/euler.pdf """ detM = matrix_determinant(M) # raises ValueError if matrix is not squared if detM == 0: ValueError("The matrix is singular.") Mt = transpose_matrix(M) if abs(Mt[3][3]) < _EPS: raise ValueError('The element [3,3] of the matrix is zero.') for i in range(4): for j in range(4): Mt[i][j] /= Mt[3][3] translation = [M[0][3], M[1][3], M[2][3]] # scale, shear, rotation # copy Mt[:3, :3] into row scale = [0.0, 0.0, 0.0] shear = [0.0, 0.0, 0.0] angles = [0.0, 0.0, 0.0] row = [[0, 0, 0] for i in range(3)] for i in range(3): for j in range(3): row[i][j] = Mt[i][j] scale[0] = norm_vector(row[0]) for i in range(3): row[0][i] /= scale[0] shear[0] = dot_vectors(row[0], row[1]) for i in range(3): row[1][i] -= row[0][i] * shear[0] scale[1] = norm_vector(row[1]) for i in range(3): row[1][i] /= scale[1] shear[0] /= scale[1] shear[1] = dot_vectors(row[0], row[2]) for i in range(3): row[2][i] -= row[0][i] * shear[1] shear[2] = dot_vectors(row[1], row[2]) for i in range(3): row[2][i] -= row[0][i] * shear[2] scale[2] = norm_vector(row[2]) for i in range(3): row[2][i] /= scale[2] shear[1] /= scale[2] shear[2] /= scale[2] if dot_vectors(row[0], cross_vectors(row[1], row[2])) < 0: scale = [-x for x in scale] row = [[-x for x in y] for y in row] # angles if row[0][2] != -1. and row[0][2] != 1.: beta1 = math.asin(-row[0][2]) # beta2 = math.pi - beta1 alpha1 = math.atan2(row[1][2] / math.cos(beta1), row[2][2] / math.cos(beta1)) # alpha2 = math.atan2(row[1][2] / math.cos(beta2), row[2][2] / math.cos(beta2)) gamma1 = math.atan2(row[0][1] / math.cos(beta1), row[0][0] / math.cos(beta1)) # gamma2 = math.atan2(row[0][1] / math.cos(beta2), row[0][0] / math.cos(beta2)) angles = [alpha1, beta1, gamma1] # TODO: check for alpha2, beta2, gamma2 needed? else: gamma = 0. if row[0][2] == -1.: beta = math.pi / 2. alpha = gamma + math.atan2(row[1][0], row[2][0]) else: # row[0][2] == 1 beta = -math.pi / 2. alpha = -gamma + math.atan2(-row[1][0], -row[2][0]) angles = [alpha, beta, gamma] # perspective if math.fabs(Mt[0][3]) > _EPS and math.fabs(Mt[1][3]) > _EPS and math.fabs( Mt[2][3]) > _EPS: P = deepcopy(Mt) P[0][3], P[1][3], P[2][3], P[3][3] = 0.0, 0.0, 0.0, 1.0 Ptinv = matrix_inverse(transpose_matrix(P)) perspective = multiply_matrix_vector( Ptinv, [Mt[0][3], Mt[1][3], Mt[2][3], Mt[3][3]]) else: perspective = [0.0, 0.0, 0.0, 1.0] return scale, shear, angles, translation, perspective
def decompose_matrix(M): """Calculates the components of rotation, translation, scale, shear, and perspective of a given transformation matrix M. Parameters ---------- M : :obj:`list` of :obj:`list` of :obj:`float` The square matrix of any dimension. Raises ------ ValueError If matrix is singular or degenerative. Returns ------- scale : :obj:`list` of :obj:`float` The 3 scale factors in x-, y-, and z-direction. shear : :obj:`list` of :obj:`float` The 3 shear factors for x-y, x-z, and y-z axes. angles : :obj:`list` of :obj:`float` The rotation specified through the 3 Euler angles about static x, y, z axes. translation : :obj:`list` of :obj:`float` The 3 values of translation. perspective : :obj:`list` of :obj:`float` The 4 perspective entries of the matrix. Examples -------- >>> trans1 = [1, 2, 3] >>> angle1 = [-2.142, 1.141, -0.142] >>> scale1 = [0.123, 2, 0.5] >>> T = matrix_from_translation(trans1) >>> R = matrix_from_euler_angles(angle1) >>> S = matrix_from_scale_factors(scale1) >>> M = multiply_matrices(multiply_matrices(T, R), S) >>> # M = compose_matrix(scale1, None, angle1, trans1, None) >>> scale2, shear2, angle2, trans2, persp2 = decompose_matrix(M) >>> allclose(scale1, scale2) True >>> allclose(angle1, angle2) True >>> allclose(trans1, trans2) True """ detM = determinant(M) # raises ValueError if matrix is not squared if detM == 0: ValueError("The matrix is singular.") Mt = transpose_matrix(M) if abs(Mt[3][3]) < _EPS: raise ValueError('The element [3,3] of the matrix is zero.') for i in range(4): for j in range(4): Mt[i][j] /= Mt[3][3] translation = [M[0][3], M[1][3], M[2][3]] # scale, shear, rotation # copy Mt[:3, :3] into row scale = [0.0, 0.0, 0.0] shear = [0.0, 0.0, 0.0] angles = [0.0, 0.0, 0.0] row = [[0, 0, 0] for i in range(3)] for i in range(3): for j in range(3): row[i][j] = Mt[i][j] scale[0] = norm_vector(row[0]) for i in range(3): row[0][i] /= scale[0] shear[0] = dot_vectors(row[0], row[1]) for i in range(3): row[1][i] -= row[0][i] * shear[0] scale[1] = norm_vector(row[1]) for i in range(3): row[1][i] /= scale[1] shear[0] /= scale[1] shear[1] = dot_vectors(row[0], row[2]) for i in range(3): row[2][i] -= row[0][i] * shear[1] shear[2] = dot_vectors(row[1], row[2]) for i in range(3): row[2][i] -= row[0][i] * shear[2] scale[2] = norm_vector(row[2]) for i in range(3): row[2][i] /= scale[2] shear[1] /= scale[2] shear[2] /= scale[2] if dot_vectors(row[0], cross_vectors(row[1], row[2])) < 0: scale = [-x for x in scale] row = [[-x for x in y] for y in row] # use base vectors?? angles[1] = math.asin(-row[0][2]) if math.cos(angles[1]): angles[0] = math.atan2(row[1][2], row[2][2]) angles[2] = math.atan2(row[0][1], row[0][0]) else: angles[0] = math.atan2(-row[2][1], row[1][1]) angles[2] = 0.0 # perspective if math.fabs(Mt[0][3]) > _EPS and math.fabs(Mt[1][3]) > _EPS and \ math.fabs(Mt[2][3]) > _EPS: P = deepcopy(Mt) P[0][3], P[1][3], P[2][3], P[3][3] = 0.0, 0.0, 0.0, 1.0 Ptinv = inverse(transpose_matrix(P)) perspective = multiply_matrix_vector(Ptinv, [Mt[0][3], Mt[1][3], Mt[2][3], Mt[3][3]]) else: perspective = [0.0, 0.0, 0.0, 1.0] return scale, shear, angles, translation, perspective