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 _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 sort_new_face(face_vertices): #, piece_center):
"""
sort a face given just by it's vertex coordinates.
# Also requires a reference point 'piece_center'
# to determine the sorting direction
"""
# calculate center of the given face
face_com = get_vec_com(face_vertices)
start_vec = face_vertices[0] - face_com
face_normal = vpy.cross(start_vec, face_vertices[1] - face_com)
# if vpy.diff_angle(face_normal, face_com-piece_center) > vpy.pi/2:
# face_normal *= -1
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
face_vertices.sort(key=sort_face)
del (sort_face)
def __apply_impulse(self, sphere):
""" Applies an impulse to the desired sphere. To be used in __update_spheres(). """
for index, time in enumerate(sphere.times):
seconds1 = (self._time - self._start_time).total_seconds()
seconds2 = (time - self._start_time).total_seconds()
if self._dt < 0:
round(seconds2, decimal_length(self._dt))
elif self._dt > 0:
# A value of 10.1 would be rounded to the nearest 10's value (ignores the decimal places).
seconds2 = round_to_place(seconds2, integer_length(self._dt) + 1)
if seconds1 == seconds2:
if isinstance(sphere.impulses[0], tuple):
delta_v = sphere.impulses[index][1]
burn_angle = sphere.impulses[index][2]
else:
delta_v = sphere.impulses[1]
burn_angle = sphere.impulses[2]
if sphere.maneuver is Hohmann or sphere.maneuver is BiElliptic:
sphere.vel += delta_v*hat(sphere.vel) + sphere.primary.vel
elif sphere.maneuver is GeneralTransfer:
axis = vector_to_np(hat(cross(sphere._position, sphere._velocity)))
sphere.vel += vector(*rodrigues_rotation(axis, burn_angle).dot(vector_to_np(delta_v*hat(sphere.vel)))) + sphere.primary.vel
elif sphere.maneuver is SimplePlaneChange:
axis = vector_to_np(hat(sphere._position))
sphere.vel += vector(*rodrigues_rotation(axis, burn_angle).dot(vector_to_np(delta_v*hat(sphere.vel)))) + sphere.primary.vel
else:
# If the user gives their own impulse instructions without a known maneuver title.
pass
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(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 update(self, dt):
# forces
axis, theta = _euler.euler2axangle(self.pqr.x, self.pqr.y, self.pqr.z)
axis = _vp.vector(axis[0], axis[1], axis[2])
up = _vp.rotate(_vp.vector(0,1,0), theta, axis)
a = _vp.vector(0, -_gravity, 0)
a = a + (self.thrust1+self.thrust2+self.thrust3+self.thrust4)/self.mass * up + self.wind/self.mass
a = a - (_lin_drag_coef * _vp.mag(self.xyz_dot)**2)/self.mass * self.xyz_dot
self.xyz_dot = self.xyz_dot + a * dt
# torques (ignoring propeller torques)
cg = self.cgpos * up
tpos1 = _vp.rotate(_vp.vector(1.3*_size,0,0), theta, axis)
tpos2 = _vp.rotate(_vp.vector(0,0,1.3*_size), theta, axis)
tpos3 = _vp.rotate(_vp.vector(-1.3*_size,0,0), theta, axis)
tpos4 = _vp.rotate(_vp.vector(0,0,-1.3*_size), theta, axis)
torque = _vp.cross(cg, _vp.vector(0, -_gravity, 0))
torque = torque + _vp.cross(tpos1, self.thrust1 * up)
torque = torque + _vp.cross(tpos2, self.thrust2 * up)
torque = torque + _vp.cross(tpos3, self.thrust3 * up)
torque = torque + _vp.cross(tpos4, self.thrust4 * up)
torque = torque - _rot_drag_coef * self.pqr_dot
aa = torque/self.inertia
if _vp.mag(aa) > 0:
aai, aaj, aak = _euler.axangle2euler((aa.x, aa.y, aa.z), _vp.mag(aa))
aa = _vp.vector(aai, aaj, aak)
self.pqr_dot = self.pqr_dot + aa * dt
else:
self.pqr_dot = _vp.vector(0,0,0)
# ground interaction
if self.xyz.y <= 0:
self.xyz.y = 0
if self.xyz_dot.y <= 0:
self.xyz_dot.x = self.xyz_dot.x * _ground_friction
self.xyz_dot.y = 0
self.xyz_dot.z = self.xyz_dot.z * _ground_friction
self.pqr_dot = self.pqr_dot * _ground_friction
# energy update
self.energy += _power_coef * (self.thrust1**1.5 + self.thrust2**1.5 + self.thrust3**1.5 + self.thrust4**1.5) * dt
# time update
self.xyz += self.xyz_dot * dt
self.pqr += self.pqr_dot * dt
# callback
if self.updated is not None:
self.updated(self)
self.draw()
def collision_node_node(body1, body2):
for pt1, pt2 in it.product(body1.vertices, body2.vertices):
if not close_points(pt1, pt2):
continue
collision_pt1 = pt1 - body1.pos
collision_pt2 = pt1 - body2.pos
collision_normal = vp.norm(body1.pos - body2.pos)
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 updateAxis(self):
if not self.colorbar:
return
forward = vp.norm(self.scene.forward)
screenUp = vp.norm(self.scene.up)
right = vp.norm(vp.cross(forward, screenUp))
up = vp.norm(vp.cross(right, forward))
dx = 0.8
x = vp.vector(dx, 0.0, 0.0)
y = vp.vector(0.0, dx, 0.0)
z = vp.vector(0.0, 0.0, dx)
self.xAx.axis = vp.vector(x.dot(right), x.dot(up), 0.0)
self.yAx.axis = vp.vector(y.dot(right), y.dot(up), 0.0)
self.zAx.axis = vp.vector(z.dot(right), z.dot(up), 0.0)
self.axisLength.text = "{:.2f} <i>u</i>m".format(
dx * 1e6 * self.scene.range * self.colorbar.width /
self.scene.width)
def make_normals(self):
# Set the normal for each vertex to be perpendicular to the lower left corner of the quad.
# The vectors a and b point to the right and up around a vertex in the xy plance.
for i in range(L * L):
x = int(i / L)
y = i % L
if x == L - 1 or y == L - 1: continue
v = self.vertices[i]
a = self.vertices[i + L].pos - v.pos
b = self.vertices[i + 1].pos - v.pos
v.normal = cross(a, b)
def partially_inelastic_collision(self):
# totally inelastic collision, but if the couple of bodies collides with others can breack
for indexs in self.collided_couples:
body0 = self.bodies[indexs[0]]
body1 = self.bodies[indexs[1]]
m0 = body0.mass
m1 = body1.mass
# grow body 1
cm_pos = (body0.pos * m0 + body1.pos * m1) / (m0 + m1)
cm_velocity = (body0.velocity * m0 + body1.velocity * m1) / (m0 +
m1)
dr0 = body0.pos - cm_pos
dv0 = body0.velocity - cm_velocity
dr1 = body1.pos - cm_pos
dv1 = body1.velocity - cm_velocity
omega0 = cross(-dv0, dr0) / mag(dr0)**2
omega1 = cross(-dv1, dr1) / mag(dr1)**2
#assert omega0 == omega1
body0.velocity = cm_velocity + cross(omega0, dr0)
body1.velocity = cm_velocity + cross(omega1, dr1)
self.collided_couples.clear()
def init_vel(centr_body, position):
''' Calculates velocity of an orbiting object with its initial position '''
if position != centr_body.position:
radius = vp.mag(position - centr_body.position)
# equate gravitational force to centripetal force gives
vel_mag = np.sqrt(G*centr_body.mass/radius)
# velocity vector is normal to the vector towards central object and the vector normal to xz-plane
vec1 = vp.vector(centr_body.position - position)
vec2 = vp.vector(0,1,0)
vel_vec = vp.cross(vec1,vec2)
velocity = vel_mag * vp.norm(vel_vec) + centr_body.velocity # taking centr_body motion into account
else:
velocity = vp.vector(0,0,0) # avoid division by 0 for sun's case
return velocity
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 quad(self,
i_1,
phi_1,
i_2,
phi_2,
i_3,
phi_3,
i_4,
phi_4,
mirror=False):
p_1 = self.point(i_1, phi_1, mirror)
p_2 = self.point(i_2, phi_2, mirror)
p_3 = self.point(i_3, phi_3, mirror)
p_4 = self.point(i_4, phi_4, mirror)
n = cross(p_2 - p_1, p_3 - p_1)
n /= mag(n)
if (mirror):
n *= -1
return quad(vs=[
self.vertex(p_1, n),
self.vertex(p_2, n),
self.vertex(p_3, n),
self.vertex(p_4, n)
])
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))
scene.autoscale = 1
# Main math is done here to calculate the orbits
while True:
r = mag(p_star.pos - s_star.pos)
F = -G * p_star.mass * s_star.mass * (p_star.pos -
s_star.pos) / r**3 #force on star A
#force on secondary star is neg of above
if r > rmax:
rmax = r
if r < rmin:
rmin = r
a = (rmin + rmax) / 2 #SEMI MAJOR AXIS
#period keplers 3rd law
P = sqrt(4 * pi**2 * a**3 / (G * M)) / Year
L = mu * cross((p_star.pos - s_star.pos), (p_star.vel - s_star.vel))
A = cross((p_star.vel - s_star.vel),
(L / mu)) - G * M * (p_star.pos - s_star.pos) / r
#accentricity
E = eccentricity
ecc1 = mag(A) / (G * M) #method 1
ecc2 = sqrt(1 + 2 * mag2(L) * E / ((G * M)**2 * mu**3))
ecc3 = (rmax - rmin) / (2 * a) #geometrically
#implement euler
p_star.vel += F / p_star.mass * h
s_star.vel -= F / s_star.mass * h
#unknown.vel += F/unknown.mass*h
p_star.pos += p_star.vel * h
s_star.pos += s_star.vel * h
p_star.trail.append(pos=p_star.pos)
def launch(sample_rate, position, orientation, COM, COP, thrust, gravity, lift, drag):
for step in range(len(position)):
position[step][2] = -position[step][2]
force_scale = 0.01
l = 2
rad = 0.09
steps = len(position)
# Initialization of enviorment
render = vp.canvas(height=600, width=1200, background=vp.vector(0.8, 0.8, 0.8), forward=vp.vector(1, 0, 0), up=vp.vector(0, 0, 1))
render.select()
render.caption = "Loading..."
render.visible = False
# Initialization of body and cone
body = vp.cylinder(pos=vp.vector(-l, 0, 0), axis=vp.vector(l*0.8, 0, 0), radius=rad)
cone = vp.cone(pos=vp.vector(-l*0.2, 0, 0), axis=vp.vector(l*0.2, 0, 0), radius=rad)
# Initialization of fins
a_fins = vp.box(pos=vp.vector(-l + (l*0.05), 0, 0), size=vp.vector(l*0.1, rad*4, rad*0.25))
b_fins = vp.box(pos=vp.vector(-l + (l*0.05), 0, 0), size=vp.vector(l*0.1, rad*0.25, rad*4))
inv_box = vp.box(pos=vp.vector(l/2, 0, 0), size=vp.vector(l, 10e-10, 10e-10), visible=False)
# Initialization of rocket
rocket = vp.compound([body, cone, a_fins, b_fins, inv_box], pos=vp.vector(0, 0, 1), axis=vp.vector(1, 0, 0), up=vp.vector(0, 0, 1), color=vp.vector(1,1,1), opacity=0.5)
COM_sphere = vp.sphere(radius = 0.04, color=vp.vector(0, 0, 0))
COP_sphere = vp.sphere(radius = 0.04, color=vp.vector(0, 0, 0))
thrust_pointer = vp.arrow(shaftwidth = 0.1, color=vp.vector(1, 1, 0))
gravity_pointer = vp.arrow(shaftwidth = 0.1, color=vp.vector(0, 1, 1))
lift_pointer = vp.arrow(shaftwidth = 0.1, color=vp.vector(1, 0, 1))
drag_pointer = vp.arrow(shaftwidth = 0.1, color=vp.vector(0, 0, 1))
a = 4
c = 0.3
b = a - c
sqr_out = [[-a, a],[a, a],[a, -a],[-a, -a], [-a, a]]
sqr_in1 = [[-b, b - 3*c],[b, b - 3*c],[b, -b],[-b, -b], [-b, b - 3*c]]
sqr_in2 = [[-b, b],[b, b], [b, b - 2*c],[-b, b - 2*c], [-b, b]]
ref_box = vp.extrusion(path=[vp.vector(-c/2, 0, 0), vp.vector(c/2, 0, 0)], shape=[sqr_out, sqr_in1, sqr_in2], color=vp.vector(0.5, 0.5, 0.5))
render.camera.follow(rocket)
render.autoscale = False
launch_pad = vp.box(pos=vp.vector(position[0][0], position[0][1], -1), size=vp.vector(16, 16, 2), color=vp.vector(0.2, 0.2, 0.2))
launch_pad = vp.box(pos=vp.vector(position[-1][0], position[-1][1], -1), size=vp.vector(16, 16, 2), color=vp.vector(0.2, 0.2, 0.2))
rocket.normal = vp.cross(rocket.up, rocket.axis)
#vp.attach_arrow(rocket, 'up', color=vp.color.green)
#vp.attach_arrow(rocket, 'axis', color=vp.color.blue)
#vp.attach_arrow(rocket, 'normal', color=vp.color.red)
roll = 0
vp.attach_trail(rocket, radius=rad/3, color=vp.color.red)
sqr_out = [[-a, a],[a, a],[a, -a],[-a, -a], [-a, a]]
sqr_in1 = [[-b, b - 3*c],[b, b - 3*c],[b, -b],[-b, -b], [-b, b - 3*c]]
sqr_in2 = [[-b, b],[b, b], [b, b - 2*c],[-b, b - 2*c], [-b, b]]
for step in range(2, steps):
if not step % 5:
a = 4
c = 0.3
b = a - c
ref = vp.extrusion(path=[vp.vector(0, 0, 0), vp.vector(c, 0, 0)], shape=[sqr_out, sqr_in1, sqr_in2], axis=vp.vector(1, 0, 0), up=vp.vector(0, 1, 0), pos=vp.vector(position[step][0], position[step][1], position[step][2]))
ref.up = vp.vector(0, 0, 1)
ref.rotate(angle=orientation[step][0], axis=vp.cross(ref.axis, ref.up))
ref.rotate(angle=orientation[step][1], axis=ref.up)
ref.rotate(angle=orientation[step][2], axis=ref.axis)
render.caption = "Running"
render.visible = True
for step in range(steps):
x = position[step][0]
y = position[step][1]
z = position[step][2]
rocket.normal = vp.cross(rocket.axis, rocket.up)
pitch, yaw, roll = orientation[step][0], orientation[step][1], orientation[step][2]
COM_x = x + COM[step] * np.cos(yaw) * np.cos(pitch)
COM_y = y + COM[step] * np.sin(yaw) * np.cos(pitch)
COM_z = z + COM[step] * np.sin(pitch)
COP_x = x + COP[step] * np.cos(yaw) * np.cos(pitch)
COP_y = y + COP[step] * np.sin(yaw) * np.cos(pitch)
COP_z = z + COP[step] * np.sin(pitch)
thrust_x = x + (-l) * np.cos(yaw) * np.cos(pitch)
thrust_y = y + (-l) * np.sin(yaw) * np.cos(pitch)
thrust_z = z + (-l) * np.sin(pitch)
thrust_mag = np.linalg.norm(thrust[step]) * force_scale
thrust_ax_x = thrust_mag * np.cos(yaw) * np.cos(pitch)
thrust_ax_y = thrust_mag * np.sin(yaw) * np.cos(pitch)
thrust_ax_z = thrust_mag * np.sin(pitch)
lift_mag = lift[step][0] * force_scale
lift_ax_x = 0
lift_ax_y = 0
lift_ax_z = lift_mag
drag_mag = np.linalg.norm(drag[step]) * force_scale
print(lift_mag)
drag_ax_x = drag_mag
drag_ax_y = 0
drag_ax_z = 0
gravity_mag = np.linalg.norm(gravity[step]) * force_scale
thrust_pointer.pos = vp.vector(thrust_x, thrust_y, thrust_z)
thrust_pointer.axis = vp.vector(thrust_ax_x, thrust_ax_y, thrust_ax_z)
gravity_pointer.pos = vp.vector(COM_x, COM_y, COM_z)
gravity_pointer.axis = vp.vector(0, 0, -gravity_mag)
lift_pointer.pos = vp.vector(COP_x, COP_y, COP_z)
lift_pointer.axis = vp.vector(lift_ax_x, lift_ax_y, lift_ax_z)
drag_pointer.pos = vp.vector(COP_x, COP_y, COP_z)
drag_pointer.axis = vp.vector(drag_ax_x,drag_ax_y, drag_ax_z)
rocket.rotate(angle=pitch, axis=rocket.normal)
rocket.rotate(angle=yaw, axis=rocket.up)
rocket.rotate(angle=roll, axis=rocket.axis)
COM_sphere.pos = vp.vector(COM_x, COM_y, COM_z)
COP_sphere.pos = vp.vector(COP_x, COP_y, COP_z)
rocket.pos = vp.vector(x, y, z)
vp.sleep(1/sample_rate)
if step + 1 != steps:
rocket.rotate(angle=-roll, axis=rocket.axis)
rocket.rotate(angle=-yaw, axis=rocket.up)
rocket.rotate(angle=-pitch, axis=rocket.normal)
render.caption = "Done"
# Axes
x_axis_line = vp.curve(x_axis.cone_tip,
3 * vec_i,
radius=0.01,
color=x_axis_color)
y_axis_line = vp.curve(y_axis.cone_tip,
3 * vec_j,
radius=0.01,
color=y_axis_color)
z_axis_line = vp.curve(z_axis.cone_tip,
3 * vec_k,
radius=0.01,
color=z_axis_color)
# Define vector A
vec_A = vp.vector(1, 2, 1)
pos_A = vp.vector(0.5, 0.5, 0.5)
col_A = vp.color.magenta
A = Arrow(vec_A, 'A(1, 2, 1)', col_A, pos_A)
# Define vector B
vec_B = vp.vector(2, 1, 3)
pos_B = vp.vector(0.5, 0.5, 0.5)
col_B = vp.color.orange
A = Arrow(vec_B, 'B(2, 1, 3)', col_B, pos_B)
vec_C = vp.cross(vec_A, vec_B)
pos_C = pos_A
col_C = vp.color.purple
C = Arrow(vec_C, f'AxB({vec_C.x}, {vec_C.y}, {vec_C.z})', col_C, pos_C)
def calculate_rotation_axis(self):
self.rotation_axis = cross(vec(0, 1, 0), self.velocity)
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)
def applyTropism(self, tropismVec, tropismStrength):
correction = tropismStrength * cross(norm(self.heading), tropismVec)
self.heading += self.heading.rotate(mag(correction), correction)
from vpython import sphere, vector, sin, cos, rate, pi, cross, curve, color
B = vector(0, 0, 5e-4)
q = -1.6e-19
m = 9e-31
s = sphere(radius=2.0e-7)
t = 0 # seconds
dt = 1e-12 # seconds
s.velocity = vector(100, 0, 0)
trail = curve(color=color.blue, pos=s.pos)
while t < 3e-6:
s.acceleration = q * cross(s.velocity, B) / m
s.velocity += s.acceleration * dt
s.pos += s.velocity * dt
trail.append(s.pos)
t = t + dt
rate(4e-7 / dt)
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
def moveView(self, event):
camAxis = self.scene.camera.axis
camDist = vp.mag(self.scene.center - self.scene.camera.pos)
dtheta = self.sensitivity
up = self.scene.up
if event.key in ["up", "k", "K"]:
self.scene.camera.pos -= up.norm() * dtheta * camDist
return
if event.key in ["down", "j", "J"]:
self.scene.camera.pos += up.norm() * dtheta * camDist
return
if event.key in ["right", "l", "L"]:
self.scene.camera.pos += vp.norm(
up.cross(camAxis)) * dtheta * camDist
return
if event.key in ["left", "h", "H"]:
self.scene.camera.pos -= vp.norm(
up.cross(camAxis)) * dtheta * camDist
return
if event.key in [".", ">"]: # Get closer, by ratio
ctr = self.scene.center
self.scene.camera.pos = ctr - camAxis / (1 + dtheta)
self.scene.camera.axis = ctr - self.scene.camera.pos
return
if event.key in [",", "<"]: # Get further
ctr = self.scene.center
self.scene.camera.pos = ctr - camAxis * (1 + dtheta)
self.scene.camera.axis = ctr - self.scene.camera.pos
return
if event.key == "p": # pitch: Rotate camera around ctr-horiz axis
self.scene.forward = vp.rotate(self.scene.forward,
angle=dtheta,
axis=vp.cross(
self.scene.forward,
self.scene.up))
return
if event.key == "P":
self.scene.forward = vp.rotate(self.scene.forward,
angle=-dtheta,
axis=vp.cross(
self.scene.forward,
self.scene.up))
return
if event.key == "y": # yaw: Rotate camera around ctr - up axis.
self.scene.forward = vp.rotate(self.scene.forward,
angle=dtheta,
axis=self.scene.up)
return
return
if event.key == "Y":
self.scene.forward = vp.rotate(self.scene.forward,
angle=-dtheta,
axis=self.scene.up)
return
if event.key == "r": # Roll, that is, change the 'up' vector
self.scene.camera.rotate(angle=dtheta,
axis=camAxis,
origin=self.scene.camera.pos)
return
if event.key == "R":
self.scene.camera.rotate(angle=-dtheta,
axis=camAxis,
origin=self.scene.camera.pos)
return
if event.key == "d": # Diameter scaling down
for dbl in self.drawables_:
dbl.diaScale *= 1.0 - self.sensitivity * 4
dbl.updateDiameter()
return
if event.key == "D":
for dbl in self.drawables_:
dbl.diaScale *= 1.0 + self.sensitivity * 4
dbl.updateDiameter()
return
if event.key == "s": # Scale down sleep time, make it faster.
self.sleep *= 1 - self.sensitivity
return
if event.key == "S": # Scale up sleep time, make it slower.
self.sleep *= 1 + self.sensitivity
return
if event.key == "a": # autoscale to fill view.
self.doAutoscale()
return
if event.key == "g":
self.hideAxis = not self.hideAxis
# show/hide the axis here.
if event.key == "t": # Turn on/off twisting/autorotate
self.doRotation = not self.doRotation
if event.key == "?": # Print out help for these commands
self.printMoogulHelp()
body.pos = vector(R, 0, 0)
attach_trail(body)
dt = 0.01
# mostra gli assi x e y
axis_x = arrow(pos=vector(0, 0, 0), axis=vector(1, 0, 0), shaftwidth=0.01)
axis_y = arrow(pos=vector(0, 0, 0), axis=vector(0, 1, 0), shaftwidth=0.01)
# definisco la velocità angolare come un vettore perpendicolare al piano di rotazione
omega = vector(0, 0, 0) # rad/s
alpha = vector(0, 0, 0.1) # rad/s**2
velocity = cross(omega, body.pos)
while True:
rate(100)
# calcola il vettore accelerazione centripeta
acceleration_C = cross(omega, cross(omega, body.pos))
acceleration_T = cross(alpha, body.pos)
acceleration = acceleration_C + acceleration_T
# aggiorna velocità e accelerazione
omega = omega + alpha * dt
velocity = velocity + acceleration * dt
body.pos = body.pos + velocity * dt
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
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)
pcmi = pcmi - 2 * pcmi.dot(rrel) * rrel # bounce in cm frame
