Commits

German Larrain committed 438dbc7

external-libs tests: new ray_collision program, which does not depend on ARS

Comments (0)

Files changed (1)

tests/external-libs/ray_collision.py

+#!/usr/bin/env python
+
+# Created on 2012.05.09
+#
+# @author: german
+
+"""
+Tests the collision of 2 spheres with a ray.
+It does not use ARS or depend on it in any way.
+"""
+
+import math
+
+import ode
+import vtk
+
+
+#===========================================================================
+# constants
+#===========================================================================
+TIMER_PERIOD = 50 # milliseconds
+TIMER_EVENT = 'TimerEvent'
+WIN_SIZE = (1000,600)
+CAM_POSITION = (30,10,50)
+SPHERE_RESOLUTION = 20
+
+BALL_CENTER = (0,3,5)
+BALL_RADIUS = 1.0
+BALL_MASS_VALUE = 1.0
+BALL2_OFFSET = (0.5,1,2)
+
+RAY_LENGTH = 1000.0
+RAY_POS = (0,0,0)
+
+FPS = 50
+DT = 1.0 / FPS
+STEPS_PER_FRAME = 100
+
+#===========================================================================
+# variables
+#===========================================================================
+world = None
+space = None
+contactGroup = None
+
+ball_body = None
+ball_actor = None
+ball2_body = None
+ball2_actor = None
+
+sim_time = 0.0
+num_iter = 0
+
+#===========================================================================
+# aux functions
+#===========================================================================
+def length3(vector):
+	"""Returns the length of a 3-dimensions vector"""
+	return math.sqrt(vector[0]**2 + vector[1]**2 + vector[2]**2)
+
+def add3(vector1, vector2):
+	"""Returns the sum of 3-vectors vector1 and vector2"""
+	return (vector1[0] + vector2[0], vector1[1] + vector2[1], vector1[2] + vector2[2])
+
+def sub3(vector1, vector2):
+	"""Returns the difference between 3-vectors vector1 and vector2"""
+	return (vector1[0] - vector2[0], vector1[1] - vector2[1], vector1[2] - vector2[2])
+
+def timerCallback(obj, event):
+	performSimStepsPerFrame()
+	pos = ball_body.getPosition()
+	pos2 = ball2_body.getPosition()
+	ball_actor.SetPosition(*pos)
+	ball2_actor.SetPosition(*pos2)
+
+	iren = obj
+	iren.GetRenderWindow().Render()
+
+#===========================================================================
+# collisions functions
+#===========================================================================
+def handle_ray_collision(ray, other_geom, contact):
+	ray_pos = ray.getPosition()
+	(pos, normal, depth, geom1, geom2) = contact.getContactGeomParams()
+	distance = length3(sub3(pos,ray_pos))
+	print('ray collision between %s and %s. distance: %f' % (ray,other_geom,distance))
+
+def is_geom_ray(geom):
+	"""Is `geom` a ray?"""
+	return isinstance(geom, ode.GeomRay)
+
+def are_geoms_connected(geom1, geom2):
+	"""
+	Are `geom1` and `geom2` connected (through the bodies they are attached to)?
+	"""
+	return ode.areConnected(geom1.getBody(), geom2.getBody())
+
+def geoms_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."""
+	contactJointBounce = 0.2
+	contactJointMu = 500 # 0-5 = very slippery, 50-500 = normal, 5000 = very sticky
+
+	world, contactgroup = args
+	ray_geom = None
+	other_geom = None
+
+	if are_geoms_connected(geom1, geom2):
+		return
+
+	# see if any of the geoms is a Ray
+	if is_geom_ray(geom1):
+		if is_geom_ray(geom2):
+			print('Weird, two rays collided. That case is not handled.')
+			return
+		else:
+			ray_geom = geom1
+			other_geom = geom2
+
+	elif is_geom_ray(geom2):
+		ray_geom = geom2
+		other_geom = geom1
+
+	# check if the objects collide and generate 'contacts'
+	contacts = ode.collide(geom1, geom2)
+
+	for c in contacts:
+		(pos, normal, depth, geom1, geom2) = c.getContactGeomParams()
+
+		# we create a ContactJoint only if both geoms are not rays
+		if ray_geom is not None:
+			handle_ray_collision(ray_geom, other_geom, c)
+		else:
+			print((pos, normal, depth))
+			c.setBounce(contactJointBounce)
+			c.setMu(contactJointMu)
+			j = ode.ContactJoint(world, contactgroup, c)
+			j.attach(geom1.getBody(), geom2.getBody())
+
+#===========================================================================
+# sim functions
+#===========================================================================
+def createOdeWorld(gravity=(0.0,-9.81,0.0), erp=0.8, cfm=1E-4):
+	"""create and configure and ODE world object"""
+	world = ode.World()
+	world.setGravity(gravity)
+	world.setERP(erp)
+	world.setCFM(cfm)
+	return world
+
+def performSimStepsPerFrame():
+
+	global STEPS_PER_FRAME, world, contactGroup, num_iter, sim_time
+
+	for i in range(STEPS_PER_FRAME):
+		# Detect collisions and create contact joints
+		space.collide((world, contactGroup), geoms_near_callback)
+
+		# Simulation step
+		timeStep = DT / STEPS_PER_FRAME
+		world.step(timeStep)
+		sim_time += timeStep
+		num_iter += 1
+
+		# Remove all contact joints
+		contactGroup.empty()
+
+if __name__ == '__main__':
+
+	world = createOdeWorld()
+	space = ode.Space()
+	contactGroup = ode.JointGroup()
+
+	#===========================================================================
+	# Ball (ODE)
+	#===========================================================================
+	ball_body = ode.Body(world)
+	ball2_body = ode.Body(world)
+
+	ball_mass = ode.Mass()
+	ball2_mass = ode.Mass()
+	ball_mass.setSphereTotal(BALL_MASS_VALUE, BALL_RADIUS)
+	ball2_mass.setSphereTotal(BALL_MASS_VALUE, BALL_RADIUS)
+	ball_body.setMass(ball_mass)
+	ball2_body.setMass(ball2_mass)
+
+	ball_geom = ode.GeomSphere(space, BALL_RADIUS)
+	ball2_geom = ode.GeomSphere(space, BALL_RADIUS)
+	ball_geom.setBody(ball_body)
+	ball2_geom.setBody(ball2_body)
+
+	ball_body.setPosition(BALL_CENTER)
+	ball2_body.setPosition(add3(BALL_CENTER, BALL2_OFFSET))
+
+	#===========================================================================
+	# Ray (ODE)
+	#===========================================================================
+	ray_geom = ode.GeomRay(space, RAY_LENGTH)
+	ray_geom.setPosition(RAY_POS)
+
+	#===========================================================================
+	# VTK
+	#===========================================================================
+	# create a rendering window and renderer
+	ren = vtk.vtkRenderer()
+	renWin = vtk.vtkRenderWindow()
+	renWin.AddRenderer(ren)
+	renWin.SetSize(WIN_SIZE)
+	ren.GetActiveCamera().SetPosition(CAM_POSITION)
+
+	# create a renderwindowinteractor
+	iren = vtk.vtkRenderWindowInteractor()
+	iren.SetRenderWindow(renWin)
+	iren.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera())
+
+	#===========================================================================
+	# draw sphere
+	#===========================================================================
+	ball_source = vtk.vtkSphereSource()
+	ball2_source = vtk.vtkSphereSource()
+	ball_source.SetRadius(BALL_RADIUS)
+	ball2_source.SetRadius(BALL_RADIUS)
+	# do NOT use ball_source.SetCenter(ball_center)
+
+	ball_source.SetPhiResolution(SPHERE_RESOLUTION) # looks more sphere-like
+	ball2_source.SetPhiResolution(SPHERE_RESOLUTION)
+	ball_source.SetThetaResolution(SPHERE_RESOLUTION)
+	ball2_source.SetThetaResolution(SPHERE_RESOLUTION)
+
+	ball_mapper = vtk.vtkPolyDataMapper()
+	ball2_mapper = vtk.vtkPolyDataMapper()
+	ball_mapper.SetInputConnection(ball_source.GetOutputPort())
+	ball2_mapper.SetInputConnection(ball2_source.GetOutputPort())
+
+	ball_actor = vtk.vtkActor()
+	ball2_actor = vtk.vtkActor()
+	ball_actor.SetMapper(ball_mapper)
+	ball2_actor.SetMapper(ball2_mapper)
+
+	ren.AddActor(ball_actor)
+	ren.AddActor(ball2_actor)
+
+	#===========================================================================
+	# start visualization
+	#===========================================================================
+	iren.Initialize() # enable user interface interactor
+	renWin.Render()
+	iren.AddObserver(TIMER_EVENT, timerCallback)
+	timerId = iren.CreateRepeatingTimer(TIMER_PERIOD)
+	iren.Start()
+	print('done')
+	print('simTime: %f, numIter: %d' % (sim_time, num_iter))