[Pyode-user] Collision detection in VPython

[Miles J. <mil...@ho...>]

I have tried to modify tutorial 3 to work with VPython.  The blocks seem to 
collide OK, but some of them come to rest on their edges.  I probably 
misunderstand the nature of the rotation matrix, or the "up" method in 
VPython.  Would appreciate any help.

Here is the code (running on Windows):

# pyODE example 3: Collision detection

# Originally by Matthias Baas.
# Attempt by Miles Jacobs to make collision detection work with VPython.

import sys, os, random, time
from math import *

from visual import *
import ode

from Scientific.Geometry import *
from Scientific.Geometry.TensorModule import *
from Scientific.Geometry.Transformation import *


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


# 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

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

    global bodies, counter, objcount

    body = create_box(world, space, 1000, 1.0,1.0,1.0)
    body.setPosition((random.gauss(0,0.1),10.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])
    matpot=Tensor([[ct, 0., -st],[0., 1., 0.],[st, 0., ct]])
    matkettle=Rotation(matpot)
    bodyaxis, bodyangle = matkettle.axisAndAngle()
    icecube = box(pos=body.getPosition(), axis=bodyaxis,
                  length=1.0, height=1.0, width=1.0,
                  
color=(random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)))
    bodies.append(body)
    cubes.append(icecube)
    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, 800000*(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, -100000 / length (l))
        b.addForce(l)
        if counter%60==0:
            b.addForce((0,100000,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())



######################################################################


# 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)
ground=box(pos=(0,-0.1,0), width=20, height=0.1, length=20, 
color=color.white)

# A list with ODE bodies and Visual objects to see the bodies
bodies = []
cubes = []

# 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 = 25
dt = 1.0/fps
running = True
state = 0
counter = 0
objcount = 0
scene.autoscale=0
scene.forward=(-5,-5,-5)

while(1):

    global counter, state

    counter += 1

    if state==0:
        if counter==20:
            drop_object()
        if objcount==10:
            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

    for b in range(len(bodies)):
        x,y,z = bodies[b].getPosition()
        R = bodies[b].getRotation()
        pot=Tensor([[R[0],R[3],R[6]],[R[1],R[4],R[7]],[R[2],R[5],R[8]]])
        kettle=Rotation(pot)
        bodyaxis, bodyangle = kettle.axisAndAngle()
        cubes[b].pos=(x,y,z)
        cubes[b].axis=bodyaxis
        cubes[b].angle=bodyangle

    print state, counter
    # 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()

    rate(10)


Miles.

_________________________________________________________________
Express yourself instantly with MSN Messenger! Download today - it's FREE! 
http://messenger.msn.click-url.com/go/onm00200471ave/direct/01/