-
- #!/usr/bin/env python
-
- import pygame
- import ode
-
- default_density = 2500
- default_radius = 0.5
- white = (255, 255, 255)
- blue = (0, 0, 255)
-
- class Pendulum(object):
- def __init__(self, ball, joint):
- self.ball = ball
- self.joint = joint
-
- class Block(object):
- def __init__(self, block, joint):
- self.block = block
- self.joint = joint
-
- def coord((x, y, z)):
- return int(100 + x*20), int(300 - y*20)
-
- def init_scene():
- # Position and density information
- pendulum_density = default_density
- base_density = default_density
-
- pendulum_balls = [(0, 8, 0), (3, 5, 0), ( 7, 4, 0), (12, 3, 0)]
- pendulum_joints = [(5, 8, 0), (7, 8, 0), (10, 8, 0), (12, 8, 0)]
-
- # Create world
- world = ode.World()
- world.setGravity((0, -9.81, 0))
-
- # Create base
- b_body = ode.Body(world)
- b_mass = ode.Mass()
- b_mass.setBox(base_density, 13, 2, 2)
- b_body.setMass(b_mass)
- b_body.setPosition((8.5, 8.5, 0))
-
- b_joint = ode.SliderJoint(world)
- b_joint.attach(b_body, ode.environment)
-
- base = Block(b_body, b_joint)
-
- # Create pendulums
- p_list = []
- for p_pos, j_pos in zip(pendulum_balls, pendulum_joints):
- p_body = ode.Body(world)
- p_mass = ode.Mass()
- p_mass.setSphere(default_density, default_radius)
- p_body.setMass(p_mass)
- p_body.setPosition(p_pos)
-
- # Create & connect joint
- joint = ode.BallJoint(world)
- joint.attach(p_body, b_body)
- joint.setAnchor(j_pos)
-
- print "connected {0} to {1}".format(p_pos, j_pos)
-
- # Instantiate container class
- p_list.append(Pendulum(p_body, joint))
-
- # return base, p_list
- return world, base, p_list
-
- def init_graphics():
- pygame.init()
- screen = pygame.display.set_mode((600, 600))
- return screen
-
- def main(screen, world, base, p_list):
- clock = pygame.time.Clock()
- fps = 60
- dt = 1.0/fps
-
- while not pygame.event.get(pygame.QUIT):
- pygame.event.get([pygame.KEYDOWN, pygame.MOUSEMOTION])
- world.step(dt)
-
- # render world
- screen.fill(white)
- for p in p_list:
- pygame.draw.circle(screen, blue, coord(p.ball.getPosition()), 10)
- pygame.draw.line(screen, blue, coord(p.joint.getAnchor()), coord(p.ball.getPosition()), 1)
-
- for p in p_list:
- pygame.draw.circle(screen, blue, coord(p.joint.getAnchor()), 10)
- pygame.draw.line(screen, blue, coord(p_list[0].joint.getAnchor()), coord(p_list[-1].joint.getAnchor()), 20)
-
- #for a, j in zip(base.anchors, base.joints):
- # pygame.draw.circle(screen, blue, coord(a.getPosition()), 10)
- # pygame.draw.line(screen, blue, coord(a.getPosition()), coord(j.getAnchor()), 1)
-
- pygame.display.flip()
-
- # tick pygame's clock
- clock.tick(fps)
-
- if __name__=="__main__":
- screen = init_graphics()
- world, base, p_list = init_scene()
- main(screen, world, base, p_list)
-