def fixed_joint_demo(screen):
bodies = []
joints = []
restitution = 0.5
fric_coeff = 0.15
r = Rect([120, 100], [60, 60],
restitution=restitution, fric_coeff=fric_coeff)
bodies.append(r)
r.add_force(ExternalForce(gravity, multiplier=100))
r2 = Rect([160, 100], [60, 60],
restitution=restitution, fric_coeff=fric_coeff)
bodies.append(r2)
joints += [FixedJoint(r, r2)]
r2.add_no_collision(r)
r2.add_force(ExternalForce(gravity, multiplier=100))
inclination = math.pi / 32
r = Rect([inclination, 500, 500], [900, 10],
restitution=restitution, fric_coeff=fric_coeff)
bodies.append(r)
joints.append(TotalConstraint(r))
recorder = None
# recorder = Recorder(DT, screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=TIME, screen=screen, recorder=recorder)
def testSlide(self):
bodies = []
joints = []
restitution = 0.5
fric_coeff = 0.2
clock = Circle([975, 575], 20, vel=[1, 0, 0])
bodies.append(clock)
p1 = Hull([500, 300], [[450, 5], [-450, 5], [-450, -5], [450, -5]],
restitution=restitution, fric_coeff=fric_coeff)
p1.rotate_verts(get_tensor(math.pi / 32))
bodies.append(p1)
joints.append(TotalConstraint(p1))
# Rectangle
# p2 = Hull([100, 100], [[30, 30], [-30, 30], [-30, -30], [30, -30]],
# restitution=restitution, fric_coeff=fric_coeff)
# Pentagon
p2 = Hull([100, 100], [[50, 0], [30, 50], [-30, 30], [-50, -30], [0, -50]],
restitution=restitution, fric_coeff=fric_coeff)
# Hexagon
p2 = Hull([100, 100], [[50, 0], [30, 50], [-30, 30], [-50, -30], [0, -50], [30, -30]],
restitution=restitution, fric_coeff=fric_coeff)
bodies.append(p2)
p2.add_force(ExternalForce(down_force, multiplier=100))
# p2.add_force(ExternalForce(hor_impulse, multiplier=-100))
recorder = None
# recorder = Recorder(DT, self.screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=TIME, screen=self.screen, recorder=recorder)
def make_world(forces, mass, num_links=10):
bodies = []
joints = []
# make chain of rectangles
link_mass = mass / num_links
r = Rect([300, 50], [20, 60], mass=link_mass)
bodies.append(r)
joints.append(Joint(r, None, [300, 30]))
for i in range(1, num_links):
if i < num_links - 1:
r = Rect([300, 50 + 50 * i], [20, 60], mass=link_mass)
else:
r = Rect([300, 50 + 50 * i], [20, 60], mass=link_mass)
r.add_force(Gravity(g=100))
bodies.append(r)
joints.append(Joint(bodies[-1], bodies[-2], [300, 25 + 50 * i]))
bodies[-1].add_no_contact(bodies[-2])
# make projectile
m = 3
c_pos = torch.tensor([50, bodies[-1].pos[1]]) # same Y as last chain link
c = Circle(c_pos, 20, restitution=1.)
bodies.append(c)
for f in forces:
c.add_force(ExternalForce(f, multiplier=500 * m))
world = World(bodies, joints, dt=DT, post_stab=True)
return world, r
def make_world(forces, mass):
bodies = []
joints = []
# make chain of rectangles
r = Rect([300, 50], [20, 60])
bodies.append(r)
joints.append(Joint(r, None, [300, 30]))
for i in range(1, 10):
if i < 9:
r = Rect([300, 50 + 50 * i], [20, 60])
else:
r = Rect([300, 50 + 50 * i], [20, 60], mass=mass)
bodies.append(r)
joints.append(Joint(bodies[-1], bodies[-2],
[300, 25 + 50 * i]))
bodies[-1].add_no_collision(bodies[-2])
bodies[-1].add_force(ExternalForce(down_force, multiplier=100))
# make projectile
m = 13
c1 = Circle([50, 500], 20)
bodies.append(c1)
for f in forces:
c1.add_force(ExternalForce(f, multiplier=100 * m))
world = World(bodies, joints, dt=DT)
return world, r
def make_world(forces, mass, num_links=10):
bodies = []
joints = []
# make chain of rectangles
r = Rect([300, 50], [20, 60])
bodies.append(r)
joints.append(Joint(r, None, [300, 30]))
for i in range(1, num_links):
if i < num_links - 1:
r = Rect([300, 50 + 50 * i], [20, 60])
else:
r = Rect([300, 50 + 50 * i], [20, 60], mass=mass)
bodies.append(r)
joints.append(Joint(bodies[-1], bodies[-2], [300, 25 + 50 * i]))
bodies[-1].add_no_collision(bodies[-2])
bodies[-1].add_force(Gravity(g=100))
# make projectile
m = 13
c1 = Circle([50, bodies[-1].pos.data[1]], 20) # same Y as last chain link
bodies.append(c1)
for f in forces:
c1.add_force(ExternalForce(f, multiplier=100 * m))
world = World(bodies, joints, dt=DT)
return world, r
def chain_demo(screen):
bodies = []
joints = []
restitution = 0.9
# make chain of rectangles
r = Rect([300, 50], [20, 60], restitution=restitution)
bodies.append(r)
joints.append(XConstraint(r))
joints.append(YConstraint(r))
for i in range(1, 10):
r = Rect([300, 50 + 50 * i], [20, 60], restitution=restitution)
bodies.append(r)
joints.append(Joint(bodies[-1], bodies[-2], [300, 25 + 50 * i]))
bodies[-1].add_no_contact(bodies[-2])
bodies[-1].add_force(Gravity(g=100))
# make projectile
c = Circle([50, 500], 20, restitution=restitution)
bodies.append(c)
c.add_force(ExternalForce(hor_impulse, multiplier=2000))
clock = Circle([975, 575], 20, vel=[1, 0, 0])
bodies.append(clock)
recorder = None
# recorder = Recorder(DT, screen)
world = World(bodies, joints, dt=DT, post_stab=True)
run_world(world, run_time=TIME * 2, screen=screen, recorder=recorder)
def testFric(self):
bodies = []
joints = []
def timed_force(t):
if 1 < t < 2:
return ExternalForce.RIGHT
else:
return ExternalForce.ZEROS
r = Rect([400, 400], [900, 10])
bodies.append(r)
r.add_force(ExternalForce(timed_force, multiplier=100))
r.add_force(ExternalForce(gravity, multiplier=100))
c = Circle([200, 364], 30)
bodies.append(c)
c.add_force(ExternalForce(gravity, multiplier=100))
c = Circle([50, 436], 30)
bodies.append(c)
joints.append(XConstraint(c))
joints.append(YConstraint(c))
c = Circle([800, 436], 30)
bodies.append(c)
joints.append(XConstraint(c))
joints.append(YConstraint(c))
recorder = None
# recorder = Recorder(DT, self.screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=10, screen=self.screen, recorder=recorder)
def testChain(self):
bodies = []
joints = []
# make chain of rectangles
r = Rect([300, 50], [20, 60])
bodies.append(r)
joints.append(XConstraint(r))
joints.append(YConstraint(r))
for i in range(1, 10):
r = Rect([300, 50 + 50 * i], [20, 60])
bodies.append(r)
joints.append(Joint(bodies[-1], bodies[-2], [300, 25 + 50 * i]))
bodies[-1].add_no_collision(bodies[-2])
bodies[-1].add_force(ExternalForce(down_force, multiplier=100))
# make projectile
c = Circle([50, 500], 20, restitution=1)
bodies.append(c)
c.add_force(ExternalForce(hor_impulse, multiplier=1000))
recorder = None
# recorder = Recorder(DT, self.screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=TIME, screen=self.screen, recorder=recorder)
def make_world(forces, controlT, controlU):
bodies = []
joints = []
# make chain of rectangles
r = Rect([0, 120, 240], [60, 60], mass=1)
r2 = Rect([0, 200, 240], [60, 60], mass=1)
#r.set_p(r.p.new_tensor([1, 1, 1]))
bodies.append(r)
bodies.append(r2)
#joints.append(Joint(r, None, [300, 30]))
r.add_force(Gravity(g=10))
r2.add_force(Gravity(g=10))
controlForceL = lambda t: controlForce(t, controlT, controlU)
cf = ExternalForce(controlForceL, multiplier=1)
controlForceL2 = lambda t: controlForce2(t, controlT, controlU)
cf2 = ExternalForce(controlForceL2, multiplier=1)
r.add_force(cf)
r2.add_force(cf2)
floor = Rect([0, 300], [1000, 30], mass=100)
floorStep = Rect([300, 240], [100, 89], mass=100)
floorStep2 = Rect([0, 240], [100, 89], mass=100)
joints.append(TotalConstraint(floor))
bodies.append(floor)
joints.append(TotalConstraint(floorStep))
bodies.append(floorStep)
joints.append(TotalConstraint(floorStep2))
bodies.append(floorStep2)
#joints.append(Joint(bodies[-1], bodies[-2], [300, 25 + 50 * i]))
#bodies[-1].add_no_contact(bodies[-2])
# make projectile
#m = 3
#c_pos = torch.tensor([50, bodies[-1].pos[1]]) # same Y as last chain link
#c = Circle(c_pos, 20, restitution=1.)
#bodies.append(c)
#for f in forces:
# c.add_force(ExternalForce(f, multiplier=500 * m))
world = World(bodies,
joints,
dt=DT,
post_stab=True,
strict_no_penetration=True)
return world, r
def testFric(self):
restitution = 0.75
fric_coeff = 1
bodies = []
joints = []
def timed_force(t):
if 1 < t < 2:
return ExternalForce.RIGHT
else:
return ExternalForce.ZEROS
r = Rect([400, 400], [900, 10],
restitution=restitution,
fric_coeff=fric_coeff)
bodies.append(r)
r.add_force(ExternalForce(timed_force, multiplier=100))
r.add_force(ExternalForce(down_force, multiplier=100))
c = Circle([200, 364],
30,
restitution=restitution,
fric_coeff=fric_coeff)
bodies.append(c)
c.add_force(ExternalForce(down_force, multiplier=100))
c = Circle([50, 436],
30,
restitution=restitution,
fric_coeff=fric_coeff)
bodies.append(c)
joints.append(XConstraint(c))
joints.append(YConstraint(c))
c = Circle([800, 436],
30,
restitution=restitution,
fric_coeff=fric_coeff)
bodies.append(c)
joints.append(XConstraint(c))
joints.append(YConstraint(c))
clock = Circle([975, 575], 20, vel=[1, 0, 0])
bodies.append(clock)
recorder = None
# recorder = Recorder(DT, screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=10, screen=self.screen, recorder=recorder)
def testDemo(self):
bodies = []
joints = []
# Ball hitting object constrained by 1 joint
for i in range(1, 3):
c = Circle([150, 150 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(ExternalForce(vert_impulse, multiplier=500))
bodies.append(c)
joints.append(Joint(bodies[-1], None, [140, 220]))
# Ball bouncing on body fixed in place
for i in range(1, 3):
c = Circle([300 + 1 * (i - 1), 150 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(ExternalForce(down_force, multiplier=100))
bodies.append(c)
joints.append(TotalConstraint(bodies[-1]))
# 2 free ball collision angled
for i in range(1, 3):
c = Circle([225 - 10 * (i - 1), 300 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(ExternalForce(down_force, multiplier=100))
bodies.append(c)
# 2 free ball collision straight
for i in range(1, 3):
c = Circle([375, 300 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(ExternalForce(vert_impulse, multiplier=500))
bodies.append(c)
r = Rect([300, 500], [40, 40])
r.add_force(ExternalForce(down_force, multiplier=-100))
r.v[0] = -1
bodies.append(r)
r = Rect([300, 50], [40, 40])
r.add_force(ExternalForce(down_force, multiplier=100))
r.v[0] = -1
for b in bodies:
b.add_no_collision(r)
# bodies.append(r)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=10, screen=self.screen)
def make_world(learned_force):
bodies = []
joints = []
target = Circle([500, 300], 30)
bodies.append(target)
c1 = Circle([250, 210], 30)
bodies.append(c1)
c1.add_force(ExternalForce(learned_force))
c1.add_no_collision(target)
c2 = Circle([400, 250], 30)
bodies.append(c2)
c2.add_no_collision(target)
world = World(bodies, joints, dt=DT)
return world, c2, target
def debug_demo(screen):
bodies = []
joints = []
# Ball hitting object constrained by 1 joint
for i in range(1, 3):
c = Circle([150, 150 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(ExternalForce(vert_impulse, multiplier=500))
bodies.append(c)
joints.append(Joint(bodies[-1], None, [140, 220]))
# Ball bouncing on body fixed in place
for i in range(1, 3):
c = Circle([300 + 1 * (i - 1), 150 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(Gravity(g=100))
# else:
# c.add_force(ExternalForce(neg_gravity, multiplier=100))
bodies.append(c)
joints.append(TotalConstraint(bodies[-1]))
# 2 free ball collision angled
for i in range(1, 3):
c = Circle([225 - 10 * (i - 1), 300 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(Gravity(g=100))
bodies.append(c)
# 2 free ball collision straight
for i in range(1, 3):
c = Circle([375, 300 + 80 * (i - 1)], 20)
if i == 1:
c.add_force(ExternalForce(vert_impulse, multiplier=500))
bodies.append(c)
r = Rect([300, 500], [40, 40], vel=[-1, 0, 0])
r.add_force(Gravity(g=-100))
bodies.append(r)
clock = Circle([975, 575], 20, vel=[1, 0, 0])
bodies.append(clock)
world = World(bodies, joints, dt=DT, post_stab=True)
run_world(world, run_time=10, screen=screen)
def testRender(self):
p1 = Hull([700, 200], [[math.sqrt(2) * 10 + 10, 0 + 10], [0 + 10, math.sqrt(2) * 10 + 10],
[math.sqrt(2) * -10 + 10, 0 + 10], [0 + 10, math.sqrt(2) * -10 + 10]])
p2 = Hull([300, 200], [[60, 0], [60, 60], [0, 60]])
p2.add_force(ExternalForce(hor_impulse, multiplier=1000))
p2.add_force(ExternalForce(rot_impulse, multiplier=1000))
p3 = Hull([700, 400], [[60, 0], [60, 60], [0, 0]])
p4 = Hull([300, 420], [[60, 0], [0, 60], [0, 0]])
p4.add_force(ExternalForce(hor_impulse, multiplier=1000))
p4.add_force(ExternalForce(rot_impulse, multiplier=1000))
p5 = Hull([500, 300], [[50, 0], [30, 60], [-30, 30], [-60, -30], [0, -60]])
p5.add_force(ExternalForce(hor_impulse, multiplier=1000))
p5.add_force(ExternalForce(rot_impulse, multiplier=1000))
# print(p5.verts)
# print(get_support(p5.verts, wrap_variable([-1, -1])))
# world = World([p5])
world = World([p1, p2, p3, p4, p5])
run_world(world, screen=self.screen, run_time=180)
def slide_demo(screen):
bodies = []
joints = []
restitution = Defaults.RESTITUTION
fric_coeff = 0.15
inclination = math.pi / 32
r = Rect([inclination, 500, 300], [900, 10],
restitution=restitution, fric_coeff=fric_coeff)
bodies.append(r)
joints.append(TotalConstraint(r))
r = Rect([100, 100], [60, 60],
restitution=restitution, fric_coeff=fric_coeff)
# r = Hull([100, 100], [[30, 30], [-30, 30], [-30, -30], [30, -30]])
bodies.append(r)
r.add_force(Gravity(g=100))
recorder = None
# recorder = Recorder(DT, screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=TIME, screen=screen, recorder=recorder)
def testSlide(self):
bodies = []
joints = []
r = Rect([500, 300], [900, 10])
r.v[0] = math.pi / 32
r.move(1)
r.v[0] = 0.
bodies.append(r)
joints.append(TotalConstraint(r))
r = Rect([100, 100], [60, 60])
r.v[0] = -math.pi / 8 * 0
r.move(1)
r.v[0] = 0.
bodies.append(r)
r.add_force(ExternalForce(down_force, multiplier=100))
# r.add_force(ExternalForce(hor_impulse, multiplier=-100))
# c = Circle([100, 150], 30)
# bodies.append(c)
# c.add_force(ExternalForce(gravity, multiplier=100))
# c = Circle([50, 550], 30)
# c.add_force(ExternalForce(rot_impulse, multiplier=1000))
# bodies.append(c)
# XXX
# c = Circle([875, 100], 30)
# bodies.append(c)
# c.add_force(ExternalForce(gravity, multiplier=100))
recorder = None
# recorder = Recorder(DT, self.screen)
world = World(bodies, joints, dt=DT)
run_world(world, run_time=TIME, screen=self.screen, recorder=recorder)
5,
mass=100000000,
vel=(0, 0, 0),
restitution=restitution,
fric_coeff=1,
name="f2")
bodies.append(c)
vel = torch.tensor(traj1)
traj_f = []
traj_f.append(Trajectory(vel=vel, name="f1"))
vel = torch.tensor(traj2)
traj_f.append(Trajectory(vel=vel, name="f2"))
# Environment
# r = Rect([0, 500, 505], [900, 10],
# restitution=restitution, fric_coeff=1)
# bodies.append(r)
# joints.append(TotalConstraint(r))
recorder = None
world = World(bodies, joints, dt=0.1)
run_world_traj(world,
run_time=5,
screen=screen,
recorder=recorder,
traj=traj_f)
print('\n')
print(world.states)
print('\n')
def make_world(paddle_params, blocks_params, ball_params, ball_vel):
bodies = []
constraints = []
# Add ball if it exists
ball_pos, ball_rad = ball_params
ball_body = Circle(ball_pos,
ball_rad,
vel=ball_vel,
mass=BALL_MASS,
restitution=STD_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=BALL_COLOR,
thickness=0)
bodies.append(ball_body)
# Add paddle
paddle_pos, paddle_dims = paddle_params
paddle_body = Rect(paddle_pos,
paddle_dims,
restitution=STD_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=PADDLE_COLOR,
thickness=0)
bodies.append(paddle_body)
constraints += [
# YConstraint(paddle_body),
# RotConstraint(paddle_body)
]
paddle_idx = len(bodies) - 1
# Add walls
left_wall = Rect([WALL_WIDTH / 2.0, STOPPER_LINE / 2.0],
[WALL_WIDTH, STOPPER_LINE],
restitution=STD_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=WALL_COLOR,
thickness=0)
constraints.append(TotalConstraint(left_wall))
left_wall.add_no_contact(paddle_body)
bodies.append(left_wall)
right_wall = Rect([
SCREEN_WIDTH - WALL_WIDTH / 2.0, (STOPPER_LINE + STOPPER_HEIGHT) / 2.0
], [WALL_WIDTH, STOPPER_LINE + STOPPER_HEIGHT],
restitution=STD_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=WALL_COLOR,
thickness=0)
constraints.append(TotalConstraint(right_wall))
paddle_body.add_no_contact(right_wall)
bodies.append(right_wall)
roof = Rect([SCREEN_WIDTH / 2.0, ROOF_LINE / 2.0],
[SCREEN_WIDTH - 2 * WALL_WIDTH, ROOF_LINE],
restitution=STD_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=WALL_COLOR,
thickness=0)
roof.add_no_contact(paddle_body)
roof.add_no_contact(left_wall)
roof.add_no_contact(right_wall)
constraints.append(TotalConstraint(roof))
bodies.append(roof)
# Add stoppers
left_stopper = Rect(
[STOPPER_WIDTH / 2.0, STOPPER_LINE + STOPPER_HEIGHT / 2.0],
[STOPPER_WIDTH, STOPPER_HEIGHT],
restitution=STOPPER_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=LEFT_STOPPER_COLOR,
thickness=0)
constraints.append(TotalConstraint(left_stopper))
bodies.append(left_stopper)
left_stopper.add_no_contact(left_wall)
left_stopper.add_no_contact(ball_body)
right_stopper = Rect([
SCREEN_WIDTH + STOPPER_WIDTH / 2.0, STOPPER_LINE + STOPPER_HEIGHT / 2.0
], [STOPPER_WIDTH, STOPPER_HEIGHT],
restitution=STOPPER_RESTITUTION,
fric_coeff=FRIC_COEFF,
col=WALL_COLOR,
thickness=0)
constraints.append(TotalConstraint(right_stopper))
bodies.append(right_stopper)
right_stopper.add_no_contact(right_wall)
right_stopper.add_no_contact(ball_body)
# Add blocks
blocks_bodies = []
for block_params in blocks_params:
block_pos, block_dims = block_params
import math # XXX
level = int((block_pos[1] - block_dims[1] / 2 - BLOCKS_TOP_LEFT[0]) /
BLOCK_HEIGHT)
# TODO Define restitution by level? (different levels have different bounces)
block_restitution = 1
block_body = Rect(block_pos,
block_dims,
restitution=block_restitution,
fric_coeff=FRIC_COEFF,
col=BLOCK_COLORS[level],
thickness=0)
bodies.append(block_body)
blocks_bodies.append(block_body)
constraints.append(TotalConstraint(block_body))
block_body.add_no_contact(left_wall)
block_body.add_no_contact(right_wall)
for other_block in blocks_bodies[:-1]:
block_body.add_no_contact(other_block)
return World(bodies, constraints,
dt=DT), ball_body, paddle_body, blocks_bodies, paddle_idx
