add ode

3 files changed, 468 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/bindings/python/demos/tutorial1.py b/libs/ode-0.16.1/bindings/python/demos/tutorial1.py
new file mode 100755
index 0000000..c4266d4
--- /dev/null
+++ b/libs/ode-0.16.1/bindings/python/demos/tutorial1.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+
+# http://pyode.sourceforge.net/tutorials/tutorial1.html
+
+# pyODE example 1: Getting started
+
+# modified by Gideon Klompje (removed literals and using
+# 'ode.Mass.setSphereTotal' instead of 'ode.Mass.setSphere')
+
+import ode
+
+# Simulation constants
+GRAVITY = (0, -9.81, 0)
+
+SPHERE_RADIUS = 0.05
+SPHERE_MASS = 1.0
+SPHERE_START_POS = (0, 2, 0)
+SPHERE_FORCE = (0, 200, 0) # Initial force to apply to the sphere
+
+TIME_STEP = 0.04
+TIME_STOP = 2.0 # When to stop the simulation
+
+# Create a world object
+world = ode.World()
+world.setGravity(GRAVITY)
+
+# Create a spherical body inside the world
+body = ode.Body(world)
+mass = ode.Mass()
+mass.setSphereTotal(SPHERE_MASS, SPHERE_RADIUS)
+body.setMass(mass)
+
+body.setPosition(SPHERE_START_POS)
+body.addForce(SPHERE_FORCE)
+
+# Do the simulation...
+if __name__ == "__main__":
+    total_time = 0.0
+    while total_time < TIME_STOP:
+        # output the body's position and velocity
+        x, y, z = body.getPosition()
+        u, v, w = body.getLinearVel()
+        print "%1.2fsec: pos=(%6.3f, %6.3f, %6.3f)  vel=(%6.3f, %6.3f, %6.3f)" % \
+            (total_time, x, y, z, u, v, w)
+
+        # advance the simulation
+        world.step(TIME_STEP)
+        total_time += TIME_STEP
+
diff --git a/libs/ode-0.16.1/bindings/python/demos/tutorial2.py b/libs/ode-0.16.1/bindings/python/demos/tutorial2.py
new file mode 100755
index 0000000..1a4a228
--- /dev/null
+++ b/libs/ode-0.16.1/bindings/python/demos/tutorial2.py
@@ -0,0 +1,135 @@
+#!/usr/bin/env python
+
+# http://pyode.sourceforge.net/tutorials/tutorial2.html
+
+# pyODE example 2: Connecting bodies with joints
+
+# modified by Gideon Klompje (removed literals and using
+# 'ode.Mass.setSphereTotal' instead of 'ode.Mass.setSphere')
+
+
+import ode
+import pygame
+
+from pygame.locals import QUIT, KEYDOWN
+
+# Constants
+WINDOW_RESOLUTION = (640, 480)
+
+DRAW_SCALE = WINDOW_RESOLUTION[0] / 5
+"""Factor to multiply physical coordinates by to obtain screen size in pixels"""
+
+DRAW_OFFSET = (WINDOW_RESOLUTION[0] / 2, 50)
+"""Screen coordinates (in pixels) that map to the physical origin (0, 0, 0)"""
+
+BACKGROUND_COLOR = (255, 255, 255)
+
+GRAVITY = (0, -9.81, 0)
+
+SPHERE1_POSITION = (1, 0, 0)
+SPHERE1_MASS = 1
+SPHERE1_RADIUS = 0.15
+SPHERE1_COLOR = (55, 0, 200)
+
+SPHERE2_POSITION = (2, 0, 0)
+SPHERE2_MASS = 1
+SPHERE2_RADIUS = 0.15
+SPHERE2_COLOR = (55, 0, 200)
+
+JOINT1_ANCHOR = (0, 0, 0)
+JOINT1_COLOR = (200, 0, 55)
+JOINT1_WIDTH = 2
+"""Width of the line (in pixels) representing the joint"""
+
+JOINT2_ANCHOR = SPHERE1_POSITION
+JOINT2_COLOR = (200, 0, 55)
+JOINT2_WIDTH = 2
+"""Width of the line (in pixels) representing the joint"""
+
+TIME_STEP = 0.04
+
+# Utility functions
+def coord(x, y, integer=False):
+    """
+    Convert world coordinates to pixel coordinates.  Setting 'integer' to
+    True will return integer coordinates.
+    """
+    xs = (DRAW_OFFSET[0] + DRAW_SCALE*x)
+    ys = (DRAW_OFFSET[1] - DRAW_SCALE*y)
+
+    if integer:
+        return int(round(xs)), int(round(ys))
+    else:
+        return xs, ys
+
+# Initialize pygame
+pygame.init()
+
+# Open a display
+screen = pygame.display.set_mode(WINDOW_RESOLUTION)
+
+# Create a world object
+world = ode.World()
+world.setGravity(GRAVITY)
+
+# Create two bodies
+body1 = ode.Body(world)
+M = ode.Mass()
+M.setSphereTotal(SPHERE1_MASS, SPHERE1_RADIUS)
+body1.setMass(M)
+body1.setPosition(SPHERE1_POSITION)
+
+body2 = ode.Body(world)
+M = ode.Mass()
+M.setSphereTotal(SPHERE2_MASS, SPHERE2_RADIUS)
+body2.setMass(M)
+body2.setPosition(SPHERE2_POSITION)
+
+# Connect body1 with the static environment
+j1 = ode.BallJoint(world)
+j1.attach(body1, ode.environment)
+j1.setAnchor(JOINT1_ANCHOR)
+
+# Connect body2 with body1
+j2 = ode.BallJoint(world)
+j2.attach(body1, body2)
+j2.setAnchor(JOINT2_ANCHOR)
+
+# Simulation loop...
+if __name__ == "__main__":
+    fps = 1.0 / TIME_STEP
+    clk = pygame.time.Clock()
+
+    sph1_rad = int(DRAW_SCALE * SPHERE1_RADIUS)
+    sph2_rad = int(DRAW_SCALE * SPHERE2_RADIUS)
+
+    loopFlag = True
+    while loopFlag:
+        for e in pygame.event.get():
+            if e.type==QUIT:
+                loopFlag=False
+            if e.type==KEYDOWN:
+                loopFlag=False
+
+        # Clear the screen
+        screen.fill(BACKGROUND_COLOR)
+
+        # Draw the two bodies and the lines representing the joints
+        x1, y1, z1 = body1.getPosition()
+        x2, y2, z2 = body2.getPosition()
+        xj1, yj1, zj1 = j1.getAnchor()
+        xj2, yj2, zj2 = j2.getAnchor()
+
+        pygame.draw.line(screen, JOINT1_COLOR, coord(xj1, yj1), coord(x1, y1), JOINT1_WIDTH)
+        pygame.draw.line(screen, JOINT2_COLOR, coord(xj2, yj2), coord(x2, y2), JOINT2_WIDTH)
+        pygame.draw.circle(screen, SPHERE1_COLOR, coord(x1, y1, integer=True), sph1_rad, 0)
+        pygame.draw.circle(screen, SPHERE2_COLOR, coord(x2, y2, integer=True), sph2_rad, 0)
+
+        pygame.display.flip()
+
+        # Next simulation step
+        world.step(TIME_STEP)
+
+        # Try to keep the specified framerate    
+        clk.tick(fps)
+
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 ()
+