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