[Visualpython-users] Collision detection with VPython

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

I've tried to make VPython work with PyODE (http://pyode.sourceforge.net).  
The blocks seem to collide properly but they often settle on their edges.  I 
probably misunderstand the rotation matrix provided by PyODE or the "up" 
method of VPython.  Any help would be appreciated.  The VPython "cubes" get 
their positional and angular information from the PyODE "bodies".


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

_________________________________________________________________
Don’t just search. Find. Check out the new MSN Search! 
http://search.msn.click-url.com/go/onm00200636ave/direct/01/