[Pyode-user] Modified tutorial3.py

[Thomas P. <tho...@fr...>]

Hello,

You will find below a modified version of the script 'tutorial3.py'.

Press either 'b' to drop boxes geoms, 's' to drop spheres, 'c' to drop 
ccylinder or 'q' to exit. Dropping boxes is the default behaviour.

Regards,

Thomas Paviot

# pyODE example 3: Collision detection
# Originally by Matthias Baas.
# Updated by Pierre Gay to work without pygame or cgkit.
# Updated by Thomas Paviot (tho...@fr...) to drop either boxes, 
spheres or cylinders

import sys, os, random, time
from math import *
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
import ode

# geoms definition
BOX = 1
CCYLINDER = 2
SPHERE = 3
GEOM_TO_DROP = BOX
quadratic = None #necessary for cylinder

# 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.
    """
    global quadratic
    quadratic = gluNewQuadric()
    # Viewport
    glViewport(0,0,800,600)
    # 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=="sphere":
        radius = body.radius
        glutSolidSphere(radius,20,20)
    elif body.shape=="box":
        sx,sy,sz = body.boxsize
        glScale(sx, sy, sz)
        glutSolidCube(1)
    elif body.shape=="ccylinder":
        radius = body.radius
        height = body.height
        gluCylinder(quadratic,radius,radius,height,32,32)
        #gluCylinder(radius,height,20,20)
       
    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

def create_sphere(world, space, density, radius):
    """Create a box body and its corresponding geom."""
    # Create body
    body = ode.Body(world)
    M = ode.Mass()
    M.setSphere(density, radius)
    body.setMass(M)
    # Set parameters for drawing the body
    body.shape = "sphere"
    body.radius = radius
    # Create a Sphere geom for collision detection
    geom = ode.GeomSphere(space, radius)
    geom.setBody(body)
    return body

def create_ccylinder(world, space, density, radius, height):
    """Create a box body and its corresponding geom."""
    # Create body
    body = ode.Body(world)
    M = ode.Mass()
    M.setCappedCylinder(density, direction=1, r = radius, h = height)
    body.setMass(M)
    # Set parameters for drawing the body
    body.shape = "ccylinder"
    body.radius = radius
    body.height = height
    # Create a CCylinder geom for collision detection
    geom = ode.GeomCCylinder(space, radius, height)
    geom.setBody(body)
    return body

# drop_object
def drop_object():
    """Drop an object into the scene."""
    global bodies, counter, objcount
    if GEOM_TO_DROP==SPHERE:
        body = create_sphere(world, space, 1000, radius = 0.2)
    elif GEOM_TO_DROP == BOX:
        body = create_box(world, space, 1000, 1.0, 0.2,0.2)
    elif GEOM_TO_DROP == CCYLINDER:
        body = create_ccylinder(world, space, 1000, radius = 0.2, height 
= 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

# 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_DOUBLE)
x = 0
y = 0
width = 800
height = 600
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-3)
# 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 = []
# 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):
    global GEOM_TO_DROP
    if c=="b":
        GEOM_TO_DROP = BOX
    elif c=="s":
        GEOM_TO_DROP = SPHERE
    elif c=="c":
        GEOM_TO_DROP = CCYLINDER
    elif c=="q":
        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 = 2
    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)
#
#
#
print "Press 'b' to drop a box"
print "Press 's' to drop a sphere"
print "Press 'c' to drop a cylinder"
print "Press 'q' to quit."
glutMainLoop ()