Example #1
0
 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))           
Example #2
0
    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))   
Example #3
0
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)
Example #4
0
    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))
Example #5
0
 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))
Example #6
0
    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
Example #7
0
    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)
Example #8
0
    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))
Example #9
0
 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")
Example #10
0
 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")
Example #11
0
    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.'
Example #12
0
 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)
Example #13
0
    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)
Example #14
0
                #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:
Example #15
0
    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)
Example #16
0
                                 (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:
Example #17
0
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