def rotate_points_2d(points, axis, angle, origin=None): """Rotates points around an arbitrary axis in 2D. Parameters: points (sequence of sequence of float): XY coordinates of the points. axis (sequence of float): The rotation axis. angle (float): the angle of rotation in radians. origin (sequence of float): Optional. The origin of the rotation axis. Default is ``[0.0, 0.0, 0.0]``. Returns: list: the rotated points References: https://en.wikipedia.org/wiki/Rotation_matrix """ if not origin: origin = [0.0, 0.0] # rotation matrix x, y = normalize_vector_2d(axis) cosa = cos(angle) sina = sin(angle) R = [[cosa, -sina], [sina, cosa]] # translate points points = translate_points_2d(points, scale_vector_2d(origin, -1.0)) # rotate points points = [multiply_matrix_vector(R, point) for point in points] # translate points back points = translate_points_2d(points, origin) return points
def transform(self, xyz): """Transforms a point, vector, xyz coordinates or a list therefrom. Should this be split into transform_xyz_list and transform_xyz TODO: should be attached to the elements. """ xyz = list(xyz) if type(xyz[0]) == float or type(xyz[0]) == int: # point, vector, xyz coordinates point = xyz + [1.] # make homogeneous coordinates point = multiply_matrix_vector(self.matrix, point) return point[:3] else: # it is a list of xyz coordinates xyz = zip(*xyz) # transpose matrix xyz += [[1] * len(xyz[0])] # make homogeneous coordinates xyz = multiply_matrices(self.matrix, xyz) return zip(*xyz[:3]) # cutoff 1 and transpose again
def rotate_points_xy(points, angle, origin=None): """Rotates points in the XY plane around the Z axis at a specific origin. Parameters ---------- points : list of point A list of points. angle : float The angle of rotation in radians. origin : point, optional The origin of the rotation axis. Default is ``[0.0, 0.0, 0.0]``. Returns ------- list The rotated points in the XY plane (Z=0). Examples -------- >>> """ if not origin: origin = [0.0, 0.0, 0.0] cosa = math.cos(angle) sina = math.sin(angle) R = [[cosa, -sina, 0.0], [sina, cosa, 0.0], [0.0, 0.0, 1.0]] # translate points points = translate_points_xy(points, scale_vector_xy(origin, -1.0)) # rotate points points = [multiply_matrix_vector(R, point) for point in points] # translate points back points = translate_points_xy(points, origin) return points
def matrix_from_axis_and_angle(axis, angle, point=None, rtype='list'): """Calculates a rotation matrix from an rotation axis, an angle and an optional point of rotation. Parameters ---------- axis : list of float Three numbers that represent the axis of rotation. angle : float The rotation angle in radians. point : list of float, optional A point to perform a rotation around an origin other than [0, 0, 0]. Returns ------- list of list of float The matrix. Notes ----- The rotation is based on the right hand rule, i.e. anti-clockwise if the axis of rotation points towards the observer. Examples -------- >>> axis1 = normalize_vector([-0.043, -0.254, 0.617]) >>> angle1 = 0.1 >>> R = matrix_from_axis_and_angle(axis1, angle1) >>> axis2, angle2 = axis_and_angle_from_matrix(R) >>> allclose(axis1, axis2) True >>> allclose([angle1], [angle2]) True """ if not point: point = [0.0, 0.0, 0.0] axis = list(axis) if length_vector(axis): axis = normalize_vector(axis) sina = math.sin(angle) cosa = math.cos(angle) R = [[cosa, 0.0, 0.0], [0.0, cosa, 0.0], [0.0, 0.0, cosa]] outer_product = [[axis[i] * axis[j] * (1.0 - cosa) for i in range(3)] for j in range(3)] R = [[R[i][j] + outer_product[i][j] for i in range(3)] for j in range(3)] axis = scale_vector(axis, sina) m = [[0.0, -axis[2], axis[1]], [axis[2], 0.0, -axis[0]], [-axis[1], axis[0], 0.0]] M = [[1. if x == y else 0. for x in range(4)] for y in range(4)] for i in range(3): for j in range(3): R[i][j] += m[i][j] M[i][j] = R[i][j] # rotation about axis, angle AND point includes also translation t = subtract_vectors(point, multiply_matrix_vector(R, point)) M[0][3] = t[0] M[1][3] = t[1] M[2][3] = t[2] if rtype == 'list': return M if rtype == 'array': from numpy import asarray return asarray(M) raise NotImplementedError
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