[Pyode-user] Collision detection and coordinates

[Matthias T. <Sta...@gm...>]

Hi!

I am very sorry that i am posting again my test program, but i have a little 
problem which i am dialing with the whole day!
I was inspired from tutorial2 and tutorial3 and i added in tutorial2 a block 
which should be connected to the 
ode.environment and with collision detection! But there is a little problem 
with the coordinates and i don't know why!
My real life coordinates are 6 (meters) in the x and 4 in the y coordinate!

Thank you!
matthias


Here is the source:

import pygame
from pygame.locals import * 
import ode

WIDTH = 900
HEIGHT = 600

srf=None
world=None
space=None

def coord(x,y):
	"Convert world coordinates to pixel coordinates."
	xf = WIDTH / 6
	yf = HEIGHT / 4
	#return int(400+150*x), int(500-150*y)
	return int(xf * x), int(HEIGHT - (yf * y))

def near_callback(args, geom1, geom2):
	
	contacts = ode.collide(geom1, geom2)
	for c in contacts:
		c.setBounce(0.5)
		e = ode.ContactJoint(world, args, c)
		e.attach(geom1.getBody(), geom2.getBody())
		print 'Position of geom1 and block(geom2): ', geom1.getPosition(), 
geom2.getPosition()
		print 'Length of the block: ', geom2.getLengths()
	

def doSimulation(body, block):
	fps = 50
	dt = 1.0 / fps
	loopFlag = True
	
	clk = pygame.time.Clock()
	
	contgrp = ode.JointGroup()
	
	while loopFlag:
		events = pygame.event.get()
		for e in events:
			if e.type == QUIT or e.type == KEYDOWN:
				loopFlag = False
		
		srf.fill ( (255,255,255) )
		
		srf.lock()
		xv, yv = 3, 4
		
		# block zeichnen
		x,y,z = block.getPosition()
		l,h = block.laenge
		
		pygame.draw.rect(srf, (0,0,0), (coord(x,y + h), (l * (WIDTH / 6), h * 
(HEIGHT / 4) )), 0)
		
		# bodies zeichnen
		for i in range(len(body)):
			x1,y1,z1 = body[i].getPosition()
			
			pygame.draw.circle(srf, (50,0,200), coord(x1,y1), int(body[i].radius * 
(WIDTH/6)) )
			pygame.draw.line(srf, (100,0,200), coord(xv,yv), coord(x1,y1), 2)
	
			xv, yv = x1, y1
		
		srf.unlock()
		pygame.display.flip()
		
		for i in range(2):
			space.collide(contgrp,near_callback)
		
			world.step(dt / 2)
			contgrp.empty()
		
		clk.tick(fps)
		
	# end while
	
def createBody(world, space):
	
	radius = 0.2
		
	body = ode.Body(world)
	
	mass = ode.Mass()
	mass.setSphere(2000, radius)
	body.setMass(mass)
		
	geom = ode.GeomSphere(space, radius)
	geom.setBody(body)
	
	body.radius = 0.2
		
	return body

def createBlock(space, pos):
	
	geom = ode.GeomBox(space, (1.5, 2.5, 0) )
	geom.setPosition((4.5,0,0))
	geom.setBody(ode.environment)
	
	geom.laenge = (1.5, 2.5)
	
	return geom

if __name__ == '__main__':

	pygame.init()
	srf = pygame.display.set_mode( (900,600) )
	
	world = ode.World()
	world.setGravity( (0,-9.81,0) )
	world.setERP(0.5)
	space = ode.Space()
	
	bodies = []
	
	for i in range(3):
		body = createBody(world, space)
		body.setPosition((i+4,4,0))
		bodies.append(body)
		
	joint = ode.BallJoint(world)
	joint.attach(ode.environment, bodies[0])
	joint.setAnchor((3,4,0))
	
	joint2 = ode.BallJoint(world)
	joint2.attach(bodies[0], bodies[1])
	joint2.setAnchor((4,4,0))
	
	joint3 = ode.BallJoint(world)
	joint3.attach(bodies[1], bodies[2])
	joint3.setAnchor((5,4,0))
	
	block = createBlock(space, (4.5,0,0))
		
	doSimulation(bodies, block)