I think I've found a problem with collision detection. Attached is a
slightly modified version of the script I posted yesterday. When you
run it, you'll see two circles to the left and right and a square in
the center.
Gravity will cause the two circles to fall downward, but the square
has no body, and only serves to detect collisions. When a circle
contacts the square, it will turn blue.
As you can see, even those the circles never visibly come in contact
with the square, PyODE is still registering collisions with the left
circle. It's almost like PyODE thinks the square is positioned at
(0,0), even though geombox.getPosition() confirms it's positioned in
the center of the screen.
At first I thought this was a problem with my PyGame drawing routine,
but I can't find any errors. Can anyone else confirm problems with
PyODE detecting collisions with bodiless geoms? Is there any way to
fix this?
Thanks,
Chris
'''
todo: Use plane2DJoint to restrict Z movement.
The method listed at http://opende.sourceforge.net/wiki/index.php/HOWTO_constrain_objects_to_2d
does not appear to work.
todo: Detect objects in geomBox. Collision detection works with geomPlane, but not geomBox
for some reason.
todo: stop objects from going through each other
'''
import sys, os, random, time
from math import sqrt
import pygame
from pygame.locals import *
import ode
from numpy import matrix
bodies = []
def create_box(world, space, position, density, lengths, color, body=1):
"""Create a box body and its corresponding geom."""
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=lengths)
if body:
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setBox(density, *lengths)
body.setMass(M)
geom.setBody(body)
# constrain to 2D
# todo: fix, this does nothing!!!
planeJoint.attach(body, None)
# Set parameters for drawing the body
geom.shape = "box"
geom.color = color
geom.setPosition(position)
global bodies
bodies.append(geom)
return geom
def create_sphere(world, space, position, density, radius, color, geom=1):
"""Create a sphere body and its corresponding geom."""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
# Create a box geom for collision detection
geom = ode.GeomSphere(space, radius)
geom.setBody(body)
# Set parameters for drawing the body
geom.shape = body.shape = "sphere"
geom.color = body.color = color
body.setPosition(position)
# constrain to 2D
# todo: fix, this does nothing!!!
planeJoint.attach(body, None)
global bodies
bodies.append(geom)
return geom
# Collision callback
viewboxContacts = set()
contactDict = {}
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.
"""
if geom1 is geom2:
return
if geom1.shape == geom2.shape == 'plane':
return # ignore static planes colliding with static planes
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
#if contacts:
# print '%i contacts!!!!!!!!!!!!!!!!' % len(contacts)
print '%s collided with %s' % (geom1.name, geom2.name)
# Create contact joints
world,contactgroup = args
for c in contacts:
# viewbox should be bodiless...
# but explicitly ignore anyways since it seems to be effecting collisions
#if geom1 is viewbox1 or geom2 is viewbox1:
# continue
c.setBounce(1)
c.setMu(5000)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
'''
# mark geoms in contact with viewbox (blue)
if geom1 is viewbox1 or geom2 is viewbox1:
#print time.time(),'intersecting viewbox!!!!!!!!!!!!',geom1,geom2
#viewboxContacts.update([g for g in (geom1,geom2) if g is not viewbox1])
#print viewboxContacts
for g in [g for g in (geom1,geom2) if g is not viewbox1]:
g.color = (0,0,255)
'''
'''
# mark geoms in contact with floor (red)
if geom1 is floor or geom2 is floor:
for g in [g for g in (geom1,geom2) if g is not viewbox1]:
g.color = (255,0,0)
'''
def viewbox_callback(args, geom1, geom2):
if geom1 is geom2:
return
print '%s vcollided with %s' % (geom1.name, geom2.name)
print 'vcbits:',geom2.getCollideBits()
print 'vpos:',geom2.getPosition()
print 'gpos:',geom1.getPosition()
if geom1 is not viewbox1:
geom1.color = (0,0,255)
if geom2 is not viewbox1:
geom2.color = (0,0,255)
def alignToZAxis(body):
rot = body.getAngularVel()
quat = list(body.getQuaternion())
quat[1] = 0
quat[2] = 0
quat_len = sqrt( quat[0] * quat[0] + quat[3] * quat[3] )
quat[0] /= quat_len
quat[3] /= quat_len
body.setQuaternion(quat)
body.setAngularVel((0, 0, rot[2]))
pixel_w,pixel_h = pixel_dimensions = 640,480
camera_w,camera_h = camera_position = pixel_w/2,pixel_h/2
#def cameraOffset(x,y):
# return camera_w+x, camera_h+y
# Initialize pygame
pygame.init()
# Open a display
srf = pygame.display.set_mode(pixel_dimensions)
# A list with ODE bodies
bodies = []
# restrict positions to these ranges
x_min,x_max = 50,590
y_min,y_max = 50,430
z_min,z_max = 0,0
# Create a world object
world = ode.World()
world.setGravity( (0,9.81,0) )
world.setERP(0.8)
world.setCFM(1E-9)
# Create a space object
space = ode.Space(type=1)
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(space, (0,-1,0), -430)
floor.shape = 'plane'
floor.name = 'floor'
bodies.append(floor)
#'''
floor2 = ode.GeomPlane(space, (1,0,0), 50)
floor2.shape = 'plane'
floor2.name = 'left wall'
bodies.append(floor2)
floor3 = ode.GeomPlane(space, (0,1,0), 50)
floor3.shape = 'plane'
floor3.name = 'right wall'
bodies.append(floor3)
#'''
# these don't work as expected...
# causes spheres to fly to extremes...
# generates "ODE Message 2: vector has zero size in dNormalize4() File odemath.cpp Line 129"
floor4 = ode.GeomPlane(space, (-1,0,0), -590)
floor4.shape = 'plane'
floor4.name = 'ceiling'
bodies.append(floor4)
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
planeJoint = ode.Plane2DJoint(world) # restricts movement to 2D
# Some variables used inside the simulation loop
fps = 50
dt = 1.0/fps
running = True
state = 0
counter = 0
objcount = 0
lasttime = time.time()
# create some objects
sphere1 = create_sphere(
world=world,
space=space,
#position=(320,150,0), # center
#position=(220,150,0), # left
position=(120,150,0), # far-left
#position=(420,150,0), # right
#position=(520,150,0), # far-right
density=220,
radius=50,
color=(255,255,255)
)
sphere1.name = 'sphere1'
sphere1 = create_sphere(
world=world,
space=space,
#position=(320,150,0), # center
#position=(220,150,0), # left
#position=(120,150,0), # far-left
#position=(420,150,0), # right
position=(520,150,0), # far-right
density=220,
radius=50,
color=(255,255,255)
)
sphere1.name = 'sphere2'
'''
sphere2 = create_sphere(
world=world,
space=space,
position=(320,270,0),
density=220,
radius=50,
color=(255,255,255)
)
sphere2.name = 'sphere2'
sphere3 = create_sphere(
world=world,
space=space,
position=(370,150,0),
density=220,
radius=50,
color=(255,255,255)
)
sphere3.name = 'sphere3'
w,h = 200,20
box1 = create_box(
world=world,
space=space,
position=(450-w/2,40-h/2,-100),
density=220,
lengths=(w,h,50),
color=(255,255,255)
)
box1.name = 'box1'
'''
# create bounding box to detect shapes in user's view
w,h = 200,200
viewbox1 = create_box(
world=world,
space=space,
position=(320-w/2,240-h/2,-100),
density=220,
lengths=(w,h,100),
color=(255,255,255),
body=0
)
viewbox1.name = 'viewbox1'
# Simulation loop...
fps = 30
dt = 1.0/fps
loopFlag = True
clk = pygame.time.Clock()
while loopFlag:
events = pygame.event.get()
for e in events:
if e.type==QUIT:
loopFlag=False
if e.type==KEYDOWN:
loopFlag=False
# Clear the screen
srf.fill((0,0,0))
# Draw the bodies
for geom in bodies:
if geom.getBody():
alignToZAxis(geom.getBody())
if geom.shape == 'sphere':
x1,y1,z1 = geom.getPosition()
#print 'z',z1
if geom.getBody():
x1 = min(max(x_min, x1), x_max)
y1 = min(max(y_min, y1), y_max)
z1 = min(max(z_min, z1), z_max)
geom.setPosition((x1,y1,0.0))
pygame.draw.circle(srf, geom.color, (x1,y1), geom.getRadius(), 1)
elif geom.shape == 'box':
x1,y1,z1 = geom.getPosition()
print 'pos:',geom.name,geom.getPosition()
lx,ly,lz = geom.getLengths()
#print 'z',z1
#print geom.getRotation()
if geom.getBody():
#print 'no body',geom
x1 = min(max(x_min, x1), x_max)
y1 = min(max(y_min, y1), y_max)
z1 = min(max(z_min, z1), z_max)
geom.setPosition((x1,y1,0))
#print Rect(x1,y1,lx,ly) # this will error if units get too large
#x1,y1 = cameraOffset(x1,y1)
p1 = x1,y1
p2 = x1+lx,y1+ly
p3 = p1[0],p2[1]
p4 = p2[0],p1[1]
points = p1,p4,p2,p3
pos = geom.getPosition()
#print 'raw:',points
# untranslate
points = map(lambda p: (p[0]-pos[0],p[1]-pos[1]), points)
#print 'untran:',points
if geom.placeable():
r = geom.getRotation()
R = matrix([r[i:i+2] for i in xrange(0,6,3)]) # find rotation matrix (take only x,y components)
points = map(matrix, points) # convert to matrix
points = map(lambda p: p*R, points) # transform points
points = map(lambda p: p.tolist()[0], points) # convert to list
#print 'roted:',points
# retranslate
points = map(lambda p: (p[0]+pos[0],p[1]+pos[1]), points)
#print 'retran:',points
#pygame.draw.rect(srf, geom.color, Rect(x1,y1,lx,ly), 1)
pygame.draw.polygon(srf, (255,255,255), points, 1)
elif geom.shape == 'plane':
normal,dist = geom.getParams()
nx,ny,nz = normal
if ny:
f = lambda x: (dist-nx*x)/ny
p1 = 0, f(0)
p2 = pixel_w, f(pixel_w)
else:
f = lambda y: (dist-ny*y)/nx
p1 = f(0), 0
p2 = f(pixel_h), pixel_h
#print p1,p2
pygame.draw.line(srf, (255,255,255), p1, p2, 1)
else:
print 'unknown shape',geom
# clear prior marking
geom.color = (255,255,255)
# Remove all contact joints and unmark bodies no longer in contact
contactgroup.empty()
viewboxContacts.clear()
# Simulate
n = 2
for i in xrange(n):
# Detect collisions and create contact joints
space.collide((world,contactgroup), near_callback)
# Simulation step
#world.step(dt/n)
world.quickStep(dt/n)
# detect objects in viewing box
ode.collide2(viewbox1, space, None, viewbox_callback)
pygame.display.flip()
# Next simulation step
world.step(dt)
# Try to keep the specified framerate
clk.tick(fps)
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Pyode-user mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/pyode-user
