1#!/usr/bin/env python
2
3# http://pyode.sourceforge.net/tutorials/tutorial2.html
4
5# pyODE example 2: Connecting bodies with joints
6
7# modified by Gideon Klompje (removed literals and using
8# 'ode.Mass.setSphereTotal' instead of 'ode.Mass.setSphere')
9
10
11import ode
12import pygame
13
14from pygame.locals import QUIT, KEYDOWN
15
16# Constants
17WINDOW_RESOLUTION = (640, 480)
18
19DRAW_SCALE = WINDOW_RESOLUTION[0] / 5
20"""Factor to multiply physical coordinates by to obtain screen size in pixels"""
21
22DRAW_OFFSET = (WINDOW_RESOLUTION[0] / 2, 50)
23"""Screen coordinates (in pixels) that map to the physical origin (0, 0, 0)"""
24
25BACKGROUND_COLOR = (255, 255, 255)
26
27GRAVITY = (0, -9.81, 0)
28
29SPHERE1_POSITION = (1, 0, 0)
30SPHERE1_MASS = 1
31SPHERE1_RADIUS = 0.15
32SPHERE1_COLOR = (55, 0, 200)
33
34SPHERE2_POSITION = (2, 0, 0)
35SPHERE2_MASS = 1
36SPHERE2_RADIUS = 0.15
37SPHERE2_COLOR = (55, 0, 200)
38
39JOINT1_ANCHOR = (0, 0, 0)
40JOINT1_COLOR = (200, 0, 55)
41JOINT1_WIDTH = 2
42"""Width of the line (in pixels) representing the joint"""
43
44JOINT2_ANCHOR = SPHERE1_POSITION
45JOINT2_COLOR = (200, 0, 55)
46JOINT2_WIDTH = 2
47"""Width of the line (in pixels) representing the joint"""
48
49TIME_STEP = 0.04
50
51# Utility functions
52def coord(x, y, integer=False):
53    """
54    Convert world coordinates to pixel coordinates.  Setting 'integer' to
55    True will return integer coordinates.
56    """
57    xs = (DRAW_OFFSET[0] + DRAW_SCALE*x)
58    ys = (DRAW_OFFSET[1] - DRAW_SCALE*y)
59
60    if integer:
61        return int(round(xs)), int(round(ys))
62    else:
63        return xs, ys
64
65# Initialize pygame
66pygame.init()
67
68# Open a display
69screen = pygame.display.set_mode(WINDOW_RESOLUTION)
70
71# Create a world object
72world = ode.World()
73world.setGravity(GRAVITY)
74
75# Create two bodies
76body1 = ode.Body(world)
77M = ode.Mass()
78M.setSphereTotal(SPHERE1_MASS, SPHERE1_RADIUS)
79body1.setMass(M)
80body1.setPosition(SPHERE1_POSITION)
81
82body2 = ode.Body(world)
83M = ode.Mass()
84M.setSphereTotal(SPHERE2_MASS, SPHERE2_RADIUS)
85body2.setMass(M)
86body2.setPosition(SPHERE2_POSITION)
87
88# Connect body1 with the static environment
89j1 = ode.BallJoint(world)
90j1.attach(body1, ode.environment)
91j1.setAnchor(JOINT1_ANCHOR)
92
93# Connect body2 with body1
94j2 = ode.BallJoint(world)
95j2.attach(body1, body2)
96j2.setAnchor(JOINT2_ANCHOR)
97
98# Simulation loop...
99if __name__ == "__main__":
100    fps = 1.0 / TIME_STEP
101    clk = pygame.time.Clock()
102
103    sph1_rad = int(DRAW_SCALE * SPHERE1_RADIUS)
104    sph2_rad = int(DRAW_SCALE * SPHERE2_RADIUS)
105
106    loopFlag = True
107    while loopFlag:
108        for e in pygame.event.get():
109            if e.type==QUIT:
110                loopFlag=False
111            if e.type==KEYDOWN:
112                loopFlag=False
113
114        # Clear the screen
115        screen.fill(BACKGROUND_COLOR)
116
117        # Draw the two bodies and the lines representing the joints
118        x1, y1, z1 = body1.getPosition()
119        x2, y2, z2 = body2.getPosition()
120        xj1, yj1, zj1 = j1.getAnchor()
121        xj2, yj2, zj2 = j2.getAnchor()
122
123        pygame.draw.line(screen, JOINT1_COLOR, coord(xj1, yj1), coord(x1, y1), JOINT1_WIDTH)
124        pygame.draw.line(screen, JOINT2_COLOR, coord(xj2, yj2), coord(x2, y2), JOINT2_WIDTH)
125        pygame.draw.circle(screen, SPHERE1_COLOR, coord(x1, y1, integer=True), sph1_rad, 0)
126        pygame.draw.circle(screen, SPHERE2_COLOR, coord(x2, y2, integer=True), sph2_rad, 0)
127
128        pygame.display.flip()
129
130        # Next simulation step
131        world.step(TIME_STEP)
132
133        # Try to keep the specified framerate
134        clk.tick(fps)
135
136