add ode

1 files changed, 284 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/bindings/python/demos/tutorial3.py b/libs/ode-0.16.1/bindings/python/demos/tutorial3.py
new file mode 100644
index 0000000..2644d62
--- /dev/null
+++ b/libs/ode-0.16.1/bindings/python/demos/tutorial3.py
@@ -0,0 +1,284 @@
+#!/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 ()
+