1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
|
#!/usr/bin/python
#http://pyode.sourceforge.net/tutorials/tutorial3.html
#collision detection with pyODE
import ode, random
from math import *
# create_box
def create_box(name, world, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setBox(density, lx, ly, lz)
body.setMass(M)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=body.boxsize)
geom.setBody(body)
geom.name = name
body.name = name
return body
# drop_object
def drop_object():
"""Drop an object into the scene."""
global bodies, counter, objcount
body = create_box("box #%s" % (objcount), world, space, 1000, 1.0,0.2,0.2)
body.setPosition( (random.gauss(0,0.1),3.0,random.gauss(0,0.1)) )
theta = random.uniform(0,2*pi)
ct = cos (theta)
st = sin (theta)
body.setRotation([ct, 0., -st, 0., 1., 0., st, 0., ct])
bodies.append(body)
counter=0
objcount+=1
return body
# Collision callback
def near_callback(args, geom1, geom2):
"""Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do.
for two colliding blocks there should be four "contacts", w
"""
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
#print "contacts = ", contacts
# Create contact joints
world,contactgroup = args
for c in contacts:
body1 = geom1.getBody()
body2 = geom2.getBody()
if not body1 is None and not body2 is None:
print "collision between %s and %s" % (body1.name, body2.name)
(position, normal, depth, geom1, geom2) = c.getContactGeomParams()
print ".. and position = ", position
restitution = c.getBounce()
print ".. and restitituion = ", restitution
#if len(contacts) > 0:
#there was definitely a collision
def configure_world(erp=0.8, cfm=1E-5, gravity=(0,0,0)):
'''returns a pyODE world object
gravity should be a triple'''
world = ode.World()
world.setGravity(gravity)
world.setERP(erp)
world.setCFM(cfm)
return world
world = configure_world()
space = ode.Space()
floor = ode.GeomPlane(space, (0,1,0), 0)
contactgroup = ode.JointGroup()
objcount = 0
bodies = []
fps = 50
dt = 1.0/fps
dt = dt/2
#start simulation
body1 = drop_object()
world.step(100)
body2 = drop_object()
#body2.addForce((0, -2, 0)) #when you uncomment this line the bodies no longer have names? (see the attribute error)
world.step(10)
space.collide((world,contactgroup), near_callback)
contactgroup.empty()
|
