def initSpace():
space = Space()
space.gravity = Vec2d(0.0, -900.0)
space.collision_bias = 0
# attach collision handlers (defined at the end of the file)
space.add_collision_handler(
Rocket.collisionType,
Platform.collisionType,
begin = rocketHandler
)
space.add_collision_handler(
Player.collisionType,
Explosion.collisionType,
begin = explosionHandler
)
space.add_collision_handler(
Player.collisionType,
Trigger.collisionType,
begin = triggerOn,
separate = triggerOff
)
space.add_collision_handler(
Player.collisionType,
Platform.collisionType,
pre_solve = notifyPlayer,
)
return space
def build_grid(self, space: pymunk.Space, collision_type_for_brick,
collision_type_for_ball, aspect_ratio):
grid_position = aspect_ratio.scale(90, 500)
brick_size = aspect_ratio.scale(100, 16)
brick_width = brick_size.x
brick_height = brick_size.y
space_between_brick = aspect_ratio.scale(10, 14)
x_space_between_brick = space_between_brick.x
brick_step_x = brick_width + x_space_between_brick
y_space_between_brick = space_between_brick.y
brick_step_y = brick_height + y_space_between_brick
for x in range(10):
pos_x = grid_position.x + x * brick_step_x
for y in range(10):
pos_y = grid_position.y + y * brick_step_y
# http://www.pymunk.org/en/latest/pymunk.html#pymunk.Body.__init__
body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
# position
body.position = pos_x, pos_y
# shape
shape = pymunk.Segment(body, (0, 0),
aspect_ratio.scale(100, 0), 8)
shape.elasticity = 0.98
shape.collision_type = collision_type_for_brick
space.add(body, shape)
handler = space.add_collision_handler(collision_type_for_brick,
collision_type_for_ball)
# http://www.pymunk.org/en/latest/pymunk.html#pymunk.CollisionHandler.separate
handler.separate = self._cb_remove_brick
def createSpace(engine_):
global engine
engine = engine_
space = Space()
space.gravity = Vec2d(0.0, 0.0)
PhysicsEntity.body = space.static_body
# attach collision handlers (defined at the end of the file)
space.add_collision_handler(
Blob.collisionType,
Blob.collisionType,
# begin = handler_blob
# separate = handler_blob
pre_solve = handler_blob
)
# space.add_collision_handler(
# Blob.collisionType,
# Wall.collisionType,
# pre_solve = handler_wall,
# )
return space
def build_dynamic_grid(self, space: pymunk.Space, collision_type_for_brick,
collision_type_for_ball, aspect_ratio):
wall_left = 50
wall_right = 1230
grid_left_corner = aspect_ratio.scale(wall_left + 40, 500)
grid_right_corner = aspect_ratio.scale(wall_right - 40,
500 + (16 + 14) * 10 - 40)
nb_bricks = Vec2d(12, 8)
spaces_between_brick = aspect_ratio.scale_V2d(Vec2d(10, 14))
grid_size = grid_right_corner - grid_left_corner
brick_size = (
grid_size -
(nb_bricks - Vec2d(1, 1)) * spaces_between_brick) / nb_bricks
brick_steps = brick_size + spaces_between_brick
for x in range(nb_bricks.x):
pos_x = grid_left_corner.x + x * brick_steps.x
for y in range(nb_bricks.y):
pos_y = grid_left_corner.y + y * brick_steps.y
# https://github.com/viblo/pymunk/blob/master/examples/breakout.py
# http://www.pymunk.org/en/latest/pymunk.html#pymunk.Body.__init__
brick_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
# position
brick_body.position = Vec2d(pos_x, pos_y) + brick_size * 0.5
brick_shape = pymunk.Poly.create_box(brick_body, brick_size)
brick_shape.elasticity = 0.98
brick_shape.collision_type = collision_type_for_brick
brick_shape.filter = pymunk.ShapeFilter(
categories=2 << collision_type_for_brick)
space.add(brick_body, brick_shape)
handler = space.add_collision_handler(collision_type_for_brick,
collision_type_for_ball)
# http://www.pymunk.org/en/latest/pymunk.html#pymunk.CollisionHandler.separate
handler.separate = self._cb_remove_brick
def __init__(self, space: pymunk.Space, collision_type_for_ball,
collision_type_for_bottom, cb_loose_ball, aspect_ratio):
"""
:param space:
:param collision_type_for_ball:
:param collision_type_for_bottom:
:param cb_loose_ball: call back function to reset game
"""
left = pymunk.Segment(space.static_body, aspect_ratio.scale(50, 50),
aspect_ratio.scale(50, 800), 2)
top = pymunk.Segment(space.static_body, aspect_ratio.scale(50, 800),
aspect_ratio.scale(1230, 800), 2)
right = pymunk.Segment(space.static_body, aspect_ratio.scale(1230, 50),
aspect_ratio.scale(1230, 800), 2)
left.elasticity = 1.0
right.elasticity = 1.0
top.elasticity = 1.0
bottom = pymunk.Segment(space.static_body, aspect_ratio.scale(50, 50),
aspect_ratio.scale(1230, 50), 2)
bottom.sensor = True
bottom.collision_type = collision_type_for_bottom
left.filter = pymunk.ShapeFilter(
categories=2 << collision_type_for_bottom)
right.filter = pymunk.ShapeFilter(
categories=2 << collision_type_for_bottom)
top.filter = pymunk.ShapeFilter(
categories=2 << collision_type_for_bottom)
bottom.filter = pymunk.ShapeFilter(
categories=2 << collision_type_for_bottom)
# http://www.pymunk.org/en/latest/pymunk.html#pymunk.CollisionHandler
handler = space.add_collision_handler(collision_type_for_ball,
collision_type_for_bottom)
handler.begin = self.reset_game
self.cb_loose_ball = cb_loose_ball
space.add(left, top, right, bottom)
class World(object):
def __init__(self):
self.items = []
init_pymunk()
self.space = Space()
self.space.gravity = (0, -0.5)
self.space.resize_static_hash()
self.space.resize_active_hash()
self.leaves = []
self.end_game = False
def add_item(self, item):
if isinstance(item, Bough):
self.leaves.append(item)
self.items.append(item)
item.create_body()
item.add_to_space(self.space)
def remove_item(self, item):
self.items.remove(item)
item.remove_from_space(self.space)
def update(self):
self.space.step(0.5)
for item in self.items:
item.update()
def remove_collided(self):
for item in self.items:
if item.status == "Collided":
self.remove_item(item)
def tick(self):
max_limit = len(self.leaves) - 1
if len(self.leaves) == 0:
max_limit = 0
## print "End Level"
self.end_game = True
else:
## print max_limit
randno = random.randint(0, max_limit)
## print randno
leaf = self.leaves.pop(randno)
## print leaf
leaf.remove_from_tree(self.space)
def add_cherry(self, x, y):
cherry = Cherry(x, y)
self.add_item(cherry)
def add_owange(self, x, y):
owange = Owange(x, y)
self.add_item(owange)
def add_collision_handler( self,
col_typ1,
col_typ2,
begin=None,
pre_solve=None,
post_solve=None,
separate=None,
**kwargs
):
self.space.add_collision_handler(
col_typ1, col_typ2,
begin, pre_solve,
post_solve, separate,
**kwargs)
class PymunkSimulation:
def __init__(self, do_render, sparse, max_motor_force):
self.do_render = do_render
self.sparse = sparse
self.max_motor_force = max_motor_force
if self.do_render:
pygame.init()
self.screen = pygame.display.set_mode((ENV_SIZE, ENV_SIZE))
self.draw_options = pygame_util.DrawOptions(self.screen)
self.clock = pygame.time.Clock()
self.motors = []
self.segment_bodies = []
self.space = Space()
self.space.iterations = 20
no_collision = self.space.add_collision_handler(NO_COLLISION_TYPE, NO_COLLISION_TYPE)
no_collision.begin = lambda a, b, c: False
ghost_collision = self.space.add_wildcard_collision_handler(GHOST_TYPE)
ghost_collision.begin = lambda a, b, c: False
def set_motor_rates(self, motor_rates):
for i, motor in enumerate(self.motors):
motor.rate = motor_rates[i]
def set_motor_max_forces(self, motor_forces):
for i, motor in enumerate(self.motors):
motor.max_force = motor_forces[i]
def step(self):
dt = 0.03
steps = 30
for i in range(steps):
self.space.step(dt / steps)
def add_arm(self, segment_lengths, anchor_position):
anchor = pymunk.Body(body_type=pymunk.Body.STATIC)
anchor.position = anchor_position
self.space.add(anchor)
segment_anchor = anchor
next_anchor_pos = anchor_position
for i, segment_length in enumerate(segment_lengths):
segment_size = segment_length, 10
segment_body = pymunk.Body(10, pymunk.moment_for_box(10, segment_size))
end_effector_shape = pymunk.Poly.create_box(segment_body, segment_size)
end_effector_shape.collision_type = NO_COLLISION_TYPE
end_effector_shape.friction = 1.0
end_effector_shape.elasticity = 0.1
alpha = random.random() * math.pi * 2
dx = np.cos(alpha) * segment_length / 2
dy = np.sin(alpha) * segment_length / 2
segment_body.position = next_anchor_pos[0] - dx, next_anchor_pos[1] - dy
next_anchor_pos = (next_anchor_pos[0] - 2 * dx, next_anchor_pos[1] - 2 * dy)
segment_body.angle = alpha
anchor_pin_pos = (0 if i == 0 else -segment_lengths[i - 1] / 2, 0)
pin = pymunk.PinJoint(segment_anchor, segment_body, anchor_pin_pos, (segment_length / 2, 0))
self.space.add(pin)
self.space.add(segment_body, end_effector_shape)
motor = pymunk.SimpleMotor(segment_anchor, segment_body, 0)
motor.max_force = self.max_motor_force
self.space.add(motor)
self.motors.append(motor)
self.segment_bodies.append(segment_body)
segment_anchor = segment_body
return segment_anchor, next_anchor_pos
class VehicleSimulator(object):
COLLISION_TYPE = IntEnum("COLLISION_TYPE",
"OBJECT VEHICLE LEFT_SENSOR RIGHT_SENSOR FEED")
DISPLAY_MARGIN = 10
ARENA_SIZE = 600
# simulation setting parameters
VEHICLE_RADIUS = 20
SENSOR_ANGLE = np.pi * 45 / 180
SENSOR_RANGE = 80
SENSOR_NOISE = 0
MOTOR_NOISE = 1.0
FEED_COLOR = (0, 0, 0)
FEED_ACTIVE_COLOR = (255, 0, 0)
FEED_EATING_TIME = 200
def __init__(self,
width=600,
height=600,
obstacle_num=5,
obstacle_radius=30,
feed_num=0,
feed_radius=5):
import pyglet
from pymunk import Space, Segment, Body, Circle, moment_for_circle, pyglet_util
super(VehicleSimulator, self).__init__()
self.__left_sensor_val = 0
self.__right_sensor_val = 0
self.__feed_sensor_val = False
self.__feed_touch_counter = {}
self.__feed_bodies = []
self.__feed_radius = feed_radius
self.__window = pyglet.window.Window(
self.ARENA_SIZE + self.DISPLAY_MARGIN * 2,
self.ARENA_SIZE + self.DISPLAY_MARGIN * 2,
vsync=False)
self.__draw_options = pyglet_util.DrawOptions()
self.__closed = False
@self.__window.event
def on_draw():
pyglet.gl.glClearColor(255, 255, 255, 255)
self.__window.clear()
self.__simulation_space.debug_draw(self.__draw_options)
@self.__window.event
def on_close():
pyglet.app.EventLoop().exit()
self.__closed = True
self.__simulation_space = Space()
self.__simulation_space.gravity = 0, 0
# arena
walls = [
Segment(
self.__simulation_space.static_body,
(self.DISPLAY_MARGIN, self.DISPLAY_MARGIN),
(self.ARENA_SIZE + self.DISPLAY_MARGIN, self.DISPLAY_MARGIN),
0),
Segment(
self.__simulation_space.static_body,
(self.ARENA_SIZE + self.DISPLAY_MARGIN, self.DISPLAY_MARGIN),
(self.ARENA_SIZE + self.DISPLAY_MARGIN,
self.ARENA_SIZE + self.DISPLAY_MARGIN), 0),
Segment(
self.__simulation_space.static_body,
(self.ARENA_SIZE + self.DISPLAY_MARGIN,
self.ARENA_SIZE + self.DISPLAY_MARGIN),
(self.DISPLAY_MARGIN, self.ARENA_SIZE + self.DISPLAY_MARGIN),
0),
Segment(
self.__simulation_space.static_body,
(self.DISPLAY_MARGIN, self.ARENA_SIZE + self.DISPLAY_MARGIN),
(self.DISPLAY_MARGIN, self.DISPLAY_MARGIN), 0)
]
for w in walls:
w.collision_type = self.COLLISION_TYPE.OBJECT
w.friction = 0.2
self.__simulation_space.add(walls)
# vehicle
mass = 1
self.__vehicle_body = Body(
mass, moment_for_circle(mass, 0, self.VEHICLE_RADIUS))
self.__vehicle_shape = Circle(self.__vehicle_body, self.VEHICLE_RADIUS)
self.__vehicle_shape.friction = 0.2
self.__vehicle_shape.collision_type = self.COLLISION_TYPE.VEHICLE
self.__simulation_space.add(self.__vehicle_body, self.__vehicle_shape)
# left sensor
sensor_l_s = Segment(self.__vehicle_body, (0, 0),
(self.SENSOR_RANGE * np.cos(self.SENSOR_ANGLE),
self.SENSOR_RANGE * np.sin(self.SENSOR_ANGLE)),
0)
sensor_l_s.sensor = True
sensor_l_s.collision_type = self.COLLISION_TYPE.LEFT_SENSOR
handler_l = self.__simulation_space.add_collision_handler(
self.COLLISION_TYPE.LEFT_SENSOR, self.COLLISION_TYPE.OBJECT)
handler_l.pre_solve = self.__left_sensr_handler
handler_l.separate = self.__left_sensr_separate_handler
self.__simulation_space.add(sensor_l_s)
# right sensor
sensor_r_s = Segment(self.__vehicle_body, (0, 0),
(self.SENSOR_RANGE * np.cos(-self.SENSOR_ANGLE),
self.SENSOR_RANGE * np.sin(-self.SENSOR_ANGLE)),
0)
sensor_r_s.sensor = True
sensor_r_s.collision_type = self.COLLISION_TYPE.RIGHT_SENSOR
handler_r = self.__simulation_space.add_collision_handler(
self.COLLISION_TYPE.RIGHT_SENSOR, self.COLLISION_TYPE.OBJECT)
handler_r.pre_solve = self.__right_sensr_handler
handler_r.separate = self.__right_sensr_separate_handler
self.__simulation_space.add(sensor_r_s)
# obstacles
for a in (np.linspace(0, np.pi * 2, obstacle_num, endpoint=False) +
np.pi / 2):
body = Body(body_type=Body.STATIC)
body.position = (self.DISPLAY_MARGIN + self.ARENA_SIZE / 2 +
self.ARENA_SIZE * 0.3 * np.cos(a),
self.DISPLAY_MARGIN + self.ARENA_SIZE / 2 +
self.ARENA_SIZE * 0.3 * np.sin(a))
shape = Circle(body, obstacle_radius)
shape.friction = 0.2
shape.collision_type = self.COLLISION_TYPE.OBJECT
self.__simulation_space.add(shape)
for i in range(feed_num):
body = Body(1, 1)
self.__feed_bodies.append(body)
shape = Circle(body, self.__feed_radius)
shape.sensor = True
shape.color = self.FEED_COLOR
shape.collision_type = self.COLLISION_TYPE.FEED
handler = self.__simulation_space.add_collision_handler(
self.COLLISION_TYPE.VEHICLE, self.COLLISION_TYPE.FEED)
handler.pre_solve = self.__feed_touch_handler
handler.separate = self.__feed_separate_handler
self.__simulation_space.add(body, shape)
self.__feed_touch_counter[shape] = 0
self.reset()
def reset(self, random_seed=None):
np.random.seed(random_seed)
self.__vehicle_body.position = self.ARENA_SIZE / 2 + self.DISPLAY_MARGIN, self.ARENA_SIZE / 2 + self.DISPLAY_MARGIN
self.__vehicle_body.angle = 0
for b in self.__feed_bodies:
b.position = self.DISPLAY_MARGIN + self.__feed_radius + np.random.rand(
2) * (self.ARENA_SIZE - self.__feed_radius * 2)
def update(self, action):
self.__vehicle_body.velocity = (0, 0)
self.__vehicle_body.angular_velocity = 0
velocity_l, velocity_r = action[0], action[1]
velocity_l += self.MOTOR_NOISE * np.random.randn()
velocity_r += self.MOTOR_NOISE * np.random.randn()
self.__vehicle_body.apply_impulse_at_local_point(
(velocity_l * self.__vehicle_body.mass, 0),
(0, self.VEHICLE_RADIUS))
self.__vehicle_body.apply_impulse_at_local_point(
(velocity_r * self.__vehicle_body.mass, 0),
(0, -self.VEHICLE_RADIUS))
lf = self.__get_lateral_velocity() * self.__vehicle_body.mass
self.__vehicle_body.apply_impulse_at_local_point(-lf, (0, 0))
self.__simulation_space.step(1 / 100)
from pyglet import clock
clock.tick()
self.__window.switch_to()
self.__window.dispatch_events()
self.__window.dispatch_event('on_draw')
self.__window.flip()
def get_sensor_data(self):
sensor_data = {
"left_distance": self.__left_sensor_val,
"right_distance": self.__right_sensor_val,
"feed_touching": self.__feed_sensor_val
}
return sensor_data
def set_bodycolor(self, color):
assert len(color) == 3
self.__vehicle_shape.color = color
def __feed_touch_handler(self, arbiter, space, data):
feed = arbiter.shapes[1]
feed.color = self.FEED_ACTIVE_COLOR
self.__feed_touch_counter[feed] += 1
self.__feed_sensor_val = True
if (self.__feed_touch_counter[feed] > self.FEED_EATING_TIME):
feed.body.position = self.DISPLAY_MARGIN + feed.radius / 2 + np.random.rand(
2) * (self.ARENA_SIZE - feed.radius)
return True
def __feed_separate_handler(self, arbiter, space, data):
feed = arbiter.shapes[1]
feed.color = self.FEED_COLOR
self.__feed_touch_counter[feed] = 0
self.__feed_sensor_val = False
return True
def __left_sensr_handler(self, arbiter, space, data):
p = arbiter.contact_point_set.points[0]
distance = self.__vehicle_body.world_to_local(p.point_b).get_length()
self.__left_sensor_val = 1 - distance / self.SENSOR_RANGE
self.__left_sensor_val += self.SENSOR_NOISE * np.random.randn()
return True
def __left_sensr_separate_handler(self, arbiter, space, data):
self.__left_sensor_val = 0
return True
def __right_sensr_handler(self, arbiter, space, data):
p = arbiter.contact_point_set.points[0]
distance = self.__vehicle_body.world_to_local(p.point_b).get_length()
self.__right_sensor_val = 1 - distance / self.SENSOR_RANGE
self.__right_sensor_val += self.SENSOR_NOISE * np.random.randn()
return True
def __right_sensr_separate_handler(self, arbiter, space, data):
self.__right_sensor_val = 0
return True
def __get_lateral_velocity(self):
from pymunk.vec2d import Vec2d
v = self.__vehicle_body.world_to_local(self.__vehicle_body.velocity +
self.__vehicle_body.position)
rn = Vec2d(0, -1)
return v.dot(rn) * rn
def __bool__(self):
return not self.__closed
class World(object):
def __init__(self):
self.items = []
init_pymunk()
self.space = Space()
self.space.gravity = (0, -0.5)
self.space.resize_static_hash()
self.space.resize_active_hash()
self.leaves = []
self.end_game = False
def add_item(self, item):
if isinstance(item, Bough):
self.leaves.append(item)
self.items.append(item)
item.create_body()
item.add_to_space(self.space)
def remove_item(self, item):
self.items.remove(item)
item.remove_from_space(self.space)
def update(self):
self.space.step(0.5)
for item in self.items:
item.update()
def remove_collided(self):
for item in self.items:
if item.status == "Collided":
self.remove_item(item)
def tick(self):
max_limit = len(self.leaves) - 1
if len(self.leaves) == 0:
max_limit = 0
## print "End Level"
self.end_game = True
else:
## print max_limit
randno = random.randint(0, max_limit)
## print randno
leaf = self.leaves.pop(randno)
## print leaf
leaf.remove_from_tree(self.space)
def add_cherry(self, x, y):
cherry = Cherry(x, y)
self.add_item(cherry)
def add_owange(self, x, y):
owange = Owange(x, y)
self.add_item(owange)
def add_collision_handler(self,
col_typ1,
col_typ2,
begin=None,
pre_solve=None,
post_solve=None,
separate=None,
**kwargs):
self.space.add_collision_handler(col_typ1, col_typ2, begin, pre_solve,
post_solve, separate, **kwargs)
