def transform_frames(frames, T): """Transform multiple frames with one transformation matrix. Parameters ---------- frames : sequence[[point, vector, vector] | :class:`compas.geometry.Frame`] A list of frames to be transformed. T : list[list[float]] | :class:`compas.geometry.Transformation` The transformation to apply on the frames. Returns ------- list[[point, vector, vector]] Transformed frames. Examples -------- >>> from compas.geometry import Frame, matrix_from_axis_and_angle >>> frames = [Frame([1, 0, 0], [1, 2, 4], [4, 7, 1]), Frame([0, 2, 0], [5, 2, 1], [0, 2, 1])] >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) >>> transformed_frames = transform_frames(frames, T) """ points_and_vectors = homogenize_and_flatten_frames(frames) return dehomogenize_and_unflatten_frames( multiply_matrices(points_and_vectors, transpose_matrix(T)))
def transpose(self): """Transpose the matrix of this transformation. Returns ------- None The transformation is transposed in-place. """ self.matrix = transpose_matrix(self.matrix)
def transform_vectors(vectors, T): """Transform multiple vectors with one transformation matrix. Parameters ---------- vectors : list of :class:`Vector` or list of list of float A list of vectors to be transformed. T : :class:`Transformation` list of list of float The transformation to apply. Examples -------- >>> vectors = [[1, 0, 0], [1, 2, 4], [4, 7, 1]] >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) >>> vectors_transformed = transform_vectors(vectors, T) """ return dehomogenize(multiply_matrices(homogenize(vectors, w=0.0), transpose_matrix(T)))
def transform_frames(frames, T): """Transform multiple frames with one transformation matrix. Parameters ---------- frames : list of :class:`Frame` A list of frames to be transformed. T : :class:`Transformation` The transformation to apply on the frames. Examples -------- >>> frames = [Frame([1, 0, 0], [1, 2, 4], [4, 7, 1]), Frame([0, 2, 0], [5, 2, 1], [0, 2, 1])] >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) >>> transformed_frames = transform_frames(frames, T) """ points_and_vectors = homogenize_and_flatten_frames(frames) return dehomogenize_and_unflatten_frames(multiply_matrices(points_and_vectors, transpose_matrix(T)))
def transform_vectors(vectors, T): """Transform multiple vectors with one transformation matrix. Parameters ---------- vectors : sequence[[float, float, float] | :class:`compas.geometry.Vector`] A list of vectors to be transformed. T : list[list[float]] | :class:`compas.geometry.Transformation` The transformation to apply. Returns ------- list[[float, float, float]] Transformed vectors. Examples -------- >>> vectors = [[1, 0, 0], [1, 2, 4], [4, 7, 1]] >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) >>> vectors_transformed = transform_vectors(vectors, T) """ return dehomogenize( multiply_matrices(homogenize(vectors, w=0.0), transpose_matrix(T)))
def decompose_matrix(M): """Calculates the components of rotation, translation, scale, shear, and perspective of a given transformation matrix M. [1]_ Parameters ---------- M : list[list[float]] The square matrix of any dimension. Raises ------ ValueError If matrix is singular or degenerative. Returns ------- scale : [float, float, float] The 3 scale factors in x-, y-, and z-direction. shear : [float, float, float] The 3 shear factors for x-y, x-z, and y-z axes. angles : [float, float, float] The rotation specified through the 3 Euler angles about static x, y, z axes. translation : [float, float, float] The 3 values of translation. perspective : [float, float, float, 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 """ fabs = math.fabs cos = math.cos atan2 = math.atan2 asin = math.asin pi = math.pi 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, angles scale = [0.0, 0.0, 0.0] shear = [0.0, 0.0, 0.0] angles = [0.0, 0.0, 0.0] # copy Mt[:3, :3] into row 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 = asin(-row[0][2]) # beta2 = pi - beta1 alpha1 = atan2(row[1][2] / cos(beta1), row[2][2] / cos(beta1)) # alpha2 = atan2(row[1][2] / cos(beta2), row[2][2] / cos(beta2)) gamma1 = atan2(row[0][1] / cos(beta1), row[0][0] / cos(beta1)) # gamma2 = atan2(row[0][1] / cos(beta2), row[0][0] / cos(beta2)) angles = [alpha1, beta1, gamma1] else: gamma = 0. if row[0][2] == -1.: beta = pi / 2. alpha = gamma + atan2(row[1][0], row[2][0]) else: # row[0][2] == 1 beta = -pi / 2. alpha = -gamma + atan2(-row[1][0], -row[2][0]) angles = [alpha, beta, gamma] # perspective if fabs(Mt[0][3]) > _EPS and fabs(Mt[1][3]) > _EPS and 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 matrix_inverse(M): """Calculates the inverse of a square matrix M. Parameters ---------- M : list[list[float]] A square matrix of any dimension. Returns ------- list[list[float]] The inverted matrix. Raises ------ ValueError If the matrix is not squared ValueError If the matrix is singular. ValueError If the matrix is not invertible. Examples -------- >>> from compas.geometry import Frame >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) >>> T = matrix_from_frame(f) >>> I = multiply_matrices(T, matrix_inverse(T)) >>> I2 = identity_matrix(4) >>> allclose(I[0], I2[0]) True >>> allclose(I[1], I2[1]) True >>> allclose(I[2], I2[2]) True >>> allclose(I[3], I2[3]) True """ D = matrix_determinant(M) if D == 0: ValueError("The matrix is singular.") if len(M) == 2: return [[M[1][1] / D, -1 * M[0][1] / D], [-1 * M[1][0] / D, M[0][0] / D]] cofactors = [] for r in range(len(M)): cofactor_row = [] for c in range(len(M)): cofactor_row.append( (-1)**(r + c) * matrix_determinant(matrix_minor(M, r, c))) cofactors.append(cofactor_row) cofactors = transpose_matrix(cofactors) for r in range(len(cofactors)): for c in range(len(cofactors)): cofactors[r][c] = cofactors[r][c] / D return cofactors
def matrix_inverse(M): """Calculates the inverse of a square matrix M. Parameters ---------- M : :obj:`list` of :obj:`list` of :obj:`float` The square matrix of any dimension. Raises ------ ValueError If the matrix is not squared ValueError If the matrix is singular. ValueError If the matrix is not invertible. Returns ------- list of list of float The inverted matrix. Examples -------- >>> from compas.geometry import Frame >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) >>> T = matrix_from_frame(f) >>> I = multiply_matrices(T, matrix_inverse(T)) >>> I2 = identity_matrix(4) >>> allclose(I[0], I2[0]) True >>> allclose(I[1], I2[1]) True >>> allclose(I[2], I2[2]) True >>> allclose(I[3], I2[3]) True """ def matrix_minor(m, i, j): return [row[:j] + row[j + 1:] for row in (m[:i] + m[i + 1:])] detM = matrix_determinant(M) # raises ValueError if matrix is not squared if detM == 0: ValueError("The matrix is singular.") if len(M) == 2: return [[M[1][1] / detM, -1 * M[0][1] / detM], [-1 * M[1][0] / detM, M[0][0] / detM]] else: cofactors = [] for r in range(len(M)): cofactor_row = [] for c in range(len(M)): minor = matrix_minor(M, r, c) cofactor_row.append( ((-1)**(r + c)) * matrix_determinant(minor)) cofactors.append(cofactor_row) cofactors = transpose_matrix(cofactors) for r in range(len(cofactors)): for c in range(len(cofactors)): cofactors[r][c] = cofactors[r][c] / detM return cofactors
def inhomogeneous_transformation(self, matrix, vector): return self.project_vector( multiply_matrices([self.embed_vector(vector)], transpose_matrix(matrix))[0])