def area_polygon(polygon): """Compute the area of a polygon. Parameters ---------- polygon : sequence The XYZ coordinates of the vertices/corners of the polygon. The vertices are assumed to be in order. The polygon is assumed to be closed: the first and last vertex in the sequence should not be the same. Returns ------- float The area of the polygon. Examples -------- .. plot:: :include-source: from compas.geometry import area_polygon from compas_plotters import Plotter plotter = Plotter() polygon = [ [0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0], [0.0, 1.0, 0.0] ] area_polygon(polygon) # 1.0 plotter.draw_polygons([{'points': polygon}]) plotter.show() .. plot:: :include-source: from compas.geometry import area_polygon from compas_plotters import Plotter plotter = Plotter() polygon = [ [0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0], [0.5, 0.0, 0.0], [0.0, 1.0, 0.0] ] area_polygon(polygon) # 0.5 plotter.draw_polygons([{'points': polygon}]) plotter.show() """ o = centroid_points(polygon) a = polygon[-1] b = polygon[0] oa = subtract_vectors(a, o) ob = subtract_vectors(b, o) n0 = cross_vectors(oa, ob) area = 0.5 * length_vector(n0) for i in range(0, len(polygon) - 1): oa = ob b = polygon[i + 1] ob = subtract_vectors(b, o) n = cross_vectors(oa, ob) if dot_vectors(n, n0) > 0: area += 0.5 * length_vector(n) else: area -= 0.5 * length_vector(n) return area
def is_intersection_line_triangle(line, triangle, epsilon=1e-6): """Verifies if a line (ray) intersects with a triangle. Notes ----- Based on the Moeller Trumbore intersection algorithm. The line is treated as continues, directed ray and not as line segment with a start and end point Parameters ---------- line : tuple Two points defining the line. triangle : sequence of sequence of float XYZ coordinates of the triangle corners. Returns ------- bool True if the line (ray) intersects with the triangle, False otherwise. Examples -------- >>> """ a, b, c = triangle # direction vector and base point of line v1 = subtract_vectors(line[1], line[0]) p1 = line[0] # Find vectors for two edges sharing V1 e1 = subtract_vectors(b, a) e2 = subtract_vectors(c, a) # Begin calculating determinant - also used to calculate u parameter p = cross_vectors(v1, e2) # if determinant is near zero, ray lies in plane of triangle det = dot_vectors(e1, p) # NOT CULLING if det > -epsilon and det < epsilon: return False inv_det = 1.0 / det # calculate distance from V1 to ray origin t = subtract_vectors(p1, a) # Calculate u parameter and make_blocks bound u = dot_vectors(t, p) * inv_det # The intersection lies outside of the triangle if u < 0.0 or u > 1.0: return False # Prepare to make_blocks v parameter q = cross_vectors(t, e1) # Calculate V parameter and make_blocks bound v = dot_vectors(v1, q) * inv_det # The intersection lies outside of the triangle if v < 0.0 or u + v > 1.0: return False t = dot_vectors(e2, q) * inv_det if t > epsilon: return True # No hit return False
def local_axes(a, b, c): u = b - a v = c - a w = cross_vectors(u, v) v = cross_vectors(w, u) return normalize_vector(u), normalize_vector(v), normalize_vector(w)
def volume_polyhedron(polyhedron): r"""Compute the volume of a polyhedron represented by a closed mesh. Parameters ---------- polyhedron : tuple The vertices and faces of the polyhedron. Returns ------- float The volume of the polyhedron. Notes ----- This implementation is based on the divergence theorem, the fact that the *area vector* is constant for each face, and the fact that the area of each face can be computed as half the length of the cross product of two adjacent edge vectors [1]_. .. math:: :nowrap: \begin{align} V = \int_{P} 1 &= \frac{1}{3} \int_{\partial P} \mathbf{x} \cdot \mathbf{n} \\ &= \frac{1}{3} \sum_{i=0}^{N-1} \int{A_{i}} a_{i} \cdot n_{i} \\ &= \frac{1}{6} \sum_{i=0}^{N-1} a_{i} \cdot \hat n_{i} \end{align} Warning ------- The volume computed by this funtion is only correct if the polyhedron is convex, has planar faces, and is positively oriented (all face normals point outwards). References ---------- .. [1] Nurnberg, R. *Calculating the area and centroid of a polygon in 2d*. Available at: http://wwwf.imperial.ac.uk/~rn/centroid.pdf """ xyz, faces = polyhedron V = 0 for vertices in faces: if len(vertices) == 3: triangles = [vertices] else: centroid = centroid_points([xyz[i] for i in vertices]) i = len(xyz) xyz.append(centroid) triangles = [] for u, v in pairwise(vertices + vertices[0:1]): triangles.append([i, u, v]) for u, v, w in triangles: a = xyz[u] b = xyz[v] c = xyz[w] ab = subtract_vectors(b, a) ac = subtract_vectors(c, a) n = cross_vectors(ab, ac) V += dot_vectors(a, n) return V / 6.
def third_tangent(b_struct, b_v_old, b_v1, b3_1, b3_2, pt_mean_3, max_len, b_v3_1, b_v3_2, pt_mean, radius, b_v0_n=None, check_collision=False): line_1 = b_struct.vertex[b_v_old]["axis_endpoints"] line_2 = b_struct.vertex[b_v1]["axis_endpoints"] b1 = b_struct.vertex[b_v_old] b2 = b_struct.vertex[b_v1] pt_b_1 = line_1[0] pt_b_2 = line_2[0] pt_b_3 = b3_1["axis_endpoints"][0] pt_b_4 = b3_2["axis_endpoints"][0] l_1 = normalize_vector(vector_from_points(line_1[0], line_1[1])) l_2 = normalize_vector(vector_from_points(line_2[0], line_2[1])) l_3 = normalize_vector( vector_from_points(b3_1["axis_endpoints"][0], b3_1["axis_endpoints"][1])) l_4 = normalize_vector( vector_from_points(b3_2["axis_endpoints"][0], b3_2["axis_endpoints"][1])) pts_axis_1 = dropped_perpendicular_points(line_1[0], line_1[1], line_2[0], line_2[1]) pt_axis_1 = centroid_points(pts_axis_1) pts_axis_2 = dropped_perpendicular_points(b3_1["axis_endpoints"][0], b3_1["axis_endpoints"][1], b3_2["axis_endpoints"][0], b3_2["axis_endpoints"][1]) pt_axis_2 = centroid_points(pts_axis_2) pt_mid = centroid_points((pt_axis_1, pt_axis_2)) axis = vector_from_points(pt_axis_1, pt_axis_2) ex = normalize_vector(cross_vectors(normalize_vector(axis), (1, 0, 0))) ey = normalize_vector(cross_vectors(normalize_vector(axis), ex)) bounds = (-100.0, 100.0) args = pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds # solutions_1 = [] # solutions_2 = [] # pts_3 = [] if check_collision == False: if b_v0_n: ind_1 = b_struct.vertex[b_v0_n]["index_sol"][0] ind_2 = b_struct.vertex[b_v0_n]["index_sol"][1] else: ind_1 = 0 ind_2 = 0 # args = pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2 # xfunc = XFunc('coop_assembly.help_functions.tangents.solve_third_tangent', radius'C:\Users\parascho\Documents\git_repos') # xfunc(pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2) # ret_stt = xfunc.data ## ret_stt = solve_third_tangent(*args) ret_stt = solve_third_tangent(pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2) # if max(b_struct.vertex.keys()) == 67: print("args", args) if ret_stt: pt3, vec_l1, vec_l2, ang_check = ret_stt else: return None # pts_3.append(pt3) # solutions_1.append(vec_l1) # solutions_2.append(vec_l2) solution_1 = vec_l1 solution_2 = vec_l2 test_1 = check_length_sol_one(solution_2, pt_mean_3, pt3, b3_1, b3_2, b_v3_1, b_v3_2, b_struct) test_2 = check_length_sol_one(solution_1, pt_mean_3, pt3, b1, b2, b_v_old, b_v1, b_struct) if not test_1 or not test_2: return None # for n in test_1: # for m in test_2: # if n[4] == m[4]: # vec_sol_31, l31, pts_b3_11, pts_b3_21, ind = n # vec_sol_32, l32, pts_b3_12, pts_b3_22, ind_2 = m vec_sol_31, l31, pts_b3_11, pts_b3_21 = test_1 vec_sol_32, l32, pts_b3_12, pts_b3_22 = test_2 pt3_e1 = add_vectors(pt3, scale_vector(vec_sol_31, l31)) pt3_e2 = add_vectors(pt3, scale_vector(vec_sol_32, -1 * l32)) end_pts_0 = (pt3_e2, pt3_e1) else: bool_test = False for i in range(4): for j in range(4): ind_1 = i ind_2 = j args = pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2 ret_stt = solve_third_tangent(*args) if ret_stt: pt3, vec_l1, vec_l2, ang_check = ret_stt else: return None # pts_3.append(pt3) # solutions_1.append(vec_l1) # solutions_2.append(vec_l2) solution_1 = vec_l1 solution_2 = vec_l2 #for j in range(4): test_1 = check_length_sol_one(solution_2, pt_mean_3, pt3, b3_1, b3_2, b_v3_1, b_v3_2, b_struct) test_2 = check_length_sol_one(solution_1, pt_mean_3, pt3, b1, b2, b_v_old, b_v1, b_struct) if not test_1 or not test_2: return None vec_sol_31, l31, pts_b3_11, pts_b3_21 = test_1 vec_sol_32, l32, pts_b3_12, pts_b3_22 = test_2 pt3_e1 = add_vectors(pt3, scale_vector(vec_sol_31, l31)) pt3_e2 = add_vectors(pt3, scale_vector(vec_sol_32, -1 * l32)) end_pts_0 = (pt3_e2, pt3_e1) ext_len = 30 end_pts_0 = (add_vectors( pt3_e2, scale_vector( normalize_vector(vector_from_points(pt3_e1, pt3_e2)), ext_len)), add_vectors( pt3_e1, scale_vector( normalize_vector( vector_from_points(pt3_e2, pt3_e1)), ext_len))) bool_col = check_colisions(b_struct, end_pts_0, radius, bar_nb=b_v0_n) if bool_col == True: end_pts_check = b_struct.vertex[b_v3_1]["axis_endpoints"] bool_col = check_colisions(b_struct, end_pts_check, radius, bar_nb=b_v0_n, bar_checking=b_v3_1) if bool_col == True: end_pts_check = b_struct.vertex[b_v3_2][ "axis_endpoints"] bool_col = check_colisions(b_struct, end_pts_check, radius, bar_nb=b_v0_n, bar_checking=b_v3_2) # bool_col = True if bool_col == False: print("COLLIDE", len(b_struct.vertex)) if i == 3 and j == 3 and bool_col == False: print("NO TANGENT 3 FOUND IN ONE BAR COMBINATION") return None if bool_col == True: bool_test = True break if bool_test == True: break # end_pts_0 = [map(float, p) for p in end_pts_0] vec_x, vec_y, vec_z = calculate_coord_sys(end_pts_0, pt_mean) # pt_o = centroid_points(end_pts_0) if not b_v0_n: # b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (2*radius, 2.0), vec_z) b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (25.0, 2.0), vec_z) else: b_v0 = b_v0_n b_struct.vertex[b_v0].update({"axis_endpoints": end_pts_0}) b_struct.vertex[b_v0].update({"index_sol": [ind_1, ind_2]}) # b_struct.vertex[b_v0].update({"gripping_plane_no_offset":(pt_o, vec_x, vec_y, vec_z)}) # calculate_gripping_plane(b_struct, b_v0, pt_mean) b_struct.vertex[b_v0].update({"mean_point": pt_mean}) b3_1.update({"axis_endpoints": pts_b3_11}) b3_2.update({"axis_endpoints": pts_b3_21}) if not b_v0_n: b_struct.connect_bars(b_v0, b_v3_1) b_struct.connect_bars(b_v0, b_v3_2) dpp_1 = dropped_perpendicular_points( b_struct.vertex[b_v0]["axis_endpoints"][0], b_struct.vertex[b_v0]["axis_endpoints"][1], b_struct.vertex[b_v3_1]["axis_endpoints"][0], b_struct.vertex[b_v3_1]["axis_endpoints"][1]) dpp_2 = dropped_perpendicular_points( b_struct.vertex[b_v0]["axis_endpoints"][0], b_struct.vertex[b_v0]["axis_endpoints"][1], b_struct.vertex[b_v3_2]["axis_endpoints"][0], b_struct.vertex[b_v3_2]["axis_endpoints"][1]) # b_struct.edge[b_v0][b_v3_1].update({"endpoints":[dpp_1[0], dpp_1[1]]}) # b_struct.edge[b_v0][b_v3_2].update({"endpoints":[dpp_2[0], dpp_2[1]]}) k_1 = list(b_struct.edge[b_v0][b_v3_1]["endpoints"].keys())[0] k_2 = list(b_struct.edge[b_v0][b_v3_2]["endpoints"].keys())[0] b_struct.edge[b_v0][b_v3_1]["endpoints"].update( {k_1: (dpp_1[0], dpp_1[1])}) b_struct.edge[b_v0][b_v3_2]["endpoints"].update( {k_2: (dpp_2[0], dpp_2[1])}) return b_v0, pt3, end_pts_0
def normal(self): """:class:`Vector` : The frame's normal (z-axis).""" return Vector(*cross_vectors(self.xaxis, self.yaxis))
def intersection_line_triangle(line, triangle, tol=1e-6): """Computes the intersection point of a line (ray) and a triangle based on the Moeller Trumbore intersection algorithm Parameters ---------- line : tuple Two points defining the line. triangle : list of list of float XYZ coordinates of the triangle corners. tol : float, optional A tolerance for membership verification. Default is ``1e-6``. Returns ------- point : tuple The intersectin point. None If the intersection does not exist. """ # a, b, c = triangle # v1 = subtract_vectors(line[1], line[0]) # p1 = line[0] # # Find vectors for two edges sharing V1 # e1 = subtract_vectors(b, a) # e2 = subtract_vectors(c, a) # # Begin calculating determinant - also used to calculate u parameter # p = cross_vectors(v1, e2) # # if determinant is near zero, ray lies in plane of triangle # det = dot_vectors(e1, p) # if det > - epsilon and det < epsilon: # return None # inv_det = 1.0 / det # # calculate distance from V1 to ray origin # t = subtract_vectors(p1, a) # # Calculate u parameter # u = dot_vectors(t, p) * inv_det # # The intersection lies outside of the triangle # if u < 0.0 or u > 1.0: # return None # # Prepare to make_blocks v parameter # q = cross_vectors(t, e1) # # Calculate V parameter # v = dot_vectors(v1, q) * inv_det # # The intersection lies outside of the triangle # if v < 0.0 or u + v > 1.0: # return None # t = dot_vectors(e2, q) * inv_det # if t > epsilon: # return add_vectors(p1, scale_vector(v1, t)) # # No hit # return None a, b, c = triangle ab = subtract_vectors(b, a) ac = subtract_vectors(c, a) n = cross_vectors(ab, ac) plane = a, n x = intersection_line_plane(line, plane, tol=tol) if x: if is_point_in_triangle(x, triangle): return x
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
def generate_first_triangle(o_struct, b_struct, radius, base_tri_pts, base_tri_ids): """[summary] Parameters ---------- o_struct : [type] to be overwritten b_struct : [type] to be overwritten radius : float bar radius, in millimeter base_tri_pts : list of lists of 3-float [[x, y, z], [x, y, z], [x, y, z]] base_tri_ids : list of int point indices for the base triangle, used for bookkeeping indices in the OverallStructure vertex Returns ------- (Bar_Structure, Overall_Structure) [description] """ pt_0, pt_1, pt_2 = base_tri_pts vec_0 = normalize_vector(vector_from_points(pt_0, pt_1)) vec_1 = normalize_vector(vector_from_points(pt_1, pt_2)) vec_2 = normalize_vector(vector_from_points(pt_2, pt_0)) c_0 = scale_vector(normalize_vector(cross_vectors(vec_0, vec_1)), 2 * radius) c_1 = scale_vector(normalize_vector(cross_vectors(vec_1, vec_2)), 2 * radius) c_2 = scale_vector(normalize_vector(cross_vectors(vec_2, vec_0)), 2 * radius) # bar i: start point to raised end point end_pts_0 = (pt_0, add_vectors(pt_1, c_0)) end_pts_1 = (pt_1, add_vectors(pt_2, c_1)) end_pts_2 = (pt_2, add_vectors(pt_0, c_2)) # pt_int = centroid_points((end_pts_0[0], end_pts_0[1], end_pts_1[0], end_pts_1[1], end_pts_2[0], end_pts_2[1])) # local coordinate system for each bar # _, _, vec_z_0 = calculate_coord_sys(end_pts_0, pt_int) # _, _, vec_z_1 = calculate_coord_sys(end_pts_1, pt_int) # _, _, vec_z_2 = calculate_coord_sys(end_pts_2, pt_int) # ? overwriting the local frame's z axis above ??? vec_z_0 = calculate_bar_z(end_pts_0) vec_z_1 = calculate_bar_z(end_pts_1) vec_z_2 = calculate_bar_z(end_pts_2) # add the three bars to the Bar_Structure as vertices, bar_type = 0 crosec_type = "tube" crosec_values = (25.0, 2.0) # ? what does this cross section value mean? # these are vertex keys in the Bar_Structure network # * each bar is a vertex in the Bar_Structure b_v0_key = b_struct.add_bar(bar_type, end_pts_0, crosec_type, crosec_values, vec_z_0) b_v1_key = b_struct.add_bar(bar_type, end_pts_1, crosec_type, crosec_values, vec_z_1) b_v2_key = b_struct.add_bar(bar_type, end_pts_2, crosec_type, crosec_values, vec_z_2) # pt_o_0 = centroid_points(end_pts_0) # pt_o_1 = centroid_points(end_pts_1) # pt_o_2 = centroid_points(end_pts_2) # b_struct.vertex[b_v0].update({"gripping_plane": (pt_o_0, vec_x_0, vec_y_0, vec_z_0)}) # b_struct.vertex[b_v1].update({"gripping_plane": (pt_o_1, vec_x_1, vec_y_1, vec_z_1)}) # b_struct.vertex[b_v2].update({"gripping_plane": (pt_o_2, vec_x_2, vec_y_2, vec_z_2)}) pt_m = [0, 0, -10000000000000] # calculate_gripping_plane(b_struct, b_v0, pt_m) # calculate_gripping_plane(b_struct, b_v1, pt_m) # calculate_gripping_plane(b_struct, b_v2, pt_m) # ? what does this mean_point mean? b_struct.vertex[b_v0_key].update({"mean_point": pt_m}) b_struct.vertex[b_v1_key].update({"mean_point": pt_m}) b_struct.vertex[b_v2_key].update({"mean_point": pt_m}) # calculate contact point projected on bar axes, (Pi, P_{ci}) between bar i and bar i+1 epts_0 = dropped_perpendicular_points( b_struct.vertex[b_v0_key]["axis_endpoints"][0], b_struct.vertex[b_v0_key]["axis_endpoints"][1], b_struct.vertex[b_v1_key]["axis_endpoints"][0], b_struct.vertex[b_v1_key]["axis_endpoints"][1]) epts_1 = dropped_perpendicular_points( b_struct.vertex[b_v1_key]["axis_endpoints"][0], b_struct.vertex[b_v1_key]["axis_endpoints"][1], b_struct.vertex[b_v2_key]["axis_endpoints"][0], b_struct.vertex[b_v2_key]["axis_endpoints"][1]) epts_2 = dropped_perpendicular_points( b_struct.vertex[b_v2_key]["axis_endpoints"][0], b_struct.vertex[b_v2_key]["axis_endpoints"][1], b_struct.vertex[b_v0_key]["axis_endpoints"][0], b_struct.vertex[b_v0_key]["axis_endpoints"][1]) b_struct.connect_bars(b_v0_key, b_v1_key, _endpoints=epts_0) b_struct.connect_bars(b_v1_key, b_v2_key, _endpoints=epts_1) b_struct.connect_bars(b_v2_key, b_v0_key, _endpoints=epts_2) # update_edges(b_struct) b_struct.update_bar_lengths() tet_id = 0 # these are vertex's index in the Overall_Structure network o_v0_key = o_struct.add_node(pt_0, v_key=base_tri_ids[0], t_key=tet_id) o_v1_key = o_struct.add_node(pt_1, v_key=base_tri_ids[1], t_key=tet_id) o_v2_key = o_struct.add_node(pt_2, v_key=base_tri_ids[2], t_key=tet_id) print('vertex key: {} added to the OverallStructure as the base triangle, original ids in the list: {}'.format(\ [o_v0_key, o_v1_key, o_v2_key], base_tri_ids)) # ? shouldn't these be assigned to tet #0 as well? # o_vi and o_vj's connection is "realized" by bar # b_v_key o_struct.add_bar(o_v0_key, o_v1_key, b_v0_key) o_struct.add_bar(o_v1_key, o_v2_key, b_v1_key) o_struct.add_bar(o_v0_key, o_v2_key, b_v2_key) # calculate and save the contact (tangent) point to each vertex o_struct.calculate_point(o_v0_key) o_struct.calculate_point(o_v1_key) o_struct.calculate_point(o_v2_key) return b_struct, o_struct
def _normal_face(face): u = subtract_vectors(points[face[1]], points[face[0]]) v = subtract_vectors(points[face[-1]], points[face[0]]) return cross_vectors(u, v)
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 orient_points(points, reference_plane, target_plane): """Orient points from one plane to another. Parameters ---------- points : list of points XYZ coordinates of the points. reference_plane : plane Base point and normal defining a reference plane. target_plane : plane Base point and normal defining a target plane. Returns ------- points : list of point XYZ coordinates of the oriented points. Notes ----- This function is useful to orient a planar problem in the xy-plane to simplify the calculation (see example). Examples -------- .. code-block:: python from compas.geometry import orient_points from compas.geometry import intersection_segment_segment_xy refplane = ([0.57735, 0.57735, 0.57735], [1.0, 1.0, 1.0]) tarplane = ([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]) points = [ [0.288675, 0.288675, 1.1547], [0.866025, 0.866025, 0.0], [1.077350, 0.077350, 0.57735], [0.077350, 1.077350, 0.57735] ] points = orient_points(points, refplane, tarplane) ab = points[0], points[1] cd = points[2], points[3] point = intersection_segment_segment_xy(ab, cd) points = orient_points([point], tarplane, refplane) print(points[0]) """ axis = cross_vectors(reference_plane[1], target_plane[1]) angle = angle_vectors(reference_plane[1], target_plane[1]) origin = reference_plane[0] if angle: points = rotate_points(points, angle, axis, origin) vector = subtract_vectors(target_plane[0], reference_plane[0]) points = translate_points(points, vector) return points
def center_of_mass_polyhedron(polyhedron): """Compute the center of mass of a polyhedron. Parameters ---------- polyhedron : tuple The coordinates of the vertices, and the indices of the vertices forming the faces. Returns ------- tuple XYZ coordinates of the center of mass. Examples -------- >>> from compas.geometry import Polyhedron >>> p = Polyhedron.generate(6) >>> center_of_mass_polyhedron((p.vertices, p.faces)) (-4.206480876464043e-17, -4.206480876464043e-17, -4.206480876464043e-17) """ vertices, faces = polyhedron V = 0 x = 0.0 y = 0.0 z = 0.0 ex = [1.0, 0.0, 0.0] ey = [0.0, 1.0, 0.0] ez = [0.0, 0.0, 1.0] for face in faces: if len(face) == 3: triangles = [face] else: centroid = centroid_points([vertices[index] for index in face]) vertices.append(centroid) w = len(vertices) - 1 triangles = [[u, v, w] for u, v in window(face + face[0:1], 2)] for triangle in triangles: a = vertices[triangle[0]] b = vertices[triangle[1]] c = vertices[triangle[2]] ab = subtract_vectors(b, a) ac = subtract_vectors(c, a) n = cross_vectors(ab, ac) V += dot_vectors(a, n) nx = dot_vectors(n, ex) ny = dot_vectors(n, ey) nz = dot_vectors(n, ez) for j in (-1, 0, 1): ab = add_vectors(vertices[triangle[j]], vertices[triangle[j + 1]]) x += nx * dot_vectors(ab, ex)**2 y += ny * dot_vectors(ab, ey)**2 z += nz * dot_vectors(ab, ez)**2 if V < 1e-9: V = 0.0 d = 1.0 / 48.0 else: V = V / 6.0 d = 1.0 / 48.0 / V x *= d y *= d z *= d return x, y, z
def centroid_polygon(polygon): r"""Compute the centroid of the surface of a polygon. Parameters ---------- polygon : list of point A sequence of polygon point coordinates. Returns ------- list The XYZ coordinates of the centroid. Examples -------- .. code-block:: python from compas.geometry import centroid_polygon polygon = [ [0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0], [0.0, 1.0, 0.0] ] c = centroid_polygon(polygon) print(c) # [0.5, 0.5, 0.0] .. code-block:: python from compas.geometry import centroid_polygon from compas.geometry import centroid_points polygon = [ [0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0], [0.5, 1.0, 0.0], [0.0, 1.0, 0.0] ] c = centroid_polygon(polygon) print(c) # [0.5, 0.5, 0.0] c = centroid_points(polygon) print(c) # [0.5, 0.6, 0.0] Notes ----- The centroid is the centre of gravity of the polygon surface if mass would be uniformly distributed over it. It is calculated by triangulating the polygon surface with respect to the centroid of the polygon vertices, and then computing the centroid of the centroids of the individual triangles, weighted by the corresponding triangle area in proportion to the total surface area. .. math:: c_x = \frac{1}{A} \sum_{i=1}^{N} A_i \cdot c_{x,i} c_y = \frac{1}{A} \sum_{i=1}^{N} A_i \cdot c_{y,i} c_z = \frac{1}{A} \sum_{i=1}^{N} A_i \cdot c_{z,i} Warning ------- The polygon need not be convex. The polygon need not be flat. However, it is unclear what the meaning of the centroid is in that case. The polygon may be self-intersecting. However, it is unclear what the meaning of the centroid is in that case. """ p = len(polygon) assert p > 2, "At least three points required" if p == 3: return centroid_points(polygon) cx, cy, cz = 0.0, 0.0, 0.0 A2 = 0 o = centroid_points(polygon) a = polygon[-1] b = polygon[0] oa = subtract_vectors(a, o) ob = subtract_vectors(b, o) n0 = cross_vectors(oa, ob) x, y, z = centroid_points([o, a, b]) a2 = length_vector(n0) A2 += a2 cx += a2 * x cy += a2 * y cz += a2 * z for i in range(1, len(polygon)): a = b b = polygon[i] oa = ob ob = subtract_vectors(b, o) n = cross_vectors(oa, ob) x, y, z = centroid_points([o, a, b]) if dot_vectors(n, n0) > 0: a2 = length_vector(n) else: a2 = -length_vector(n) A2 += a2 cx += a2 * x cy += a2 * y cz += a2 * z if A2 == 0: return polygon[0] return [cx / A2, cy / A2, cz / A2]
def second_tangent(b2_1, b2_2, pt_mean_2, b_v2_1, b_v2_2, b_struct, b_v_old, pt1, radius, max_len, pt_mean, b_v0_n=None, check_collision=False): line = b_struct.vertex[b_v_old]["axis_endpoints"] vec_l_0 = vector_from_points(line[0], line[1]) ex = normalize_vector(cross_vectors(normalize_vector(vec_l_0), (1, 0, 0))) ey = normalize_vector(cross_vectors(normalize_vector(vec_l_0), ex)) ptM = pt1 pt_b_1 = b2_1["axis_endpoints"][0] pt_b_1_2 = b2_1["axis_endpoints"][1] l_1 = vector_from_points(pt_b_1, pt_b_1_2) pt_b_2 = b2_2["axis_endpoints"][0] pt_b_2_2 = b2_2["axis_endpoints"][1] l_2 = vector_from_points(pt_b_2, pt_b_2_2) # sols_test = tangent_from_point(pt_b_1, l_1, pt_b_2, l_2, ptM, 2*radius, 2*radius) if check_collision == False: if b_v0_n: ind = b_struct.vertex[b_v0_n]["index_sol"][0] else: ind = 0 sols_test = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ptM, 2 * radius, 2 * radius, ind) if not sols_test: return None # args = ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2*radius, 2*radius, ind # xfunc = XFunc('coop_assembly.help_functions.tangents.solve_second_tangent', radius'C:\Users\parascho\Documents\git_repos') # xfunc(ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2*radius, 2*radius, ind) # ret_sst = xfunc.data ## ret_sst = solve_second_tangent(*args) ret_sst = solve_second_tangent(ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2 * radius, 2 * radius, ind) if ret_sst: pt2, vec_l = ret_sst else: return None solution = vec_l ret_cls = check_length_sol_one(solution, pt_mean_2, pt2, b2_1, b2_2, b_v2_1, b_v2_2, b_struct) if not ret_cls: return None vec_sol_2, l2, pts_b2_1, pts_b2_2 = ret_cls pt2_e = add_vectors(pt2, scale_vector(vec_sol_2, l2)) end_pts_0 = (pt2, pt2_e) else: for ind in range(4): sols_test = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ptM, 2 * radius, 2 * radius, ind) if ind == 3 and sols_test == None: return None if sols_test == None: continue args = ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2 * radius, 2 * radius, ind ret_sst = solve_second_tangent(*args) if ret_sst: pt2, vec_l = ret_sst else: return None solution = vec_l ret_cls = check_length_sol_one(solution, pt_mean_2, pt2, b2_1, b2_2, b_v2_1, b_v2_2, b_struct) if not ret_cls: return None vec_sol_2, l2, pts_b2_1, pts_b2_2 = ret_cls pt2_e = add_vectors(pt2, scale_vector(vec_sol_2, l2)) end_pts_0 = (pt2, pt2_e) ext_len = 30 end_pts_0 = (add_vectors( pt2, scale_vector(normalize_vector(vector_from_points(pt2_e, pt2)), ext_len)), add_vectors( pt2_e, scale_vector( normalize_vector( vector_from_points(pt2, pt2_e)), ext_len))) bool_col = check_colisions(b_struct, end_pts_0, radius, bar_nb=b_v0_n) if bool_col == True: end_pts_check = b_struct.vertex[b_v2_1]["axis_endpoints"] bool_col = check_colisions(b_struct, end_pts_check, radius, bar_nb=b_v0_n, bar_checking=b_v2_1) if bool_col == True: end_pts_check = b_struct.vertex[b_v2_2]["axis_endpoints"] bool_col = check_colisions(b_struct, end_pts_check, radius, bar_nb=b_v0_n, bar_checking=b_v2_2) # bool_col = True if bool_col == False: print("COLLIDE", len(b_struct.vertex)) if ind == 3 and bool_col == False: print("NO TANGENT 2 FOUND IN ONE BAR COMBINATION") return None if bool_col == True: break # end_pts_0 = [map(float, p) for p in end_pts_0] vec_x, vec_y, vec_z = calculate_coord_sys(end_pts_0, pt_mean) # pt_o = centroid_points(end_pts_0) if not b_v0_n: # b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (2*radius, 2.0), vec_z) b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (25.0, 2.0), vec_z) else: b_v0 = b_v0_n b_struct.vertex[b_v0].update({"axis_endpoints": end_pts_0}) b_struct.vertex[b_v0].update({"index_sol": [ind]}) # b_struct.vertex[b_v0].update({"gripping_plane_no_offset":(pt_o, vec_x, vec_y, vec_z)}) # calculate_gripping_plane(b_struct, b_v0, pt_mean) b_struct.vertex[b_v0].update({"mean_point": pt_mean}) b2_1.update({"axis_endpoints": pts_b2_1}) b2_2.update({"axis_endpoints": pts_b2_2}) if not b_v0_n: b_struct.connect_bars(b_v0, b_v2_1) b_struct.connect_bars(b_v0, b_v2_2) dpp_1 = dropped_perpendicular_points( b_struct.vertex[b_v0]["axis_endpoints"][0], b_struct.vertex[b_v0]["axis_endpoints"][1], b_struct.vertex[b_v2_1]["axis_endpoints"][0], b_struct.vertex[b_v2_1]["axis_endpoints"][1]) dpp_2 = dropped_perpendicular_points( b_struct.vertex[b_v0]["axis_endpoints"][0], b_struct.vertex[b_v0]["axis_endpoints"][1], b_struct.vertex[b_v2_2]["axis_endpoints"][0], b_struct.vertex[b_v2_2]["axis_endpoints"][1]) # b_struct.edge[b_v0][b_v2_1].update({"endpoints":[dpp_1[0], dpp_1[1]]}) # b_struct.edge[b_v0][b_v2_2].update({"endpoints":[dpp_2[0], dpp_2[1]]}) k_1 = list(b_struct.edge[b_v0][b_v2_1]["endpoints"].keys())[0] k_2 = list(b_struct.edge[b_v0][b_v2_2]["endpoints"].keys())[0] b_struct.edge[b_v0][b_v2_1]["endpoints"].update( {k_1: (dpp_1[0], dpp_1[1])}) b_struct.edge[b_v0][b_v2_2]["endpoints"].update( {k_2: (dpp_2[0], dpp_2[1])}) return b_v0, pt2, end_pts_0
def centroid_polyhedron(polyhedron): """Compute the center of mass of a polyhedron. Parameters ---------- polyhedron : tuple The coordinates of the vertices, and the indices of the vertices forming the faces. Returns ------- list XYZ coordinates of the center of mass. Warning ------- This function assumes that the vertex cycles of the faces are such that the face normals are consistently pointing outwards, resulting in a *positive* polyhedron. Examples -------- >>> from compas.geometry import Polyhedron >>> p = Polyhedron.generate(6) >>> centroid_polyhedron(p) [0.0, 0.0, 0.0] """ vertices, faces = polyhedron V = 0 x = 0.0 y = 0.0 z = 0.0 ex = [1.0, 0.0, 0.0] ey = [0.0, 1.0, 0.0] ez = [0.0, 0.0, 1.0] for face in faces: if len(face) == 3: triangles = [face] else: centroid = centroid_points([vertices[index] for index in face]) w = len(vertices) vertices.append(centroid) triangles = [[w, u, v] for u, v in pairwise(face + face[0:1])] for triangle in triangles: a = vertices[triangle[0]] b = vertices[triangle[1]] c = vertices[triangle[2]] ab = subtract_vectors(b, a) ac = subtract_vectors(c, a) n = cross_vectors(ab, ac) V += dot_vectors(a, n) nx = dot_vectors(n, ex) ny = dot_vectors(n, ey) nz = dot_vectors(n, ez) ab = add_vectors(a, b) bc = add_vectors(b, c) ca = add_vectors(c, a) ab_x2 = dot_vectors(ab, ex)**2 bc_x2 = dot_vectors(bc, ex)**2 ca_x2 = dot_vectors(ca, ex)**2 x += nx * (ab_x2 + bc_x2 + ca_x2) ab_y2 = dot_vectors(ab, ey)**2 bc_y2 = dot_vectors(bc, ey)**2 ca_y2 = dot_vectors(ca, ey)**2 y += ny * (ab_y2 + bc_y2 + ca_y2) ab_z2 = dot_vectors(ab, ez)**2 bc_z2 = dot_vectors(bc, ez)**2 ca_z2 = dot_vectors(ca, ez)**2 z += nz * (ab_z2 + bc_z2 + ca_z2) # for j in (-1, 0, 1): # ab = add_vectors(vertices[triangle[j]], vertices[triangle[j + 1]]) # x += nx * dot_vectors(ab, ex) ** 2 # y += ny * dot_vectors(ab, ey) ** 2 # z += nz * dot_vectors(ab, ez) ** 2 V = V / 6.0 if V < 1e-9: d = 1.0 / (2 * 24) else: d = 1.0 / (2 * 24 * V) x *= d y *= d z *= d return [x, y, z]
def calculate_gripping_plane(b_struct, v, pt_mean, nb_rot=8, nb_trans=8, planes_rot=True, planes_trans=True): """calculate gripping planes for a given bar structure and the vertex key (representing a bar) Parameters ---------- b_struct : [type] [description] v : [type] [description] pt_mean : [type] [description] nb_rot : int, optional number of rotational division, by default 8 nb_trans : int, optional number of translational division, by default 8 planes_rot : bool, optional [description], by default True planes_trans : bool, optional [description], by default True """ end_pts_0 = b_struct.vertex[v]["axis_endpoints"] # local coordinate system vec_x, vec_y, vec_z = calculate_coord_sys(end_pts_0, pt_mean) # mid point of the bar axis pt_o = centroid_points(end_pts_0) b_struct.vertex[v].update({"gripping_plane": (pt_o, vec_x, vec_y, vec_z)}) gripping_plane = b_struct.vertex[v]["gripping_plane"] frames_all = [] if planes_trans == True: # extend both end points for 30 mm vec_bar = scale_vector( normalize_vector(subtract_vectors(end_pts_0[1], end_pts_0[0])), 30) pt1 = add_vectors(end_pts_0[0], vec_bar) vec_bar = scale_vector(vec_bar, -1) pt2 = add_vectors(end_pts_0[1], vec_bar) vec_n = subtract_vectors(pt2, pt1) len_vec = length_vector(vec_n) len_new = len_vec / (nb_trans - 1) for i in range(nb_trans): origin = add_vectors( pt1, scale_vector(normalize_vector(vec_n), len_new * i)) frame_n = [origin, gripping_plane[1], gripping_plane[2]] if not planes_rot: frames_all.append(frame_n) # if planes_flip == True: # frame_n = [frame_n[0], scale_vector(frame_n[1], -1), scale_vector(frame_n[2], -1)] # frames_all.append(frame_n) else: ang = math.radians(360 / nb_rot) for n in range(nb_rot): gripping_plane = frame_n vecs_n = rotate_points( [gripping_plane[1], gripping_plane[2]], angle=n * ang, axis=subtract_vectors(end_pts_0[1], end_pts_0[0]), origin=(0, 0, 0)) frame_n = [gripping_plane[0], vecs_n[0], vecs_n[1]] frames_all.append(frame_n) # if planes_flip == True: # frame_n = [frame_n[0], scale_vector(frame_n[1], -1), scale_vector(frame_n[2], -1)] # frames_all.append(frame_n) elif planes_rot == True: ang = math.radians(360 / nb_rot) for n in range(nb_rot): vecs_n = rotate_points([gripping_plane[1], gripping_plane[2]], angle=n * ang, axis=subtract_vectors( end_pts_0[1], end_pts_0[0]), origin=(0, 0, 0)) frame_n = [gripping_plane[0], vecs_n[0], vecs_n[1]] frames_all.append(frame_n) # if planes_flip == True: # frame_n = [frame_n[0], scale_vector(frame_n[1], -1), scale_vector(frame_n[2], -1)] # frames_all.append(frame_n) for i, f in enumerate(frames_all): z_vec = cross_vectors(f[1], f[2]) frames_all[i].append(z_vec) b_struct.vertex[v].update({"gripping_planes_all": frames_all})