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.
"""
o = centroid_points(polygon)
u = subtract_vectors(polygon[-1], o)
v = subtract_vectors(polygon[0], o)
a = 0.5 * length_vector(cross_vectors(u, v))
for i in range(0, len(polygon) - 1):
u = v
v = subtract_vectors(polygon[i + 1], o)
a += 0.5 * length_vector(cross_vectors(u, v))
return a
def normal_polygon(points, unitized=True):
"""Compute the normal of a polygon defined by a sequence of points.
Parameters:
points (sequence): A sequence of points.
Returns:
list: The normal vector.
Raises:
ValueError: If less than three points are provided.
Notes:
The points in the list should be unique. For example, the first and last
point in the list should not be the same.
"""
p = len(points)
assert p > 2, "At least three points required"
nx = 0
ny = 0
nz = 0
o = centroid_points(points)
a = subtract_vectors(points[-1], o)
for i in range(p):
b = subtract_vectors(points[i], o)
n = cross_vectors(a, b)
a = b
nx += n[0]
ny += n[1]
nz += n[2]
if not unitized:
return nx, ny, nz
l = length_vector([nx, ny, nz])
return nx / l, ny / l, nz / l
def calculate_point(self, v_key):
cons = self.connectors(v_key)
pts = []
for con in cons:
points_1 = self.struct_bar.vertex[con[0]]["axis_endpoints"]
points_2 = self.struct_bar.vertex[con[1]]["axis_endpoints"]
dpp = dropped_perpendicular_points(points_1[0], points_1[1],
points_2[0], points_2[1])
pts.append(centroid_points(
dpp)) # contact point is the mid point of P_{i} and P_{Ci}
# ? why take centroid again?
contact_pt = centroid_points(pts)
self.vertex[v_key].update({
"x": contact_pt[0],
"y": contact_pt[1],
"z": contact_pt[2],
"point_xyz": contact_pt
})
return contact_pt
def normal_polygon(polygon, unitized=True):
"""Compute the normal of a polygon defined by a sequence of points.
Parameters
----------
polygon : list of list
A list of polygon point coordinates.
Returns
-------
list
The normal vector.
Raises
------
ValueError
If less than three points are provided.
Notes
-----
The points in the list should be unique. For example, the first and last
point in the list should not be the same.
"""
p = len(polygon)
assert p > 2, "At least three points required"
nx = 0
ny = 0
nz = 0
o = centroid_points(polygon)
a = polygon[-1]
oa = subtract_vectors(a, o)
for i in range(p):
b = polygon[i]
ob = subtract_vectors(b, o)
n = cross_vectors(oa, ob)
oa = ob
nx += n[0]
ny += n[1]
nz += n[2]
if not unitized:
return nx, ny, nz
l = length_vector([nx, ny, nz])
return nx / l, ny / l, nz / l
def calculate_offset_pos_two_side_two_point_locked(b_struct, v_key, vecs_con_1,
vecs_con_2, pts_con_1,
pts_con_2, d_o_1, d_o_2):
"""calculate offsetted plane when the bar's both ends have two contact points
"""
assert len(vecs_con_1) == 2 and len(pts_con_1) == 2
assert len(vecs_con_2) == 2 and len(pts_con_2) == 2
map(normalize_vector, vecs_con_1)
map(normalize_vector, vecs_con_2)
v1_1, v1_2 = vecs_con_1
v2_1, v2_2 = vecs_con_2
pt_1_1, pt_1_2 = pts_con_1
pt_2_1, pt_2_2 = pts_con_2
vm_1 = scale_vector(normalize_vector(add_vectors(v1_1, v1_2)), -1. * d_o_1)
# original contact point (assuming two bars have the same radius)
pt_1 = centroid_points([pt_1_1, pt_1_2])
pt_1_new = translate_points([pt_1], vm_1)[0]
vm_2 = scale_vector(normalize_vector(add_vectors(v2_1, v2_2)), -1. * d_o_2)
pt_2 = centroid_points([pt_2_1, pt_2_2])
pt_2_new = translate_points([pt_2], vm_2)[0]
vec_x_new = normalize_vector(vector_from_points(pt_1_new, pt_2_new))
x_ax = b_struct.vertex[v_key]["gripping_plane"][1]
if not angle_vectors(x_ax, vec_x_new, deg=True) < 90:
vec_x_new = scale_vector(vec_x_new, -1.)
pt_o = b_struct.vertex[v_key]["gripping_plane"][0]
y_ax = b_struct.vertex[v_key]["gripping_plane"][2]
vec_z = cross_vectors(vec_x_new, y_ax)
l_n = (pt_1_new, pt_2_new)
pt_o_n = closest_point_on_line(pt_o, l_n)
return pt_o_n, vec_x_new, y_ax, vec_z
def offset_polyline(polyline, distance, normal=[0., 0., 1.]):
"""Offset a polyline by a distance.
Parameters:
polyline (sequence of sequence of floats): The XYZ coordinates of the
vertices of a polyline.
distance (float or list of tuples of floats): The offset distance as float.
A single value determines a constant offset globally. Alternatively, pairs of local
offset values per line segment can be used to create variable offsets.
Distance > 0: offset to the "left", distance < 0: offset to the "right"
normal (tuple): The normal of the offset plane.
Returns:
offset polyline (sequence of sequence of floats): The XYZ coordinates of the resulting polyline.
"""
if isinstance(distance, list) or isinstance(distance, tuple):
distances = distance
if len(distances) < len(polyline):
distances = distances + [distances[-1]
] * (len(polyline) - len(distances) - 1)
else:
distances = [[distance, distance]] * len(polyline)
lines = [polyline[i:i + 2] for i in range(len(polyline[:-1]))]
lines_offset = []
for i, line in enumerate(lines):
lines_offset.append(offset_line(line, distances[i], normal))
polyline_offset = []
polyline_offset.append(lines_offset[0][0])
for i in range(len(lines_offset[:-1])):
intx_pt1, intx_pt2 = intersection_line_line(lines_offset[i],
lines_offset[i + 1])
if intx_pt1 and intx_pt2:
polyline_offset.append(centroid_points([intx_pt1, intx_pt2]))
else:
polyline_offset.append(lines_offset[i][0])
polyline_offset.append(lines_offset[-1][1])
return polyline_offset
def centroid(self):
"""int: The centroid of the polygon."""
return Point(* centroid_points(self.points))
from compas_plotters import Plotter
plotter = Plotter(figsize=(10, 7))
points = [[0, 0, 0], [1.0, 0, 0], [1.0, 1.0, 0], [0.5, 0.0, 0],
[0, 1.0, 0]]
polygon = points[::-1]
print(polygon)
print(area_polygon(polygon))
n = normal_polygon(polygon, unitized=False)
print(n)
if n[2] > 0:
color = '#ff0000'
else:
color = '#0000ff'
polygons = [{'points': polygon}]
points = [{
'pos': centroid_points(polygon),
'radius': 0.025,
'facecolor': color
}]
plotter.draw_polygons(polygons)
plotter.draw_points(points)
plotter.show()
def offset_polygon(polygon, distance):
"""Offset a polygon (closed) by a distance.
Parameters:
polygon (sequence of sequence of floats): The XYZ coordinates of the
corners of the polygon. The first and last coordinates must be identical.
distance (float or list of tuples of floats): The offset distance as float.
A single value determines a constant offset globally. Alternatively, pairs of local
offset values per line segment can be used to create variable offsets.
Distance > 0: offset to the outside, distance < 0: offset to the inside
Returns:
offset polygon (sequence of sequence of floats): The XYZ coordinates of the
corners of the offset polygon. The first and last coordinates are identical.
Note:
The offset direction is determined by the normal of the polygon. The
algorithm works also for spatial polygons that do not perfectly fit a plane.
Examples:
.. code-block:: python
polygon = [
(0.0, 0.0, 0.0),
(3.0, 0.0, 1.0),
(3.0, 3.0, 2.0),
(1.5, 1.5, 2.0),
(0.0, 3.0, 1.0),
(0.0, 0.0, 0.0)
]
distance = 0.5 # constant offset
polygon_offset = offset_polygon(polygon, distance)
print(polygon_offset)
distance = [
(0.1, 0.2),
(0.2, 0.3),
(0.3, 0.4),
(0.4, 0.3),
(0.3, 0.1)
] # variable offset
polygon_offset = offset_polygon(polygon, distance)
print(polygon_offset)
"""
normal = normal_polygon(polygon)
if isinstance(distance, list):
distances = distance
if len(distances) < len(polygon):
distances = distances + [distances[-1]] * (len(polygon) - len(distances) - 1)
else:
distances = [[distance, distance]] * len(polygon)
lines = [polygon[i:i + 2] for i in xrange(len(polygon[:-1]))]
lines_offset = []
for i, line in enumerate(lines):
lines_offset.append(offset_line(line, distances[i], normal))
polygon_offset = []
for i in xrange(len(lines_offset)):
intx_pt1, intx_pt2 = intersection_line_line(lines_offset[i - 1], lines_offset[i])
if intx_pt1 and intx_pt2:
polygon_offset.append(centroid_points([intx_pt1, intx_pt2]))
else:
polygon_offset.append(lines_offset[i][0])
polygon_offset.append(polygon_offset[0])
return polygon_offset
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 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 bestfit_plane_from_points(points):
"""Fit a plane to a list of (more than three) points.
Parameters
----------
points : list of list
A list of points represented by their XYZ coordinates.
Returns
-------
plane : tuple
Base point and normal vector (normalized).
References
----------
http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
Warning
-------
This method will minimize the squares of the residuals as perpendicular
to the main axis, not the residuals perpendicular to the plane. If the
residuals are small (i.e. your points all lie close to the resulting plane),
then this method will probably suffice. However, if your points are more
spread then this method may not be the best fit.
See also
--------
compas.numerical.geometry.bestfit_plane
Examples
--------
>>>
"""
centroid = centroid_points(points)
xx, xy, xz = 0., 0., 0.
yy, yz, zz = 0., 0., 0.
for point in points:
rx, ry, rz = subtract_vectors(point, centroid)
xx += rx * rx
xy += rx * ry
xz += rx * rz
yy += ry * ry
yz += ry * rz
zz += rz * rz
det_x = yy * zz - yz * yz
det_y = xx * zz - xz * xz
det_z = xx * yy - xy * xy
det_max = max(det_x, det_y, det_z)
if det_max == det_x:
a = (xz * yz - xy * zz) / det_x
b = (xy * yz - xz * yy) / det_x
normal = (1., a, b)
elif det_max == det_y:
a = (yz * xz - xy * zz) / det_y
b = (xy * xz - yz * xx) / det_y
normal = (a, 1., b)
else:
a = (yz * xy - xz * yy) / det_z
b = (xz * xy - yz * xx) / det_z
normal = (a, b, 1.)
return centroid, normalize_vector(normal)
def add_tetra(o_struct,
b_struct,
connected_edges_from_vert,
new_vertex_pt,
new_vertex_id,
radius,
bool_add=True,
b_vert_ids=None,
o_v_key=None,
correct=True,
check_collision=False):
"""adds a new point and tetrahedron to the structure
input: nodes, bars from o_struct as vertex_key_integer and edge_vertex_key_tuples
.. image:: ../images/three_bar_group_generation.png
:scale: 60 %
:align: center
Parameters
----------
o_struct : OverallStructure
[description]
b_struct : BarStructure
[description]
tri_node_ids : list of three int
connected_edges_from_vert : dict
{OverallS vertex key : list of OverallS's edges}
dict keys: OverallStructure's vertex id triplets, representing the "ideal vertex" where multiple bars meet together.
dict value: each entry is a list of OverallS's edges connected to ideal vertex tri_node_ids[0], each representing a potential new bar (edges in OverallS represents bars)
new_vertex_pt : list, three floats
[x, y, z] coordinate of the newly added ideal vertex in OverallS.
new_vertex_id : int
vertex key of the newly added ideal vertex in OverallS.
radius : float
radius of the bar, in millimeter
bool_add : bool, optional
generate new vertex, not using given b_vi, by default True
b_vert_ids : list of three ints, optional
BarS vertex ids, if specified, b_struct's corresponding vertices attributes will be updated, by default None
o_v_key : int, optional
if specified, o_struct's corresponding vertex pt will be updated, by default None
correct : bool, optional
perform angle/distance based vertex correction if True, by default True
check_collision : bool, optional
perform collision-based correction if True, by default False
"""
# len_vec_min = 500
# len_vec_max = 1400
# len_vec = (random.random()*(len_vec_max - len_vec_min))+len_vec_min
max_len = 1800
assert bool_add or (b_vert_ids is not None and len(b_vert_ids) == 3)
assert len(connected_edges_from_vert) == 3
tri_node_ids = list(connected_edges_from_vert.keys())
comb_bars_1, comb_bars_2, comb_bars_3 = connected_edges_from_vert.values()
# * finding the mean point?
jnd = 0
bars1 = comb_bars_1[jnd]
bars2 = comb_bars_2[jnd]
bars3 = comb_bars_3[jnd]
print('bars 1 {} | bars 2 {} | bars 3 {}'.format(bars1, bars2, bars3))
# vertex id in BarS
# TODO: write a function to find mean point given bar ids
# two bars at vertex 0
b_v1_1 = o_struct.edge[bars1[0][0]][bars1[0][1]]["vertex_bar"]
b1_1 = b_struct.vertex[b_v1_1]
b_v1_2 = o_struct.edge[bars1[1][0]][bars1[1][1]]["vertex_bar"]
b1_2 = b_struct.vertex[b_v1_2]
# two bars at vertex 1
b_v2_1 = o_struct.edge[bars2[0][0]][bars2[0][1]]["vertex_bar"]
b2_1 = b_struct.vertex[b_v2_1]
b_v2_2 = o_struct.edge[bars2[1][0]][bars2[1][1]]["vertex_bar"]
b2_2 = b_struct.vertex[b_v2_2]
# two bars at vertex 2
b_v3_1 = o_struct.edge[bars3[0][0]][bars3[0][1]]["vertex_bar"]
b3_1 = b_struct.vertex[b_v3_1]
b_v3_2 = o_struct.edge[bars3[1][0]][bars3[1][1]]["vertex_bar"]
b3_2 = b_struct.vertex[b_v3_2]
# center points of the bar axes to obtain the central point of the base triangle
dpp1 = dropped_perpendicular_points(b1_1["axis_endpoints"][0],
b1_1["axis_endpoints"][1],
b1_2["axis_endpoints"][0],
b1_2["axis_endpoints"][1])
pt_mean_1 = centroid_points(dpp1)
dpp2 = dropped_perpendicular_points(b2_1["axis_endpoints"][0],
b2_1["axis_endpoints"][1],
b2_2["axis_endpoints"][0],
b2_2["axis_endpoints"][1])
pt_mean_2 = centroid_points(dpp2)
dpp3 = dropped_perpendicular_points(b3_1["axis_endpoints"][0],
b3_1["axis_endpoints"][1],
b3_2["axis_endpoints"][0],
b3_2["axis_endpoints"][1])
pt_mean_3 = centroid_points(dpp3)
pt_mean = centroid_points([pt_mean_1, pt_mean_2, pt_mean_3])
# if new_vertex_pt:
pt_new = new_vertex_pt
# check if new point is inside of structure
# if not new_vertex_pt:
# for t in o_struct.tetrahedra:
# if len(o_struct.tetrahedra[t]) > 3:
# if not o_struct.isOutside(pt_new, t):
# vec_n = scale_vector(vec_n, -1)
# pt_new = add_vectors(pt_mean, vec_n)
if correct:
pt_new = correct_point(b_struct,
o_struct,
pt_new, [(b_v1_1, b_v1_2), (b_v2_1, b_v2_2),
(b_v3_1, b_v3_2)],
o_v_key=o_v_key)
pt1 = pt_new
for j, bar_jnd_1 in enumerate(comb_bars_1):
bars1 = bar_jnd_1
b_v1_1 = o_struct.edge[bars1[0][0]][bars1[0][1]]["vertex_bar"]
b1_1 = b_struct.vertex[b_v1_1]
b_v1_2 = o_struct.edge[bars1[1][0]][bars1[1][1]]["vertex_bar"]
b1_2 = b_struct.vertex[b_v1_2]
if correct:
pt_new = correct_point(b_struct,
o_struct,
pt_new, [(b_v1_1, b_v1_2), (b_v2_1, b_v2_2),
(b_v3_1, b_v3_2)],
o_v_key=o_v_key)
# ! is this a mistype? shouldn't we plug in the corrected pt pt_new?
ret_ft = first_tangent(pt1,
b1_1,
b1_2,
pt_mean_1,
max_len,
b_v1_1,
b_v1_2,
b_struct,
pt_mean,
radius,
b_v0_n=None if bool_add else b_v0,
check_collision=check_collision)
if ret_ft:
b_v0, end_pts_0 = ret_ft
break
else:
# print("tangent 1 not found")
if j == len(comb_bars_1) - 1:
# print("no point found for first tangent calculation - 430, add_tetra")
raise RuntimeError(
"no point found for first tangent calculation - 430, add_tetra"
)
for j, bar_jnd_2 in enumerate(comb_bars_2):
bars2 = bar_jnd_2
b2_1 = b_struct.vertex[o_struct.edge[bars2[0][0]][bars2[0][1]]
["vertex_bar"]]
b_v2_1 = o_struct.edge[bars2[0][0]][bars2[0][1]]["vertex_bar"]
b2_2 = b_struct.vertex[o_struct.edge[bars2[1][0]][bars2[1][1]]
["vertex_bar"]]
b_v2_2 = o_struct.edge[bars2[1][0]][bars2[1][1]]["vertex_bar"]
if correct:
pt_new = correct_point(b_struct,
o_struct,
pt_new, [(b_v1_1, b_v1_2), (b_v2_1, b_v2_2),
(b_v3_1, b_v3_2)],
o_v_key=o_v_key)
if bool_add:
ret_st = second_tangent(b2_1,
b2_2,
pt_mean_2,
b_v2_1,
b_v2_2,
b_struct,
b_v0,
pt1,
radius,
max_len,
pt_mean,
check_collision=check_collision)
else:
ret_st = second_tangent(b2_1,
b2_2,
pt_mean_2,
b_v2_1,
b_v2_2,
b_struct,
b_v0,
pt1,
radius,
max_len,
pt_mean,
b_v1,
check_collision=check_collision)
if ret_st:
b_v1, pt2, end_pts_1 = ret_st
break
else:
# print("tangent 2 not found")
if j == len(comb_bars_2) - 1:
# print("no point found for second tangent calculation - 430, add_tetra")
raise RuntimeError(
"no point found for second tangent calculation - 430, add_tetra"
)
for j, bar_jnd_3 in enumerate(comb_bars_3):
bars3 = bar_jnd_3
b3_1 = b_struct.vertex[o_struct.edge[bars3[0][0]][bars3[0][1]]
["vertex_bar"]]
b_v3_1 = o_struct.edge[bars3[0][0]][bars3[0][1]]["vertex_bar"]
b3_2 = b_struct.vertex[o_struct.edge[bars3[1][0]][bars3[1][1]]
["vertex_bar"]]
b_v3_2 = o_struct.edge[bars3[1][0]][bars3[1][1]]["vertex_bar"]
if correct:
pt_new = correct_point(b_struct,
o_struct,
pt_new, [(b_v1_1, b_v1_2), (b_v2_1, b_v2_2),
(b_v3_1, b_v3_2)],
o_v_key=o_v_key)
if bool_add:
ret_tt = third_tangent(b_struct,
b_v0,
b_v1,
b3_1,
b3_2,
pt_mean_3,
max_len,
b_v3_1,
b_v3_2,
pt_mean,
radius,
check_collision=check_collision)
else:
ret_tt = third_tangent(b_struct,
b_v0,
b_v1,
b3_1,
b3_2,
pt_mean_3,
max_len,
b_v3_1,
b_v3_2,
pt_mean,
radius,
b_v2,
check_collision=check_collision)
if ret_tt:
b_v2, pt3, end_pts_2 = ret_tt
break
else:
# print("tangent 3 not found")
if j == len(comb_bars_3) - 1:
# print("no point found for third tangent calculation - 430, add_tetra")
raise RuntimeError(
"no point found for third tangent calculation - 430, add_tetra"
)
# * BarStructure update
if bool_add:
# adding contact edge information in BarS
b_struct.connect_bars(b_v0, b_v1)
b_struct.connect_bars(b_v1, b_v2)
b_struct.connect_bars(b_v2, b_v0)
# contact edge coordinate
dpp_1 = dropped_perpendicular_points(
b_struct.vertex[b_v1]["axis_endpoints"][0],
b_struct.vertex[b_v1]["axis_endpoints"][1],
b_struct.vertex[b_v2]["axis_endpoints"][0],
b_struct.vertex[b_v2]["axis_endpoints"][1])
key = list(b_struct.edge[b_v1][b_v2]["endpoints"].keys())[0]
b_struct.edge[b_v1][b_v2]["endpoints"].update({key: (dpp_1[0], dpp_1[1])})
dpp_2 = dropped_perpendicular_points(
b_struct.vertex[b_v2]["axis_endpoints"][0],
b_struct.vertex[b_v2]["axis_endpoints"][1],
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1])
key = list(b_struct.edge[b_v2][b_v0]["endpoints"].keys())[0]
b_struct.edge[b_v2][b_v0]["endpoints"].update({key: (dpp_2[0], dpp_2[1])})
dpp_3 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v1]["axis_endpoints"][0],
b_struct.vertex[b_v1]["axis_endpoints"][1])
key = list(b_struct.edge[b_v0][b_v1]["endpoints"].keys())[0]
b_struct.edge[b_v0][b_v1]["endpoints"].update({key: (dpp_3[0], dpp_3[1])})
# * OverallStructure update
if bool_add:
o_n_new = o_struct.add_node(pt_new, v_key=new_vertex_id)
### check length of bar and adjust gripper position ###
# pt_bar_1 = b_struct.vertex[b_v0]["axis_endpoints"]
# pt_bar_2 = b_struct.vertex[b_v1]["axis_endpoints"]
# pt_bar_3 = b_struct.vertex[b_v2]["axis_endpoints"]
# adjust_gripping_plane(pt_bar_1, pt_new, b_struct, b_v0)
# adjust_gripping_plane(pt_bar_2, pt_new, b_struct, b_v1)
# adjust_gripping_plane(pt_bar_3, pt_new, b_struct, b_v2)
### ###
if bool_add:
o_n1 = tri_node_ids[0]
o_n2 = tri_node_ids[1]
o_n3 = tri_node_ids[2]
o_struct.add_bar(o_n_new, o_n1, b_v0)
o_struct.add_bar(o_n_new, o_n2, b_v1)
o_struct.add_bar(o_n_new, o_n3, b_v2)
# adjust newly added bars' length
find_bar_ends(b_struct, b_v0)
find_bar_ends(b_struct, b_v1)
find_bar_ends(b_struct, b_v2)
# adjust neighbor bars' length
# find_bar_ends(b_struct, b_v1_1)
# find_bar_ends(b_struct, b_v1_2)
# find_bar_ends(b_struct, b_v2_1)
# find_bar_ends(b_struct, b_v2_2)
# find_bar_ends(b_struct, b_v3_1)
# find_bar_ends(b_struct, b_v3_2)
return o_struct, b_struct
def bestfit_plane(points):
"""Fit a plane to a list of (more than three) points.
Parameters
----------
points : list of list
A list of points represented by their XYZ coordinates.
Returns
-------
plane : tuple
Base point and normal vector (normalized).
Notes
-----
This method will minimize the squares of the residuals as perpendicular
to the main axis, not the residuals perpendicular to the plane. If the
residuals are small (i.e. your points all lie close to the resulting plane),
then this method will probably suffice. However, if your points are more
spread then this method may not be the best fit. For more information see
[ernerfeldt2015]_
References
----------
.. [ernerfeldt2015] Ernerfeldt, E. *Fitting a plane to many points in 3D*.
Available at: http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
Examples
--------
.. code-block:: python
#
See also
--------
:func:`bestfit_plane_numpy` : Variation using NumPy.
"""
centroid = centroid_points(points)
xx, xy, xz = 0., 0., 0.
yy, yz, zz = 0., 0., 0.
for point in points:
rx, ry, rz = subtract_vectors(point, centroid)
xx += rx * rx
xy += rx * ry
xz += rx * rz
yy += ry * ry
yz += ry * rz
zz += rz * rz
det_x = yy * zz - yz * yz
det_y = xx * zz - xz * xz
det_z = xx * yy - xy * xy
det_max = max(det_x, det_y, det_z)
if det_max == det_x:
a = (xz * yz - xy * zz) / det_x
b = (xy * yz - xz * yy) / det_x
normal = (1., a, b)
elif det_max == det_y:
a = (yz * xz - xy * zz) / det_y
b = (xy * xz - yz * xx) / det_y
normal = (a, 1., b)
else:
a = (yz * xy - xz * yy) / det_z
b = (xz * xy - yz * xx) / det_z
normal = (a, b, 1.)
return centroid, normalize_vector(normal)
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})
