def on_ball_paddle_collision(self, ball_body, paddle_body, normal): # Adjusts the ball direction if the paddle is moving when the ball collides with it angle = math.acos(dot(normal, ball_body.direction)) # Angle between the reflected direction and the normal delta_angle = abs(((math.pi * 0.5) - angle) * 0.5) # Half the angle that remains if were to perform a 90 degree reflection if paddle_body.direction.x > 0: # Clockwise rotation because the paddle is moving to the right ball_body.direction = normalize(rotate(ball_body.direction, delta_angle)) elif paddle_body.direction.x < 0: # Counter-clockwise rotation because the paddle is moving to the left ball_body.direction = normalize(rotate(ball_body.direction, -delta_angle))
def on_ball_left_right_collision(self, ball_body, wall_body, normal): angle = math.acos(dot(normal, ball_body.direction)) # Angle between the reflected direction and the normal # If the angle is too flat, add a small rotation to the reflected direction if angle < 0.1: delta_angle = 0.2 if ball_body.direction.y > 0: # Counter-clockwise rotation because the ball is moving downwards ball_body.direction = normalize(rotate(ball_body.direction, -delta_angle)) elif ball_body.direction.y <= 0: # Clockwise rotation because the ball is moving upwards ball_body.direction = normalize(rotate(ball_body.direction, delta_angle))
def draw_directed_circle(screen, field, color, center, radius, dir, thickness=2): draw_circle(screen, field, color, center, radius, thickness) top_angle = 40.0 * pi / 180.0 x = radius * sin(top_angle) y = radius * cos(top_angle) ang = signed_angle(Vector(0.0, 1.0), dir) pa = center + rotate(Vector(-x, -y), ang) pb = center + rotate(Vector( x, -y), ang) pc = center + rotate(Vector(0.0, radius), ang) draw_line(screen, field, color, pa, pb, thickness) draw_line(screen, field, color, pa, pc, thickness) draw_line(screen, field, color, pb, pc, thickness)
def on_ball_left_right_collision(self, ball_body, wall_body, normal): angle = math.acos( dot(normal, ball_body.direction )) # Angle between the reflected direction and the normal # If the angle is too flat, add a small rotation to the reflected direction if angle < 0.1: delta_angle = 0.2 if ball_body.direction.y > 0: # Counter-clockwise rotation because the ball is moving downwards ball_body.direction = normalize( rotate(ball_body.direction, -delta_angle)) elif ball_body.direction.y <= 0: # Clockwise rotation because the ball is moving upwards ball_body.direction = normalize( rotate(ball_body.direction, delta_angle))
def on_ball_paddle_collision(self, ball_body, paddle_body, normal): # Adjusts the ball direction if the paddle is moving when the ball collides with it angle = math.acos( dot(normal, ball_body.direction )) # Angle between the reflected direction and the normal delta_angle = abs( ((math.pi * 0.5) - angle) * 0.5 ) # Half the angle that remains if were to perform a 90 degree reflection if paddle_body.direction.x > 0: # Clockwise rotation because the paddle is moving to the right ball_body.direction = normalize( rotate(ball_body.direction, delta_angle)) elif paddle_body.direction.x < 0: # Counter-clockwise rotation because the paddle is moving to the left ball_body.direction = normalize( rotate(ball_body.direction, -delta_angle))
def rotate_rel(self, angle, axis): logging.info('rotating vertices by a relative angle of ' '%.2f degrees along the %s axis' % (angle, self.axis_letter_map[axis])) angle = angle % 360 self.vertices = vector.rotate(self.vertices, angle, *axis) self.rotation_angle[axis] += angle self.invalidate_bounding_box() self.modified = True
def _add_bullet(self, position=None, angle=None): position = position or self.game.player.position angle = angle or self.game.player.angle perturbation = random.gauss(0, self.spread) angle = angle + perturbation bullet = physics.Bullet(position, v.rotate([(0, 200)], angle, (0, 0))[0], angle) self._bullets.append(bullet)
def load_data(self, model_data, callback=None): t_start = time.time() vertex_list = [] color_list = [] self.layer_stops = [0] arrow_list = [] num_layers = len(model_data) for layer_idx, layer in enumerate(model_data): for movement in layer: vertex_list.append(movement.src) vertex_list.append(movement.dst) arrow = self.arrow # position the arrow with respect to movement arrow = vector.rotate(arrow, movement.angle(), 0.0, 0.0, 1.0) arrow_list.extend(arrow) vertex_color = self.movement_color(movement) color_list.append(vertex_color) self.layer_stops.append(len(vertex_list)) if callback: callback(layer_idx + 1, num_layers) self.vertices = numpy.array(vertex_list, 'f') self.colors = numpy.array(color_list, 'f') self.arrows = numpy.array(arrow_list, 'f') # by translating the arrow vertices outside of the loop, we achieve a # significant performance gain thanks to numpy. it would be really nice # if we could rotate in a similar fashion... self.arrows = self.arrows + self.vertices[1::2].repeat(3, 0) # for every pair of vertices of the model, there are 3 vertices for the arrow assert len(self.arrows) == ((len(self.vertices) // 2) * 3), \ 'The 2:3 ratio of model vertices to arrow vertices does not hold.' self.max_layers = len(self.layer_stops) - 1 self.num_layers_to_draw = self.max_layers self.arrows_enabled = True self.initialized = False t_end = time.time() logging.info('Initialized Gcode model in %.2f seconds' % (t_end - t_start)) logging.info('Vertex count: %d' % len(self.vertices))
def calc_pos(self, pos, grip_angle=0.0): """Calculate servo values for arm position, returns dict(servos)""" position = Vector(pos) grip_angle = float(grip_angle) # unit vector translation of position on xy plane xy_unit = Vector(position.x, position.y, 0).unit # get a grip... vector gripper = (xy_unit * self.beams['gripper'].mag) # ... and rotate to angle specified gripper = rotate(gripper, crossproduct(gripper, Z), radians(grip_angle)) # Subtract to get Sub Arm (sum of vectors 0 and 1) composite = position - gripper # Calculate sub-vectors # Get angle betweens try: arm2compangle = trisss([ self.beams['arm'].mag, self.beams['forearm'].mag, composite.mag, ])[1] except ValueError, m: raise ValueError("Position is beyond range of motion")
def create_vertex_arrays(self, model_data): """ Construct vertex lists from gcode data. """ vertex_list = [] color_list = [] self.layer_stops = [0] arrow_list = [] for layer in model_data: for movement in layer: a, b = movement.point_a, movement.point_b vertex_list.append([a.x, a.y, a.z]) vertex_list.append([b.x, b.y, b.z]) arrow = self.arrow # position the arrow with respect to movement arrow = vector.rotate(arrow, movement.angle(), 0.0, 0.0, 1.0) arrow_list.extend(arrow) vertex_color = self.movement_color(movement) color_list.append(vertex_color) self.layer_stops.append(len(vertex_list)) self.vertices = numpy.array(vertex_list, 'f') self.colors = numpy.array(color_list, 'f') self.arrows = numpy.array(arrow_list, 'f') # by translating the arrow vertices outside of the loop, we achieve a # significant performance gain thanks to numpy. it would be really nice # if we could rotate in a similar fashion... self.arrows = self.arrows + self.vertices[1::2].repeat(3, 0) # for every pair of vertices of the model, there are 3 vertices for the arrow assert len(self.arrows) == ((len(self.vertices) // 2) * 3), \ 'The 2:3 ratio of model vertices to arrow vertices does not hold.'
def add_velocity(self, dt_ratio, sub=False): additional_velocity = v.rotate([(0, 20)], self.angle, (0, 0))[0] scale = -dt_ratio if sub else dt_ratio additional_velocity = v.vec_scale(additional_velocity, scale) self.velocity = v.vec_add(self.velocity, additional_velocity)
def load_data(self, model_data, callback=None): t_start = time.time() vertex_list = [] color_list = [] self.layer_stops = [0] self.layer_heights = [] arrow_list = [] layer_markers_list = [] self.layer_marker_stops = [0] num_layers = len(model_data) callback_every = max(1, int(math.floor(num_layers / 100))) # the first movement designates the starting point start = prev = model_data[0][0] del model_data[0][0] for layer_idx, layer in enumerate(model_data): first = layer[0] for movement in layer: vertex_list.append(prev.v) vertex_list.append(movement.v) arrow = self.arrow # position the arrow with respect to movement arrow = vector.rotate(arrow, movement.angle(prev.v), 0.0, 0.0, 1.0) arrow_list.extend(arrow) vertex_color = self.movement_color(movement) color_list.append(vertex_color) prev = movement self.layer_stops.append(len(vertex_list)) self.layer_heights.append(first.v[2]) # add the layer entry marker if layer_idx > 0 and len(model_data[layer_idx - 1]) > 0: layer_markers_list.extend(self.layer_entry_marker + model_data[layer_idx - 1][-1].v) elif layer_idx == 0 and len(layer) > 0: layer_markers_list.extend(self.layer_entry_marker + layer[0].v) # add the layer exit marker if len(layer) > 1: layer_markers_list.extend(self.layer_exit_marker + layer[-1].v) self.layer_marker_stops.append(len(layer_markers_list)) if callback and layer_idx % callback_every == 0: callback(layer_idx + 1, num_layers) self.vertices = numpy.array(vertex_list, 'f') self.colors = numpy.array(color_list, 'f') self.arrows = numpy.array(arrow_list, 'f') self.layer_markers = numpy.array(layer_markers_list, 'f') # by translating the arrow vertices outside of the loop, we achieve a # significant performance gain thanks to numpy. it would be really nice # if we could rotate in a similar fashion... self.arrows = self.arrows + self.vertices[1::2].repeat(3, 0) # for every pair of vertices of the model, there are 3 vertices for the arrow assert len(self.arrows) == ((len(self.vertices) // 2) * 3), \ 'The 2:3 ratio of model vertices to arrow vertices does not hold.' self.max_layers = len(self.layer_stops) - 1 self.num_layers_to_draw = self.max_layers self.arrows_enabled = True self.initialized = False self.vertex_count = len(self.vertices) t_end = time.time() logging.info('Initialized Gcode model in %.2f seconds' % (t_end - t_start)) logging.info('Vertex count: %d' % self.vertex_count)
#only move on an axes if doing so does not cause an intersection if level.map[int(player.pos[0] + player.dir[0] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades))][int(player.pos[1])] == False: player.pos[0] += player.dir[0] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades) if level.map[int(player.pos[0])][int(player.pos[1] + player.dir[1] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades))] == False: player.pos[1] += player.dir[1] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades) if keys_pressed[K_DOWN]: #only move on an axes if doing so does not cause an intersection if level.map[int(player.pos[0] - player.dir[0] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades))][int(player.pos[1])] == False: player.pos[0] -= player.dir[0] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades) if level.map[int(player.pos[0])][int(player.pos[1] - player.dir[1] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades))] == False: player.pos[1] -= player.dir[1] * moveSpeed * dt * speed_multiplyer * (1.02 ** speed_upgrades) if keys_pressed[K_LEFT]: #rotate the direction and the plane so that it stays perpendicular to the direction player.dir = vector.rotate(player.dir,turnSpeed * dt) player.plane = vector.rotate(player.plane,turnSpeed * dt) if keys_pressed[K_RIGHT]: #rotate the direction and the plane so that it stays perpendicular to the direction player.dir = vector.rotate(player.dir,-turnSpeed * dt) player.plane = vector.rotate(player.plane,-turnSpeed * dt) for entity in level.entities: #enter the shop if the player goes close enough to the object that represents it if health > 0 and wave_num < len(waves): if not entity.__class__.__name__ == "Explosion" and entity.type == 3: #allow the player to move away from the shop once they exit the shop gameState if vector.distance(entity.pos, [player.pos[1],player.pos[0]]) < 0.5: if shop == False:
def load_data(self, model_data, callback=None): t_start = time.time() vertex_list = [] color_list = [] self.layer_stops = [0] self.layer_heights = [] arrow_list = [] layer_markers_list = [] self.layer_marker_stops = [0] num_layers = len(model_data) callback_every = max(1, int(math.floor(num_layers / 100))) # the first movement designates the starting point start = prev = model_data[0][0] del model_data[0][0] for layer_idx, layer in enumerate(model_data): first = layer[0] for movement in layer: vertex_list.append(prev.v) vertex_list.append(movement.v) arrow = self.arrow # position the arrow with respect to movement arrow = vector.rotate(arrow, movement.angle(prev.v), 0.0, 0.0, 1.0) arrow_list.extend(arrow) vertex_color = self.movement_color(movement) color_list.append(vertex_color) prev = movement self.layer_stops.append(len(vertex_list)) self.layer_heights.append(first.v[2]) # add the layer entry marker if layer_idx > 0 and len(model_data[layer_idx - 1]) > 0: layer_markers_list.extend(self.layer_entry_marker + model_data[layer_idx-1][-1].v) elif layer_idx == 0 and len(layer) > 0: layer_markers_list.extend(self.layer_entry_marker + layer[0].v) # add the layer exit marker if len(layer) > 1: layer_markers_list.extend(self.layer_exit_marker + layer[-1].v) self.layer_marker_stops.append(len(layer_markers_list)) if callback and layer_idx % callback_every == 0: callback(layer_idx + 1, num_layers) self.vertices = numpy.array(vertex_list, 'f') self.colors = numpy.array(color_list, 'f') self.arrows = numpy.array(arrow_list, 'f') self.layer_markers = numpy.array(layer_markers_list, 'f') # by translating the arrow vertices outside of the loop, we achieve a # significant performance gain thanks to numpy. it would be really nice # if we could rotate in a similar fashion... self.arrows = self.arrows + self.vertices[1::2].repeat(3, 0) # for every pair of vertices of the model, there are 3 vertices for the arrow assert len(self.arrows) == ((len(self.vertices) // 2) * 3), \ 'The 2:3 ratio of model vertices to arrow vertices does not hold.' self.max_layers = len(self.layer_stops) - 1 self.num_layers_to_draw = self.max_layers self.arrows_enabled = True self.initialized = False self.vertex_count = len(self.vertices) t_end = time.time() logging.info('Initialized Gcode model in %.2f seconds' % (t_end - t_start)) logging.info('Vertex count: %d' % self.vertex_count)
(1.02**speed_upgrades))][int( player.pos[1])] == False: player.pos[0] -= player.dir[ 0] * moveSpeed * dt * speed_multiplyer * ( 1.02**speed_upgrades) if level.map[int( player.pos[0])][int(player.pos[1] - player.dir[1] * moveSpeed * dt * speed_multiplyer * (1.02**speed_upgrades))] == False: player.pos[1] -= player.dir[ 1] * moveSpeed * dt * speed_multiplyer * ( 1.02**speed_upgrades) if keys_pressed[K_LEFT]: #rotate the direction and the plane so that it stays perpendicular to the direction player.dir = vector.rotate(player.dir, turnSpeed * dt) player.plane = vector.rotate(player.plane, turnSpeed * dt) if keys_pressed[K_RIGHT]: #rotate the direction and the plane so that it stays perpendicular to the direction player.dir = vector.rotate(player.dir, -turnSpeed * dt) player.plane = vector.rotate(player.plane, -turnSpeed * dt) for entity in level.entities: #enter the shop if the player goes close enough to the object that represents it if health > 0 and wave_num < len(waves): if not entity.__class__.__name__ == "Explosion" and entity.type == 3: #allow the player to move away from the shop once they exit the shop gameState if vector.distance(entity.pos, [player.pos[1], player.pos[0]]) < 0.5:
class Al5x(object): """Represents an AL5x robot arm from Lynxmotion The state of the arm is represented with by a dict with these keys: pos: the gripper position in 3-dimensions gripper_angle: the grippers angle from horizon in degrees grip: the grippers distance between fingers wrist_rotate: the angle from center in degrees Initiation and usage: arm = Al5x(AL5D, servo_controller=None, parked_state=dict(pos=(0,8,3)), dt=0.010) a.move(dict(pos=(-4,6,6), grip_angle=15.0, grip=0.0)) a.move(dict(pos=(4,4,10), grip_angle=0.0, grip=0.5)) a.park() """ def __init__(self, beams, servo_controller=None, parked_state=None, servo_map=None, avg_speed=15.0, dt=0.007): self.beams = dict( zip(['arm', 'forearm', 'gripper'], map(Vector, beams))) if servo_controller is not None: self.sc = servo_controller else: self.sc = NullServo() self.current_state = dict() # set up parked_state p = sum(self.beams.values()) ga = 0.0 g = 0.0 wr = 0.0 self.parked_state = dict(pos=p, grip_angle=ga, grip=g, wrist_rotate=wr) if parked_state is not None: self.parked_state.update(parked_state) if servo_map is not None: self.servo_map = servo_map else: self.servo_map = SERVO_MAP self.avg_speed = avg_speed self.dt = dt self.immediate_move(self.parked_state) def get_state(self): """Return current state""" return dict(self.current_state) def __zip_state(self, new_state): """Returns combined state of current_state and new_state""" state = dict(self.current_state) state.update(new_state) return state def immediate_move(self, new_state, time=0): """Move arm to new_state without interpolation This bypasses the straight-line and accelleration calculations and simply finds the servo positions for the new state and steps servos to it. This will cause a non-linear gripper movement. """ state = self.__zip_state(new_state) servos = dict() servos.update(self.calc_pos(state['pos'], state['grip_angle'])) servos.update(self.calc_grip(state['grip'])) self.sc.servos(servos, time) self.current_state.update(state) def park(self): """Moves arm to parked state""" self.move(self.parked_state) def calc_grip(self, val): """Calculate grip servo value, returns dict(servo)""" return dict({self.servo_map['grip']: val}) def set_grip(self, val): self.sc.servos(self.calc_grip(val)) self.current_state.update(dict(grip=val)) def calc_pos(self, pos, grip_angle=0.0): """Calculate servo values for arm position, returns dict(servos)""" position = Vector(pos) grip_angle = float(grip_angle) # unit vector translation of position on xy plane xy_unit = Vector(position.x, position.y, 0).unit # get a grip... vector gripper = (xy_unit * self.beams['gripper'].mag) # ... and rotate to angle specified gripper = rotate(gripper, crossproduct(gripper, Z), radians(grip_angle)) # Subtract to get Sub Arm (sum of vectors 0 and 1) composite = position - gripper # Calculate sub-vectors # Get angle betweens try: arm2compangle = trisss([ self.beams['arm'].mag, self.beams['forearm'].mag, composite.mag, ])[1] except ValueError, m: raise ValueError("Position is beyond range of motion") # get arm vector arm = composite.unit * self.beams['arm'].mag # ... and rotate to calculated angle arm = rotate(arm, crossproduct(arm, Z), arm2compangle) # the easy part... forearm = composite - arm # set servo values servo_values = dict() servo_values[self.servo_map['base']] = rad2float(angle(X, xy_unit)) servo_values[self.servo_map['shoulder']] = rad2float( angle(xy_unit, arm)) servo_values[self.servo_map['elbow']] = rad2float(angle(arm, forearm)) servo_values[self.servo_map['wrist']] = rad2float( pi / 2 - angle(forearm, gripper) * sign(forearm.unit.z - gripper.unit.z)) return servo_values