def _sort_faces(self):
"""
sort the vertex indices for every face such that they are listed clockwise as seen from the inside
since the faces of a convex polyhedron are convex polygons, the center of a face is always inside this polygon.
therefor the angles between the vectors connecting each vertex to the face center are unique
"""
# print(self.faces)
# self.show_faces()
for face_com, face_indices in zip(self.face_centers, self.faces):
start_vec = self.vertices[face_indices[0]].pos - face_com
face_normal = vpy.cross(
start_vec, self.vertices[face_indices[1]].pos - face_com)
if vpy.diff_angle(face_normal, face_com - self.pos) > vpy.pi / 2:
face_normal *= -1
# if self.debug:
# vpy.arrow(axis=face_normal/face_normal.mag*5,
# pos=face_com, color=vpy.vec(0, 0.8, 0))
def sort_face(vertex_index):
"""
return the angle between the starting vector and 'self.vertices[vertex_index] - face_com'
"""
current_vec = self.vertices[vertex_index].pos - face_com
angle = vpy.diff_angle(start_vec, current_vec) * \
sign(vpy.dot(vpy.cross(start_vec, current_vec), face_normal))
return angle
face_indices.sort(key=sort_face)
del (sort_face) # clean up the internal function
def calc_rotate_pair(point_A, point_B, com, PUZZLE_COM=vpy.vec(0, 0, 0)):
"""
calculate everything necessary to rotate 'point_A' to 'point_B' around
the point 'com'
inputs:
-------
point_A - (vpython 3d object) - a vpython object with .pos attribute
this object will be moved
point_B - (vpython 3d object) - a vpython object with .pos attribute
this is the position of 'point_A' after the rotation
com - (vpy.vector) - the point around which 'point_A' will be rotated
returns:
--------
(float) - angle of rotation in radians
(vpy.vector) - axis of rotation
(vpy.vector) - origin for the rotation
"""
v_A = point_A.pos - com
v_B = point_B.pos - com
# check for linear dependence
if abs(abs(vpy.dot(v_A, v_B)) - v_A.mag * v_B.mag) <= 1e-12:
mid_point = (point_A.pos + point_B.pos) / 2
axis = mid_point - PUZZLE_COM
angle = vpy.pi
return angle, axis, mid_point
axis = vpy.cross(v_A, v_B)
angle = vpy.diff_angle(v_A, v_B)
return angle, axis, com
def sort_face(point_index):
"""
return three values for sorting (in that order):
1. angle between the starting vector and `point_coordinates[point_index] - piece_com`
2. angle between `piece_com` and `point_coordinates[point_index] - piece_com`
3. magnitude of `point_coordinates[point_index] - piece_com`
inputs:
-------
index of an element in piece
"""
current_vec = point_coordinates[point_index] - piece_com
angle = vpy.diff_angle(start_vec, current_vec) * \
sign(vpy.dot(vpy.cross(start_vec, current_vec), piece_com))
return (angle, vpy.diff_angle(piece_com,
current_vec), current_vec.mag)
def sort_face(vec):
"""
return the angle between the starting vector and 'self.vertices[vertex_index] - face_com'
"""
current_vec = vec - face_com
angle = vpy.diff_angle(start_vec, current_vec) * \
sign(vpy.dot(vpy.cross(start_vec, current_vec), face_normal))
return angle
def vertex_is_inside(vertex, plane_normal, plane_anchor):
"""
Checks whether or not a given vertex is below (True) or above (False) a given plane.
The planes normal vector points towards the 'above' side of the plane.
If the point is on the plane, also returns True
inputs:
-------
vertex - (vpy.vector) - any 3D point as a vpython vector
plane_normal - (vpy.vector) - the normal vector of the plane of interest
plane_anchor - (vpy.vector) - any point on the plane
returns:
--------
(bool) - True if vertex is below or on the plane,
False if it is above the plane
"""
if vpy.diff_angle(vertex - plane_anchor, plane_normal) >= vpy.pi / 2:
return True
return False
def intersect(self, other, debug=False, color=None, **poly_properties):
"""
define intersection of two polyhedra
equal to the intersection of the sets of all points inside the polyhedra
edge cases are treated as empty intersections:
if the intersection has no volume (i.e. if it is just a point or line),
returns None
inputs:
-------
other - (Polyhedron) - another 3D polyhedron
returns:
--------
(NoneType) or (Polyhedron) - None if the intersection is empty,
otherwise return a new Polyhedron object
raises:
-------
TypeError - if 'other' is not a Polyhedron object.
"""
if not isinstance(other, Polyhedron):
raise TypeError(
f"second object should be of type 'Polyhedron' but is of type {type(other)}."
)
# # check for bounding box overlap. This is very quick and can eliminate many cases with empty intersections
# dist_vec = other.obj.pos - self.obj.pos
# if abs(dist_vec.x) > other.obj.size.x/2 + self.obj.size.x/2 \
# and abs(dist_vec.y) > other.obj.size.y/2 + self.obj.size.y/2 \
# and abs(dist_vec.z) > other.obj.size.z/2 + self.obj.size.z/2:
# return None
# del(dist_vec)
changed_polyhedron = False # variable to check if the polyhedron was changed
# We will work on the list of faces but the faces are lists of vpython vectors,
# not just refrences to self.vertices
new_poly_faces = [[r_vec(self.vertices[i].pos) for i in face]
for face in self.faces]
for clip_center, clip_face in zip(other.face_centers, other.faces):
# calculate normal vector of the clip plane
clip_vec_0 = other.vertices[clip_face[0]].pos - clip_center
clip_vec_1 = other.vertices[clip_face[1]].pos - clip_center
clip_normal_vec = vpy.cross(clip_vec_0, clip_vec_1)
del (clip_vec_0, clip_vec_1, clip_face) #cleanup
# calculate for each point whether it's above or below the clip plane
relation_dict = get_vertex_plane_relations(new_poly_faces,
clip_normal_vec,
clip_center,
eps=self.eps)
if debug:
debug_list = [
vpy.arrow(axis=clip_normal_vec / clip_normal_vec.mag,
pos=clip_center,
color=color,
shaftwidth=0.05,
headlength=0.2,
headwidth=0.2)
]
debug_list += [
vpy.sphere(pos=vpy.vec(*key),
radius=0.05,
color=vpy.vec(1 - val, val, 0))
for key, val in relation_dict.items()
]
# skip calculation if this plane does not intersect the polyhedron
relation_set = set(relation_dict.values())
if relation_set == {False}: # all points are above the clip plane
if debug:
for obj in debug_list:
obj.visible = False
del (debug_list)
return None
if len(relation_set) == 1: # all point are below the clip plane
if debug:
for obj in debug_list:
obj.visible = False
del (debug_list)
continue
del (relation_set)
changed_polyhedron = True
intersected_vertices = list()
for face in new_poly_faces: # loop over all faces of the polyhedron
point_1 = face[-1]
new_face = list()
for point_2 in face: # loop over all edges of each face
point_1_rel = relation_dict[vpy_vec_tuple(point_1)]
point_2_rel = relation_dict[vpy_vec_tuple(point_2)]
if debug:
edge = point_2 - point_1
debug_list.append(
vpy.arrow(
pos=point_1,
axis=edge,
shaftwidth=0.05,
headlength=0.2,
headwidth=0.2,
color=vpy.vec(1, 0, 1),
opacity=0.75)) #show directional debug edge
print(
f"calc intersection: {bool(len(set((point_1_rel, point_2_rel)))-1)}"
)
if point_1_rel: #point_1 is below the clip face -> possibly still in the clip polyhedron
if not point_1 in new_face:
new_face.append(point_1)
if point_1_rel != point_2_rel:
# if one point is above and the other below, calculate the intersection point:
edge_vec = point_2 - point_1
# counteract numerical errors:
if abs(
vpy.diff_angle(clip_normal_vec, edge_vec) -
vpy.pi / 2) < 1e-5:
# edge on clip plane
continue
div = vpy.dot(edge_vec, clip_normal_vec)
if div != 0:
t = vpy.dot(clip_center - point_1,
clip_normal_vec) / div
# try to prevent numerical errors:
if abs(t) < 1e-5: # point_1 on clip plane
intersect_point = point_1
elif abs(t - 1) < 1e-5: # point 2 on clip plane
intersect_point = point_2
else: # normal intersection calculation
intersect_point = r_vec(point_1 + t * edge_vec)
# process intersection point
if not intersect_point in new_face:
new_face.append(intersect_point)
if not intersect_point in intersected_vertices:
intersected_vertices.append(intersect_point)
relation_dict[vpy_vec_tuple(
intersect_point)] = True
if debug:
debug_list.append(
vpy.sphere(pos=intersect_point,
color=vpy.vec(1, 0, 1),
radius=0.06))
point_1 = point_2 # go to next edge
if debug:
while not isinstance(debug_list[-1], vpy.arrow):
debug_list[-1].visible = False # hide debug points
del (debug_list[-1])
debug_list[-1].visible = False # hide debug edge
if len(new_face) > 2:
face[:] = new_face
else:
face.clear()
if debug:
for obj in debug_list:
obj.visible = False
del (debug_list)
if len(intersected_vertices) > 2:
sort_new_face(intersected_vertices)
new_poly_faces.append(
intersected_vertices) # add a single new face
del_empties(new_poly_faces) # delete empty faces
if debug:
self.toggle_visible(False)
try:
temp.toggle_visible(False)
except UnboundLocalError:
pass
temp = poly_from_faces(
new_poly_faces,
debug=debug,
color=color,
opacity=0.5,
show_faces=True,
show_edges=True,
show_corners=False,
sort_faces=True,
edge_color=poly_properties["edge_color"],
corner_color=poly_properties["corner_color"],
edge_radius=poly_properties["edge_radius"],
corner_radius=poly_properties["corner_radius"],
pos=poly_properties["pos"])
print("next")
# if not changed_polyhedron: # no faces of 'self' and 'other intersected'
# return self # 'self' is completely inside of 'other'
# the intersection is a new polyhedron
if len(new_poly_faces) == 0:
return None
return poly_from_faces(new_poly_faces,
debug=debug,
color=color,
**poly_properties)
1 / 2 * (L - 2 * R) * cos(alpha + theta2)) - meu_2 * cos(alpha) * (
m + M) * g * R) / (I_sys + (m + M) * R ** 2)) * dt
time += dt
capsule.pos += vec(R * dtheta2 * cos(alpha), - R * dtheta2 * sin(alpha), 0)
origin = vec(capsule.pos.x + (0.5 * L - R) * cos(alpha + theta2),
capsule.pos.y - (0.5 * L - R) * sin(alpha + theta2),
capsule.pos.z)
# white_mark = sphere(pos=origin, radius=0.5, color=color.white)
capsule.rotate(angle=dtheta2, axis=vec(0, 0, -1), origin=origin)
ball.pos += vec(R * dtheta2 * cos(alpha), - R * dtheta2 * sin(alpha), 0)
if ball.pos.y < 0:
break
if ball.pos.y < 0:
break
check_vec = capsule.pos - vec(0.5 * B, 0, 0)
if diff_angle(check_vec, - vec(0.5 * B, 0, 0)) != alpha:
print("Simulation and upgradation of positions and numerical values with dt = 1 micro sec ",
"resulted in an angle of inclination of the simulated capsule to be: ", diff_angle(check_vec, - vec(0.5 * B, 0, 0)))
print("The true angle of inclination is: ", alpha)
print("Adjusting the position of capsule for the simulation to visually seem smooth...")
"""
This is python's numerical computation fault :(. Further refer to:
https://www-uxsup.csx.cam.ac.uk/courses/moved.NumericalPython/paper_1.pdf
to see how python creates this gap for numerical calculation errors.
"""
# the following lines added to keep simulation outlook smooth
capsule.pos = vec(cap_ref.x + R * pi * cos(alpha), cap_ref.y - R * pi * sin(alpha), cap_ref.z)
origin = vec(capsule.pos.x + (0.5 * L - R) * cos(alpha),
capsule.pos.y - (0.5 * L - R) * sin(alpha),
capsule.pos.z)
capsule.rotate(angle=pi, axis=vec(0, 0, -1), origin=origin)