# test code: http://pyode.sourceforge.net/tutorials/tutorial3.html
#
#i want selecting object in pyode test code.
# what's wrong?
# under modify code
# see selectObject() function
# 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)
obj_map = {}
def _mousefunc(b, s, x, y):
if b == 0 and s == 0:
selectObject(x, y)
def selectObject(x, y):
print x, y
glSelectBuffer( 512)
viewport = glGetIntegerv(G L_VIEWPORT)
glMatrixMode(GL _PROJECTION)
glPushMatrix()
glRenderMode(GL _SELECT)
glLoadIdentity( )
gluPickMatrix(x , viewport[3]-y, 2, 2, viewport)
gluPerspective (45,1.333,0.1,1 00)
gluLookAt (2.4, 3.6, 4.8, 0.5, 0.5, 0, 0, 1, 0)
glMatrixMode(GL _MODELVIEW)
glLoadIdentity( )
_drawfunc()
buffer = glRenderMode(GL _RENDER)
for hit_record in buffer:
print hit_record
if buffer:
obj_name = buffer[0][2][0]
else:
obj_name = 0
glMatrixMode(GL _PROJECTION)
glPopMatrix()
glMatrixMode(GL _MODELVIEW)
# prepare_GL
def prepare_GL():
"""Prepare drawing.
"""
# Viewport
glViewport(0,0, 640,480)
# Initialize
glClearColor(0. 8,0.8,0.9,0)
glClear(GL_COLO R_BUFFER_BIT | GL_DEPTH_BUFFER _BIT);
glEnable(GL_DEP TH_TEST)
glDisable(GL_LI GHTING)
glEnable(GL_LIG HTING)
glEnable(GL_NOR MALIZE)
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_LI GHT0,GL_POSITIO N,[0,0,1,0])
glLightfv(GL_LI GHT0,GL_DIFFUSE ,[1,1,1,1])
glLightfv(GL_LI GHT0,GL_SPECULA R,[1,1,1,1])
glEnable(GL_LIG HT0)
# 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.
"""
global obj_map
x,y,z = body.getPositio n()
R = body.getRotatio n()
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(r ot)
if body.shape=="bo x":
sx,sy,sz = body.boxsize
glScale(sx, sy, sz)
glPushName(obj_ map[body])
glutSolidCube(1 )
glPopName()
glPopMatrix()
# create_box
def create_box(worl d, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setBox(densit y, 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(spa ce, lengths=body.bo xsize)
geom.setBody(bo dy)
return body
# drop_object
def drop_object():
"""Drop an object into the scene."""
global bodies, counter, objcount
body = create_box(worl d, space, 1000, 1.0,0.2,0.2)
body.setPositio n( (random.gauss(0 ,0.1),3.0,rando m.gauss(0,0.1)) )
theta = random.uniform( 0,2*pi)
ct = cos (theta)
st = sin (theta)
body.setRotatio n([ct, 0., -st, 0., 1., 0., st, 0., ct])
bodies.append(b ody)
counter=0
objcount+=1
global obj_map
obj_map[body] = objcount
# 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,1 0000,0))
# Collision callback
def near_callback(a rgs, 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(geo m1, geom2)
# Create contact joints
world,contactgr oup = args
for c in contacts:
c.setBounce(0.2 )
c.setMu(5000)
j = ode.ContactJoin t(world, contactgroup, c)
j.attach(geom1. getBody(), geom2.getBody() )
############### ############### ############### ############### ##########
# Initialize Glut
glutInit ([])
# Open a window
glutInitDisplay Mode (GLUT_RGB | GLUT_DOUBLE)
x = 0
y = 0
width = 640
height = 480
glutInitWindowP osition (x, y);
glutInitWindowS ize (width, height);
glutCreateWindo w ("testode")
# Create a world object
world = ode.World()
world.setGravit y( (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(s pace, (0,1,0), 0)
# A list with ODE bodies
bodies = []
# 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)
glutKeyboardFun c (_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
glutPostRedispl ay ()
# Simulate
n = 2
for i in range(n):
# Detect collisions and create contact joints
space.collide(( world,contactgr oup), near_callback)
# Simulation step
world.step(dt/n)
# Remove all contact joints
contactgroup.em pty()
lasttime = time.time()
glutIdleFunc (_idlefunc)
glutMouseFunc(_ mousefunc)
glutMainLoop ()
#
#i want selecting object in pyode test code.
# what's wrong?
# under modify code
# see selectObject() function
# 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)
obj_map = {}
def _mousefunc(b, s, x, y):
if b == 0 and s == 0:
selectObject(x, y)
def selectObject(x, y):
print x, y
glSelectBuffer( 512)
viewport = glGetIntegerv(G L_VIEWPORT)
glMatrixMode(GL _PROJECTION)
glPushMatrix()
glRenderMode(GL _SELECT)
glLoadIdentity( )
gluPickMatrix(x , viewport[3]-y, 2, 2, viewport)
gluPerspective (45,1.333,0.1,1 00)
gluLookAt (2.4, 3.6, 4.8, 0.5, 0.5, 0, 0, 1, 0)
glMatrixMode(GL _MODELVIEW)
glLoadIdentity( )
_drawfunc()
buffer = glRenderMode(GL _RENDER)
for hit_record in buffer:
print hit_record
if buffer:
obj_name = buffer[0][2][0]
else:
obj_name = 0
glMatrixMode(GL _PROJECTION)
glPopMatrix()
glMatrixMode(GL _MODELVIEW)
# prepare_GL
def prepare_GL():
"""Prepare drawing.
"""
# Viewport
glViewport(0,0, 640,480)
# Initialize
glClearColor(0. 8,0.8,0.9,0)
glClear(GL_COLO R_BUFFER_BIT | GL_DEPTH_BUFFER _BIT);
glEnable(GL_DEP TH_TEST)
glDisable(GL_LI GHTING)
glEnable(GL_LIG HTING)
glEnable(GL_NOR MALIZE)
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_LI GHT0,GL_POSITIO N,[0,0,1,0])
glLightfv(GL_LI GHT0,GL_DIFFUSE ,[1,1,1,1])
glLightfv(GL_LI GHT0,GL_SPECULA R,[1,1,1,1])
glEnable(GL_LIG HT0)
# 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.
"""
global obj_map
x,y,z = body.getPositio n()
R = body.getRotatio n()
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(r ot)
if body.shape=="bo x":
sx,sy,sz = body.boxsize
glScale(sx, sy, sz)
glPushName(obj_ map[body])
glutSolidCube(1 )
glPopName()
glPopMatrix()
# create_box
def create_box(worl d, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setBox(densit y, 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(spa ce, lengths=body.bo xsize)
geom.setBody(bo dy)
return body
# drop_object
def drop_object():
"""Drop an object into the scene."""
global bodies, counter, objcount
body = create_box(worl d, space, 1000, 1.0,0.2,0.2)
body.setPositio n( (random.gauss(0 ,0.1),3.0,rando m.gauss(0,0.1)) )
theta = random.uniform( 0,2*pi)
ct = cos (theta)
st = sin (theta)
body.setRotatio n([ct, 0., -st, 0., 1., 0., st, 0., ct])
bodies.append(b ody)
counter=0
objcount+=1
global obj_map
obj_map[body] = objcount
# 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,1 0000,0))
# Collision callback
def near_callback(a rgs, 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(geo m1, geom2)
# Create contact joints
world,contactgr oup = args
for c in contacts:
c.setBounce(0.2 )
c.setMu(5000)
j = ode.ContactJoin t(world, contactgroup, c)
j.attach(geom1. getBody(), geom2.getBody() )
############### ############### ############### ############### ##########
# Initialize Glut
glutInit ([])
# Open a window
glutInitDisplay Mode (GLUT_RGB | GLUT_DOUBLE)
x = 0
y = 0
width = 640
height = 480
glutInitWindowP osition (x, y);
glutInitWindowS ize (width, height);
glutCreateWindo w ("testode")
# Create a world object
world = ode.World()
world.setGravit y( (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(s pace, (0,1,0), 0)
# A list with ODE bodies
bodies = []
# 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)
glutKeyboardFun c (_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
glutPostRedispl ay ()
# Simulate
n = 2
for i in range(n):
# Detect collisions and create contact joints
space.collide(( world,contactgr oup), near_callback)
# Simulation step
world.step(dt/n)
# Remove all contact joints
contactgroup.em pty()
lasttime = time.time()
glutIdleFunc (_idlefunc)
glutMouseFunc(_ mousefunc)
glutMainLoop ()