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})
