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 rotate_points_xy(points, angle, origin=None): """Rotates points in the XY plane around the Z axis at a specific origin. Parameters ---------- points : sequence[[float, float, float] | :class:`compas.geometry.Point`] A list of points. angle : float The angle of rotation in radians. origin : [float, float, float] | :class:`compas.geometry.Point`, optional The origin of the rotation axis. Default is ``[0.0, 0.0, 0.0]``. Returns ------- list[[float, float, 0.0]] 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 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_from_axis_and_angle(axis, angle, point=None): """Calculates a rotation matrix from an rotation axis, an angle and an optional point of rotation. Parameters ---------- axis : [float, float, float] Three numbers that represent the axis of rotation. angle : float The rotation angle in radians. point : [float, float, float] | :class:`compas.geometry.Point`, optional A point to perform a rotation around an origin other than [0, 0, 0]. Returns ------- list[list[float]] A 4x4 transformation matrix representing a rotation. 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 = identity_matrix(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] return M
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