def __init__(self, space, rect, playfield=None):
super(PlungerAssembly, self).__init__()
self.chute_counter = 0
self.rect = Rect(0, 0, 1, 1)
spring_strength = 100 * plunger_mass
chute_opening = playfield.position + rect.center - (rect.width / 2. -
ball_radius * 4, 0)
plunger_rect = Rect(0, 0, rect.width * .2, ball_radius / 2.)
anchor0 = chute_opening - playfield.position - (ball_radius * 3., 0)
anchor1 = anchor0 + (rect.width * .8, 0)
anchor2 = -plunger_rect.width / 2., 0
plunger_body = Body(plunger_mass, pymunk.inf)
plunger_shape = Poly.create_box(plunger_body, plunger_rect.size)
plunger_shape.layers = 1
plunger_shape.friction = 0.1
plunger_shape.elasticity = 1.0
plunger_shape.collision_type = plunger_type
plunger_body.position = chute_opening + (plunger_rect.width / 2., 0)
j0 = GrooveJoint(playfield, plunger_body, anchor0, anchor1, anchor2)
j1 = DampedSpring(playfield, plunger_body, anchor0, anchor2, 0,
spring_strength, 5)
s0 = Circle(Body(), ball_radius / 2.)
s0.layers = 1
s0.sensor = True
s0.collision_type = sensor0_type
s0.body.position = chute_opening + (ball_radius * 4., 0.0)
s1 = Circle(Body(), ball_radius * 3.)
s1.layers = 1
s1.sensor = True
s1.collision_type = sensor1_type
s1.body.position = chute_opening
def inc_counter(space, arbiter):
self.chute_counter += 1
return True
def dec_counter(space, arbiter):
self.chute_counter -= 1
f = space.add_collision_handler
f(sensor1_type, plunger_type, begin=inc_counter, separate=dec_counter)
self.playfield = playfield
self.plunger_offset = playfield.position - plunger_body.position + (
ball_radius * 3, 0)
self.spring = j1
self.spring_length = rect.width / 2.
self.spring_strength = spring_strength
self.plunger_body = plunger_body
self.ball_chute = Rect(0, 0, ball_radius * 2., ball_radius * 2.)
self.ball_chute.center = chute_opening
self._original_image = pygame.Surface(plunger_rect.size)
self._original_image.fill((192, 255, 255))
self.shapes = [plunger_shape, s0, s1, j0, j1]
self.visible = 0
def __init__(self,
arm_anchor_point,
arm_segment_lengths,
target_position,
do_render=False,
sparse=True,
sparse_distance=SPARSE_DISTANCE):
super().__init__(do_render, sparse, MAX_MOTOR_FORCE)
self.sparse_distance = sparse_distance
self.target_position = target_position
self.space.gravity = (0.0, 0.0)
self.end_effector_body = None
end_anchor, end_pos = self.add_arm(arm_segment_lengths,
arm_anchor_point)
self.end_effector_body = pymunk.Body(1, 1)
self.end_effector_body.position = end_pos[0], end_pos[1]
end_effector_shape = Circle(self.end_effector_body, 8)
end_effector_shape.collision_type = NO_COLLISION_TYPE
self.space.add(self.end_effector_body, end_effector_shape)
pin = pymunk.PinJoint(end_anchor, self.end_effector_body,
(-arm_segment_lengths[-1] / 2, 0), (0, 0))
self.space.add(pin)
def add_circle(self, x, y, r, dynamic=True):
body = Body(body_type=(Body.STATIC, Body.DYNAMIC)[int(dynamic)])
body.position = x, y
circle = Circle(body, r)
circle.density = Environment.DEFAULT_DENSITY
self.space.add(body, circle)
self.bodies.append(body)
return body
def __createCircleShape(info: 'Dict[str, Any]') -> 'Circle':
shape = Circle(None, info['radius'], (info.get('x', 0), info.get('y', 0)))
shape.mass = info['mass']
shape.elasticity = info.get('elasticity', 0.5)
shape.friction = info.get('friction', 0.5)
return shape
def init_space():
sp = Space()
sp.gravity = (0, 50)
chain = make_pivot_chain(sp, (0, 0), (240, 30), 30)
sp.add(constraint.PivotJoint(chain[0], sp.static_body, chain[0].position))
# Cria quadrado
L = 25
player = Body(mass=1, moment=100)
shape = Poly(player, [(-L, -L), (L, -L), (L, L), (-L, L)])
player.position = (90, 60)
player.velocity = (-25, 25)
shape.elasticity = 1.0
shape.color = pyxel.COLOR_RED
shape.collision_type = 42
ball = Body(mass=1, moment=200)
ball_shape = Circle(ball, 20)
ball.position = (player.position.x, 130)
ball_shape.elasticity = 1.0
shape.color = pyxel.COLOR_NAVY
ball_shape.collision_type = 42
joint1 = constraint.DampedSpring(player, ball, (0, 0), (20, 0), 20, 3, 0.5)
joint2 = constraint.PivotJoint(sp.static_body, player, (65, 35))
joint1.collide_bodies = False
sp.add(joint1, joint2)
body2 = Body(1, 100)
sp.add(body2)
sp.add(Poly(body2, [(-3, 3), (3, 3), (3, -3), (-3, -3)]))
body2.position = 220, 50
sp.add(constraint.DampedRotarySpring(body2, ball, 0, 2, 1))
sp.body2 = body2
# Cria margens
line = Body(body_type=Body.STATIC)
e = 0
lines = [
Segment(line, (-e, -e), (240 + e, -e), 2),
Segment(line, (-e, 180 + e), (240 + e, 180 + e), 2),
Segment(line, (-e, -e), (-e, 180 + e), 2),
Segment(line, (240 + e, -e), (240 + e, 180 + e), 2),
]
for line in lines:
line.elasticity = 1.0
lines = []
# Adiciona elementos ao espaço
sp.add(player, shape, ball, ball_shape, *lines)
sp.player = player
#handler = sp.add_collision_handler(42, 42)
#handler.begin = lambda *args: False
return sp
def __init__(
self,
space: Space,
shape_type: str,
pos: Tuple[float, float],
scale: float = 1.,
mass: float = 1,
static: bool = False,
friction: float = 1.,
color: Tuple[float, float, float] = (1., 1., 1.),
):
self.space = space
self.shape_type = shape_type
self.static = static
self._scale = scale
self.color = color
padding = 0.03
if not static:
if self.shape_type == "box":
moment = pymunk.moment_for_box(mass, (self.scale, self.scale))
elif self.shape_type == "circle":
moment = pymunk.moment_for_circle(mass, 0, self.scale / 2.)
else:
raise ValueError(f"Invalid shape_type '{self.shape_type}'")
self.mass = mass
self.body = Body(mass, moment)
self.body.position = pos
self._static_position = None
if self.shape_type == "box":
self.shape = Poly.create_box(self.body, (1, 1))
elif self.shape_type == "circle":
self.shape = Circle(self.body, self.scale / 2. - padding)
# static
else:
self.mass = 0
self.body = self.space.static_body
self._static_position = Vec2d(*pos)
if self.shape_type == "box":
self.shape = Poly(
self.space.static_body,
[(pos[0] + p[0] * scale, pos[1] + p[1] * scale)
for p in SHAPE_TYPES[self.shape_type]["polygon"]])
self.shape.get_vertices()
elif self.shape_type == "circle":
self.shape = Circle(self.space.static_body,
self.scale / 2. - padding,
offset=pos)
self.shape.friction = friction
def init_space():
sp = Space()
player = Body(mass=1, moment=1)
shape = Circle(player, 10)
player.position = (20, 90)
player.velocity = (5, 0)
shape.color = pyxel.COLOR_YELLOW
line = Body(body_type=Body.STATIC)
line_shape = Segment(line, (0, 1), (240, 1), 2)
line_shape.color = pyxel.COLOR_RED
sp.add(player, shape, line, line_shape)
sp.player = player
return sp
def __init__(self, center, road, side):
# setup shape
mass = 90
radius = 2.5
inertia = moment_for_circle(mass, 0, radius * 2, (0, 0))
body = Body(mass, inertia)
body.position = center
self.shape = Circle(body, radius * 2, (0, 0))
self.shape.color = (0, 255, 255)
self.shape.collision_type = CollisionType.Pedestrian
self.shape.elasticity = 0.05
# Walk parameter
self.lenRange = road.length
self.widthRange = (road.nLanes + 1) * road.width * 2
self.side = side
# Bool flags
self.dead = False
# Move parameters
self.moving = 0
self.direction = road.direction
self.normal = road.normal
self.speed = random.randint(3, 6)
# Flags for crossing
self.crossing = False
self.beginCrossing = False
def __init__(self, mask):
self.body = Body(0, 0, Body.STATIC)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
#self.radius = 9 * CM_TO_PIXELS
self.radius = 6 * CM_TO_PIXELS
self.shape = Circle(self.body, self.radius)
self.mask = mask
self.shape.filter = ShapeFilter(mask=ShapeFilter.ALL_MASKS ^
(ANY_PUCK_MASK | ROBOT_MASK),
categories=mask)
if mask == ARC_LANDMARK_MASK:
self.shape.color = 255, 0, 255
elif mask == ENTER_ARC_LANDMARK_MASK:
self.shape.color = 0, 255, 0
elif mask == EXIT_ARC_LANDMARK_MASK:
self.shape.color = 255, 0, 0
elif mask == POLE_LANDMARK_MASK:
self.shape.color = 0, 0, 255
elif mask == BLAST_LANDMARK_MASK:
self.shape.color = 255, 255, 255
else:
sys.exit("Unknown landmark mask: " + str(mask))
def __init__(self, mask, radius):
self.body = Body(0, 0, Body.STATIC)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
self.shape = Circle(self.body, radius)
self.mask = mask
self.shape.filter = ShapeFilter(categories=mask)
if mask == ARC_LANDMARK_MASK:
self.shape.color = 0, 255, 0
elif mask == POLE_LANDMARK_MASK:
self.shape.color = 0, 0, 255
elif mask == BLAST_LANDMARK_MASK:
self.shape.color = 255, 0, 0
else:
sys.exit("Unknown landmark mask: " + str(mask))
# The following is just to set the appropriate params to visualize below
config = ConfigSingleton.get_instance()
self.vis_range_max = \
config.getfloat("RangeScan:landmarks", "range_max") \
+ radius
self.vis_inside_radius = \
config.getfloat("LandmarkCircleController", "inside_radius") \
+ radius
self.vis_outside_radius = \
config.getfloat("LandmarkCircleController", "outside_radius") \
+ radius
def __init__(self, space, rect, playfield=None):
super(PlungerAssembly, self).__init__()
self.chute_counter = 0
self.rect = pygame.Rect(0, 0, 0, 0)
spring_strength = 100 * plunger_mass
chute_opening = playfield.position + rect.center - (rect.width / 2 - ball_radius * 4, 0)
plunger_rect = pygame.Rect(0, 0, rect.width * .2, ball_radius / 2)
anchor0 = chute_opening - playfield.position - (ball_radius * 3, 0)
anchor1 = anchor0 + (rect.width * .8, 0)
anchor2 = -plunger_rect.width / 2, 0
plunger_body = Body(plunger_mass, pymunk.inf)
plunger_shape = Poly.create_box(plunger_body, plunger_rect.size)
plunger_shape.layers = 1
plunger_shape.friction = 0
plunger_shape.elasticity = 1.0
plunger_shape.collision_type = plunger_type
plunger_body.position = chute_opening + (plunger_rect.width / 2, 0)
j0 = GrooveJoint(playfield, plunger_body, anchor0, anchor1, anchor2)
j1 = DampedSpring(playfield, plunger_body, anchor0, anchor2, 0, spring_strength, 5)
s0 = Circle(Body(), ball_radius / 2)
s0.layers = 1
s0.sensor = True
s0.collision_type = sensor0_type
s0.body.position = chute_opening + (ball_radius * 4, 0)
s1 = Circle(Body(), ball_radius * 3)
s1.layers = 1
s1.sensor = True
s1.collision_type = sensor1_type
s1.body.position = chute_opening
def inc_counter(space, arbiter):
self.chute_counter += 1
return True
def dec_counter(space, arbiter):
self.chute_counter -= 1
f = space.add_collision_handler
f(sensor1_type, plunger_type, begin=inc_counter, separate=dec_counter)
self.playfield = playfield
self.plunger_offset = playfield.position - plunger_body.position + (ball_radius * 3, 0)
self.spring = j1
self.spring_length = rect.width / 2
self.spring_strength = spring_strength
self.plunger_body = plunger_body
self.ball_chute = pygame.Rect(0, 0, ball_radius * 2, ball_radius * 2)
self.ball_chute.center = chute_opening
self._original_image = pygame.Surface(plunger_rect.size)
self._original_image.fill((192, 255, 255))
self.shapes = [plunger_shape, s0, s1, j0, j1]
self.visible = 0
def __init__(self, space, rect, playfield=None):
super(Spinner, self).__init__()
r, cy = rect.width / 2, rect.height / 2
assert (r == cy)
body = Body(.1, moment_for_circle(.1, 0, r))
body.position = rect.center
top = Circle(body, r)
top.layers = 2
rect2 = pygame.Rect((-r, -cy), rect.size)
cross0 = Segment(body, rect2.midleft, rect2.midright, 1)
cross1 = Segment(body, rect2.midtop, rect2.midbottom, 1)
j0 = PivotJoint(playfield, body, body.position)
j1 = SimpleMotor(playfield, body, 0)
j1.max_force = 200
self.shapes = [top, cross0, cross1, j0, j1]
self.rect = pygame.Rect(rect)
self._original_image = prepare.GFX['pachinko-spinner']
def __init__(self, space, rect, playfield=None):
super(Spinner, self).__init__()
r, cy = rect.width / 2, rect.height / 2
assert (r == cy)
body = Body(.1, moment_for_circle(.1, 0, r))
body.position = rect.center
top = Circle(body, r)
top.layers = 2
rect2 = pygame.Rect((-r, -cy), rect.size)
cross0 = Segment(body, rect2.midleft, rect2.midright, 1)
cross1 = Segment(body, rect2.midtop, rect2.midbottom, 1)
j0 = PivotJoint(playfield, body, body.position)
j1 = SimpleMotor(playfield, body, 0)
j1.max_force = 200
self.shapes = [top, cross0, cross1, j0, j1]
self.rect = pygame.Rect(rect)
self._original_image = prepare.GFX['pachinko-spinner']
def __init__(self, x, y, side, dir):
body = Body(0, 0, Body.STATIC)
body.position = x, y
self.side = side
self.dir = dir
self.shape = Circle(body, self.radius * 2, (0, 0))
self.shape.color = (0, 0, 255, 0)
self.shape.elasticity = 0.95
self.shape.collision_type = CollisionType.Goalpost
def _create_poly(self) -> Circle:
"""
Create the polygon used for ray-casting. (Ultrasonic sensor)
:return: a Pymunk Circle object.
"""
body = Body(body_type=Body.STATIC)
body.position = Vec2d(self.center_x, self.center_y)
return Circle(body, self.radius)
def circ(self, x, y, r, **kwargs):
"""
Creates a circle at radius r centered at (x, y).
"""
body = self._make_body(**kwargs)
shape = self._make_shape(Circle(body, r), **kwargs)
body.position = (x, y)
self.space.add(body, shape)
return body
def init_space():
sp = Space()
sp.gravity = (0, 50)
sp.damping = 1.0
floor = Body(body_type=Body.STATIC)
stick = Body(mass=100, moment=100 * 50**2)
L = 20
shapes = [
Poly(stick, [(-L, -L), (L, -L), (L, L), (0, L + L / 2), (-L, L)],
radius=3),
Segment(floor, (1, 179), (239, 179), 1),
Segment(floor, (1, 1), (239, 1), 1),
Segment(floor, (1, 1), (1, 179), 1),
Segment(floor, (239, 1), (239, 179), 1),
]
stick.position = (120, L)
bodies = []
for _ in range(L):
r = random.uniform(2, 6)
mass = pi * r**2
body = Body(mass=mass, moment=mass * r**2 / 2)
circle = Circle(body, r)
x = random.uniform(r, 240 - r)
y = random.uniform(r, 180 - r)
body.position = (x, y)
vx = random.uniform(-L, L)
vy = random.uniform(-L, L)
body.velocity = (vx, vy)
bodies.append(body)
shapes.append(circle)
circle.color = random.randint(1, 15)
for shape in shapes:
shape.elasticity = 1.0
sp.add(floor, stick, *bodies, *shapes)
return sp
def __init__(self, world, x=0, y=0, r=8):
x, y = int(x), int(y)
shape = Circle(world.phys_space.static_body, r, (x + r, y + r))
super().__init__(world, shape, x=x, y=y)
self.spriteobject = SpriteObject(world.get_texture("orb"),
x,
y,
batch="objects")
sw = self.spriteobject.sprite.width
ratio = (r * 2) / sw
self.spriteobject.sprite.scale = ratio
def __init__(self, x, y, car, mass=1, radius=8):
super().__init__()
self.position = car.position + (x, y)
self.offset = Vec2d(x, y)
self.car = car
self.shape = Circle(self, radius)
self.shape.mass = mass
self.shape.visible = True
self.shape.friction = 0.9
def __createCircleShape(self, info: 'Dict[str, Any]',
default_elasticity: float = None,
default_friction: float = None) -> 'pymunk.Shape':
shape = Circle(None, info['radius'],
(info.get('x', 0), info.get('y', 0)))
self.__setGeneralProperties(shape, info,
default_elasticity=default_elasticity,
default_friction=default_friction)
return shape
def __init__(self, rect):
super(Ball, self).__init__()
radius = rect.width / 2
body = Body()
body.position = rect.center
self.shape = Circle(body, radius)
self.shape.mass = ball_mass
self.shape.elasticity = .25
self.shape.friction = 1
self.rect = Rect(0, 0, rect.width, rect.width)
self.original_image = resources.gfx("yarnball.png", convert_alpha=True)
self.pymunk_shapes = (body, self.shape)
def __init__(self, space, rect):
super(Ball, self).__init__()
radius = rect.width / 2
body = Body(ball_mass, moment_for_circle(ball_mass, 0, radius))
body.position = rect.center
self.shape = Circle(body, radius)
self.shape.elasticity = .5
self.shape.friction = 0
self.shape.layers = 1
self.shape.collision_type = ball_type
self.rect = pygame.Rect(0, 0, rect.width, rect.width)
image = smoothscale(prepare.GFX.get('ball-bearing'), self.rect.size)
self._original_image = image.convert_alpha()
def __init__(self, space, rect):
super(Handle, self).__init__()
color = (192, 192, 220)
radius = rect.width / 2
body = Body()
body.position = rect.center
shape = Circle(body, radius)
rect2 = pygame.Rect(0, 0, rect.width, rect.width)
image = pygame.Surface(rect2.size, pygame.SRCALPHA)
pygame.draw.circle(image, color, rect2.center, int(radius // 4))
pygame.draw.line(image, (0, 0, 255), rect2.center, rect2.midtop, 8)
self.shapes = [shape]
self.rect = rect
self._original_image = image
def create_circle(r=1, x=0, y=0, m=1, bt=Body.DYNAMIC):
'''
given radius (r), x-position (x), y-position (y), mass (m), body_type (bt)
return the (body, shape) tuple for a circle
'''
given_bt = bt
bt = Body.DYNAMIC if given_bt == 'dynamic' else Body.DYNAMIC
bt = Body.STATIC if given_bt == 'static' else Body.DYNAMIC
bt = Body.KINEMATIC if given_bt == 'kinematic' else Body.DYNAMIC
moment = moment_for_circle(mass=m, inner_radius=0, outer_radius=r)
body = Body(mass=m, moment=moment, body_type=bt)
shape = Circle(body=body, radius=r)
body.position = (x, y)
return body, shape
def __init__(self):
self.mass = 1 # 1 kg
# e-puck: 0.037 meter radius, converted to pixels for display
#self.radius = 3.7 * CM_TO_PIXELS
# Bupimo: 9 cm radius, converted to pixels for display
self.radius = 9 * CM_TO_PIXELS
self.circular = False;
if self.circular:
rob_I = moment_for_circle(self.mass, 0, self.radius)
self.body = Body(self.mass, rob_I)
self.shape = Circle(self.body, self.radius)
else:
r = self.radius
d = self.radius * 1.5 # Amount the wedge part pokes out.
vertices = [(0, -r),
(d, 0),
(0, r)]
#vertices = [(0, -r),
# (d/4, 0),
# (d/2, r)]
# Now add the semicircular back part
n = 5
angles = [pi/2 + i*(pi/n) for i in range(1, n)]
for a in angles:
vertices.append((r*cos(a), r*sin(a)))
rob_I = moment_for_poly(self.mass, vertices)
self.body = Body(self.mass, rob_I)
self.shape = Poly(self.body, vertices)
# EXPERIMENTAL: Add bristles to robot
# rest_length = 100
# stiffness = 500
# damping = 10
# self.bristle_body = pymunk.DampedSpring(self.body, body, \
# (0,0), (0,0), rest_length, stiffness, damping)
# self.sim.env.add(self.spring_body)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
self.shape.color = 0, 255, 0
self.shape.filter = ShapeFilter(categories = ROBOT_MASK)
self.command = Twist()
def __init__(self, world, x=0, y=0):
self.movable = Movable()
self.spriteobject = SpriteObject(world.get_texture("player"),
x,
y,
batch="player")
self.directionalsprite = DirectionalSprite(world, "player")
phys_body = Body(5, inf)
phys_body.position = x, y
shape = Circle(phys_body, 8, (8, 8))
self.physicsbody = PhysicsBody(shape)
world.phys_space.add(self.physicsbody.body, self.physicsbody.shape)
self.groundingobject = GroundingObject()
self.jumpobject = JumpObject()
self.inputobject = InputObject("kb")
def __init__(self):
rob_mass = 1 # 1 kg
# 0.1 meter radius, converted to pixels for display
rob_radius = 0.1 * M_TO_PIXELS
# moment of inertia for disk
rob_I = moment_for_circle(rob_mass, 0, rob_radius)
self.body = Body(rob_mass, rob_I)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
self.shape = Circle(self.body, rob_radius)
self.shape.color = 255, 0, 0 # red
self._command = Twist()
def __init__(self, kind, immobile=False):
self.immobile = immobile
self.mass = 0.1 # 0.1 kg
# 0.1 meter radius, converted to pixels for display
#self.radius = 0.1 * M_TO_PIXELS
# Hockey puck radius = 23mm = 0.023
self.radius = 0.023 * M_TO_PIXELS
# Plate puck radius = 12.8cm = 0.128
#self.radius = 0.128 * M_TO_PIXELS
# moment of inertia for disk
moment_inertia = moment_for_circle(self.mass, 0, self.radius)
if not immobile:
self.body = Body(self.mass, moment_inertia)
else:
self.body = Body(0, 0, Body.STATIC)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
self.shape = Circle(self.body, self.radius)
self.kind = kind
if kind == 0:
self.shape.color = 200, 100, 100
self.shape.filter = ShapeFilter(categories=RED_PUCK_MASK)
elif kind == 1:
self.shape.color = 100, 200, 100
self.shape.filter = ShapeFilter(categories=GREEN_PUCK_MASK)
elif kind == 2:
self.shape.color = 100, 100, 200
self.shape.filter = ShapeFilter(categories=BLUE_PUCK_MASK)
else:
sys.exit("Unknown puck kind: " + kind)
if immobile:
self.shape.color = self.shape.color[0] / 3, self.shape.color[
1] / 3, self.shape.color[2] / 3
def __init__(self,x,y,radius):
# setup shape
mass = 10
inertia = moment_for_circle(mass, 0, radius*2, (0, 0))
body = Body(mass, inertia)
body.position = x, y
body.velocity_func = friction_ball
self.shape = Circle(body, radius*2, (0, 0))
self.shape.color = (255, 0, 0, 0)
self.shape.elasticity = 0.98
self.shape.friction = 3.0
self.shape.collision_type = CollisionType.Ball
# Initial and previous positions
self.initPos = x,y
self.prevPos = Vec2d(x,y)
# List of robots who last touched the ball
self.lastKicked = []
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 handle_object_type_pin(data, body):
pin = Circle(body, 2, get_rect(data).center)
pin.elasticity = 1.0
yield pin
class Object:
def __init__(
self,
space: Space,
shape_type: str,
pos: Tuple[float, float],
scale: float = 1.,
mass: float = 1,
static: bool = False,
friction: float = 1.,
color: Tuple[float, float, float] = (1., 1., 1.),
):
self.space = space
self.shape_type = shape_type
self.static = static
self._scale = scale
self.color = color
padding = 0.03
if not static:
if self.shape_type == "box":
moment = pymunk.moment_for_box(mass, (self.scale, self.scale))
elif self.shape_type == "circle":
moment = pymunk.moment_for_circle(mass, 0, self.scale / 2.)
else:
raise ValueError(f"Invalid shape_type '{self.shape_type}'")
self.mass = mass
self.body = Body(mass, moment)
self.body.position = pos
self._static_position = None
if self.shape_type == "box":
self.shape = Poly.create_box(self.body, (1, 1))
elif self.shape_type == "circle":
self.shape = Circle(self.body, self.scale / 2. - padding)
# static
else:
self.mass = 0
self.body = self.space.static_body
self._static_position = Vec2d(*pos)
if self.shape_type == "box":
self.shape = Poly(
self.space.static_body,
[(pos[0] + p[0] * scale, pos[1] + p[1] * scale)
for p in SHAPE_TYPES[self.shape_type]["polygon"]])
self.shape.get_vertices()
elif self.shape_type == "circle":
self.shape = Circle(self.space.static_body,
self.scale / 2. - padding,
offset=pos)
self.shape.friction = friction
@property
def position(self) -> Vec2d:
if self._static_position:
return self._static_position
return self.body.position
@property
def rotation(self) -> float:
return self.body.angle
@property
def scale(self) -> float:
return self._scale
@property
def texture_idx(self) -> int:
return 13 if self.shape_type == "circle" else 10
def __repr__(self):
return f"Object({self.mass}, {self.position})"
def apply_impulse(self,
impulse: Tuple[float, float],
point: Optional[Tuple[float, float]] = (0, 0)):
if not self.static:
point = self.position + point
self.body.apply_impulse_at_world_point(impulse, point)
def add_to_mesh(self, mesh: TriangleMesh):
offset = self.body.position
rotation = self.body.angle
if isinstance(self.shape, Poly):
vertices = [
glm.vec3(v.rotated(rotation) + offset, 0)
for v in self.shape.get_vertices()
]
for i in range(len(vertices) - 1):
mesh.add_triangle(
vertices[0],
vertices[i + 1],
vertices[i],
)
elif self.shape_type == "circle":
num_seg = 4
center = glm.vec3(offset, 0)
vertices = [
glm.vec3(
Vec2d(math.sin(s / num_seg * math.pi),
math.cos(s / num_seg * math.pi)).rotated(rotation) *
self.scale / 2. + offset, 0)
for s in range(num_seg * 2 + 1)
]
for i in range(len(vertices) - 1):
mesh.add_triangle(
center,
vertices[i + 1],
vertices[i],
)
def update(self, time: float, dt: float):
pass
def handle_object_type_pin(data, body):
pin = Circle(body, 2, get_rect(data).center)
pin.elasticity = 1.0
yield pin
