"""Classes and functions to interface with the collision libraryincluded in ODE."""fromabcimportABCMetaimportodefrom.importbasefrom.importode_objects_factoriesfrom.baseimportNearCallbackArgs#==============================================================================# Environment#==============================================================================classOdeEngine(base.Engine):"""Adapter to the ODE collision engine."""#==========================================================================# Functions and methods not overriding base class functions and methods#==========================================================================@staticmethoddefnear_callback(args,geom1,geom2):"""Callback function for the collide() method (in ODE). This function checks if the given geoms do collide and creates contact joints (c_joint) if they do, except if they are connected."""# Contact joint parameters:# -bounce: default=0.2# -mu: default=500.# very slippery 0-5, normal 50-500, very sticky 5000c_joint_bounce=0.2c_joint_mu=500world=args.world._inner_objectcontact_group=args.contact_groupray_geom=Noneother_geom=Noneifargs.ignore_connectedandOdeEngine.are_geoms_connected(geom1,geom2):return#======================================================================# Ray's special case#======================================================================ifOdeEngine.is_ray(geom1):ifOdeEngine.is_ray(geom2):print('Weird, ODE says two rays may collide. ''That case is not handled.')returnelse:ray_geom=geom1other_geom=geom2elifOdeEngine.is_ray(geom2):ray_geom=geom2other_geom=geom1#======================================================================# create contact joints#======================================================================# check if the objects collidecontacts=ode.collide(geom1,geom2)forcincontacts:ifray_geomisnotNone:OdeEngine.handle_ray_collision(ray_geom,other_geom,c)else:# we create a ContactJoint only if both geoms are not rays# set contact parametersc.setBounce(c_joint_bounce)c.setMu(c_joint_mu)# create contact jointsj=ode.ContactJoint(world,contact_group,c)j.attach(geom1.getBody(),geom2.getBody())@staticmethoddefare_geoms_connected(geom1,geom2):"""Are `geom1` and `geom2` connected (through the bodies they are attached to)? """returnode.areConnected(geom1.getBody(),geom2.getBody())@staticmethoddefis_ray(geom):"""Return whether ``geom`` is a :class:`ode.GeomRay` object or not. :param geom: :type geom: :class:`ode.GeomObject` :return: True if ``geom`` is an instance of :class:`ode.GeomRay` :rtype: bool """returnisinstance(geom,ode.GeomRay)@staticmethoddefhandle_ray_collision(ray,other_geom,contact):# pos: intersection position# depth: distance(pos,normal,depth,geom1,geom2)=contact.getContactGeomParams()ray_contact=base.RayContactData(ray,other_geom,pos,normal,depth)try:ray.outer_object.set_last_contact(ray_contact)exceptAttributeError:print("`ray` has no attribute `outer_object` ")exceptException:print("Ray's encapsulating object's last_contact attribute could not be set")classSpace(base.Space):"""Adapter to :class:`ode.SimpleSpace`."""def__init__(self):self._inner_object=ode_objects_factories.create_ode_simple_space()defcollide(self,args,callback=None):ifcallbackisNone:self._inner_object.collide(args,OdeEngine.near_callback)else:self._inner_object.collide(args,callback)#==============================================================================# Parents#==============================================================================classOdeGeom(base.Geom):"""Abstract class, sort of equivalent to :class:`ode.GeomObject`."""defattach_body(self,body):super(OdeGeom,self).attach_body(body)self._inner_object.setBody(body.inner_object)#==========================================================================# Getters and setters#==========================================================================defget_position(self):"""Get the position of the geom. :return: position :rtype: 3-sequence of floats """returnself._inner_object.getPosition()defget_rotation(self):"""Get the orientation of the geom. :return: rotation matrix :rtype: 9-sequence of floats """returnself._inner_object.getRotation()defset_position(self,pos):"""Set the position of the geom. :param pos: position :type pos: 3-sequence of floats """self._inner_object.setPosition(pos)defset_rotation(self,rot):"""Set the orientation of the geom. :param rot: rotation matrix :type rot: 9-sequence of floats """self._inner_object.setRotation(rot)classOdeBasicShape(OdeGeom):__metaclass__=ABCMeta#==============================================================================# Other shapes#==============================================================================classPlane(OdeBasicShape,base.Plane):"""Plane, different from a box"""def__init__(self,space,normal,dist):OdeBasicShape.__init__(self)base.Plane.__init__(self,space,normal,dist)self._inner_object=ode_objects_factories.create_ode_plane(space.inner_object,normal,dist)classRay(OdeGeom,base.Ray):def__init__(self,space,length):OdeGeom.__init__(self)base.Ray.__init__(self,space,length)self._inner_object=ode_objects_factories.create_ode_ray(space.inner_object,length)self._inner_object.outer_object=self# the encapsulating objectdefget_length(self):returnself._inner_object.getLength()defset_length(self,length):self._inner_object.setLength(length)classTrimesh(OdeGeom,base.Trimesh):def__init__(self,space,vertices,faces,center):OdeGeom.__init__(self)base.Trimesh.__init__(self,space,vertices,faces,center)self._inner_object=ode_objects_factories.create_ode_trimesh(space.inner_object,vertices,faces)# FIXME: setting pos here is not consistent with the other constructorsself.set_position(center)#==============================================================================# Basic Shapes#==============================================================================classBox(OdeBasicShape,base.Box):"""Box shape, aligned along the X, Y and Z axii by default"""def__init__(self,space,size):OdeBasicShape.__init__(self)base.Box.__init__(self,space,size)self._inner_object=ode_objects_factories.create_ode_box(space.inner_object,size)classSphere(OdeBasicShape,base.Sphere):"""Spherical shape"""def__init__(self,space,radius):OdeBasicShape.__init__(self)base.Sphere.__init__(self,space,radius)self._inner_object=ode_objects_factories.create_ode_sphere(space.inner_object,radius)classCapsule(OdeBasicShape,base.Capsule):"""Capsule shape, aligned along the Z-axis by default"""def__init__(self,space,length,radius):OdeBasicShape.__init__(self)base.Capsule.__init__(self,space,length,radius)self._inner_object=ode_objects_factories.create_ode_capsule(space.inner_object,length,radius)classCylinder(OdeBasicShape,base.Cylinder):"""Cylinder shape, aligned along the Z-axis by default"""def__init__(self,space,length,radius):OdeBasicShape.__init__(self)base.Cylinder.__init__(self,space,length,radius)self._inner_object=ode_objects_factories.create_ode_cylinder(space.inner_object,length,radius)