285 lines
6.0 KiB
Python
285 lines
6.0 KiB
Python
#!/usr/bin/env python
|
|
|
|
# http://pyode.sourceforge.net/tutorials/tutorial3.html
|
|
|
|
# pyODE example 3: Collision detection
|
|
|
|
# Originally by Matthias Baas.
|
|
# Updated by Pierre Gay to work without pygame or cgkit.
|
|
|
|
import sys, os, random, time
|
|
from math import *
|
|
from OpenGL.GL import *
|
|
from OpenGL.GLU import *
|
|
from OpenGL.GLUT import *
|
|
|
|
import ode
|
|
|
|
# geometric utility functions
|
|
def scalp (vec, scal):
|
|
vec[0] *= scal
|
|
vec[1] *= scal
|
|
vec[2] *= scal
|
|
|
|
def length (vec):
|
|
return sqrt (vec[0]**2 + vec[1]**2 + vec[2]**2)
|
|
|
|
# prepare_GL
|
|
def prepare_GL():
|
|
"""Prepare drawing.
|
|
"""
|
|
|
|
# Viewport
|
|
glViewport(0,0,640,480)
|
|
|
|
# Initialize
|
|
glClearColor(0.8,0.8,0.9,0)
|
|
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
|
glEnable(GL_DEPTH_TEST)
|
|
glDisable(GL_LIGHTING)
|
|
glEnable(GL_LIGHTING)
|
|
glEnable(GL_NORMALIZE)
|
|
glShadeModel(GL_FLAT)
|
|
|
|
# Projection
|
|
glMatrixMode(GL_PROJECTION)
|
|
glLoadIdentity()
|
|
gluPerspective (45,1.3333,0.2,20)
|
|
|
|
# Initialize ModelView matrix
|
|
glMatrixMode(GL_MODELVIEW)
|
|
glLoadIdentity()
|
|
|
|
# Light source
|
|
glLightfv(GL_LIGHT0,GL_POSITION,[0,0,1,0])
|
|
glLightfv(GL_LIGHT0,GL_DIFFUSE,[1,1,1,1])
|
|
glLightfv(GL_LIGHT0,GL_SPECULAR,[1,1,1,1])
|
|
glEnable(GL_LIGHT0)
|
|
|
|
# View transformation
|
|
gluLookAt (2.4, 3.6, 4.8, 0.5, 0.5, 0, 0, 1, 0)
|
|
|
|
# draw_body
|
|
def draw_body(body):
|
|
"""Draw an ODE body.
|
|
"""
|
|
|
|
x,y,z = body.getPosition()
|
|
R = body.getRotation()
|
|
rot = [R[0], R[3], R[6], 0.,
|
|
R[1], R[4], R[7], 0.,
|
|
R[2], R[5], R[8], 0.,
|
|
x, y, z, 1.0]
|
|
glPushMatrix()
|
|
glMultMatrixd(rot)
|
|
if body.shape=="box":
|
|
sx,sy,sz = body.boxsize
|
|
glScalef(sx, sy, sz)
|
|
glutSolidCube(1)
|
|
glPopMatrix()
|
|
|
|
|
|
# create_box
|
|
def create_box(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)
|
|
|
|
return body, geom
|
|
|
|
# drop_object
|
|
def drop_object():
|
|
"""Drop an object into the scene."""
|
|
|
|
global bodies, geom, counter, objcount
|
|
|
|
body, geom = create_box(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)
|
|
geoms.append(geom)
|
|
counter=0
|
|
objcount+=1
|
|
|
|
# explosion
|
|
def explosion():
|
|
"""Simulate an explosion.
|
|
|
|
Every object is pushed away from the origin.
|
|
The force is dependent on the objects distance from the origin.
|
|
"""
|
|
global bodies
|
|
|
|
for b in bodies:
|
|
l=b.getPosition ()
|
|
d = length (l)
|
|
a = max(0, 40000*(1.0-0.2*d*d))
|
|
l = [l[0] / 4, l[1], l[2] /4]
|
|
scalp (l, a / length (l))
|
|
b.addForce(l)
|
|
|
|
# pull
|
|
def pull():
|
|
"""Pull the objects back to the origin.
|
|
|
|
Every object will be pulled back to the origin.
|
|
Every couple of frames there'll be a thrust upwards so that
|
|
the objects won't stick to the ground all the time.
|
|
"""
|
|
global bodies, counter
|
|
|
|
for b in bodies:
|
|
l=list (b.getPosition ())
|
|
scalp (l, -1000 / length (l))
|
|
b.addForce(l)
|
|
if counter%60==0:
|
|
b.addForce((0,10000,0))
|
|
|
|
# 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.
|
|
"""
|
|
|
|
# Check if the objects do collide
|
|
contacts = ode.collide(geom1, geom2)
|
|
|
|
# Create contact joints
|
|
world,contactgroup = args
|
|
for c in contacts:
|
|
c.setBounce(0.2)
|
|
c.setMu(5000)
|
|
j = ode.ContactJoint(world, contactgroup, c)
|
|
j.attach(geom1.getBody(), geom2.getBody())
|
|
|
|
|
|
|
|
######################################################################
|
|
|
|
# Initialize Glut
|
|
glutInit ([])
|
|
|
|
# Open a window
|
|
glutInitDisplayMode (GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE)
|
|
|
|
x = 0
|
|
y = 0
|
|
width = 640
|
|
height = 480
|
|
glutInitWindowPosition (x, y);
|
|
glutInitWindowSize (width, height);
|
|
glutCreateWindow ("testode")
|
|
|
|
# Create a world object
|
|
world = ode.World()
|
|
world.setGravity( (0,-9.81,0) )
|
|
world.setERP(0.8)
|
|
world.setCFM(1E-5)
|
|
|
|
# Create a space object
|
|
space = ode.Space()
|
|
|
|
# Create a plane geom which prevent the objects from falling forever
|
|
floor = ode.GeomPlane(space, (0,1,0), 0)
|
|
|
|
# A list with ODE bodies
|
|
bodies = []
|
|
|
|
# The geoms for each of the bodies
|
|
geoms = []
|
|
|
|
# A joint group for the contact joints that are generated whenever
|
|
# two bodies collide
|
|
contactgroup = ode.JointGroup()
|
|
|
|
# Some variables used inside the simulation loop
|
|
fps = 50
|
|
dt = 1.0/fps
|
|
running = True
|
|
state = 0
|
|
counter = 0
|
|
objcount = 0
|
|
lasttime = time.time()
|
|
|
|
|
|
# keyboard callback
|
|
def _keyfunc (c, x, y):
|
|
sys.exit (0)
|
|
|
|
glutKeyboardFunc (_keyfunc)
|
|
|
|
# draw callback
|
|
def _drawfunc ():
|
|
# Draw the scene
|
|
prepare_GL()
|
|
for b in bodies:
|
|
draw_body(b)
|
|
|
|
glutSwapBuffers ()
|
|
|
|
glutDisplayFunc (_drawfunc)
|
|
|
|
# idle callback
|
|
def _idlefunc ():
|
|
global counter, state, lasttime
|
|
|
|
t = dt - (time.time() - lasttime)
|
|
if (t > 0):
|
|
time.sleep(t)
|
|
|
|
counter += 1
|
|
|
|
if state==0:
|
|
if counter==20:
|
|
drop_object()
|
|
if objcount==30:
|
|
state=1
|
|
counter=0
|
|
# State 1: Explosion and pulling back the objects
|
|
elif state==1:
|
|
if counter==100:
|
|
explosion()
|
|
if counter>300:
|
|
pull()
|
|
if counter==500:
|
|
counter=20
|
|
|
|
glutPostRedisplay ()
|
|
|
|
# Simulate
|
|
n = 4
|
|
|
|
for i in range(n):
|
|
# Detect collisions and create contact joints
|
|
space.collide((world,contactgroup), near_callback)
|
|
|
|
# Simulation step
|
|
world.step(dt/n)
|
|
|
|
# Remove all contact joints
|
|
contactgroup.empty()
|
|
|
|
lasttime = time.time()
|
|
|
|
glutIdleFunc (_idlefunc)
|
|
|
|
glutMainLoop ()
|
|
|