Make sure to set the CA env variable

drop2.py

@@ -0,0 +1,317 @@
+# 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()