drop2.py

# 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)
        glColor3i(255,0,0)
        glutSolidCube(1)
    glPopMatrix()


# create_box
# need a body to set the position of the geom
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


# drop_object
def create_silder():
    """Drop an object into the scene."""

    global bodies, geom, counter, objcount

    body, geom = create_box(world, space, 1000, 0.5, 0.5, 0.5)
    body.setPosition((0, 1, 0))
    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)
    sliders.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.5)
        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 = 640
height = 480
glutInitWindowPosition(x, y);
glutInitWindowSize(width, height);
glutCreateWindow("testode")

# Create a world object
world = ode.World()
world.setGravity((0, -10, 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 = []

sliders = []

# 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)

create_silder()

# 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 == 1:
            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)

glutMainLoop()