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 set_blend_radius(print_organizer, d_fillet=10, buffer=0.3): """Sets the blend radius (filleting) for the robotic motion. Parameters ---------- print_organizer: :class:`compas_slicer.slicers.BasePrintOrganizer` d_fillet: float Value to attempt to fillet with. Defaults to 10 mm. buffer: float Buffer to make sure that the blend radius is never too big. Defaults to 0.3. """ logger.info("Setting blend radius") for printpoint, i, j, k in print_organizer.printpoints_indices_iterator(): layer_key = 'layer_%d' % i path_key = 'path_%d' % j neighboring_items = print_organizer.get_printpoint_neighboring_items( layer_key, path_key, k) if not printpoint.wait_time: radius = d_fillet if neighboring_items[0]: radius = min( radius, norm_vector( Vector.from_start_end(neighboring_items[0].pt, printpoint.pt)) * buffer) if neighboring_items[1]: radius = min( radius, norm_vector( Vector.from_start_end(neighboring_items[1].pt, printpoint.pt)) * buffer) radius = round(radius, 5) else: radius = 0.0 # 0.0 blend radius for points where the robot will pause and wait printpoint.blend_radius = radius
def orthonormalize_axes(xaxis, yaxis): """Corrects xaxis and yaxis to be unit vectors and orthonormal. Parameters ---------- xaxis: [float, float, float] | :class:`compas.geometry.Vector` The first axis. yaxis: [float, float, float] | :class:`compas.geometry.Vector` The second axis. Returns ------- [float, float, float] The corrected x axis. [float, float, float] The corrected y axis. Raises ------ ValueError If xaxis and yaxis cannot span a plane. Examples -------- >>> from compas.geometry import allclose >>> 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. [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