def setup(self, *args, **kwargs) -> None:
super().setup(*args, **kwargs)
# Physics. This joint setup was taken form tank.py in the pymunk
# examples.
if self.shape_type == ShapeType.SQUARE:
side_len = math.sqrt(math.pi) * self.shape_size
shapes = self._make_square(side_len)
elif self.shape_type == ShapeType.CIRCLE:
shapes = self._make_circle()
elif self.shape_type == ShapeType.STAR:
star_npoints = 5
star_out_rad = 1.3 * self.shape_size
star_in_rad = 0.5 * star_out_rad
shapes, convex_parts = self._make_star(star_npoints, star_out_rad,
star_in_rad)
else:
# These are free-form shapes b/c no helpers exist in Pymunk.
try:
factor, num_sides = POLY_TO_FACTOR_SIDE_PARAMS[self.shape_type]
except KeyError:
raise NotImplementedError("haven't implemented",
self.shape_type)
side_len = factor * gtools.regular_poly_circ_rad_to_side_length(
num_sides, self.shape_size)
shapes, poly_verts = self._make_regular_polygon(
num_sides, side_len)
for shape in shapes:
shape.friction = 0.5
self.add_to_space(shape)
trans_joint = pm.PivotJoint(self.space.static_body, self.shape_body,
(0, 0), (0, 0))
trans_joint.max_bias = 0
trans_joint.max_force = self.phys_vars.shape_trans_joint_max_force
self.add_to_space(trans_joint)
rot_joint = pm.GearJoint(self.space.static_body, self.shape_body, 0.0,
1.0)
rot_joint.max_bias = 0
rot_joint.max_force = self.phys_vars.shape_rot_joint_max_force
self.add_to_space(rot_joint)
# Graphics.
geoms_outer = []
if self.shape_type == ShapeType.SQUARE:
geoms = [r.make_square(side_len, outline=True)]
elif self.shape_type == ShapeType.CIRCLE:
geoms = [r.make_circle(self.shape_size, 100, True)]
elif self.shape_type == ShapeType.STAR:
star_short_verts = gtools.compute_star_verts(
star_npoints,
star_out_rad - SHAPE_LINE_THICKNESS,
star_in_rad - SHAPE_LINE_THICKNESS,
)
short_convex_parts = autogeom.convex_decomposition(
star_short_verts + star_short_verts[:1], 0)
geoms = []
for part in short_convex_parts:
geoms.append(r.Poly(part, outline=False))
geoms_outer = []
for part in convex_parts:
geoms_outer.append(r.Poly(part, outline=False))
elif (self.shape_type == ShapeType.OCTAGON
or self.shape_type == ShapeType.HEXAGON
or self.shape_type == ShapeType.PENTAGON
or self.shape_type == ShapeType.TRIANGLE):
geoms = [r.Poly(poly_verts, outline=True)]
else:
raise NotImplementedError("haven't implemented", self.shape_type)
if self.shape_type == ShapeType.STAR:
for g in geoms_outer:
g.color = darken_rgb(self.color)
for g in geoms:
g.color = self.color
else:
for g in geoms:
g.color = self.color
g.outline_color = darken_rgb(self.color)
self.shape_xform = r.Transform()
shape_compound = r.Compound(geoms_outer + geoms)
shape_compound.add_transform(self.shape_xform)
self.viewer.add_geom(shape_compound)
def main():
N = int(DURATION / DT) + 1
space = pymunk.Space()
space.gravity = (0, 0)
box_body = pymunk.Body()
box_body.position = (1, 0.3)
box_r = 0.5
box_corners = [(-box_r, box_r), (-box_r, -box_r), (box_r, -box_r),
(box_r, box_r)]
box = pymunk.Poly(box_body, box_corners, radius=0.01)
# box = pymunk.Circle(box_body, 0.5)
box.mass = M
box.friction = 0.5
box.collision_type = 1
space.add(box.body, box)
# linear friction
pivot = pymunk.PivotJoint(space.static_body, box.body, box.body.position)
pivot.max_force = MU * M * G # μmg
pivot.max_bias = 0 # disable correction so that body doesn't bounce back
space.add(pivot)
# angular friction
gear = pymunk.GearJoint(space.static_body, box.body, 0, 1)
gear.max_force = 2
gear.max_bias = 0
space.add(gear)
control_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
control_body.position = (0, 0)
control_shape = pymunk.Circle(control_body, 0.1, (0, 0))
control_shape.friction = 1
control_shape.collision_type = 1
space.add(control_shape.body, control_shape)
watcher = CollisionWatcher()
space.add_collision_handler(1, 1).post_solve = watcher.update
plt.ion()
fig = plt.figure()
ax = plt.gca()
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_xlim([-5, 5])
ax.set_ylim([-3, 3])
ax.grid()
ax.set_aspect('equal')
options = pymunk.matplotlib_util.DrawOptions(ax)
options.flags = pymunk.SpaceDebugDrawOptions.DRAW_SHAPES
space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
pd = np.array([3, 0])
# pd = np.array([-3, 0])
# pd = np.array([2, -2])
ax.plot(pd[0], pd[1], 'o', color='r')
v_max = 0.2
kp = 0.5
for i in range(N - 1):
t = i * DT
# step the sim
space.step(DT)
p = np.array(control_body.position)
Δp = pd - p
# Δp_unit = unit(Δp)
c = np.array(box.body.position)
# Δp = pd - c
# if watcher.first_contact:
# # φ = np.arccos(watcher.nf @ Δp_unit)
# φ = 1 - watcher.nf @ Δp_unit
#
# # k is still a tuning parameter -- depends to some extent on object
# # curvature and friction
# # k = 1 / (1 + φ**2)
# k = 1
# kφ = k * φ # / np.linalg.norm(Δp)
# print(φ)
# R = np.array([[np.cos(kφ), -np.sin(kφ)],
# [np.sin(kφ), np.cos(kφ)]])
# v_unit = R @ watcher.nf
# v = min(v_max, np.linalg.norm(Δp)) * v_unit
# else:
# v = v_max * np.array([1, 0])
if watcher.first_contact:
v_traj = kp * Δp
v_mag = max(min(watcher.nf @ v_traj, v_max), 0)
# reflect over nf
α = 0.5
d = (1 + α) * watcher.nf * (watcher.nf @ Δp) - α * Δp
# project onto d
v = (v_traj @ unit(d)) * unit(d)
# v_unit = unit(d)
# v = v_mag * v_unit
else:
v = v_max * np.array([1, 0])
control_body.velocity = (v[0], v[1])
if i % PLOT_PERIOD == 0:
ax.cla()
ax.set_xlim([-5, 5])
ax.set_ylim([-3, 3])
ax.grid()
ax.plot(pd[0], pd[1], 'o', color='r')
plot_line(ax, p, p + unit(v), color='r')
plot_line(ax, p, p + watcher.nf, color='b')
space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
def run():
config = pyglet.gl.Config(sample_buffers=1, samples=2, double_buffer=True)
window = pyglet.window.Window(config=config, vsync = False)
space = pymunk.Space()
space.gravity = 0,-900
space.damping = .999
c = Vec2d(window.width /2., window.height / 2.)
### WEB
web_group = 1
bs = []
dist = .3
cb = pymunk.Body(1,1)
cb.position = c
s = pymunk.Circle(cb, 15) # to have something to grab
s.filter = pymunk.ShapeFilter(group = web_group)
s.ignore_draw = True
space.add(cb, s)
#generate each crossing in the net
for x in range(0,101):
b = pymunk.Body(1, 1)
v = Vec2d.unit()
v.angle_degrees = x*18
scale = window.height / 2. / 6. * .5
dist += 1/18.
dist = dist ** 1.005
offset = 0
offset = [0.0, -0.80, -1.0, -0.80][((x*18) % 360)//18 % 4]
offset = .8 + offset
offset *= dist**2.8 / 100.
#print "offset", offset
v.length = scale * (dist + offset)
b.position = c + v
s = pymunk.Circle(b, 15)
s.filter = pymunk.ShapeFilter(group = web_group)
s.ignore_draw = True
space.add(b,s)
bs.append(b)
def add_joint(a,b):
rl = a.position.get_distance(b.position) * 0.9
stiffness = 5000.
damping = 100
j = pymunk.DampedSpring(a, b, (0,0), (0,0), rl, stiffness, damping)
j.max_bias = 1000
#j.max_force = 50000
space.add(j)
for b in bs[:20]:
add_joint(cb,b)
for i in range(len(bs)-1):
add_joint(bs[i], bs[i+1])
i2 = i+20
if len(bs) > i2:
add_joint(bs[i], bs[i2])
### WEB ATTACH POINTS
static_bs = []
for b in bs[-17::4]:
static_body = pymunk.Body(body_type = pymunk.Body.STATIC)
static_body.position = b.position
static_bs.append(static_body)
j = pymunk.PivotJoint(static_body, b, static_body.position)
j = pymunk.DampedSpring(static_body, b, (0,0), (0,0), 0, 0, 0)
j.damping = 100
j.stiffness = 20000
space.add(j)
### ALL SETUP DONE
def update(dt):
# Note that we dont use dt as input into step. That is because the
# simulation will behave much better if the step size doesnt change
# between frames.
r = 10
for x in range(r):
space.step(1./30./r)
pyglet.clock.schedule_interval(update, 1/30.)
selected = None
selected_joint = None
mouse_body = pymunk.Body(body_type = pymunk.Body.KINEMATIC)
@window.event
def on_mouse_press(x, y, button, modifiers):
mouse_body.position = x,y
hit = space.point_query_nearest((x,y), 10, pymunk.ShapeFilter())
if hit != None:
global selected
body = hit.shape.body
rest_length = mouse_body.position.get_distance(body.position)
stiffness = 1000
damping = 10
selected = pymunk.DampedSpring(mouse_body, body, (0,0), (0,0), rest_length, stiffness, damping)
space.add(selected)
@window.event
def on_mouse_release(x, y, button, modifiers):
global selected
if selected != None:
space.remove(selected)
selected = None
@window.event
def on_mouse_drag(x, y, dx, dy, buttons, modifiers):
mouse_body.position = x,y
@window.event
def on_key_press(symbol, modifiers):
if symbol == pyglet.window.key.P:
pyglet.image.get_buffer_manager().get_color_buffer().save('spiderweb.png')
fps_display = pyglet.clock.ClockDisplay()
@window.event
def on_draw():
pyglet.gl.glClearColor(240,240,240,255)
window.clear()
fps_display.draw()
# static attach points
pyglet.gl.glColor3f(1,0,1)
pyglet.gl.glPointSize(6)
a = []
for b in static_bs:
a += [b.position.x, b.position.y]
pyglet.graphics.draw(len(a)//2, pyglet.gl.GL_POINTS, ('v2f',a))
# web crossings / bodies
pyglet.gl.glColor3f(.8,.8,.8)
a = []
for b in bs:
a += [b.position.x, b.position.y]
pyglet.gl.glPointSize(4)
pyglet.graphics.draw(len(a)//2, pyglet.gl.GL_POINTS, ('v2f',a))
# web net / constraints
a = []
for j in space.constraints:
a += [j.a.position.x, j.a.position.y, j.b.position.x, j.b.position.y]
pass
pyglet.graphics.draw(len(a)//2, pyglet.gl.GL_LINES, ('v2f',a))
# anything else
pyglet.app.run()
for i in range(len(bs) - 1):
add_joint(bs[i], bs[i + 1])
i2 = i + 20
if len(bs) > i2:
add_joint(bs[i], bs[i2])
### WEB ATTACH POINTS
static_bs = []
for b in bs[-17::4]:
static_body = pymunk.Body(body_type=pymunk.Body.STATIC)
static_body.position = b.position
static_bs.append(static_body)
j = pymunk.PivotJoint(static_body, b, static_body.position)
j = pymunk.DampedSpring(static_body, b, (0, 0), (0, 0), 0, 0, 0)
j.damping = 100
j.stiffness = 20000
space.add(j)
### ALL SETUP DONE
def update(dt):
# Note that we dont use dt as input into step. That is because the
# simulation will behave much better if the step size doesnt change
# between frames.
r = 10
for x in range(r):
space.step(1. / 30. / r)
def main():
running = True
pygame.init()
screen = pygame.display.set_mode((DISPLAY_WIDTH, DISPLAY_HEIGHT))
pygame.display.set_caption('SwingUp!')
clock = pygame.time.Clock()
### init our pymunk space and apply gravity to it
space = pymunk.Space()
space.gravity = (0.0, -4000)
# to prevent the pendulum from blowing up, we introduce
# a system wide damping
space.damping = 0.6
draw_options = pymunk.pygame_util.DrawOptions(screen)
agent_body, agent_shape = setup_agent()
pole_body, pole_shape = setup_pole()
move_x_joint = pymunk.GrooveJoint(space.static_body, agent_body,
(0, DISPLAY_HEIGHT / 2),
(DISPLAY_WIDTH, DISPLAY_HEIGHT / 2),
(0, 0))
pj = pymunk.PivotJoint(pole_body, agent_body, (
0,
75,
), (0, 0))
pj.distance = 0
pj.error_bias = pow(1.0 - 1.0, 60.0)
pj.collide_bodies = False
space.add(agent_body, agent_shape, pole_body, pole_shape, move_x_joint, pj)
keys = {'left': False, 'right': False}
force_direction = 0
# We want to apply a constant force to our dynamic cart as we are keeping
# our respective arrow key pressed. If we were to apply this force in our
# event for loop, this would lead us to a situation in which the force
# would be applied in one single step only as we are solely tracking
# keyup / keydown behaviours for performance reasons. Hence, we rather
# need to keep track on which action should be executed and perform on
# every step, meaning after our for event loop which figures out whether
# something changed.
#
# To achieve this behaviour, we introduce a variable named `force_direction`
# which will act as an indicator if we need to apply force and if so,
# as a multiplier in order to apply the right direction:
#
# y ^
# |
# |
# |
# ------------------------- > x
# <- negative | positive ->
# action= - 1 | action = 1
# |
#
# action = 0
#
# + If we do not have any user input, action is 0
# + If the user presses `<` , action is -1
# + If the user presses `>` , action is 1
# + If the user releases `<` or `>` a and no other key is pressed
# action is 0
# + If the user releases `<` or `>` and the opposite direction key
# meaning `>` or `<` is pressed, the action is inverted:
# -1 -> 1 | 1 -> -1
while running:
for event in pygame.event.get():
if event.type == QUIT or \
event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
running = False
break
elif event.type == KEYDOWN and event.key == K_LEFT:
keys['left'] = True
force_direction = -1
elif event.type == KEYUP and event.key == K_LEFT:
keys['left'] = False
if not keys['right']:
force_direction = 0
else:
force_direction = 1
elif event.type == KEYDOWN and event.key == K_RIGHT:
keys['right'] = True
force_direction = 1
elif event.type == KEYUP and event.key == K_RIGHT:
keys['right'] = False
if not keys['left']:
force_direction = 0
else:
force_direction = -1
f = (force_direction * 7000, 0)
agent_body.apply_force_at_local_point(f, agent_body.position)
# NOTE: `step` HAS to be float/double
space.step(1 / FRAMERATE)
screen.fill(WHITE)
space.debug_draw(draw_options)
pygame.display.flip()
clock.tick(FRAMERATE)
def testMaxBias(self):
a, b = p.Body(10, 10), p.Body(10, 10)
j = p.PivotJoint(a, b, (0, 0))
self.assertEqual(j.max_bias, p.inf)
j.max_bias = 10
self.assertEqual(j.max_bias, 10)
def testA(self):
a, b = p.Body(10, 10), p.Body(10, 10)
j = p.PivotJoint(a, b, (0, 0))
self.assertEqual(j.a, a)
def setup(self, *args, **kwargs):
super().setup(*args, **kwargs)
# physics setup, starting with main body
# signature: moment_for_circle(mass, inner_rad, outer_rad, offset)
inertia = pm.moment_for_circle(self.mass, 0, self.radius, (0, 0))
self.robot_body = body = pm.Body(self.mass, inertia)
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
# For control. The rough joint setup was taken form tank.py in the
# pymunk examples.
self.control_body = control_body = pm.Body(body_type=pm.Body.KINEMATIC)
control_body.position = self.init_pos
control_body.angle = self.init_angle
self.add_to_space(control_body)
pos_control_joint = pm.PivotJoint(control_body, body, (0, 0), (0, 0))
pos_control_joint.max_bias = 0
pos_control_joint.max_force = self.phys_vars.robot_pos_joint_max_force
self.add_to_space(pos_control_joint)
rot_control_joint = pm.GearJoint(control_body, body, 0.0, 1.0)
rot_control_joint.error_bias = 0.0
rot_control_joint.max_bias = 2.5
rot_control_joint.max_force = self.phys_vars.robot_rot_joint_max_force
self.add_to_space(rot_control_joint)
# googly eye control bodies & joints
self.pupil_bodies = []
for eye_side in [-1, 1]:
eye_mass = self.mass / 10
eye_radius = self.radius
eye_inertia = pm.moment_for_circle(eye_mass, 0, eye_radius, (0, 0))
eye_body = pm.Body(eye_mass, eye_inertia)
eye_body.angle = self.init_angle
eye_joint = pm.DampedRotarySpring(body, eye_body, 0, 0.1, 3e-3)
eye_joint.max_bias = 3.0
eye_joint.max_force = 0.001
self.pupil_bodies.append(eye_body)
self.add_to_space(eye_body, eye_joint)
# finger bodies/controls (annoying)
finger_thickness = 0.25 * self.radius
finger_upper_length = 1.1 * self.radius
finger_lower_length = 0.7 * self.radius
self.finger_bodies = []
self.finger_motors = []
finger_vertices = []
finger_inner_vertices = []
self.target_finger_angle = 0.0
for finger_side in [-1, 1]:
# basic finger body
finger_verts = make_finger_vertices(
upper_arm_len=finger_upper_length,
forearm_len=finger_lower_length,
thickness=finger_thickness,
side_sign=finger_side)
finger_vertices.append(finger_verts)
finger_inner_verts = make_finger_vertices(
upper_arm_len=finger_upper_length - ROBOT_LINE_THICKNESS * 2,
forearm_len=finger_lower_length - ROBOT_LINE_THICKNESS * 2,
thickness=finger_thickness - ROBOT_LINE_THICKNESS * 2,
side_sign=finger_side)
finger_inner_verts = [[(x, y + ROBOT_LINE_THICKNESS)
for x, y in box]
for box in finger_inner_verts]
finger_inner_vertices.append(finger_inner_verts)
# these are movement limits; they are useful below, but also
# necessary to make initial positioning work
if finger_side < 0:
lower_rot_lim = -self.finger_rot_limit_inner
upper_rot_lim = self.finger_rot_limit_outer
if finger_side > 0:
lower_rot_lim = -self.finger_rot_limit_outer
upper_rot_lim = self.finger_rot_limit_inner
finger_mass = self.mass / 8
finger_inertia = pm.moment_for_poly(finger_mass,
sum(finger_verts, []))
finger_body = pm.Body(finger_mass, finger_inertia)
if finger_side < 0:
delta_finger_angle = upper_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
else:
delta_finger_angle = lower_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
# position of finger relative to body
finger_rel_pos = (finger_side * self.radius * 0.45,
self.radius * 0.1)
finger_rel_pos_rot = gtools.rotate_vec(finger_rel_pos,
self.init_angle)
finger_body.position = gtools.add_vecs(body.position,
finger_rel_pos_rot)
self.add_to_space(finger_body)
self.finger_bodies.append(finger_body)
# pin joint to keep it in place (it will rotate around this point)
finger_pin = pm.PinJoint(
body,
finger_body,
finger_rel_pos,
(0, 0),
)
finger_pin.error_bias = 0.0
self.add_to_space(finger_pin)
# rotary limit joint to stop it from getting too far out of line
finger_limit = pm.RotaryLimitJoint(body, finger_body,
lower_rot_lim, upper_rot_lim)
finger_limit.error_bias = 0.0
self.add_to_space(finger_limit)
# motor to move the fingers around (very limited in power so as not
# to conflict with rotary limit joint)
finger_motor = pm.SimpleMotor(body, finger_body, 0.0)
finger_motor.rate = 0.0
finger_motor.max_bias = 0.0
finger_motor.max_force = self.phys_vars.robot_finger_max_force
self.add_to_space(finger_motor)
self.finger_motors.append(finger_motor)
# For collision. Main body circle. Signature: Circle(body, radius,
# offset).
robot_group = 1
body_shape = pm.Circle(body, self.radius, (0, 0))
body_shape.filter = pm.ShapeFilter(group=robot_group)
body_shape.friction = 0.5
self.add_to_space(body_shape)
# the fingers
finger_shapes = []
for finger_body, finger_verts, finger_side in zip(
self.finger_bodies, finger_vertices, [-1, 1]):
finger_subshapes = []
for finger_subverts in finger_verts:
finger_subshape = pm.Poly(finger_body, finger_subverts)
finger_subshape.filter = pm.ShapeFilter(group=robot_group)
# grippy fingers
finger_subshape.friction = 5.0
finger_subshapes.append(finger_subshape)
self.add_to_space(*finger_subshapes)
finger_shapes.append(finger_subshapes)
# graphics setup
# draw a circular body
circ_body_in = r.make_circle(radius=self.radius - ROBOT_LINE_THICKNESS,
res=100)
circ_body_out = r.make_circle(radius=self.radius, res=100)
robot_colour = COLOURS_RGB['grey']
dark_robot_colour = darken_rgb(robot_colour)
light_robot_colour = lighten_rgb(robot_colour, 4)
circ_body_in.set_color(*robot_colour)
circ_body_out.set_color(*dark_robot_colour)
# draw the two fingers
self.finger_xforms = []
finger_outer_geoms = []
finger_inner_geoms = []
for finger_outer_subshapes, finger_inner_verts, finger_side in zip(
finger_shapes, finger_inner_vertices, [-1, 1]):
finger_xform = r.Transform()
self.finger_xforms.append(finger_xform)
for finger_subshape in finger_outer_subshapes:
vertices = [(v.x, v.y) for v in finger_subshape.get_vertices()]
finger_outer_geom = r.make_polygon(vertices)
finger_outer_geom.set_color(*robot_colour)
finger_outer_geom.add_attr(finger_xform)
finger_outer_geoms.append(finger_outer_geom)
for vertices in finger_inner_verts:
finger_inner_geom = r.make_polygon(vertices)
finger_inner_geom.set_color(*light_robot_colour)
finger_inner_geom.add_attr(finger_xform)
finger_inner_geoms.append(finger_inner_geom)
for geom in finger_outer_geoms:
self.viewer.add_geom(geom)
for geom in finger_inner_geoms:
self.viewer.add_geom(geom)
# draw some cute eyes
eye_shapes = []
self.pupil_transforms = []
for x_sign in [-1, 1]:
eye = r.make_circle(radius=0.2 * self.radius, res=20)
eye.set_color(1.0, 1.0, 1.0)
eye_base_transform = r.Transform().set_translation(
x_sign * 0.4 * self.radius, 0.3 * self.radius)
eye.add_attr(eye_base_transform)
pupil = r.make_circle(radius=0.12 * self.radius, res=10)
pupil.set_color(0.1, 0.1, 0.1)
# The pupils point forward slightly
pupil_transform = r.Transform()
pupil.add_attr(r.Transform().set_translation(
0, self.radius * 0.07))
pupil.add_attr(pupil_transform)
pupil.add_attr(eye_base_transform)
self.pupil_transforms.append(pupil_transform)
eye_shapes.extend([eye, pupil])
# join them together
self.robot_xform = r.Transform()
robot_compound = r.Compound([circ_body_out, circ_body_in, *eye_shapes])
robot_compound.add_attr(self.robot_xform)
self.viewer.add_geom(robot_compound)
def setup(self, *args, **kwargs):
super().setup(*args, **kwargs)
# Physics. This joint setup was taken form tank.py in the pymunk
# examples.
if self.shape_type == ShapeType.SQUARE:
self.shape_body = body = pm.Body()
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
side_len = math.sqrt(math.pi) * self.shape_size
shape = pm.Poly.create_box(
body,
(side_len, side_len),
# slightly bevelled corners
0.01 * side_len)
# FIXME: why is this necessary? Do I need it for the others?
shape.mass = self.mass
shapes = [shape]
del shape
elif self.shape_type == ShapeType.CIRCLE:
inertia = pm.moment_for_circle(self.mass, 0, self.shape_size,
(0, 0))
self.shape_body = body = pm.Body(self.mass, inertia)
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
shape = pm.Circle(body, self.shape_size, (0, 0))
shapes = [shape]
del shape
elif self.shape_type == ShapeType.STAR:
star_npoints = 5
star_out_rad = 1.3 * self.shape_size
star_in_rad = 0.5 * star_out_rad
star_verts = gtools.compute_star_verts(star_npoints, star_out_rad,
star_in_rad)
# create an exact convex decpomosition
convex_parts = autogeom.convex_decomposition(
star_verts + star_verts[:1], 0)
star_hull = autogeom.to_convex_hull(star_verts, 1e-5)
star_inertia = pm.moment_for_poly(self.mass, star_hull, (0, 0), 0)
self.shape_body = body = pm.Body(self.mass, star_inertia)
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
shapes = []
star_group = self.generate_group_id()
for convex_part in convex_parts:
shape = pm.Poly(body, convex_part)
# avoid self-intersection with a shape filter
shape.filter = pm.ShapeFilter(group=star_group)
shapes.append(shape)
del shape
else:
# these are free-form shapes b/c no helpers exist in Pymunk
if self.shape_type == ShapeType.TRIANGLE:
# shrink to make it look more sensible and easier to grasp
factor = 0.8
num_sides = 3
elif self.shape_type == ShapeType.PENTAGON:
factor = 1.0
num_sides = 5
elif self.shape_type == ShapeType.HEXAGON:
factor = 1.0
num_sides = 6
elif self.shape_type == ShapeType.OCTAGON:
factor = 1.0
num_sides = 8
else:
raise NotImplementedError("haven't implemented",
self.shape_type)
side_len = factor * gtools.regular_poly_circ_rad_to_side_length(
num_sides, self.shape_size)
poly_verts = gtools.compute_regular_poly_verts(num_sides, side_len)
inertia = pm.moment_for_poly(self.mass, poly_verts, (0, 0), 0)
self.shape_body = body = pm.Body(self.mass, inertia)
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
shape = pm.Poly(body, poly_verts)
shapes = [shape]
del shape
for shape in shapes:
shape.friction = 0.5
self.add_to_space(shape)
trans_joint = pm.PivotJoint(self.space.static_body, body, (0, 0),
(0, 0))
trans_joint.max_bias = 0
trans_joint.max_force = self.phys_vars.shape_trans_joint_max_force
self.add_to_space(trans_joint)
rot_joint = pm.GearJoint(self.space.static_body, body, 0.0, 1.0)
rot_joint.max_bias = 0
rot_joint.max_force = self.phys_vars.shape_rot_joint_max_force
self.add_to_space(rot_joint)
# Drawing
if self.shape_type == ShapeType.SQUARE:
geoms_inner = [r.make_square(side_len - 2 * SHAPE_LINE_THICKNESS)]
geoms_outer = [r.make_square(side_len)]
elif self.shape_type == ShapeType.CIRCLE:
geoms_inner = [
r.make_circle(radius=self.shape_size - SHAPE_LINE_THICKNESS,
res=100),
]
geoms_outer = [r.make_circle(radius=self.shape_size, res=100)]
elif self.shape_type == ShapeType.STAR:
star_short_verts = gtools.compute_star_verts(
star_npoints, star_out_rad - SHAPE_LINE_THICKNESS,
star_in_rad - SHAPE_LINE_THICKNESS)
short_convex_parts = autogeom.convex_decomposition(
star_short_verts + star_short_verts[:1], 0)
geoms_inner = []
for part in short_convex_parts:
geoms_inner.append(r.make_polygon(part))
geoms_outer = []
for part in convex_parts:
geoms_outer.append(r.make_polygon(part))
elif self.shape_type == ShapeType.OCTAGON \
or self.shape_type == ShapeType.HEXAGON \
or self.shape_type == ShapeType.PENTAGON \
or self.shape_type == ShapeType.TRIANGLE:
apothem = gtools.regular_poly_side_length_to_apothem(
num_sides, side_len)
short_side_len = gtools.regular_poly_apothem_to_side_legnth(
num_sides, apothem - SHAPE_LINE_THICKNESS)
short_verts = gtools.compute_regular_poly_verts(
num_sides, short_side_len)
geoms_inner = [r.make_polygon(short_verts)]
geoms_outer = [r.make_polygon(poly_verts)]
else:
raise NotImplementedError("haven't implemented", self.shape_type)
for g in geoms_inner:
g.set_color(*self.colour)
for g in geoms_outer:
g.set_color(*darken_rgb(self.colour))
self.shape_xform = r.Transform()
shape_compound = r.Compound(geoms_outer + geoms_inner)
shape_compound.add_attr(self.shape_xform)
self.viewer.add_geom(shape_compound)
def main():
# initialize the environment
pm.pygame_util.positive_y_is_up = False
pygame.init()
screen = pygame.display.set_mode((1600, 960))
clock = pygame.time.Clock()
space = pm.Space()
space.gravity = (0.0, 0.0)
# draw_options = pm.pygame_util.DrawOptions(screen)
fps = 144
static = [
pm.Segment(space.static_body, (0, 0), (0, 960), 5),
pm.Segment(space.static_body, (0, 960), (1600, 960), 5),
pm.Segment(space.static_body, (1600, 960), (1600, 0), 5),
pm.Segment(space.static_body, (0, 0), (1600, 0), 5),
]
for s in static:
s.collision_type = 0
space.add(static)
mouse_joint = None
mouse_body = pm.Body(body_type=pm.Body.KINEMATIC)
# Draw daylight map in to space
# Test code start##########Enable this to test in Pycharm################
f = open(
'D:/Delft Courses/Graduation/PythonProject/daylighthour_layout_pathfinding/4_5_info_exchange.txt',
mode='r',
encoding='utf-8')
daylighthours = []
for line in f:
line = line.strip()
daylighthours.append(int(line))
map_height = 96
map_width = 160
# Test code end########################################################
sdmap = site_daylight_map(daylighthours, map_height)
pixel_list = draw_map(space, sdmap, map_width, map_height)
# rooms_shape_list, spring_list = room_placement(screen, space, G, order)
rooms_shape_list, rooms_body_list, connection_list = room_placement_dlh(
screen, space, G, phase_index, rooms_shape_list_pos, body_type_list,
order)
spring_list = []
while True:
pause = False
for event in pygame.event.get():
if event.type == KEYUP:
if event.key == K_p:
pause = True
if event.type == QUIT:
exit()
elif event.type == KEYDOWN and event.key == K_ESCAPE:
exit()
elif event.type == MOUSEBUTTONDOWN:
if mouse_joint != None:
space.remove(mouse_joint)
mouse_joint = None
p = Vec2d(event.pos)
hit = space.point_query_nearest(p, 5, pm.ShapeFilter())
if hit != None and hit.shape.body.body_type == pm.Body.DYNAMIC:
shape = hit.shape
# Use the closest point on the surface if the click is outside
# of the shape.
if hit.distance > 0:
nearest = hit.point
else:
nearest = p
mouse_joint = pm.PivotJoint(
mouse_body, shape.body, (0, 0),
shape.body.world_to_local(nearest))
mouse_joint.max_force = 50000000
mouse_joint.error_bias = (1 - 0.15)**60
space.add(mouse_joint)
elif event.type == MOUSEBUTTONUP:
if mouse_joint != None:
space.remove(mouse_joint)
mouse_joint = None
while pause == True:
for event in pygame.event.get():
if event.type == KEYUP:
if event.key == K_p:
pause = False
if event.type == QUIT:
exit()
elif event.type == KEYDOWN and event.key == K_ESCAPE:
exit()
elif event.type == KEYDOWN and event.key == K_c:
spring_list = connect_rooms(space, G, rooms_body_list,
order, phase_index)
elif event.type == KEYDOWN and event.key == K_g: # 判断按下g
rooms_shape_list_pos_out = []
body_type_list_out = []
room_info_lst = []
order_generate = []
for shape in rooms_shape_list:
i = rooms_shape_list.index(shape)
room = order[i]
v = shape.body.position
vx = int(v.x)
vy = int(v.y)
body_type = shape.body.body_type
shape_pos = (vx, vy)
room_info = (room, shape_pos, body_type, i, vy)
room_info_lst.append(room_info)
order_info_lst = sorted(room_info_lst,
key=lambda x: (-x[4], x[3]))
for i in range(len(order_info_lst)):
room_node = order_info_lst[i][0]
shape_pos_order = order_info_lst[i][1]
body_type_order = order_info_lst[i][2]
order_generate.append(room_node)
rooms_shape_list_pos_out.append(shape_pos_order)
body_type_list_out.append(body_type_order)
f = open(
'D:/Delft Courses/Graduation/PythonProject/daylighthour_layout_pathfinding/4_5_info_exchange.txt',
mode='w',
encoding='utf-8')
f.write(f'{phase_index}')
f.write('\n')
f.write(f'{rooms_shape_list_pos_out}')
f.write('\n')
f.write(f'{body_type_list_out}')
f.write('\n')
f.write(f'{order_generate}')
f.write('\n')
f.write(f'{rooms}')
f.write('\n')
f.write(f'{sizes}')
f.write('\n')
f.write(f'{lights}')
f.write('\n')
f.write(f'{door_direction}')
f.write('\n')
f.write(f'{circulations}')
f.write('\n')
f.write(f'{cir_Weight}')
for shape in rooms_shape_list:
i = rooms_shape_list.index(shape)
v = shape.body.position
x = order[i]
name = G.nodes[x]['room']
print(f'{name} pos = ', v)
screen.fill(pygame.color.THECOLORS["white"])
draw_map_pg(screen, pixel_list)
draw_room(screen, G, rooms_shape_list, connection_list)
draw_helptext(screen)
mouse_pos = pygame.mouse.get_pos()
mouse_body.position = mouse_pos
space.step(1. / fps)
# space.debug_draw(draw_options)
pygame.display.flip()
clock.tick(fps)
pygame.display.set_caption("fps: " + str(clock.get_fps()))
def __init__(self, space: pymunk.Space, theta: int = 0):
self.space = space
# Define initial angle
angle = Angle(theta)
# Define bodies
self.top = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.bottom = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.rod1 = pymunk.Body(5, 10000)
self.rod2 = pymunk.Body(5, 10000)
self.rod3 = pymunk.Body(5, 10000)
self.seat = pymunk.Body(10, 10000)
self.torso = pymunk.Body(20, 10000)
self.legs = pymunk.Body(20, 10000)
self.head = pymunk.Body(10, 10000)
# Create shapes
top_shape = pymunk.Poly.create_box(self.top, size=(1200,50))
bottom_shape = pymunk.Poly.create_box(self.bottom, size=(1200,50))
rod1_shape = pymunk.Segment(self.rod1, (0,0), angle.rod1, radius=3)
rod2_shape = pymunk.Segment(self.rod2, (0,0), angle.rod2, radius=3)
rod3_shape = pymunk.Segment(self.rod3, (0,0), angle.rod3, radius=3)
seat_shape = pymunk.Segment(self.seat, angle.seat[0], angle.seat[1], radius=5)
torso_shape = pymunk.Poly.create_box(self.torso, size=(10,100))
legs_shape = pymunk.Poly.create_box(self.legs, size=(10,50))
head_shape = pymunk.Circle(self.head, radius=20)
# Set layer of shapes
rod1_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
rod2_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
rod3_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
seat_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
torso_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
legs_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
head_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# Set positions of bodies
self.top.position = (600, 720)
self.bottom.position = (600, 0)
self.rod1.position = (600, 695)
self.rod2.position = angle.point1
self.rod3.position = angle.point2
self.seat.position = angle.point4
self.torso.position = (angle.point4[0] + angle.seat[1][0] + 50 * math.sin(angle.theta), angle.point4[1] + angle.seat[1][1] + 50 * math.cos(angle.theta))
self.torso._set_angle(-angle.theta)
self.legs.position = (angle.point4[0] + angle.seat[0][0] - 25 * math.sin(angle.theta), angle.point4[1] + angle.seat[0][1] - 25 * math.cos(angle.theta))
self.legs._set_angle(-angle.theta)
self.head.position = (angle.point4[0] + angle.seat[1][0] + 90 * math.sin(angle.theta), angle.point4[1] + angle.seat[1][1] + 90 * math.cos(angle.theta))
# Create pivot joints for bodies
self.pivot1 = pymunk.PivotJoint(self.top, self.rod1, (600, 695))
self.pivot2 = pymunk.PivotJoint(self.rod1, self.rod2, angle.point1)
self.pivot3 = pymunk.PivotJoint(self.rod2, self.rod3, angle.point2)
self.pivot4 = pymunk.PivotJoint(self.rod3, self.seat, angle.point4)
self.pivot5 = pymunk.PinJoint(self.rod3, self.seat, anchor_a=angle.rod3, anchor_b=angle.seat[0])
self.pivot6 = pymunk.PinJoint(self.rod3, self.seat, anchor_a=angle.rod3, anchor_b=angle.seat[1])
self.pivot7 = pymunk.PivotJoint(self.torso, self.seat, (angle.point4[0] + angle.seat[1][0], angle.point4[1] + angle.seat[1][1]))
self.pivot8 = pymunk.PivotJoint(self.seat, self.legs, (angle.point4[0] + angle.seat[0][0], angle.point4[1] + angle.seat[0][1]))
self.gear1 = pymunk.GearJoint(self.torso, self.seat, angle.theta, 1.0)
self.gear2 = pymunk.GearJoint(self.seat, self.legs, -angle.theta, 1.0)
self.pivot9 = pymunk.PivotJoint(self.head, self.torso, self.head.position)
# pin = pymunk.DampedSpring(self.rod3, self.torso, anchor_a=(0,0), anchor_b=(0,0), rest_length=100, stiffness=5, damping=0.1)#, min=20, max=100)
# Add damping
# damping1 = pymunk.DampedSpring(self.top, self.rod1, anchor_a=(-600,-25), anchor_b=(0,-rod1_length / 2), rest_length=100, stiffness=5, damping=3)
# damping2 = pymunk.DampedSpring(self.top, self.rod1, anchor_a=(600,-25), anchor_b=(0,-rod1_length / 2), rest_length=100, stiffness=5, damping=3)
# Disable collisions between rods
self.pivot1.collide_bodies = False
self.pivot2.collide_bodies = False
self.pivot3.collide_bodies = False
self.pivot4.collide_bodies = False
self.pivot7.collide_bodies = False
self.pivot8.collide_bodies = False
self.pivot9.collide_bodies = False
# Add bodies and pivots to space
self.space.add(self.top, top_shape, self.bottom, bottom_shape, self.rod1, rod1_shape, self.rod2, rod2_shape, self.rod3, rod3_shape, self.seat, seat_shape, self.torso, torso_shape, self.legs, legs_shape, self.head, head_shape, self.pivot1, self.pivot2, self.pivot3, self.pivot4, self.pivot5, self.pivot6, self.pivot7, self.pivot8, self.pivot9, self.gear1, self.gear2)
def add_arm(space, config={}):
default_config = {
"arm_center": (300, 300),
"lower_arm_length": 200,
"lower_arm_starting_angle": 15,
"lower_arm_mass": 10,
"brach_rest_length": 5,
"brach_stiffness": 450,
"brach_damping": 200,
"tricep_rest_length": 30,
"tricep_stiffness": 50,
"tricep_damping": 400
}
# Like dict.update() if it had an overwrite=False option
config.update({
k: v
for k, v in default_config.items() if k not in list(config.keys())
})
# Upper Arm
upper_arm_length = 200
upper_arm_body = pymunk.Body(body_type=pymunk.Body.STATIC)
upper_arm_body.position = config["arm_center"]
upper_arm_body.angle = np.deg2rad(-45)
upper_arm_line = pymunk.Segment(upper_arm_body, (0, 0),
(-upper_arm_length, 0), 5)
upper_arm_line.sensor = True # Disable collision
space.add(upper_arm_body)
space.add(upper_arm_line)
# Lower Arm
lower_arm_body = pymunk.Body(
0, 0) # Pymunk will calculate moment based on mass of attached shape
lower_arm_body.position = config["arm_center"]
lower_arm_body.angle = np.deg2rad(config["lower_arm_starting_angle"])
elbow_extension_length = 20
lower_arm_start = (-elbow_extension_length, 0)
lower_arm_line = pymunk.Segment(lower_arm_body, lower_arm_start,
(config["lower_arm_length"], 0), 5)
lower_arm_line.mass = config["lower_arm_mass"]
lower_arm_line.friction = 1.0
space.add(lower_arm_body)
space.add(lower_arm_line)
# Pivot (Elbow)
elbow_body = pymunk.Body(body_type=pymunk.Body.STATIC)
elbow_body.position = config["arm_center"]
elbow_joint = pymunk.PivotJoint(elbow_body, lower_arm_body,
config["arm_center"])
space.add(elbow_joint)
# Spring (Brachialis Muscle)
brach_spring = pymunk.constraint.DampedSpring(
upper_arm_body,
lower_arm_body,
(-(upper_arm_length *
(1 / 2)), 0), # Connect half way up the upper arm
(config["lower_arm_length"] / 5,
0), # Connect near the bottom of the lower arm
config["brach_rest_length"],
config["brach_stiffness"],
config["brach_damping"])
space.add(brach_spring)
# Spring (Tricep Muscle)
tricep_spring = pymunk.constraint.DampedSpring(
upper_arm_body, lower_arm_body, (-(upper_arm_length * (3 / 4)), 0),
lower_arm_start, config["tricep_rest_length"],
config["tricep_stiffness"], config["tricep_damping"])
space.add(tricep_spring)
# Elbow stop (prevent under/over extension)
elbow_stop_point = pymunk.Circle(upper_arm_body,
radius=5,
offset=(-elbow_extension_length, -3))
elbow_stop_point.friction = 1.0
space.add(elbow_stop_point)
return brach_spring, tricep_spring
def __init__(self, wheel_radius=30, speed=0, wheel_type="round"):
momentum = 1
body_size = (200, 40)
moment = pymunk.moment_for_box(momentum, body_size)
self.body = pymunk.Body(momentum, moment)
self.shape = pymunk.Poly.create_box(self.body, body_size)
self.shape.mass = 5
self.body.position = (200, 200)
self.shape.body = self.body
self.shape.color = (0, 0, 0, 255)
human_moment = pymunk.moment_for_box(momentum, (30, 50))
self.human_body = pymunk.Body(momentum, human_moment)
self.human_shape = pymunk.Poly.create_box(self.human_body, (30, 50))
self.human_body.position = (self.body.position.x,
self.body.position.y + 70)
self.human_shape.body = self.human_body
self.human_shape.color = (255, 0, 0, 255)
self.human_joint = pymunk.PivotJoint(self.human_body, self.body,
(-15, -20), (-15, 20))
self.human_joint2 = pymunk.PivotJoint(self.human_body, self.body,
(15, -20), (15, 20))
head_moment = pymunk.moment_for_circle(momentum, 1, 1)
self.head_body = pymunk.Body(momentum, head_moment)
self.head_body.position = (self.human_body.position.x,
self.human_body.position.y + 70)
self.head_shape = pymunk.Circle(self.head_body, 30)
self.head_joint = pymunk.PivotJoint(self.head_body, self.human_body,
(-10, -30), (-10, 20))
self.head_joint2 = pymunk.PivotJoint(self.head_body, self.human_body,
(10, -30), (10, 20))
self.head_shape.collision_type = 0
momentum = 100
wheel_moment = pymunk.moment_for_circle(momentum, 10, wheel_radius)
self.wheel1_body = pymunk.Body(momentum, wheel_moment)
self.wheel1_body.position = self.body.position + (-60, -20)
self.wheel1_shape = pymunk.Circle(self.wheel1_body, wheel_radius)
self.wheel2_body = pymunk.Body(momentum, wheel_moment)
self.wheel2_body.position = self.body.position + (60, -20)
self.wheel2_shape = pymunk.Circle(self.wheel2_body, wheel_radius)
self.wheel1_joint = pymunk.PivotJoint(self.body, self.wheel1_body,
(-60, -20), (0, 0))
self.wheel1_motor = pymunk.SimpleMotor(self.wheel1_body, self.body, 0)
self.wheel2_joint = pymunk.PivotJoint(self.body, self.wheel2_body,
(60, -20), (0, 0))
self.wheel1_shape.friction = 1
self.wheel1_shape.color = (200, 200, 200)
self.wheel2_shape.friction = 1
self.wheel2_shape.color = (200, 200, 200)
space.add(self.body, self.shape)
space.add(self.human_body, self.human_shape, self.human_joint,
self.human_joint2)
space.add(self.head_body, self.head_shape, self.head_joint,
self.head_joint2)
space.add(self.wheel1_body, self.wheel1_shape, self.wheel1_joint,
self.wheel1_motor)
space.add(self.wheel2_body, self.wheel2_shape, self.wheel2_joint)
shape_filter = pymunk.ShapeFilter(group=1)
self.shape.filter = shape_filter
self.human_shape.filter = shape_filter
self.head_shape.filter = shape_filter
self.wheel1_shape.filter = shape_filter
self.wheel1_motor.filter = shape_filter
self.wheel2_shape.filter = shape_filter
self.is_dead = False
self.tick = 0
self.prev_dist = 9999
self.speed = speed
self.face = pygame.image.load("normal.png")
self.face = pygame.transform.scale(self.face, (90, 90))
self.check = 0
self.prev_check = 0
self.timer = 0
def main():
N = int(DURATION / DT) + 1
space = pymunk.Space()
space.gravity = (0, 0)
box_body = pymunk.Body()
box_body.position = (1, 0.3)
box_r = 0.5
box_corners = [(-box_r, box_r), (-box_r, -box_r), (box_r, -box_r),
(box_r, box_r)]
box = pymunk.Poly(box_body, box_corners, radius=0.01)
# box = pymunk.Circle(box_body, 0.5)
box.mass = M
box.friction = 0.5
box.collision_type = 1
space.add(box.body, box)
# linear friction
pivot = pymunk.PivotJoint(space.static_body, box.body, box.body.position)
pivot.max_force = MU * M * G # μmg
pivot.max_bias = 0 # disable correction so that body doesn't bounce back
space.add(pivot)
# angular friction
gear = pymunk.GearJoint(space.static_body, box.body, 0, 1)
gear.max_force = 2
gear.max_bias = 0
space.add(gear)
control_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
control_body.position = (0, 0)
control_shape = pymunk.Circle(control_body, 0.1, (0, 0))
control_shape.friction = 1
control_shape.collision_type = 1
space.add(control_shape.body, control_shape)
watcher = CollisionWatcher()
space.add_collision_handler(1, 1).post_solve = watcher.update
plt.ion()
fig = plt.figure()
ax = plt.gca()
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.set_xlim([-5, 5])
ax.set_ylim([-3, 3])
ax.grid()
ax.set_aspect('equal')
options = pymunk.matplotlib_util.DrawOptions(ax)
options.flags = pymunk.SpaceDebugDrawOptions.DRAW_SHAPES
space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
control_body.velocity = (0.25, 0)
for i in range(N - 1):
t = i * DT
# step the sim
space.step(DT)
if i % PLOT_PERIOD == 0:
ax.cla()
ax.set_xlim([-5, 5])
ax.set_ylim([-3, 3])
ax.grid()
space.debug_draw(options)
fig.canvas.draw()
fig.canvas.flush_events()
def testMaxForce(self):
a, b = p.Body(10, 10), p.Body(10, 10)
j = p.PivotJoint(a, b, (0, 0))
self.assertEqual(j.max_force, p.inf)
j.max_force = 10
self.assertEqual(j.max_force, 10)
def addToSpace(self, space):
line_width = 4.0
p1 = Vec2d(-self.width / 2.0, self.depth / 2.0)
p2 = Vec2d(-self.width / 2.0, -self.depth / 2.0)
p3 = Vec2d(self.width / 2.0, -self.depth / 2.0)
p4 = Vec2d(self.width / 2.0, self.depth / 2.0)
p5 = Vec2d(-self.width / 2.0,
self.depth / 2.0 + line_width * 2) # left door joint
t1 = p1.rotated(
self.rotation
) + self.position #transform(p1, self.position, self.rotation)
t2 = p2.rotated(
self.rotation
) + self.position #transform(p2, self.position, self.rotation)
t3 = p3.rotated(
self.rotation
) + self.position #transform(p3, self.position, self.rotation)
t4 = p4.rotated(
self.rotation
) + self.position #transform(p4, self.position, self.rotation)
t5 = p5.rotated(
self.rotation
) + self.position #transform(p5, self.position, self.rotation)
static = [
pymunk.Segment(space.static_body, t1, t2, line_width),
pymunk.Segment(space.static_body, t2, t3, line_width),
pymunk.Segment(space.static_body, t3, t4, line_width)
]
for s in static:
s.friction = 1.
s.group = 1
space.add(static)
# samples for door
# each sample contains information about surface point, normal and maximum radius of contacting body
self.door_samples = []
for t in np.linspace(0.1, 0.9, 5, endpoint=True):
self.door_samples.append(
(Vec2d(self.width / 2.0 * t,
-line_width / 2.0), Vec2d(0, -1), float("inf")))
radius_tab = [100, 50, 15, 5, 5]
idx = 0
for t in np.linspace(0.1, 0.9, 5, endpoint=True):
self.door_samples.append(
(Vec2d(self.width / 2.0 * t,
line_width / 2.0), Vec2d(0, 1), radius_tab[idx]))
idx += 1
self.door_samples.append((Vec2d(self.width / 2.0 * 0.8 - 4 * 2.0,
4 * 4.0), Vec2d(0, 1), float("inf")))
self.door_samples.append((Vec2d(self.width / 2.0 * 0.8 - 4 * 2.0,
4 * 4.0), Vec2d(0, -1), 5))
# polygon for left door
p6 = Vec2d(0, -line_width / 2.0)
p7 = Vec2d(0, line_width / 2.0)
p8 = Vec2d(self.width / 2.0 - line_width, line_width / 2.0)
p9 = Vec2d(self.width / 2.0 - line_width, -line_width / 2.0)
# ph1 = Vec2d(self.width/2.0*0.9, line_width/2.0)
# ph2 = Vec2d(self.width/2.0*0.9, line_width*4.0)
# ph3 = Vec2d(self.width/2.0*0.9-line_width*4.0, line_width*4.0)
# ph4 = Vec2d(self.width/2.0*0.9-line_width*4.0, line_width*3.0)
# ph5 = Vec2d(self.width/2.0*0.9-line_width*1.0, line_width*3.0)
# ph6 = Vec2d(self.width/2.0*0.9-line_width*1.0, line_width/2.0)
self.fp = [p9, p8, p7, p6]
mass = 100.0
moment = pymunk.moment_for_poly(mass, self.fp)
# left door
self.l_door_body = pymunk.Body(mass, moment)
self.l_door_body.position = t5
self.l_door_body.angle = self.rotation
l_door_shape = pymunk.Poly(self.l_door_body, self.fp, radius=1)
l_door_shape.group = 1
# handle
left_h1_shape = pymunk.Segment(
self.l_door_body, Vec2d(self.width / 2.0 * 0.8, line_width * 0.5),
Vec2d(self.width / 2.0 * 0.8, line_width * 4.0), line_width)
left_h1_shape.group = 1
left_h2_shape = pymunk.Segment(
self.l_door_body,
Vec2d(self.width / 2.0 * 0.8 - line_width * 4.0, line_width * 4.0),
Vec2d(self.width / 2.0 * 0.8, line_width * 4.0), line_width)
left_h2_shape.group = 1
space.add(self.l_door_body, l_door_shape, left_h1_shape, left_h2_shape)
j = pymunk.PivotJoint(self.l_door_body, space.static_body,
self.l_door_body.position)
s = pymunk.DampedRotarySpring(self.l_door_body, space.static_body, 0,
0, 90000)
space.add(j, s)
j2 = pymunk.RotaryLimitJoint(self.l_door_body, space.static_body,
-self.rotation - math.pi / 2.0,
-self.rotation)
space.add(j2)
def testErrorBias(self):
a, b = p.Body(10, 10), p.Body(10, 10)
j = p.PivotJoint(a, b, (0, 0))
self.assertAlmostEqual(j.error_bias, pow(1.0 - 0.1, 60.0))
j.error_bias = 0.3
self.assertEqual(j.error_bias, 0.3)
b2 = add_ball(space, (150, 60), box_offset) c = pymunk.PinJoint(b1, b2, (20, 0), (-20, 0)) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size, 0 b1 = add_ball(space, (50, 60), box_offset) b2 = add_ball(space, (150, 60), box_offset) c = pymunk.SlideJoint(b1, b2, (20, 0), (-20, 0), 40, 80) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size * 2, 0 b1 = add_ball(space, (50, 60), box_offset) b2 = add_ball(space, (150, 60), box_offset) c = pymunk.PivotJoint(b1, b2, Vec2d(box_offset) + (100, 60)) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size * 3, 0 b1 = add_ball(space, (50, 60), box_offset) b2 = add_ball(space, (150, 60), box_offset) c = pymunk.GrooveJoint(b1, b2, (50, 50), (50, -50), (-50, 0)) txts[box_offset] = inspect.getdoc(c) space.add(c) box_offset = box_size * 4, 0 b1 = add_ball(space, (50, 60), box_offset) b2 = add_ball(space, (150, 60), box_offset) c = pymunk.DampedSpring(b1, b2, (30, 0), (-30, 0), 20, 5, 0.3) txts[box_offset] = inspect.getdoc(c)
def testCollideBodies(self):
a, b = p.Body(10, 10), p.Body(10, 10)
j = p.PivotJoint(a, b, (0, 0))
self.assertEqual(j.collide_bodies, True)
j.collide_bodies = False
self.assertEqual(j.collide_bodies, False)
def render(self):
running = True
mpos = pygame.mouse.get_pos()
self.mouse_body.position = self.from_pygame(Vec2d(mpos))
self.mouse_body.angle = 0
self.mouse_body.angular_velocity = 0
for event in pygame.event.get():
if event.type == QUIT:
running = False
elif event.type == KEYDOWN and event.key == K_ESCAPE:
running = False
elif event.type == MOUSEBUTTONDOWN and event.button == 1: # LMB
#selected = space.point_query_first(self.from_pygame(Vec2d(mpos)))
#if selected != None:
p1_body = self.players[0].body
p1_body.position = self.mouse_body.position
self.joint1 = pymunk.PivotJoint(self.mouse_body, p1_body,
(0, 0), (0, 0))
self.space.add(self.joint1)
elif event.type == MOUSEBUTTONUP:
if self.joint1 != None:
self.space.remove(self.joint1)
self.joint1 = None
### Clear screen
self.screen.fill(THECOLORS["black"])
### Draw
self.draw_table()
for puck in self.pucks:
p = self.to_pygame(puck.body.position)
if p[0] < 0 or p[0] > self.width:
self.remove_puck(puck)
self.add_puck()
pygame.draw.circle(self.screen, THECOLORS["red"], p,
int(puck.radius), 0)
for p_shape in self.players:
p_body = p_shape.body
p_body.angular_velocity = 0
p = self.to_pygame(p_body.position)
pygame.draw.circle(self.screen, THECOLORS["darkgreen"], p,
int(p_shape.radius), 0)
pygame.draw.circle(self.screen, THECOLORS["black"], p,
int(p_shape.radius + 1), 2)
pygame.draw.circle(self.screen, THECOLORS["black"], p,
int(p_shape.radius / 2), 1)
### Update physics
dt = 1.0 / 60.0 / 5.
for x in range(5):
self.space.step(dt)
### Flip screen
pygame.display.flip()
self.clock.tick(50)
pygame.display.set_caption("fps: " + str(self.clock.get_fps()))
return running
def load_elements(self):
self.add_element(self.position, (22, 20), 60.0,
pg.Rect(58, 32, 30, 25))
self.add_element(self.position + pm.Vec2d(0, -14), (12, 12), 4.0,
pg.Rect(58, 106, 12, 12))
self.add_element(self.position + pm.Vec2d(-11, -8), (11, 7), 2.0,
pg.Rect(85, 20, 11, 7))
self.add_element(self.position + pm.Vec2d(-19, -8), (14, 8), 2.0,
pg.Rect(71, 19, 14, 8))
self.add_element(self.position + pm.Vec2d(-6, 14), (11, 12), 8.0,
pg.Rect(181, 64, 11, 12))
self.add_element(self.position + pm.Vec2d(-5, 25), (11, 12), 4.0,
pg.Rect(181, 76, 11, 12))
self.add_element(self.position + pm.Vec2d(11, -8), (11, 7), 2.0,
pg.Rect(160, 20, 11, 7))
self.add_element(self.position + pm.Vec2d(19, -8), (14, 8), 2.0,
pg.Rect(171, 19, 14, 8))
self.add_element(self.position + pm.Vec2d(6, 14), (11, 12), 8.0,
pg.Rect(64, 64, 11, 12))
self.add_element(self.position + pm.Vec2d(5, 25), (11, 12), 4.0,
pg.Rect(64, 76, 11, 12))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[1][0],
self.position + pm.Vec2d(0, -11)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[1][0],
self._elements[0][0].position + pm.Vec2d(0, -15)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[2][0],
self.position + pm.Vec2d(-10, -7)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[2][0],
math.radians(-90), math.radians(90)))
self.add_joint(
pm.PivotJoint(self._elements[2][0], self._elements[3][0],
self.position + pm.Vec2d(-16, -11)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[2][0], self._elements[3][0],
math.radians(0), math.radians(110)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[4][0],
self.position + pm.Vec2d(-5, 10)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[4][0],
math.radians(0), math.radians(90)))
self.add_joint(
pm.PivotJoint(self._elements[4][0], self._elements[5][0],
self.position + pm.Vec2d(-4, 18)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[4][0], self._elements[5][0],
math.radians(-110), math.radians(0)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[6][0],
self.position + pm.Vec2d(10, -7)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[6][0],
math.radians(-90), math.radians(90)))
self.add_joint(
pm.PivotJoint(self._elements[6][0], self._elements[7][0],
self.position + pm.Vec2d(16, -8)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[6][0], self._elements[7][0],
math.radians(-110), math.radians(0)))
self.add_joint(
pm.PivotJoint(self._elements[0][0], self._elements[8][0],
self.position + pm.Vec2d(5, 10)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[0][0], self._elements[8][0],
math.radians(-90), math.radians(0)))
self.add_joint(
pm.PivotJoint(self._elements[8][0], self._elements[9][0],
self.position + pm.Vec2d(4, 18)))
self.add_joint(
pm.RotaryLimitJoint(self._elements[8][0], self._elements[9][0],
math.radians(0), math.radians(110)))
def fill_space(space, custom_color=(255, 255, 0, 255)):
captions = []
### Static
captions.append(((50, 680), "Static Shapes"))
# Static Segments
segments = [
pymunk.Segment(space.static_body, (10, 400), (10, 600), 0),
pymunk.Segment(space.static_body, (20, 400), (20, 600), 1),
pymunk.Segment(space.static_body, (30, 400), (30, 600), 3),
pymunk.Segment(space.static_body, (50, 400), (50, 600), 5),
]
space.add(*segments)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (40, 630)
b.angle = 3.14 / 7
s = pymunk.Segment(b, (-30, 0), (30, 0), 2)
space.add(b, s)
# Static Circles
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 630)
s = pymunk.Circle(b, 10)
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 630)
s = pymunk.Circle(b, 10, (-30, 0))
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 560)
b.angle = 3.14 / 4
s = pymunk.Circle(b, 40)
space.add(b, s)
# Static Polys
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 460)
b.angle = 3.14 / 4
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)])
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (120, 500)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], t, radius=1)
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (50, 430)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(
b,
[
(0.0, -30.0),
(19.0, -23.0),
(30.0, -5.0),
(26.0, 15.0),
(10.0, 28.0),
(-10.0, 28.0),
(-26.0, 15.0),
(-30.0, -5.0),
(-19.0, -23.0),
],
t,
)
space.add(b, s)
### Kinematic
captions.append(((220, 680), "Kinematic Shapes"))
# Kinematic Segments
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
segments = [
pymunk.Segment(b, (180, 400), (180, 600), 0),
pymunk.Segment(b, (190, 400), (190, 600), 1),
pymunk.Segment(b, (200, 400), (200, 600), 3),
pymunk.Segment(b, (220, 400), (220, 600), 5),
]
space.add(b, *segments)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (210, 630)
b.angle = 3.14 / 7
s = pymunk.Segment(b, (-30, 0), (30, 0), 2)
space.add(b, s)
# Kinematic Circles
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 630)
s = pymunk.Circle(b, 10)
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 630)
s = pymunk.Circle(b, 10, (-30, 0))
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 560)
b.angle = 3.14 / 4
s = pymunk.Circle(b, 40)
space.add(b, s)
# Kinematic Polys
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 460)
b.angle = 3.14 / 4
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)])
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (290, 500)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)], t, radius=3)
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (230, 430)
t = pymunk.Transform(ty=-100)
s = pymunk.Poly(
b,
[
(19.0, -50.0),
(30.0, -5.0),
(26.0, 15.0),
(10.0, 38.0),
(-10.0, 38.0),
(-26.0, 15.0),
(-30.0, -5.0),
(-19.0, -50.0),
],
t,
)
space.add(b, s)
### Dynamic
captions.append(((390, 680), "Dynamic Shapes"))
# Dynamic Segments
b = pymunk.Body(1, 1)
segments = [
pymunk.Segment(b, (350, 400), (350, 600), 0),
pymunk.Segment(b, (360, 400), (360, 600), 1),
pymunk.Segment(b, (370, 400), (370, 600), 3),
pymunk.Segment(b, (390, 400), (390, 600), 5),
]
space.add(b, *segments)
b = pymunk.Body(1, 1)
b.position = (380, 630)
b.angle = 3.14 / 7
s = pymunk.Segment(b, (-30, 0), (30, 0), 7)
space.add(b, s)
# Dynamic Circles
b = pymunk.Body(1, 1)
b.position = (460, 630)
s = pymunk.Circle(b, 10)
space.add(b, s)
b = pymunk.Body(1, 1)
b.position = (460, 630)
s = pymunk.Circle(b, 10, (-30, 0))
space.add(b, s)
b = pymunk.Body(1, 1)
b.position = (460, 560)
b.angle = 3.14 / 4
s = pymunk.Circle(b, 40)
space.add(b, s)
# Dynamic Polys
b = pymunk.Body(1, 1)
b.position = (460, 460)
b.angle = 3.14 / 4
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)])
space.add(b, s)
b = pymunk.Body(1, 1)
b.position = (460, 500)
s = pymunk.Poly(b, [(0, -25), (30, 25), (-30, 25)],
pymunk.Transform(ty=-100),
radius=5)
space.add(b, s)
b = pymunk.Body(1, 1)
b.position = (400, 430)
s = pymunk.Poly(b, [(0, -50), (50, 0), (30, 50), (-30, 50), (-50, 0)],
pymunk.Transform(ty=-100))
space.add(b, s)
###Constraints
# PinJoints
captions.append(((560, 660), "Pin Joint"))
a = pymunk.Body(1, 1)
a.position = (550, 600)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 620)
sb = pymunk.Circle(b, 20)
j = pymunk.PinJoint(a, b, anchor_a=(0, 0), anchor_b=(0, -20))
space.add(sa, sb, a, b, j)
# SlideJoints
captions.append(((560, 560), "Slide Joint"))
a = pymunk.Body(1, 1)
a.position = (550, 500)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 520)
sb = pymunk.Circle(b, 20)
j = pymunk.SlideJoint(a,
b,
anchor_a=(0, 20),
anchor_b=(0, -20),
min=10,
max=30)
space.add(sa, sb, a, b, j)
# PivotJoints
captions.append(((560, 460), "Pivot Joint"))
a = pymunk.Body(1, 1)
a.position = (550, 400)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (650, 420)
sb = pymunk.Circle(b, 20)
j = pymunk.PivotJoint(a, b, (600, 390))
space.add(sa, sb, a, b, j)
# GrooveJoints
captions.append(((760, 660), "Groove Joint"))
a = pymunk.Body(1, 1)
a.position = (750, 600)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 620)
sb = pymunk.Circle(b, 20)
j = pymunk.GrooveJoint(a, b, (40, 40), (40, -40), (-60, 0))
space.add(sa, sb, a, b, j)
# DampedSpring
captions.append(((760, 550), "Damped Spring"))
a = pymunk.Body(1, 1)
a.position = (750, 480)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 500)
sb = pymunk.Circle(b, 20)
j = pymunk.DampedSpring(a, b, (0, 0), (0, 10), 100, 1, 1)
space.add(sa, sb, a, b, j)
# DampedRotarySpring
captions.append(((740, 430), "Damped Rotary Spring"))
a = pymunk.Body(1, 1)
a.position = (750, 350)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 380)
sb = pymunk.Circle(b, 20)
j = pymunk.DampedRotarySpring(a, b, 10, 1, 1)
space.add(sa, sb, a, b, j)
# RotaryLimitJoint
captions.append(((740, 300), "Rotary Limit Joint"))
a = pymunk.Body(1, 1)
a.position = (750, 220)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 250)
sb = pymunk.Circle(b, 20)
j = pymunk.RotaryLimitJoint(a, b, 1, 2)
b.angle = 3
space.add(sa, sb, a, b, j)
# RatchetJoint
captions.append(((740, 170), "Ratchet Joint"))
a = pymunk.Body(1, 1)
a.position = (750, 100)
sa = pymunk.Circle(a, 20)
b = pymunk.Body(1, 1)
b.position = (850, 120)
sb = pymunk.Circle(b, 20)
j = pymunk.RatchetJoint(a, b, 1, 0.1)
b.angle = 3
space.add(sa, sb, a, b, j)
# GearJoint and SimpleMotor omitted since they are similar to the already
# added joints
# TODO: more stuff here :)
### Other
# Objects in custom color
captions.append(((150, 150), "Custom Color (static, kinematic & dynamic)"))
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (200, 200)
s = pymunk.Circle(b, 30)
s.color = custom_color
space.add(b, s)
b = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
b.position = (300, 200)
s = pymunk.Circle(b, 30)
s.color = custom_color
space.add(b, s)
b = pymunk.Body(1, 1)
b.position = (400, 200)
s = pymunk.Circle(b, 30)
s.color = custom_color
space.add(b, s)
# Collision
captions.append(((550, 150), "Collisions"))
b = pymunk.Body(body_type=pymunk.Body.STATIC)
b.position = (570, 200)
s = pymunk.Circle(b, 40)
space.add(b, s)
b = pymunk.Body(1, 1)
b.position = (590, 250)
s = pymunk.Circle(b, 40)
space.add(b, s)
# Sleeping
captions.append(((50, 150), "Sleeping"))
b = pymunk.Body(1, 1)
b.position = (75, 200)
space.sleep_time_threshold = 0.01
s = pymunk.Circle(b, 40)
space.add(s, b)
b.sleep()
space.step(0.000001)
return captions
if mouse_joint != None:
space.remove(mouse_joint)
mouse_joint = None
p = Vec2d(event.pos)
hit = space.point_query_nearest(p, 5, pymunk.ShapeFilter())
if hit != None and hit.shape.body.body_type == pymunk.Body.DYNAMIC:
shape = hit.shape
# Use the closest point on the surface if the click is outside
# of the shape.
if hit.distance > 0:
nearest = hit.point
else:
nearest = p
mouse_joint = pymunk.PivotJoint(
mouse_body, shape.body, (0, 0),
shape.body.world_to_local(nearest))
mouse_joint.max_force = 50000
mouse_joint.error_bias = (1 - 0.15)**60
space.add(mouse_joint)
elif event.type == MOUSEBUTTONUP:
if mouse_joint != None:
space.remove(mouse_joint)
mouse_joint = None
for b in space.bodies:
b.force = -b.velocity
screen.fill(pygame.color.THECOLORS["white"])
def build(space, group):
scale = 2
normal_rect = pygame.Rect(0, 0, 32 * scale, 40 * scale)
model = CatModel()
sprites = list()
pymunk_objects = list()
seat_surface = resources.gfx("seat.png", convert_alpha=True)
feet_surface = resources.gfx("wheel.png", convert_alpha=True)
filter1 = pymunk.ShapeFilter(group=0b000001)
filter2 = pymunk.ShapeFilter(group=0b000010)
# Main body
body_body = pymunk.Body(0.00001, pymunk.inf)
body_body.position = normal_rect.topleft
model.main_body = body_body
pymunk_objects.append(body_body)
# seat
seat_body = pymunk.Body()
seat_body.center_of_gravity = normal_rect.midbottom
seat_body.position = normal_rect.topleft
seat_shape = make_hitbox(seat_body, normal_rect)
seat_shape.mass = .5
seat_shape.elasticity = 0
seat_shape.friction = 2
seat_shape.filter = filter1
seat_sprite = ShapeSprite(seat_surface, seat_shape)
seat_sprite._layer = 1
sprites.append(seat_sprite)
pymunk_objects.append(seat_body)
pymunk_objects.append(seat_shape)
# build feet
radius = normal_rect.width * .55
feet_body = pymunk.Body()
model.feet = feet_body
feet_shape = pymunk.Circle(feet_body, radius, (0, 0))
feet_shape.mass = 1
feet_shape.elasticity = 0
feet_shape.friction = 100
feet_shape.filter = filter1
feet_sprite = ShapeSprite(feet_surface, feet_shape)
feet_sprite._layer = 0
sprites.append(feet_sprite)
pymunk_objects.append(feet_body)
pymunk_objects.append(feet_shape)
# adjust the position of the feet and body
feet_body.position = normal_rect.midbottom
# motor and joints for feet
motor = pymunk.SimpleMotor(body_body, feet_body, 0.0)
model.motor = motor
pymunk_objects.append(motor)
x, y = normal_rect.midbottom
y -= 10
joint = pymunk.PivotJoint(body_body, feet_body, (x, y), (0, 0))
pymunk_objects.append(joint)
joint = pymunk.PivotJoint(seat_body, feet_body, (x, y), (0, 0))
pymunk_objects.append(joint)
# cat
cat_surface = resources.gfx("cat.png", convert_alpha=True)
cat_rect = pygame.Rect(0, 0, 64, 48)
cat_body = pymunk.Body()
cat_shape = make_hitbox(cat_body, cat_rect)
cat_body.position = normal_rect.x, normal_rect.y - cat_rect.height - 10
cat_shape.mass = 0.001
cat_shape.elasticity = .1
cat_shape.friction = 10.0
cat_shape.filter = filter2
cat_sprite = ShapeSprite(cat_surface, cat_shape, 1.5)
cat_sprite._layer = 2
sprites.append(cat_sprite)
pymunk_objects.append(cat_body)
pymunk_objects.append(cat_shape)
# hold cat the the seat
spring = pymunk.DampedSpring(seat_body, cat_body, normal_rect.midtop,
cat_rect.midbottom, 0, 1, 0)
pymunk_objects.append(spring)
# tilt corrector
spring = pymunk.DampedRotarySpring(body_body, seat_body, radians(0), 60000,
20000)
spring.collide_bodies = False
pymunk_objects.append(spring)
model.sprites = sprites
model.pymunk_objects = pymunk_objects
space.add(pymunk_objects)
group.add(*sprites)
return model
def init():
space = pymunk.Space()
space.iterations = 10
space.sleep_time_threshold = 0.5
static_body = space.static_body
# Create segments around the edge of the screen.
shape = pymunk.Segment(static_body, (1, 1), (1, 480), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
shape = pymunk.Segment(static_body, (640, 1), (640, 480), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
shape = pymunk.Segment(static_body, (1, 1), (640, 1), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
shape = pymunk.Segment(static_body, (1, 480), (640, 480), 1.0)
space.add(shape)
shape.elasticity = 1
shape.friction = 1
for _ in range(50):
body = add_box(space, 20, 1)
pivot = pymunk.PivotJoint(static_body, body, (0, 0), (0, 0))
space.add(pivot)
pivot.max_bias = 0 # disable joint correction
pivot.max_force = 1000 # emulate linear friction
gear = pymunk.GearJoint(static_body, body, 0.0, 1.0)
space.add(gear)
gear.max_bias = 0 # disable joint correction
gear.max_force = 5000 # emulate angular friction
# We joint the tank to the control body and control the tank indirectly by modifying the control body.
global tank_control_body
tank_control_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)
tank_control_body.position = 320, 240
space.add(tank_control_body)
global tank_body
tank_body = add_box(space, 30, 10)
tank_body.position = 320, 240
for s in tank_body.shapes:
s.color = (0, 255, 100, 255)
pivot = pymunk.PivotJoint(tank_control_body, tank_body, (0, 0), (0, 0))
space.add(pivot)
pivot.max_bias = 0 # disable joint correction
pivot.max_force = 10000 # emulate linear friction
gear = pymunk.GearJoint(tank_control_body, tank_body, 0.0, 1.0)
space.add(gear)
gear.error_bias = 0 # attempt to fully correct the joint each step
gear.max_bias = 1.2 # but limit it's angular correction rate
gear.max_force = 50000 # emulate angular friction
return space
def __init__(self, space: pymunk.Space, theta: int = 0):
'''
Generate swing and robot and add to space.
'''
self.space = space
# Define initial angle
angle = Angle(theta)
# Define bodies
self.top = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.bottom = pymunk.Body(50, 10000, pymunk.Body.STATIC)
self.rod1 = pymunk.Body(10, 10000)
# self.rod2 = pymunk.Body(5, 10000)
# self.rod3 = pymunk.Body(5, 10000)
self.seat = pymunk.Body(10, 10000)
self.torso = pymunk.Body(10, 10000)
self.legs = pymunk.Body(10, 10000)
self.head = pymunk.Body(5, 10000)
# Create shapes
self.top_shape = pymunk.Poly.create_box(self.top, size=(1200, 50))
self.bottom_shape = pymunk.Poly.create_box(self.bottom,
size=(1200, 50))
self.rod1_shape = pymunk.Segment(self.rod1, (0, 0),
angle.rod1,
radius=3)
# self.rod2_shape = pymunk.Segment(self.rod2, (0, 0), angle.rod2, radius=3)
# self.rod3_shape = pymunk.Segment(self.rod3, (0, 0), angle.rod3, radius=3)
self.seat_shape = pymunk.Segment(self.seat,
angle.seat[0],
angle.seat[1],
radius=5)
self.torso_shape = pymunk.Poly.create_box(self.torso,
size=(10, torso_length))
self.legs_shape = pymunk.Poly.create_box(self.legs,
size=(10, legs_length))
self.head_shape = pymunk.Circle(self.head, radius=20)
# Set layer of shapes (disables collisions between bodies)
self.rod1_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# self.rod2_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# self.rod3_shape.filter = pymunk.ShapeFilter(categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.seat_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.torso_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.legs_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
self.head_shape.filter = pymunk.ShapeFilter(
categories=0b100, mask=pymunk.ShapeFilter.ALL_MASKS ^ 0b100)
# Set positions of bodies
self.top.position = (600, 720)
self.bottom.position = (600, 0)
self.rod1.position = (600, 695)
# self.rod2.position = angle.point1
# self.rod3.position = angle.point2
self.seat.position = angle.point2
self.torso.position = (angle.point2[0] + angle.seat[1][0] +
(torso_length / 2) * math.sin(angle._theta),
angle.point2[1] + angle.seat[1][1] +
(torso_length / 2) * math.cos(angle._theta))
self.torso._set_angle(-angle._theta)
self.legs.position = (angle.point2[0] + angle.seat[0][0] -
(legs_length / 2) * math.sin(angle._theta),
angle.point2[1] + angle.seat[0][1] -
(legs_length / 2) * math.cos(angle._theta))
self.legs._set_angle(-angle._theta)
self.head.position = (angle.point2[0] + angle.seat[1][0] +
(torso_length - 10) * math.sin(angle._theta),
angle.point2[1] + angle.seat[1][1] +
(torso_length - 10) * math.cos(angle._theta))
# Create pivot joints for bodies
self.pivot1 = pymunk.PivotJoint(self.top, self.rod1, (600, 695))
self.pivot2 = pymunk.PivotJoint(self.rod1, self.seat, angle.point1)
# self.pivot3 = pymunk.PivotJoint(self.rod2, self.rod3, angle.point2)
# self.pivot4 = pymunk.PivotJoint(self.rod3, self.seat, angle.point4)
self.pivot5 = pymunk.PinJoint(self.rod1,
self.seat,
anchor_a=angle.rod1,
anchor_b=angle.seat[0])
self.pivot6 = pymunk.PinJoint(self.rod1,
self.seat,
anchor_a=angle.rod1,
anchor_b=angle.seat[1])
self.pivot7 = pymunk.PivotJoint(self.torso, self.seat,
(angle.point2[0] + angle.seat[1][0],
angle.point2[1] + angle.seat[1][1]))
self.pivot8 = pymunk.PivotJoint(self.seat, self.legs,
(angle.point2[0] + angle.seat[0][0],
angle.point2[1] + angle.seat[0][1]))
self.pivot9 = pymunk.PivotJoint(self.head, self.torso,
self.head.position)
# self.arms = pymunk.DampedSpring(self.torso, self.rod3, anchor_a=(0,0), anchor_b=(0,0), rest_length=25, stiffness=100, damping=0.5)
self.gear1 = pymunk.RotaryLimitJoint(self.torso, self.seat, -0.1, 0.5)
self.gear2 = pymunk.RotaryLimitJoint(self.seat, self.legs, -1.2, 0.9)
# Disable collisions between rods
self.pivot1.collide_bodies = False
self.pivot2.collide_bodies = False
# self.pivot3.collide_bodies = False
# self.pivot4.collide_bodies = False
self.pivot7.collide_bodies = False
self.pivot8.collide_bodies = False
self.pivot9.collide_bodies = False
# Add bodies and pivots to space
self.space.add(self.top, self.top_shape, self.bottom,
self.bottom_shape, self.rod1, self.rod1_shape,
self.seat, self.seat_shape, self.torso,
self.torso_shape, self.legs, self.legs_shape, self.head,
self.head_shape, self.pivot1, self.pivot2, self.pivot5,
self.pivot6, self.pivot7, self.pivot8, self.pivot9,
self.gear1, self.gear2)
def _add_arm(self):
config = {
"arm_center":
(self.screen.get_width() / 2, self.screen.get_height() / 2),
"lower_arm_length":
170,
"lower_arm_starting_angle":
15,
"lower_arm_mass":
10,
"brach_rest_length":
5,
"brach_stiffness":
450,
"brach_damping":
200,
"tricep_rest_length":
30,
"tricep_stiffness":
50,
"tricep_damping":
400
}
# Upper Arm
upper_arm_length = 200
upper_arm_body = pymunk.Body(body_type=pymunk.Body.STATIC)
upper_arm_body.position = config["arm_center"]
upper_arm_body.angle = np.deg2rad(-45)
upper_arm_line = pymunk.Segment(upper_arm_body, (0, 0),
(-upper_arm_length, 0), 5)
upper_arm_line.sensor = True # Disable collision
self.space.add(upper_arm_body)
self.space.add(upper_arm_line)
# Lower Arm
lower_arm_body = pymunk.Body(
0,
0) # Pymunk will calculate moment based on mass of attached shape
lower_arm_body.position = config["arm_center"]
lower_arm_body.angle = np.deg2rad(config["lower_arm_starting_angle"])
elbow_extension_length = 20
lower_arm_start = (-elbow_extension_length, 0)
lower_arm_line = pymunk.Segment(lower_arm_body, lower_arm_start,
(config["lower_arm_length"], 0), 5)
lower_arm_line.mass = config["lower_arm_mass"]
lower_arm_line.friction = 1.0
self.space.add(lower_arm_body)
self.space.add(lower_arm_line)
# Hand
hand_width = hand_height = 15
start_x = config["lower_arm_length"]
start_y = 14
self.hand_shape = pymunk.Circle(lower_arm_body, 20, (start_x, start_y))
self.space.add(self.hand_shape)
# Pivot (Elbow)
elbow_body = pymunk.Body(body_type=pymunk.Body.STATIC)
elbow_body.position = config["arm_center"]
elbow_joint = pymunk.PivotJoint(elbow_body, lower_arm_body,
config["arm_center"])
self.space.add(elbow_joint)
# Spring (Brachialis Muscle)
brach_spring = pymunk.constraint.DampedSpring(
upper_arm_body,
lower_arm_body,
(-(upper_arm_length *
(1 / 2)), 0), # Connect half way up the upper arm
(config["lower_arm_length"] / 5,
0), # Connect near the bottom of the lower arm
config["brach_rest_length"],
config["brach_stiffness"],
config["brach_damping"])
self.space.add(brach_spring)
# Spring (Tricep Muscle)
tricep_spring = pymunk.constraint.DampedSpring(
upper_arm_body, lower_arm_body, (-(upper_arm_length * (3 / 4)), 0),
lower_arm_start, config["tricep_rest_length"],
config["tricep_stiffness"], config["tricep_damping"])
self.space.add(tricep_spring)
# Elbow stop (prevent under/over extension)
elbow_stop_point = pymunk.Circle(upper_arm_body,
radius=5,
offset=(-elbow_extension_length, -3))
elbow_stop_point.friction = 1.0
self.space.add(elbow_stop_point)
return brach_spring, tricep_spring
def testAnchorByPivot(self):
a, b = p.Body(10, 10), p.Body(20, 20)
a.position = (5, 7)
j = p.PivotJoint(a, b, (1, 2))
self.assertEqual(j.anchor_a, (-4, -5))
self.assertEqual(j.anchor_b, (1, 2))
def stick_arrow_to_target(arrow_body, target_body, position, space):
pivot_joint = pymunk.PivotJoint(arrow_body, target_body, position)
phase = target_body.angle - arrow_body.angle
gear_joint = pymunk.GearJoint(arrow_body, target_body,phase,1)
space.add(pivot_joint)
space.add(gear_joint)
def __init__(self, name, context, pos, color, manager, width= 2.5, length=2.5, mass = 120, maxForce = 400, maxTorque = 20000, maxSpeed = 30, maxAngSpeed = 1): #force in lbs
self.name = name
self.context = context
self.pPickUp = dict.fromkeys(RET_NAMES) # probability of picking up retrievable
self.tPickUp = dict.fromkeys(RET_NAMES) # avg time spent picking up retrievable
self.maxPickUp = dict.fromkeys(RET_NAMES) # max # of rets in possession
self.stPickUp = dict.fromkeys(RET_NAMES) # std dev of pickup time?
self.scores = dict.fromkeys(SCORE_NAMES) # attainable scores in scorespots
self.pScores = dict.fromkeys(SCORE_NAMES) # probability of getting score at scorespots
self.tScores = dict.fromkeys(SCORE_NAMES) # avg time spent scoring at scorespot (seconds)
self.stScores = dict.fromkeys(SCORE_NAMES) # std dev of score time?
self.width = width * SCALE
self.length = length * SCALE
self.mass=mass
self.pos = pos * SCALE
self.maxSpeed = maxSpeed * SCALE
self.maxAngSpeed = maxAngSpeed * SCALE
self.angFriction = maxTorque * SCALE * 9
self.maxForce = maxForce * SCALE
self.force = Vec2d(0,0)
self.maxTorque = maxTorque * SCALE
self.torque = 0
self.color = color
self.scale = SCALE
# create body
points = [(-self.width/2, -self.length/2),(-self.width/2,self.length/2),(self.width/2,self.length/2),(self.width/2,-self.length/2)]
self.inertia = pymunk.moment_for_poly(self.mass,points)
self.body = pymunk.Body(self.mass, self.inertia)
self.body.position = self.pos
# setup control body
self.control = pymunk.Body(pymunk.inf, pymunk.inf, body_type = pymunk.cp.CP_BODY_TYPE_KINEMATIC)
self.control.position = self.body.position
self.controlPivot = pymunk.PivotJoint(self.body, self.control,(0,0),(0,0))
self.controlPivot.max_bias = 0
self.controlPivot.max_force = 100 * self.maxForce
self.controlGear = pymunk.GearJoint(self.body, self.control, 0, 1)
self.controlGear.max_bias = 0
self.controlGear.max_force = 250 * self.maxTorque
#create shape
self.shape = pymunk.Poly(self.body,points)
self.shape.elasticity = 0.95
self.shape.friction = 1000
self.shape.color = pygame.color.THECOLORS[self.color]
if color is "blue":
self.score = self.context.blueScore
self.enemyScore = self.context.redScore
self.multiplier = -1
else:
self.score = self.context.redScore
self.enemyScore = self.context.blueScore
self.multiplier = 1
self.VisField = SensorField("VisField",self.context,self.body, self.multiplier * 10, self.multiplier * 15, owner = self)# make variables for vis field dims
self.RetField = SensorField("RetField", self.context, self.body, self.multiplier * 2, self.multiplier * 10, owner = self)
self.shape.collision_type = collision_types[self.name]
self.context.objects[self.shape._get_shapeid()] = self
self.Randomize()
self.damping = 1
self.friction = maxForce * SCALE * 6
self.canPickup = False
self.canScore = False
self.immobileTime = 0
self.inputs = []
self.RNNInputs = []
self.logits = []
self.actions = []
self.teamScores =[]
self.objectList = []
self.rets = defaultdict(lambda:[])
self.prev = 0 # team's score 1 second ago
self.numObstacles = 0
