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