def other_balls_collision(self, rack):
for another_ball in rack:
if another_ball.ball.visible:
pos_a = self.ball.pos
pos_b = another_ball.ball.pos
vel_a = self.velocity
vel_b = another_ball.velocity
radius = self.ball.radius
if self != another_ball and mag(pos_a - pos_b) < 2 * radius:
a = mag(vel_a - vel_b)**2
b = dot(-2 * (pos_a - pos_b), (vel_a - vel_b))
c = mag(pos_a - pos_b)**2 - (2 * radius)**2
delta = b**2 - 4 * a * c
if a != 0 and delta > 0:
dtprim = (-b + sqrt(delta)) / (2 * a)
pos_a = pos_a - vel_a * dtprim
pos_b = pos_b - vel_b * dtprim
tmp = (dot(vel_a - vel_b,
(pos_a - pos_b) / mag(pos_a - pos_b))) * (
(pos_a - pos_b) / mag(pos_a - pos_b))
vel_a -= tmp
vel_b += tmp
pos_a += vel_a * dtprim
pos_b += vel_b * dtprim
self.ball.pos = pos_a
self.set_velocity(vel_a)
another_ball.pos = pos_b
another_ball.set_velocity(vel_b)
def urto_elastico(body0, body1):
'''collisione elastica in 3 dimensioni'''
vrel = body0.velocity - body1.velocity
rrel = body0.pos - body1.pos
distance = mag(rrel)
ratio0 = 2 * body1.mass / (body0.mass + body1.mass)
ratio1 = 2 * body0.mass / (body0.mass + body1.mass)
body0.velocity += -ratio0 * dot(vrel, rrel) / distance**2 * rrel
body1.velocity += -ratio1 * dot(-vrel, -rrel) / distance**2 * (-rrel)
def elastic_collision(self):
for indexs in self.collided_couples:
if indexs in self.attached_couples: continue
body0 = self.bodies[indexs[0]]
body1 = self.bodies[indexs[1]]
vrel = body0.velocity - body1.velocity
rrel = body0.pos - body1.pos
a = rrel.mag2 # magnitude squared
ratio0 = 2 * body1.mass / (body0.mass + body1.mass)
ratio1 = 2 * body0.mass / (body0.mass + body1.mass)
body0.velocity += -ratio0 * dot(vrel, rrel) / a * rrel
body1.velocity += -ratio1 * dot(-vrel, -rrel) / a * (-rrel)
self.attached_couples = self.collided_couples.copy()
self.collided_couples.clear()
def collision_node_edge(body1, body2):
DISTANCE_TOLERANCE = 0.03
for pt1, (pt2, pt2_next) in it.product(body1.vertices, zip(body2.vertices, body2.vertices[1:]+body2.vertices[:1])):
edge = pt2_next - pt2
edge_normal = vp.norm(edge)
p = pt1 - pt2
proj_on_edge = edge_normal * vp.dot(p, edge_normal)
if vp.mag(proj_on_edge + edge) <= edge.mag or proj_on_edge.mag > edge.mag:
continue
# Perpendicular distance to edge
dist_to_edge = vp.mag(vp.cross(p, edge_normal))
if dist_to_edge > DISTANCE_TOLERANCE:
continue
collision_pt1 = pt1 - body1.pos
collision_pt2 = pt1 - body2.pos
collision_normal = vp.norm(vp.cross(vp.cross(edge_normal, p), edge_normal))
vel1 = body1.vel + vp.cross(body1.ang_vel, collision_pt1)
vel2 = body2.vel + vp.cross(body2.ang_vel, collision_pt2)
relative_vel = vel1 - vel2
if is_collision_course(relative_vel, collision_normal):
return Collision(body1, body2, collision_pt1, collision_pt2, relative_vel, collision_normal)
return None
def check_for_collisions(particles):
collisions = []
for particle in chain.from_iterable(particles):
if particle.locked:
continue
# Check for collisions with ground
if (particle.pos.y <= COLLISION_TOLERANCE) and (particle.vel.y <
VELOCITY_TOLERANCE):
collisions.append(
Collision(particle=particle,
normal=vp.norm(vp.vector(0, 1, 0))))
# Check for collisions with flag pole
# Center of mass of a pole is at the same height as particle. So distance (vector) between two
# mass centers will be also vector perpendicular to the edge of collision - page 248
dist = particle.pos - vp.vector(0, particle.pos.y, 0)
n = dist.norm()
relative_vel_n = vp.dot(particle.vel, n)
if dist.mag <= (FLAG_POLE_RADIUS + COLLISION_TOLERANCE) and \
(0 < particle.pos.y < FLAG_POLE_HEIGHT) and (relative_vel_n < VELOCITY_TOLERANCE):
collisions.append(Collision(particle=particle, normal=n))
return collisions
def calc_forces(particles, struct_springs):
# Process gravity and drag forces
for particle in chain.from_iterable(particles):
if particle.locked:
continue
# Gravity
particle.force = vp.vector(0, GRAVITY_ACC * particle.mass, 0)
# Viscous drag - page 17. Surface without rotation calculation.
# https://pl.wikipedia.org/wiki/Op%C3%B3r_aero(hydro)dynamiczny#Formu%C5%82y_na_wielko%C5%9B%C4%87_oporu_aero(hydro)dynamicznego
drag_vector = -vp.norm(particle.vel)
particle.force += drag_vector * DRAG_COEFFICIENT * AIR_DENSITY * 0.5 \
* particle.vel.mag2 * particle.surface
# Wind force
particle.force += wind_force()
# Process spring forces - page 82
for spring in struct_springs:
l = spring.particle1.pos - spring.particle2.pos
relative_vel = spring.particle1.vel - spring.particle2.vel
f1 = -(spring.k * (l.mag - spring.length) + spring.d *
(vp.dot(relative_vel, l) / l.mag)) * l.norm()
f2 = -f1
if not spring.particle1.locked:
spring.particle1.force += f1
if not spring.particle2.locked:
spring.particle2.force += f2
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 resolve_collisions(collisions):
for c in collisions:
# Impulse - page 115
# https://en.wikipedia.org/wiki/Collision_response#Impulse-based_reaction_model
vel_relative = c.particle.vel
impluse = (-(E_RESTITUTION + 1) *
vp.dot(vel_relative, c.normal)) / (1 / c.particle.mass)
c.particle.vel += (impluse * c.normal) / c.particle.mass
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 colision_part_2(obj_1, obj_2):
if vp.mag(obj_1.pos - obj_2.pos) < 2 * r_p:
delta_v_1 = obj_1.vel - obj_2.vel
delta_p_1 = obj_1.pos - obj_2.pos
delta_v_2 = obj_2.vel - obj_1.vel
delta_p_2 = obj_2.pos - obj_1.pos
coef_1 = (vp.dot(delta_v_1, delta_p_1)) / (vp.dot(
delta_p_1, delta_p_1))
coef_2 = (vp.dot(delta_v_2, delta_p_2)) / (vp.dot(
delta_p_2, delta_p_2))
obj_1.vel = obj_1.vel - coef_1 * (obj_1.pos - obj_2.pos)
obj_2.vel = obj_2.vel - coef_2 * (obj_2.pos - obj_1.pos)
lambda_1 = (2 * r_p -
(vp.mag(obj_1.pos - obj_2.pos))) / (vp.mag(obj_1.vel))
obj_1.pos = obj_1.pos + lambda_1 * obj_1.vel
def resolve_collisions(dt, collisions):
# https://en.wikipedia.org/wiki/Collision_response#Computing_impulse-based_reaction
for c in collisions:
impulse = (-(1+COEFFICIENT_OF_RESTITUTION) * vp.dot(c.relative_vel, c.collision_normal)) / \
(1/c.body1.mass + 1/c.body2.mass + \
vp.dot(c.collision_normal, vp.cross(vp.cross(c.collision_pt1, c.collision_normal) / c.body1.moment_inertia, c.collision_pt1)) + \
vp.dot(c.collision_normal, vp.cross(vp.cross(c.collision_pt2, c.collision_normal) / c.body2.moment_inertia, c.collision_pt2)))
c.body1.vel += impulse * c.collision_normal / c.body1.mass
c.body1.pos += c.body1.vel * dt
c.body2.vel -= impulse * c.collision_normal / c.body2.mass
c.body2.pos += c.body2.vel * dt
c.body1.ang_vel += vp.cross(c.collision_pt1, (impulse * c.collision_normal)) / c.body1.moment_inertia
angle_diff = c.body1.ang_vel.z * dt
c.body1.theta += angle_diff
c.body1.rotate(angle=angle_diff, axis=vp.vector(0, 0, 1))
c.body2.ang_vel -= vp.cross(c.collision_pt2, (impulse * c.collision_normal)) / c.body2.moment_inertia
angle_diff = c.body2.ang_vel.z * dt
c.body2.theta += angle_diff
c.body2.rotate(angle=angle_diff, axis=vp.vector(0, 0, 1))
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 penetration_by_node(body1, body2):
for pt1 in body1.vertices:
penetration = True
for pt2, pt2_next in zip(body2.vertices, body2.vertices[1:]+body2.vertices[:1]):
edge = pt2_next - pt2
p = pt1 - pt2
mag_proj_on_edge = vp.dot(p, vp.norm(edge))
if mag_proj_on_edge < 0:
penetration = False
break
if penetration:
collision_normal = vp.norm(body1.pos - body2.pos)
relative_vel = body1.vel - body2.vel
return Collision(body1, body2, pt1, pt1, relative_vel, collision_normal)
return None
def calc_forces(links, springs):
# Process gravity and drag forces
for particle in links:
if particle.locked:
continue
# Gravity
particle.force = vp.vector(0, GRAVITY_ACC * particle.mass, 0)
# Process spring forces - page 82
for spring in springs:
l = spring.particle1.pos - spring.particle2.pos
relative_vel = spring.particle1.vel - spring.particle2.vel
f1 = -(spring.k * (l.mag - spring.length) + spring.d *
(vp.dot(relative_vel, l) / l.mag)) * l.norm()
f2 = -f1
if not spring.particle1.locked:
spring.particle1.force += f1
if not spring.particle2.locked:
spring.particle2.force += f2
def angle_between(v1, v2):
"""
Berechne den Winkel zwischen zwei Vektoren.
Das Ergebnis ist in Grad.
"""
return acos(dot(v1, v2) / (v1.mag * v2.mag)) * 180.0 / pi
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)
width=0.05)
sticker.rotate(angle=angulo2,
axis=v.vector(1, 0, 0),
origin=v.vector(0, 0, 0))
stickers.append(sticker)
angulo2 = -(m.pi) / 2
while True:
pass
# key = v.scene.keyboard.getkey()
if keyboard.is_pressed("r"):
angle = m.pi / 2
for r in v.arange(0, angle, angle / fps):
v.rate(fps)
for sticker in stickers:
if v.dot(sticker.pos, v.vector(1, 0, 0)) > 0.5:
sticker.rotate(angle=(angle / fps),
axis=v.vector(1, 0, 0),
origin=v.vector(0, 0, 0))
elif keyboard.is_pressed("l"):
angle = -m.pi / 2
for r in v.arange(0, angle, angle / fps):
v.rate(fps)
for sticker in stickers:
if v.dot(sticker.pos, v.vector(-1, 0, 0)) > 0.5:
sticker.rotate(angle=(angle / fps),
axis=v.vector(-1, 0, 0),
origin=v.vector(0, 0, 0))
elif keyboard.is_pressed("u"):
angle = -m.pi / 2
for r in v.arange(0, angle, angle / fps):
def drawCameraFrame(): # create frame and draw its contents
global cam_box, cent_plane, cam_lab, cam_tri, range_lab, linelen, fwd_line
global fwd_arrow, mouse_line, mouse_arrow, mouse_lab, fov, range_x, cam_dist, cam_frame
global ray
cam_frame = vs.frame(pos=vs.vector(
0,
2,
2,
), axis=(0, 0, 1))
# NB: contents are rel to this frame. start with camera looking "forward"
# origin is at simulated scene.center
fov = vs.pi / 3.0 # 60 deg
range_x = 6 # simulates scene.range.x
cam_dist = range_x / vs.tan(
fov / 2.0) # distance between camera and center.
ray = vs.vector(-20.0, 2.5,
3.0).norm() # (unit) direction of ray vector (arbitrary)
# REL TO CAMERA FRAME
cam_box = vs.box(frame=cam_frame,
length=1.5,
height=1,
width=1.0,
color=clr.blue,
pos=(cam_dist, 0, 0)) # camera-box
cent_plane = vs.box(frame=cam_frame,
length=0.01,
height=range_x * 1.3,
width=range_x * 2,
pos=(0, 0, 0),
opacity=0.5) # central plane
cam_lab = vs.label(frame=cam_frame,
text='U',
pos=(cam_dist, 0, 0),
height=9,
xoffset=6)
cam_tri = vs.faces(frame=cam_frame,
pos=[(0, 0, 0), (0, 0, -range_x), (cam_dist, 0, 0)])
cam_tri.make_normals()
cam_tri.make_twosided()
range_lab = vs.label(frame=cam_frame,
text='R',
pos=(0, 0, -range_x),
height=9,
xoffset=6)
linelen = scene_size + vs.mag(cam_frame.axis.norm() * cam_dist +
cam_frame.pos)
# len of lines from camera
fwd_line = drawLine(vs.vector(cam_dist, 0, 0), linelen,
vs.vector(-1, 0, 0))
fwd_arrow = vs.arrow(frame=cam_frame,
axis=(-2, 0, 0),
pos=(cam_dist, 0, 0),
shaftwidth=0.08,
color=clr.yellow)
vs.label(frame=cam_frame,
text='C',
pos=(0, 0, 0),
height=9,
xoffset=6,
color=clr.yellow)
mouse_line = drawLine(vs.vector(cam_dist, 0, 0), linelen, ray)
mouse_arrow = vs.arrow(frame=cam_frame,
axis=ray * 2,
pos=(cam_dist, 0, 0),
shaftwidth=0.08,
color=clr.red)
mouse_lab = vs.label(frame=cam_frame,
text='M',
height=9,
xoffset=10,
color=clr.red,
pos=-ray * (cam_dist / vs.dot(ray, (1, 0, 0))) +
(cam_dist, 0, 0))
if not qPy: vss.forward = -cam_frame.axis
elif mode == 'r': # demonstrate altering scene.range. Alters size of camera triangle.
if qPy: gearing = 4
else: gearing = 1
cam_dist = cam_dist + (mouse_change.x + mouse_change.y +
mouse_change.z) * gearing
if cam_dist <= 0: cam_dist = 0.001 # allow only positive
limit = scene_size * 2
if cam_dist > limit: cam_dist = limit # limit size
reDrawLines()
cam_box.pos = (cam_dist, 0, 0)
cam_lab.pos = (cam_dist, 0, 0)
fwd_arrow.pos = (cam_dist, 0, 0)
mouse_arrow.pos = (cam_dist, 0, 0)
mouse_lab.pos = -ray * (cam_dist / vs.dot(ray, (1, 0, 0))) + (cam_dist,
0, 0)
range_x = cam_dist * vs.tan(fov / 2.0)
cent_plane.width = range_x * 2
cent_plane.height = range_x * 4.0 / 3
reDrawTri() # redraw the camera triangle
range_lab.pos = (0, 0, -range_x)
mode_lab.SetLabel('scene.range:\n' + format(range_x, "9.3"))
if not qPy: vss.range = range_x
elif mode == 'v': # demonstrate altering scene.fov
cam_dist = cam_dist + (mouse_change.x + mouse_change.y +
mouse_change.z) * 4
if cam_dist <= 0: cam_dist = 0.001 # allow only positive
ray = (mouse_lab.pos - (cam_dist, 0, 0)).norm()
reDrawLines()
dt = 0.1
g = 9.8
s = sphere()
s.pos = vector(-7, 10, 0)
#s.velocity = vector(5, 19.2, 0)
s.velocity = vector(0, 0, 0)
s.prev_pos = s.pos - (s.velocity * dt)
s.accel = vector(0, -g, 0)
s.mass = 2.0
t = 0
while t < 10:
kinetic_energy = 0.5 * s.mass * dot(s.velocity, s.velocity)
potential_energy = s.mass * 9.8 * dot(s.pos, vector(0, 1, 0))
kinetic.plot(pos=(t, kinetic_energy))
potential.plot(pos=(t, potential_energy))
energy.plot(pos=(t, kinetic_energy + potential_energy))
temp_pos = copy.copy(s.pos)
s.pos = 2 * s.pos - s.prev_pos + s.accel * dt**2
s.prev_pos = temp_pos
s.velocity = (s.pos - s.prev_pos) / dt
t += dt
rate(abs(1.0 / dt))
if not -10.0 < s.pos.x < 10.0:
s.pos.x, s.prev_pos.x = -s.pos.x, -s.prev_pos.x
if not 0.0 < s.pos.y < 19.0:
def __and__(self, other):
"""
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
"""
# if not isinstance(other, Polyhedron):
# raise TypeError(f"second object should be of type 'Polyhedron' but is of type {type(other)}.")
dist_vec = other.obj.pos - self.obj.pos
# check for bounding box overlap. This is very quick and can eliminate lots of cases with empty intersections
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 = [[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) #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)
# 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
return None
if len(relation_set) == 1: # all point are below the clip plane
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 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).count(True) == 1:
# if one point is above and the other below, calculate the intersection point:
edge_vec = point_2 - point_1
t = vpy.dot(clip_center - point_1,
clip_normal_vec) / vpy.dot(
edge_vec, clip_normal_vec)
intersect_point = point_1 + t * edge_vec
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
point_1 = point_2 # go to next edge
if len(new_face) > 2:
face[:] = new_face
else:
face.clear() #
if len(intersected_vertices) > 2:
new_poly_faces.append(
intersected_vertices) # add a single new face
del_empties(new_poly_faces) # delete empty faces
if not changed_polyhedron: # no faces of 'self' and 'other intersected'
return self # 'self' is completely inside of 'other'
return poly_from_faces(
new_poly_faces) # the intersection is a new polyhedron
m = 2 # kg
g = vector(0, -9.8, 0) # m/s^2
ymax = 20.0
xmax = 20.0
zmax = 20.0
while t < 10:
# the verlet integration:
s.force = m * g
olderpos = s.oldpos
s.oldpos = 1 * s.pos
s.pos = 2 * s.pos - olderpos + s.force * dt ** 2 / m
# work out the energy using a centered derivative
# for the velocity:
velocity = (s.pos - olderpos) / (2 * dt)
kineticEnergy = 0.5 * m * dot(velocity, velocity)
potentialEnergy = -m * dot(g, s.oldpos + ymax * vector(0, 1, 0))
print(kineticEnergy + potentialEnergy)
# Now let’s keep the ball in the box:
if s.pos.x > xmax or s.pos.x < -xmax:
s.pos.x, s.oldpos.x = s.oldpos.x, s.pos.x
if s.pos.y > ymax or s.pos.y < -ymax:
s.pos.y, s.oldpos.y = s.oldpos.y, s.pos.y
if s.pos.z > zmax or s.pos.z < -zmax:
s.pos.z, s.oldpos.z = s.oldpos.z, s.pos.z
kineticEnergy = 0.5 * m * dot(velocity, velocity)
potentialEnergy = -m * dot(g, s.pos + ymax * vector(0, 1, 0))
kinetic.plot(pos=(t, kineticEnergy))
potential.plot(pos=(t, potentialEnergy))
energy.plot(pos=(t, kineticEnergy + potentialEnergy))
d = 2
t = 0
dt = 0.01 # s
s = sphere()
spring = helix(radius=0.6, thickness=0.3)
s.velocity = vector(-10, 0, 0)
s.pos = vector(10, 0, 0) * dt # m
s.prev_pos = s.pos - s.velocity * dt # m
thermal_energy = 0
while t < 12.0 * pi:
kinetic_energy = 0.5 * m * dot(s.velocity, s.velocity)
potential_energy = 0.5 * k * dot(s.prev_pos, s.prev_pos)
thermal_energy += d * dot(s.velocity, s.velocity) * dt
s.force = -k * s.pos - d * s.velocity + 3.0 * sin(1.0 * t) * vector(
1, 0, 0)
temp = copy(s.prev_pos)
s.pos, s.prev_pos = 2 * s.pos - s.prev_pos + s.force * dt**2 / m, 1 * s.pos
s.velocity = (s.pos - temp) / (2 * dt)
spring.axis = s.pos - spring.pos
t += dt
rate(1 / dt)
kinetic.plot(pos=(t, kinetic_energy))
potential.plot(pos=(t, potential_energy))
thermal.plot(pos=(t, thermal_energy))
def is_collision_course(relative_vel, collision_normal):
relative_vel_n = vp.dot(relative_vel, collision_normal)
return relative_vel_n < 0
def chicharron_int(p_0, v_0, r):
b = 2 * (vp.dot(p_0, v_0))
c = (vp.mag(p_0))**2 - r**2
a = (vp.mag(v_0))**2
t = (-b - np.sqrt((b**2) - (4 * a * c))) / (2 * a)
return (t)
def redireccion(v_0, normal):
v_f = v_0 - 2 * (vp.dot(v_0, normal)) * normal
return (v_f)
def __init__(self,
vel=(0.0, 0.0, 0.0),
mass=1.0,
rotation_speed=0.0,
name='Sphere',
simple=False,
massive=True,
labelled=False,
luminous=False,
primary=None,
light_color='white',
impulses=None,
maneuver=None,
preset=None,
show_axes=False,
obliquity=0,
real_radius=None,
synchronous=False,
**kwargs):
"""
:param pos: (tuple/VPython vector) Position of the sphere; if primary is given, will be w.r.t the primary
(default is vector(0, 0, 0)).
:param vel: (tuple/VPython vector) Velocity of the sphere; if primary is given, will be w.r.t the primary
(default is vector(0, 0, 0)).
:param mass: (float) The mass of the sphere (default is 1.0).
:param rotation_speed: (float) The rotational_speed of the Sphere in rads/s (default is 0.0).
:param name: (str) The name of the Sphere (default is 'Sphere').
:param simple: (bool) If True will generate VPython simple_sphere instead of sphere (default is False).
:param massive: (bool) False indicates that the Sphere' mass can be considered negligible (default is True).
:param luminous: (bool) If True, will create a VPython local_light object at the Sphere pos (default is False).
:param labelled: (bool) If True, creates a VPython label that contains some Sphere attribute values
(default is False).
:param primary: (Sphere) The self.pos and self.vel values are with respect to the pos and vel of this primary
(default is None).
:param light_color: (str) The color of the class attribute, light (default is 'white').
:param impulses: (tuple/tuple(tuples)) The impulse instructions
((time, delta v, burn angle), ...) or (time, delta v, burn angle) for only one impulse (default is None).
:param maneuver: (class) A named maneuver class; will automatically created impulses instructions based off of
the maneuver; will need to add the appropriate instance attributes for the maneuver.
:param preset: (params class) Some pre-made parameters class (contains radius, mass, rotational speed, etc)
(default is None).
:param show_axes: (bool) If True, will display the cartesian axes and labels of the sphere;
can be removed and/or replaced afterwards using toggle_axes() (default is False).
:param obliquity: (float) The angle of obliquity in radians (default is 0).
:param real_radius: (float) The radius value that is used in collision calculations (default is radius).
:param synchronous: (bool) True if the Sphere has a synchronous rotation with respect to its primary;
only useful if using a specific celestial body texture like the moon or Io (default is False).
The parameters from simple_sphere, sphere, and Maneuver are also available.
"""
self.primary = primary
self.massive = massive
self.light_color = light_color
self._labelled = labelled
self.preset = preset
self.synchronous = synchronous
self._ring = None
keys = kwargs.keys()
# If a maneuver class is given, will create the appropriate impulse instructions.
# If impulses is given with no maneuver class given, will use those instructions instead.
if maneuver is not None:
attrs = [
'initial_radius', 'final_radius', 'transfer_apoapsis',
'transfer_eccentricity', 'inclination_change', 'start_time'
]
params = {elem: kwargs.pop(elem) for elem in attrs if elem in keys}
Maneuver.__init__(
self,
maneuver=maneuver,
gravitational_parameter=self.primary.grav_parameter,
**params)
else:
self.impulses = impulses
# If a maneuver class is given or own impulse instructions given, will get a list of the impulse times.
if self.impulses is not None:
try:
self.times = list(zip(*self.impulses))[0]
except TypeError:
self.times = [self.impulses[0]]
self._vel = self._velocity = self.vpython_rotation(
self.__try_vector(vel))
if 'pos' in keys:
self._position = kwargs['pos'] = self.vpython_rotation(
self.__try_vector(kwargs['pos']))
else:
self._position = vector(0, 0, 0)
# If primary is another Sphere, pos and vel are with respect to the primary;
# This will update pos and vel to be with respect to the origin of the VPython reference frame.
if isinstance(self.primary, Sphere):
kwargs['pos'] = self._position + self.primary.pos
self._vel = self._velocity + self.primary.vel
self._k_hat = hat(cross(self._position, self._velocity))
if isinstance(self.primary, Sphere):
self._up = self._k_hat
else:
self._up = self._zaxis
# If a preset is given, uses preset attributes instead of the given attributes.
if self.preset:
self.mass = self.preset.mass
self.rotation_speed = self.preset.angular_rotation
self.grav_parameter = self.preset.gravitational_parameter
self.name = str(self.preset())
kwargs['radius'] = self.preset.radius
kwargs['texture'] = self.preset.texture
if isinstance(self.primary, Sphere):
self.obliquity = self.preset.obliquity
angles = np.arange(0, 2 * math.pi, 0.05)
arbitrary_r = rotate_y(ran.choice(angles)).dot(
np.array([self._xaxis.x, self._xaxis.y, self._xaxis.z]))
self._up = vector(
*rodrigues_rotation(arbitrary_r, self.obliquity).dot(
np.array([self._up.x, self._up.y, self._up.z])))
if self.name == 'Saturn':
s = shapes.circle(radius=self.preset.ring_outer, thickness=0.4)
p = paths.line(start=kwargs['pos'],
end=(kwargs['pos'] + (7 * self._up)))
self._ring = extrusion(shape=s,
path=p,
texture=self.preset.ring_texture,
shininess=self.shininess,
up=self._up)
else:
self.mass = mass
self.rotation_speed = rotation_speed
self.name = name
self.grav_parameter = self.mass * gravity()
self.obliquity = obliquity
for col in ['color', 'trail_color', 'light_color']:
if col == 'light_color':
self.light_color = getattr(color, self.light_color)
elif col in keys:
if isinstance(kwargs[col], str):
kwargs[col] = getattr(color, kwargs[col])
else:
kwargs[col] = self.__try_vector(kwargs[col])
if 'texture' in keys and isinstance(kwargs['texture'], str):
try:
kwargs['texture'] = getattr(textures, kwargs['texture'])
except AttributeError:
pass
if self.synchronous:
kwargs['up'] = self._k_hat
else:
kwargs['up'] = self._up
kwargs['shininess'] = self.shininess
if simple:
simple_sphere.__init__(self, **kwargs)
else:
sphere.__init__(self, **kwargs)
# If the Sphere has a synchronous rotation with its primary, will rotate the Sphere so that the proper side
# is facing the primary (before obliquity is added to the Sphere).
if self.synchronous:
axis = vector_to_np(hat(cross(vector(0, 1, 0), self.up)))
angle = math.acos(dot(self.up, vector(0, 1, 0)))
face = vector_to_np(getattr(SphereFace, self.preset.name))
rot_z = -1 * self.__try_vector(
rodrigues_rotation(axis, angle).dot(face))
dot_product = dot(hat(cross(self._position, rot_z)), self.up)
theta = math.acos(dot(hat(self._position), rot_z))
if theta == math.pi:
self.rotate(angle=theta, axis=self.pos + self.up)
elif math.isclose(dot_product, 1.0):
self.rotate(angle=-theta, axis=self.up)
elif math.isclose(dot_product, -1.0):
self.rotate(angle=theta, axis=self.up)
self.up = self._up
self._luminous = self.emissive = luminous
if self._luminous:
self.__make_light()
if self._labelled:
self.__make_label()
self.show_axes = show_axes
if self.show_axes:
self.__axes()
if real_radius:
self.real_radius = real_radius
else:
self.real_radius = self.radius
j = ij[1]
ptot = p[i] + p[j]
posi = apos[i]
posj = apos[j]
vi = p[i] / mass
vj = p[j] / mass
vrel = vj - vi
a = vrel.mag2
if a == 0:
continue # exactly same velocities
rrel = posi - posj
if rrel.mag > Ratom:
continue # one atom went all the way through another
# theta is the angle between vrel and rrel:
dx = vp.dot(rrel, vrel.hat) # rrel.mag*cos(theta)
dy = vp.cross(rrel, vrel.hat).mag # rrel.mag*sin(theta)
# alpha is the angle of the triangle composed of rrel, path of atom j, and a line
# from the center of atom i to the center of atom j where atome j hits atom i:
alpha = vp.asin(dy / (2 * Ratom))
# distance traveled into the atom from first contact
d = (2 * Ratom) * vp.cos(alpha) - dx
# time spent moving from first contact to position inside atom
deltat = d / vrel.mag
posi = posi - vi * deltat # back up to contact configuration
posj = posj - vj * deltat
mtot = 2 * mass
pcmi = p[i] - ptot * mass / mtot # transform momenta to cm frame
pcmj = p[j] - ptot * mass / mtot
rrel = vp.norm(rrel)
