# -*- test-case-name: mv3d.test.phys.test_collider -*- # Copyright (C) 2006-2012 Mortal Coil Games # See LICENSE for details. """ """ import sys import ode from twisted.spread import pb from zope.interface import implements from mv3d.util.math3d import Vector from mv3d.net.pb import Copyable from mv3d.util.persist import Persistable, FloatVector, IDTuple, List, Float, \ Boolean, Text, UntypedReference, Integer, IntVector, Inclusive from mv3d.phys.iphys import ICollider, IBoxCollider, ISphereCollider, \ ICylinderCollider, IPlaneCollider, IMeshCollider, IMeshGenCollider, IRayCast, \ ISpace, IPhysicsSimulation, IBallJoint, IBody class ColliderError(Exception): """ Raised when there is an issue with a collider """ class ColliderBase(Copyable): """ Something that can be collided with in the physical simulation """ implements(ICollider) _baseType = Inclusive() mu = Float(default=10000.0, transmit=True) mu2 = Float(default=0.0, transmit=True) bounce = Float(default=0.1, transmit=True) bounce_vel = Float(default=0.0, transmit=True) soft_erp = Float(default=0.0, transmit=True) soft_cfm = Float(default=0.0, transmit=True) slip1 = Float(default=0.0, transmit=True) slip2 = Float(default=0.0, transmit=True) rollfrict = Float(default=0.0, transmit=True) oid = IDTuple(transmit=True) contacttype = List(Text(), transmit=True) oncollide = None geom = None tgeom = None density = None position = None rotation = None def __init__(self, **kw): Persistable.__init__(self) self.contacttype = ["ContactBounce"] for k, v in kw.iteritems(): if not hasattr(self.__class__, k): raise ValueError("Inalid keyword argument %s to %s" % (k, self.__class__.__name__)) setattr(self, k, v) def __getstate__(self): """ Skip a few things """ state = self.__dict__.copy() if state.has_key("geom"): del state["geom"] if state.has_key("tgeom"): del state["tgeom"] if state.has_key("oncollide"): del state["oncollide"] return state def setOID(self, oid): """ Sets the object id """ self.oid = oid if self.geom is not None: self.geom.oid = oid if self.tgeom is not None: self.tgeom.oid = oid def setGeomParams(self, geom): """ Set misc parameters for the geom. """ geom.contact_type = 0 for contact in self.contacttype: geom.contact_type |= getattr(ode, contact) geom.contact_mu = self.mu geom.contact_mu2 = self.mu2 geom.contact_bounce = self.bounce geom.contact_bounce_vel = self.bounce_vel geom.contact_soft_erp = self.soft_erp geom.contact_soft_cfm = self.soft_cfm geom.contact_slip1 = self.slip1 geom.contact_slip2 = self.slip2 geom.oid = self.oid geom.oncollide = self.oncollide if hasattr(self, "density"): geom.density = self.density geom.rollfrict = self.rollfrict def destroy(self): """ Destroy this geom """ if self.geom is not None: deleteGeom(self.geom) self.geom = None if hasattr(self, "tgeom") and self.tgeom is not None: deleteGeom(self.tgeom) self.tgeom = None def getPosition(self): """ Return the position of the collider """ return self.position def setPosition(self, pos): """ Set the position of the geom """ self.position = pos if self.geom is not None: self.geom.setPosition(pos) def getRotation(self): """ Return the rotation of the collider """ return self.rotation def setRotation(self, rot): """ Set the rotation of the geom """ self.rotation = rot if self.geom is not None: self.geom.setQuaternion(rot) class ODEBox(ColliderBase, Persistable): """ A box from the ode Physics engine """ implements(IBoxCollider) _typeName = "odeBox" # backwards compatibility position = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) rotation = FloatVector(default=(1, 0, 0, 0), autoSave=True, partialSave=True, transmit=True) size = FloatVector(default=(1, 1, 1), autoSave=True, partialSave=True, transmit=True) density = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) stationary = Boolean(default=False, autoSave=True, partialSave=True, transmit=True) def copy(self): """ Return a copy of this collider """ return self.__class__(position=self.position, rotation=self.rotation, size=self.size, density=self.density, stationary=self.stationary, contacttype=self.contacttype, mu=self.mu, mu2=self.mu2, bounce=self.bounce, bounce_vel=self.bounce_vel, soft_erp=self.soft_erp, soft_cfm=self.soft_cfm, slip1=self.slip1, slip2=self.slip2, oid=self.oid, oncollide=self.oncollide, rollfrict=self.rollfrict) def build(self, body=None, space=None): """ Create the box in the space attached to the body (if either is provided) """ if self.stationary: body = None if space is not None: space = space.geom # print "box size", size, "dens", density, "bod", body if self.density < 0: raise ValueError("Density can not be less than 0!") if self.size[0] <= 0 or self.size[1] <= 0 or self.size[2] <= 0: raise ValueError("Size can not be less than 0!") self.size = tuple([abs(sz) for sz in self.size]) if self.density: M = ode.Mass() M.setBox(self.density, *list(self.size)) if body is not None: if self.density: body.setMass(M + body.getMass()) self.geom = ode.GeomBox(lengths=self.size) else: self.geom = ode.GeomBox(space, lengths=self.size) self.geom.setPosition(self.position) if len(self.rotation) != 4: self.geom.setRotation(self.rotation) else: self.geom.setQuaternion(self.rotation) self.setGeomParams(self.geom) if body is not None: self.tgeom = ode.GeomTransform(space) self.tgeom.setGeom(self.geom) self.tgeom.setBody(body.body) self.geom.body = body.body self.setGeomParams(self.tgeom) class ODESphere(ColliderBase, Persistable): """ A box from the ode Physics engine """ implements(ISphereCollider) _typeName = "odeSphere" # backwards compatibility position = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) rotation = FloatVector(default=(1, 0, 0, 0), autoSave=True, partialSave=True, transmit=True) radius = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) density = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) stationary = Boolean(default=False, autoSave=True, partialSave=True, transmit=True) def copy(self): """ Return a copy of this collider """ return ODESphere(position=self.position, rotation=self.rotation, radius=self.radius, density=self.density, stationary=self.stationary, contacttype=self.contacttype, mu=self.mu, mu2=self.mu2, bounce=self.bounce, bounce_vel=self.bounce_vel, soft_erp=self.soft_erp, soft_cfm=self.soft_cfm, slip1=self.slip1, slip2=self.slip2, oid=self.oid, oncollide=self.oncollide, rollfrict=self.rollfrict) def build(self, body=None, space=None): """ Create the cylinder """ if self.stationary: body = None if space is not None: space = space.geom if self.density < 0: raise ValueError("Density can not be less than 0!") if self.radius <= 0: raise ValueError("Radius can not be less than 0!") if self.density: M = ode.Mass() M.setSphere(self.density, self.radius) if body is not None: self.geom = ode.GeomSphere(radius=self.radius) if self.density: body.setMass(M + body.getMass()) else: self.geom = ode.GeomSphere(space, radius=self.radius) self.geom.setPosition(self.position) if len(self.rotation) != 4: self.geom.setRotation(self.rotation) else: self.geom.setQuaternion(self.rotation) self.setGeomParams(self.geom) if body is not None: self.tgeom = ode.GeomTransform(space) self.tgeom.setGeom(self.geom) self.tgeom.setBody(body.body) self.geom.body = body.body self.setGeomParams(self.tgeom) class ODECylinder(ColliderBase, Persistable): """ A box from the ode Physics engine """ implements(ICylinderCollider) _typeName = "odeCylinder" # backwards compatibility position = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) rotation = FloatVector(default=(1, 0, 0, 0), autoSave=True, partialSave=True, transmit=True) radius = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) length = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) density = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) stationary = Boolean(default=False, autoSave=True, partialSave=True, transmit=True) def copy(self): """ Return a copy of this collider """ return ODECylinder(position=self.position, rotation=self.rotation, radius=self.radius, length=self.length, density=self.density, stationary=self.stationary, contacttype=self.contacttype, mu=self.mu, mu2=self.mu2, bounce=self.bounce, bounce_vel=self.bounce_vel, soft_erp=self.soft_erp, soft_cfm=self.soft_cfm, slip1=self.slip1, slip2=self.slip2, oid=self.oid, oncollide=self.oncollide, rollfrict=self.rollfrict) def build(self, body=None, space=None): """ Build the cylinder """ if self.stationary: body = None if space is not None: space = space.geom if self.density < 0: raise ValueError("Density can not be less than 0!") if self.radius <= 0: raise ValueError("Radius can not be less than 0!") if self.length <= 0: raise ValueError("Length can not be less than 0!") if self.density: M = ode.Mass() M.setCappedCylinder(self.density, 1, self.radius, self.length) if body is not None: self.geom = ode.GeomCCylinder(radius=self.radius, length=self.length) if self.density: body.setMass(M + body.getMass()) else: self.geom = ode.GeomCCylinder(space, radius=self.radius, length=self.length) self.geom.setPosition(self.position) if len(self.rotation) != 4: self.geom.setRotation(self.rotation) else: self.geom.setQuaternion(self.rotation) self.setGeomParams(self.geom) if body is not None: self.tgeom = ode.GeomTransform(space) self.tgeom.setGeom(self.geom) self.tgeom.setBody(body.body) self.geom.body = body.body self.setGeomParams(self.tgeom) class ODEPlane(ColliderBase, Persistable): """ A box from the ode Physics engine """ implements(IPlaneCollider) _typeName = "odePlane" # backwards compatibility _schemaVersion = 2 normal = FloatVector(default=(0, 1, 0), autoSave=True, partialSave=True, transmit=True) distance = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) @classmethod def upgrade(cls, oldData): oldVersion = oldData["_schemaVersion"] if oldVersion > 1: raise ValueError("Old version is %d, which is > 1" % oldVersion) newData = dict() newData.update(oldData) newData["normal"] = "0,1,0" return newData def copy(self): """ Return a copy of this collider """ return ODEPlane( normal=self.normal, distance=self.distance, contacttype=self.contacttype, mu=self.mu, mu2=self.mu2, bounce=self.bounce, bounce_vel=self.bounce_vel, soft_erp=self.soft_erp, soft_cfm=self.soft_cfm, slip1=self.slip1, slip2=self.slip2, oid=self.oid, oncollide=self.oncollide, rollfrict=self.rollfrict) def build(self, body=None, space=None): """ Build the plane """ assert body is None if space is not None: space = space.geom self.geom = ode.GeomPlane(space, normal=self.normal, dist=self.distance) self.setGeomParams(self.geom) class ODEMesh(ColliderBase, Persistable): """ A mesh from the ode Physics engine """ implements(IMeshCollider) _typeName = "odeMesh" # backwards compatibility tris = List(IntVector(), transmit=True) verts = List(FloatVector(), transmit=True) geom = None tgeom = None density = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) position = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) rotation = FloatVector(default=(1, 0, 0, 0), autoSave=True, partialSave=True, transmit=True) stationary = Boolean(default=False, autoSave=True, partialSave=True, transmit=True) def copy(self): """ Return a copy of this collider """ return ODEMesh( tris=self.tris, verts=self.verts, density=self.density, contacttype=self.contacttype, stationary=self.stationary, mu=self.mu, mu2=self.mu2, bounce=self.bounce, bounce_vel=self.bounce_vel, soft_erp=self.soft_erp, soft_cfm=self.soft_cfm, slip1=self.slip1, slip2=self.slip2, oid=self.oid, oncollide=self.oncollide, rollfrict=self.rollfrict) def build(self, body=None, space=None): """ Build the mesh """ if self.stationary: body = None if space is not None: space = space.geom if self.density < 0: raise ValueError("Density can not be less than 0!") if self.density: M = ode.Mass() M.setBox(self.density, 10, 10, 10) data = ode.TriMeshData() data.build(self.verts, self.tris) if body is not None: self.geom = ode.GeomTriMesh(data) if self.density: body.setMass(M + body.getMass()) else: self.geom = ode.GeomTriMesh(data, space) self.geom.setPosition(self.position) if len(self.rotation) != 4: self.geom.setRotation(self.rotation) else: self.geom.setQuaternion(self.rotation) self.setGeomParams(self.geom) if body is not None: self.tgeom = ode.GeomTransform(space) self.tgeom.setGeom(self.geom) self.tgeom.setBody(body.body) self.geom.body = body.body self.setGeomParams(self.tgeom) class ODEMeshGen(ColliderBase, Persistable): """ A mesh from the ode Physics engine """ implements(IMeshGenCollider) _typeName = "odeMeshGen" # backwards compatibility meshGen = UntypedReference(autoSave=True, partialSave=True, transmit=True) lodfactor = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) geom = None tgeom = None density = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) position = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) rotation = FloatVector(default=(1, 0, 0, 0), autoSave=True, partialSave=True, transmit=True) stationary = Boolean(default=False, autoSave=True, partialSave=True, transmit=True) def copy(self): """ Return a copy of this collider """ return ODEMeshGen(meshGen=self.meshGen, lodfactor=self.lodfactor, density=self.density, contacttype=self.contacttype, stationary=self.stationary, mu=self.mu, mu2=self.mu2, bounce=self.bounce, bounce_vel=self.bounce_vel, soft_erp=self.soft_erp, soft_cfm=self.soft_cfm, slip1=self.slip1, slip2=self.slip2, oid=self.oid, oncollide=self.oncollide, rollfrict=self.rollfrict) def build(self, body=None, space=None): """ Build this collider from a mesh generator """ d = self.meshGen.genMesh(lodfactor=self.lodfactor) if space is not None: space = space.geom def gotMesh(mesh, body, space): if mesh is None: return if self.stationary: body = None if self.density < 0: raise ValueError("Density can not be less than 0!") if self.density: M = ode.Mass() M.setBox(self.density, 10, 10, 10) data = ode.TriMeshData() data.build(mesh.verts, mesh.tris) if body is not None: self.geom = ode.GeomTriMesh(data) if self.density: body.setMass(M + body.getMass()) else: self.geom = ode.GeomTriMesh(data, space) self.geom.setPosition(self.position) if len(self.rotation) != 4: self.geom.setRotation(self.rotation) else: self.geom.setQuaternion(self.rotation) self.setGeomParams(self.geom) if body is not None: self.tgeom = ode.GeomTransform(space) self.tgeom.setGeom(self.geom) self.tgeom.setBody(body) self.geom.body = body self.setGeomParams(self.tgeom) d.addCallback(gotMesh, body, space) return d class ODERay(Copyable, Persistable): """ A ray from the ode Physics engine """ geom = None tgeom = None position = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) delta = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) length = Float(default=1.0, autoSave=True, partialSave=True, transmit=True) oid = IDTuple(autoSave=True, partialSave=True, transmit=True) oncollide = None def __init__(self, **kw): Persistable.__init__(self) for k, v in kw.iteritems(): if not hasattr(self.__class__, k): raise ValueError("Inalid keyword argument %s to ODERay" % k) setattr(self, k, v) def copy(self): """ Return a copy of this collider """ return ODERay(position=self.position, length=self.length, delta=self.delta, oid=self.oid, oncollide=self.oncollide) def build(self, body=None, space=None): """ Create the ray """ assert body is None assert self.delta != Vector(0, 0, 0) if space is not None: space = space.geom self.geom = ode.GeomRay(space, rlen=self.length) self.geom.set(self.position, self.delta) self.geom.oid = self.oid self.geom.oncollide = self.oncollide class ODESpace(Copyable, Persistable): """ A space from the ode Physics engine """ implements(ISpace) _typeName = "odeSpace" # backwards compatibility geom = None type = Text(default="simple", autoSave=True, partialSave=True, transmit=True) center = FloatVector(default=(0, 0, 0), autoSave=True, partialSave=True, transmit=True) extents = FloatVector(default=(100000, 100000, 100000), autoSave=True, partialSave=True, transmit=True) depth = Integer(default=4, autoSave=True, partialSave=True, transmit=True) oid = IDTuple(autoSave=True, partialSave=True, transmit=True) def __init__(self, world, **kw): Persistable.__init__(self) self.world = world for k, v in kw.iteritems(): if not hasattr(self.__class__, k): raise ValueError("Inalid keyword argument %s to ODESpace" % k) setattr(self, k, v) def __len__(self): """ Returns the number of colliders in the space. """ return len(self.geom) def copy(self): """ Return a copy of this collider """ return ODESpace(world=self.world, center=self.center, extents=self.extents, depth=self.depth, oid=self.oid, type=self.type) def build(self, body=None, space=None): """ Create the space """ assert body is None if space is not None: space = space.geom if self.type == "simple": self.geom = ode.SimpleSpace(space) elif self.type == "hash": self.geom = ode.HashSpace(space) elif self.type == "quadtree": self.geom = ode.QuadTreeSpace(self.center, self.extents, self.depth, space) # ?? different arg order from docs self.geom.oid = self.oid self.geom.owner = self def castRay(self, startPosition, endPosition, ignoreColliders=None, hitOnlyColliders=None): """ Cast a ray in space from startPosition to endPosition. Any colliders in ignoreColliders will be ignored. If hitOnlyColliders is specified result will only include colliders from that list. Returns an instance of an IRayCast class """ rcast = RayCast() rcast.ignorelist = ignoreColliders rcast.hitonly = hitOnlyColliders delta = Vector(endPosition) - startPosition length = delta.length() delta = delta.normalize() myray = ode.GeomRay() myray.set(startPosition, delta) myray.setLength(length) rcast.ray = myray if hitOnlyColliders is not None: hitOnlyColliders.append(myray) ode.collide2(myray, self.geom, 0, rcast.callback) deleteGeom(myray) return rcast def destroy(self): """ Destroy this space """ if self.geom is not None: deleteGeom(self.geom) self.geom = None def getBoundingBox(self): """ Returns the bounding box of the space. """ return self.geom.getAABB() def deleteGeom(geom): """ Really remove a geom """ if geom is None: return # first remove it from the space its in if geom.getSpace(): geom.getSpace().remove(geom) if ode._geom_c2py_lut.has_key(geom._id()): del ode._geom_c2py_lut[geom._id()] return raise ColliderError("Couldn't delete geom!") class RayCast(object): """ Helper class for casting a ray """ implements(IRayCast) ray = None def __init__(self): self.mindepth = 9999999999 self.mingeom = None self.maxdepth = -999999999 self.maxgeom = None self.hitlist = [] self.ignorelist = [] self.hitonly = [] self.minpos = None def callback(self, _, geom1, geom2): """ Called when there is a potential collision """ if self.ignorelist is not None: if geom1 in self.ignorelist: return if geom2 in self.ignorelist: return if self.hitonly is not None: if not geom2 in self.hitonly: return if not geom1 in self.hitonly: return contacts = ode.collide(geom1, geom2) for contact in contacts: pos, _, depth, hit1, hit2 = contact.getContactGeomParams() if depth > self.maxdepth: self.maxdepth = depth if hit1._id() == self.ray._id(): self.maxgeom = hit2 else: self.maxgeom = hit1 if depth < self.mindepth: self.mindepth = depth self.minpos = pos if hit1._id() == self.ray._id(): self.mingeom = hit2 else: self.mingeom = hit1 if not hit2 in self.hitlist: self.hitlist.append(hit1) class ODEBody(object): """ This represents a body in the world. """ implements(IBody) def __init__(self, world): self.body = ode.Body(world.world) self.world = world def _getMVBody(self): return self.body.mvbody def _setMVBody(self, body): self.body.mvbody = body mvbody = property(_getMVBody, _setMVBody) def getPosition(self): """ Returns the current position """ return self.body.getPosition() def setPosition(self, position): """ Sets the current position """ self.body.setPosition(position) def getOrientation(self): """ Returns the current orientation as a quaternion """ return self.body.getQuaternion() def setOrientation(self, orientation): """ Sets the current orientation from a quaternion """ self.body.setQuaternion(orientation) def getLinearVelocity(self): """ Returns the current linear velocity """ return self.body.getLinearVel() def setLinearVelocity(self, velocity): """ Sets the current linear velocity """ self.body.setLinearVel(velocity) def getRelativeLinearVelocity(self): """ Returns the relative linear velocity """ return self.body.getRelPointVel((0, 0, 0)) def getAngularVelocity(self): """ Returns the angular velocity. """ return self.body.getAngularVel() def setAngularVelocity(self, velocity): """ Sets the current angular velocity """ self.body.setAngularVel(velocity) def addForce(self, force, position=None): """ Adds force at the specified position. """ if position is None: self.body.addForce(force) else: self.body.addForceAtPos(force, position) def addTorque(self, torque): """ Adds the specified amount of torque. """ self.body.addTorque(torque) def getMass(self): """ Returns the mass of the body. """ return self.body.getMass() def setMass(self, mass): """ Sets the mass of the body. """ self.body.setMass(mass) def disable(self): """ Disable the body. """ self.body.disable() def enable(self): """ Enable the body. """ self.body.enable() def isEnabled(self): """ Returns true if this body is currently enabled. """ return self.body.isEnabled() def isConnectedTo(self, otherBody): """ Returns True if this body is connected to another body via a joint or collision. """ return ode.areConnected(self.body, otherBody.body) class ODEPhysics(object): """ This is the MV3D wrapper for the ODE physics engine. """ implements(IPhysicsSimulation) defaultBody = None def __init__(self): self.world = ode.World() self.world.setQuickStepNumIterations(10) self.toenable = [] self.cglen = 0 self.contactgroup = ode.JointGroup() self.exception = None @classmethod def getPhysicsClass(cls, interface): """ Returns the class for this engine that implements the specified physics interface. """ if interface is IBoxCollider: return ODEBox if interface is ISphereCollider: return ODESphere if interface is ICylinderCollider: return ODECylinder if interface is IPlaneCollider: return ODEPlane if interface is IMeshCollider: return ODEMesh if interface is IMeshGenCollider: return ODEMeshGen if interface is ISpace: return ODESpace if interface is IRayCast: return RayCast if interface is IBallJoint: return ode.BallJoint #TPDO: Fix me if interface is IBody: return ODEBody raise NotImplementedError("No ODE implementation of %s" % interface) def collideInternal(self, space): """ Collide all geoms in space with each other """ self.exception = None space.geom.collide(0, self._nearCallback) if self.exception is not None: raise self.exception[0], self.exception[1], self.exception[2] def getContactsForGeoms(self, geom1, geom2): """ Returns ODE specific collision data (todo: make the API work with this) """ return ode.collide(geom1, geom2) def collideGeoms(self, geom1, geom2): """ Collide two ODE geoms (spaces or otherwise) """ self.exception = None ode.collide2(geom1, geom2, 0, self._nearCallback) if self.exception is not None: raise self.exception[0], self.exception[1], self.exception[2] def collide(self, geom1, geom2): """ Collide two colliders (spaces or otherwise) """ self.exception = None ode.collide2(geom1.geom, geom2.geom, 0, self._nearCallback) if self.exception is not None: raise self.exception[0], self.exception[1], self.exception[2] def clearCollisions(self): """ Clear any contacts made by collisions """ self.contactgroup.empty() self.cglen = 0 def iterate(self, deltaTime): """ Iterate the simulation forward by the specified time. """ self.world.quickStep(deltaTime) def getGravity(self): """ Return the direction of gravity as a vector. """ return self.world.getGravity() def setGravity(self, gravity): """ Sets the gravity of the world. """ self.world.setGravity(gravity) def setERP(self, erp): """ Sets the ERP for the world. """ self.world.setERP(erp) def getERP(self): """ Returns the ERP for the world. """ return self.world.getERP() def setCFM(self, cfm): """ Sets the CFM for the world. """ self.world.setCFM(cfm) def getCFM(self): """ Returns the current CFM for the world """ return self.world.getCFM() def setAutoDisable(self, enable, linearThreshold, angularThreshold, steps): """ Set auto disable options for the world. """ self.world.setAutoDisableFlag(enable) self.world.setAutoDisableLinearThreshold(linearThreshold) self.world.setAutoDisableAngularThreshold(angularThreshold) self.world.setAutoDisableSteps(steps) def _nearCallback(self, _args, geom1, geom2): """ This function is called when two geoms might collide. """ try: # exceptions caught in C callbacks cause strange things if geom1.isSpace(): if len(geom1) == 0: return return ode.collide2(geom1, geom2, 0, self._nearCallback) if geom2.isSpace(): return ode.collide2(geom2, geom1, 0, self._nearCallback) body1, body2 = geom1.getBody(), geom2.getBody() if (body1 == None and body2 == None) or (body1 and body2 and ode.areConnected(body1, body2)): return contacts = ode.collide(geom1, geom2) if not len(contacts): return if geom1.oncollide: if geom1.oncollide(geom2, geom1): if geom2.oncollide: geom2.oncollide(geom1, geom2) return if geom2.oncollide: if geom2.oncollide(geom1, geom2): return ctype = geom1.contact_type | geom2.contact_type mu = geom1.contact_mu + geom2.contact_mu mu2 = geom1.contact_mu2 + geom2.contact_mu2 bounce = (geom1.contact_bounce + geom2.contact_bounce) / 2.0 bouncevel = (geom1.contact_bounce_vel + geom2.contact_bounce_vel) / 2.0 softcfm = (geom1.contact_soft_cfm + geom2.contact_soft_cfm) / 2.0 softerp = (geom1.contact_soft_erp + geom2.contact_soft_erp) / 2.0 slip1 = (geom1.contact_slip1 + geom2.contact_slip1) slip2 = (geom1.contact_slip2 + geom2.contact_slip2) if geom1.oid and not geom1.oid in self.toenable: self.toenable.append(geom1.oid) if geom2.oid and not geom2.oid in self.toenable: self.toenable.append(geom2.oid) if self.defaultBody is not None: body2 = self.defaultBody for c in contacts: c.setMode(ctype) c.setBounce(bounce) c.setMu2(mu2) c.setMu(mu) c.setBounceVel(bouncevel) c.setSoftCFM(softcfm) c.setSoftERP(softerp) c.setSlip1(slip1) c.setSlip2(slip2) # p, n, d, g1, g2 = c.getContactGeomParams() # print d ode.ContactJoint(self.world, self.contactgroup, c).attach( body1, body2) self.cglen += 1 except Exception: self.exception = sys.exc_info() pb.setUnjellyableForClass(ODEBox, ODEBox) pb.setUnjellyableForClass(ODESphere, ODESphere) pb.setUnjellyableForClass(ODECylinder, ODECylinder) pb.setUnjellyableForClass(ODEPlane, ODEPlane) pb.setUnjellyableForClass(ODEMesh, ODEMesh) pb.setUnjellyableForClass(ODEMeshGen, ODEMeshGen) pb.setUnjellyableForClass(ODESpace, ODESpace) pb.setUnjellyableForClass(ODERay, ODERay)