home *** CD-ROM | disk | FTP | other *** search
- # ***** BEGIN LICENSE BLOCK *****
- # Version: MPL 1.1/GPL 2.0/LGPL 2.1
- #
- # The contents of this file are subject to the Mozilla Public License Version
- # 1.1 (the "License"); you may not use this file except in compliance with
- # the License. You may obtain a copy of the License at
- # http://www.mozilla.org/MPL/
- #
- # Software distributed under the License is distributed on an "AS IS" basis,
- # WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
- # for the specific language governing rights and limitations under the
- # License.
- #
- # The Original Code is the Python Computer Graphics Kit.
- #
- # The Initial Developer of the Original Code is Matthias Baas.
- # Portions created by the Initial Developer are Copyright (C) 2004
- # the Initial Developer. All Rights Reserved.
- #
- # Contributor(s):
- #
- # Alternatively, the contents of this file may be used under the terms of
- # either the GNU General Public License Version 2 or later (the "GPL"), or
- # the GNU Lesser General Public License Version 2.1 or later (the "LGPL"),
- # in which case the provisions of the GPL or the LGPL are applicable instead
- # of those above. If you wish to allow use of your version of this file only
- # under the terms of either the GPL or the LGPL, and not to allow others to
- # use your version of this file under the terms of the MPL, indicate your
- # decision by deleting the provisions above and replace them with the notice
- # and other provisions required by the GPL or the LGPL. If you do not delete
- # the provisions above, a recipient may use your version of this file under
- # the terms of any one of the MPL, the GPL or the LGPL.
- #
- # ***** END LICENSE BLOCK *****
- # $Id: odedynamics.py,v 1.8 2006/05/26 17:08:04 mbaas Exp $
-
- ## \file odedynamics.py
- ## Contains the ODEDynamics class.
-
- """This module contains a Dynamics component using the ODE rigid body
- dynamics package."""
-
- import protocols, weakref
- from Interfaces import *
- from component import Component
- from scene import getScene
- from eventmanager import eventManager
- from cgtypes import *
- from worldobject import WorldObject
- from spheregeom import SphereGeom
- from ccylindergeom import CCylinderGeom
- from boxgeom import BoxGeom
- from trimeshgeom import TriMeshGeom
- from planegeom import PlaneGeom
- from joints import *
- from events import *
- from slots import *
- import cmds
- import _core
- try:
- import ode
- has_ode = True
- except:
- has_ode = False
-
- #import numarray
- #import numarray.linear_algebra
-
- ODE_COLLISION = "ODECollision"
-
- # ODECollisionEvent
- class ODECollisionEvent:
- """Event object that is used as argument for collision events.
- """
-
- def __init__(self, obj1, obj2, contacts, contactproperties):
- self.obj1 = obj1
- self.obj2 = obj2
- self.contacts = contacts
- self.contactproperties = contactproperties
-
- def __str__(self):
- if self.obj1==None:
- n1 = "None"
- else:
- n1 = '"%s"'%self.obj1.name
- if self.obj2==None:
- n2 = "None"
- else:
- n2 = '"%s"'%self.obj2.name
- return '<ODECollisionEvent: obj1:%s obj2:%s #Contacts:%d>'%(n1, n2, len(self.contacts))
-
- # averageContactGeom
- def averageContactGeom(self):
- """Return the average contact position, normal and depth.
- """
- pos = vec3(0)
- normal = vec3(0)
- depth = 0.0
- for c in self.contacts:
- p,n,d,g1,g2 = c.getContactGeomParams()
- pos += vec3(p)
- normal += vec3(n)
- depth += d
-
- # n is not 0 (the event wouldn't have been generated if it was)
- n = len(self.contacts)
- pos /= n
- normal /= n
- depth /= n
-
- return pos,normal,depth
-
-
- # ODEDynamics
- class ODEDynamics(Component):
- """Dynamics component using the ODE rigid body dynamics package.
- """
-
- def __init__(self,
- name="ODEDynamics",
- gravity = 9.81,
- substeps = 1,
- cfm = None,
- erp = None,
- defaultcontactproperties = None,
- enabled = True,
- show_contacts = False,
- contactmarkersize = 0.1,
- contactnormalsize = 1.0,
- collision_events = False,
- auto_add = False,
- auto_insert=True):
- """Constructor.
-
- \param name (\c str) Component name
- \param auto_add (\c bool) Automatically add the world objects to the simulation
- \param auto_insert (\c bool) Automatically add the component to the scene
- """
- Component.__init__(self, name=name, auto_insert=auto_insert)
-
- scene = getScene()
-
- self.substeps = substeps
- self.collision_events = collision_events
-
- self.world = ode.World()
- g = -gravity*scene.up
- self.world.setGravity(g)
- if cfm!=None:
- self.world.setCFM(cfm)
- if erp!=None:
- self.world.setERP(erp)
-
- # self.world.setAutoDisableFlag(True)
-
- self.enabled = enabled
-
- self.eventmanager = eventManager()
- self.eventmanager.connect(STEP_FRAME, self)
- self.eventmanager.connect(RESET, self.reset)
-
- # Space object
- self.space = ode.Space()
-
- # Joint group for the contact joints
- self.contactgroup = ode.JointGroup()
-
- # A dictionary with WorldObjects as keys and ODEBodies as values
- self.body_dict = {}
- # A list of all bodies
- self.bodies = []
- # A list of all joints
- self.joints = []
-
- # A dictionary with contact properties
- # Key is a 2-tuple (material1, material2). Value is a
- # ODEContactProperties object.
- # The value of (mat1, mat2) should always be the same than
- # the value of (mat2, mat1).
- self.contactprops = {}
-
- # Default contact properties
- if defaultcontactproperties==None:
- defaultcontactproperties = ODEContactProperties()
- self.defaultcontactprops = defaultcontactproperties
-
- self.show_contacts = show_contacts
- self.contactmarkersize = contactmarkersize
- self.contactnormalsize = contactnormalsize
-
- # A list of weakrefs to body manipulators
- self.body_manips = []
-
- # Debug statistics (the number of contacts per simulation step)
- self.numcontacts = 0
-
- # Automatically add world objects
- if auto_add:
- # Add all rigid bodies first...
- for obj in scene.worldRoot().iterChilds():
- try:
- obj = protocols.adapt(obj, IRigidBody)
- self.add(obj)
- except NotImplementedError:
- pass
-
- # Then add all joints...
- for obj in scene.worldRoot().iterChilds():
- if isinstance(obj, ODEJointBase):
- self.add(obj)
-
- # add
- def add(self, objs, categorybits=None, collidebits=None):
- """Add a world object to the simulation.
-
- \param objs World objects to be simulated (given as names or objects)
- """
- objs = cmds.worldObjects(objs)
- for obj in objs:
- self._add(obj, categorybits, collidebits)
-
- def _add(self, obj, categorybits, collidebits):
-
- if isinstance(obj, ODEJointBase):
- obj.activate(self)
- self.joints.append(obj)
- return
-
- try:
- obj = protocols.adapt(obj, IRigidBody)
- except NotImplementedError:
- print 'Object "%s" is not a rigid body.'%obj.name
- return
-
- # Should the object be ignored?
- if not obj.dynamics:
- return
-
- # print "#####################",obj.name,"#########################"
- # print "Old center of gravity:",obj.cog
- # print "Old inertiatensor:"
- # print obj.inertiatensor
- # print "Old Offset transform:"
- P = obj.getOffsetTransform()
- # print P
- obj.pivot = P*obj.cog
- # print "Setting pivot to", obj.pivot
- # print "New center of gravity:", obj.cog
- # print "Intermediate inertia tensor:"
- # print obj.inertiatensor
- # I = obj.inertiatensor
- # a = numarray.array([I.getRow(0), I.getRow(1), I.getRow(2)], type=numarray.Float64)
- # vals, vecs = numarray.linear_algebra.eigenvectors(a)
- # print "Eigenvalues:",vals
- # Eigenvektoren sind ungenau! (bei cube_base_cog grosse Abweichungen)
- # b1 = vec3(vecs[0]).normalize()
- # b2 = vec3(vecs[1]).normalize()
- # b3 = vec3(vecs[2]).normalize()
- # P = obj.getOffsetTransform()
- # Pnull = P*vec3(0,0,0)
- # b1 = P*b1 - Pnull
- # b2 = P*b2 - Pnull
- # b3 = P*b3 - Pnull
- # I2 = mat3(b1,b2,b3)
- # print I2
- # if I2.determinant()<0:
- # I2.setColumn(0, -I2.getColumn(0))
- # print "Det of new basis",I2.determinant()
- # P = obj.getOffsetTransform()
- # P.setMat3(I2)
- # obj.setOffsetTransform(P)
- # print "New offset transform:"
- # print P
- # print "New center of gravity:",obj.cog
- # print "New inertia tensor:"
- # print obj.inertiatensor
-
- body = ODEBody(obj, self, categorybits=categorybits, collidebits=collidebits)
- self.bodies.append(body)
- self.body_dict[obj] = body
-
- # reset
- def reset(self):
- """Reset the body states.
-
- All bodies are reset to their position and velocity at the time
- they were added.
-
- This method is also called when the RESET event is issued.
- """
- for b in self.bodies:
- b.reset()
- b.updateObj()
-
- for j in self.joints:
- j.reset()
-
- # setContactProperties
- def setContactProperties(self, (mat1, mat2), props):
- """Set the contact properties of a pair of materials.
-
- The contact properties \a props are applied whenever an object
- with material \a mat1 collides with an object with material \a mat2.
-
- \param mat1 (\c Material) Material 1
- \param mat2 (\c Material) Material 2
- \param props (\c ODEContactProperties) Contact properties
- """
- self.contactprops[(mat1,mat2)] = props
- self.contactprops[(mat2,mat1)] = props
-
- # getContactProperties
- def getContactProperties(self, matpair):
- """Return the contact properties for a material pair.
-
- \param matpair A 2-tuple of two Material objects
- \return Contact properties (\c ODEContactProperties)
- """
- cp = self.contactprops.get(matpair)
- if cp==None:
- # print 'ODEDynamics: Warning: No contact properties defined for material "%s" and "%s"'%(matpair[0].name, matpair[1].name)
- cp = self.defaultcontactprops
- return cp
-
- # createBodyManipulator
- def createBodyManipulator(self, obj):
- # return self.body_dict[obj].odebody
- bm = ODEBodyManipulator(self.body_dict[obj])
- self.body_manips.append(weakref.ref(bm))
- return bm
-
- # nearCallback
- def nearCallback(self, args, geom1, geom2):
- # Check if the objects do collide
- contacts = ode.collide(geom1, geom2)
-
- # No contacts? then return immediately
- if len(contacts)==0:
- return
-
- # print len(contacts),"contacts"
- self.numcontacts += len(contacts)
-
- # Get the contact properties
- cp = self.getContactProperties((geom1.material, geom2.material))
-
- # Create a collision event?
- if self.collision_events:
- obj1 = getattr(geom1, "worldobj", None)
- obj2 = getattr(geom2, "worldobj", None)
- evt = ODECollisionEvent(obj1, obj2, contacts, cp)
- self.eventmanager.event(ODE_COLLISION, evt)
-
- # Create contact joints
- for c in contacts:
- if self.show_contacts:
- p,n,d,g1,g2 = c.getContactGeomParams()
- cmds.drawMarker(p, size=self.contactmarkersize, color=(1,0,0))
- cmds.drawLine(p, vec3(p)+self.contactnormalsize*vec3(n), color=(1,0.5,0.5))
- # print p,n,d
-
- # Set the properties
- cp.apply(c)
- # Create the contact joint
- j = ode.ContactJoint(self.world, self.contactgroup, c)
- b1 = geom1.getBody()
- b2 = geom2.getBody()
- # if b1==None:
- # b1=ode.environment
- # if b2==None:
- # b2=ode.environment
- j.attach(b1, b2)
-
- # onStepFrame
- def onStepFrame(self):
- """Callback for the StepFrame event.
- """
- if self.substeps==0 or not self.enabled:
- return
-
- if self.show_contacts:
- cmds.drawClear()
-
- # Remove dead body manipulators...
- self.body_manips = filter(lambda x: x() is not None, self.body_manips)
-
- # Sim loop...
- subdt = getScene().timer().timestep/self.substeps
- for i in range(self.substeps):
- self.numcontacts = 0
-
- # Apply body manipulators
- for bmref in self.body_manips:
- bm = bmref()
- if bm is not None:
- bm._apply()
-
- # Detect collisions and create contact joints
- self.space.collide(None, self.nearCallback)
- # print "#Contacts:",self.numcontacts
- # Simulation step
- # self.world.step(subdt)
- self.world.quickStep(subdt)
- # Remove all contact joints
- self.contactgroup.empty()
-
- # Update the world objects
- for body in self.bodies:
- body.updateObj()
-
- # Reset body manipulators
- for bmref in self.body_manips:
- bm = bmref()
- if bm is not None:
- bm._reset()
-
-
- ######################################################################
-
- # ODEContactProperties
- class ODEContactProperties:
-
- """Contact properties.
-
- This class stores contact properties that are used whenever
- two objects collide. The attributes are used to initialize
- ODE contact joints.
- """
-
- def __init__(self,
- mode = 0,
- mu = 0.3,
- mu2 = None,
- bounce = None,
- bounce_vel = None,
- soft_erp = None,
- soft_cfm = None,
- motion1 = None,
- motion2 = None,
- slip1 = None,
- slip2 = None,
- fdir1 = None):
- """Constructor.
-
- See the ODE manual for an explanation of the parameters.
-
- The flags for the mode parameter are automatically set.
- However, you can initially set the ContactApprox flags.
- """
-
- if mu2!=None:
- mode |= ode.ContactMu2
- else:
- mu2 = 0.0
-
- if bounce!=None:
- mode |= ode.ContactBounce
- else:
- bounce = 0.0
-
- if bounce_vel==None:
- bounce_vel = 0.0
-
- if soft_erp!=None:
- mode |= ode.ContactSoftERP
- else:
- soft_erp = 0.0
-
- if soft_cfm!=None:
- mode |= ode.ContactSoftCFM
- else:
- soft_cfm = 0.0
-
- if motion1!=None:
- mode |= ode.ContactMotion1
- else:
- motion1 = 0.0
-
- if motion2!=None:
- mode |= ode.ContactMotion2
- else:
- motion2 = 0.0
-
- if slip1!=None:
- mode |= ode.ContactSlip1
- else:
- slip1 = 0.0
-
- if slip2!=None:
- mode |= ode.ContactSlip2
- else:
- slip2 = 0.0
-
- if fdir1!=None:
- mode |= ode.ContactFDir1
- else:
- fdir1 = (0,0,0)
-
- self.mode = mode
- # print "ODEContactProps: mode =",mode
- self.mu = mu
- self.mu2 = mu2
- self.bounce = bounce
- self.bounce_vel = bounce_vel
- self.soft_erp = soft_erp
- self.soft_cfm = soft_cfm
- self.motion1 = motion1
- self.motion2 = motion2
- self.slip1 = slip1
- self.slip2 = slip2
- self.fdir1 = fdir1
-
- # apply
- def apply(self, contact):
- """Apply the contact properties to a contact joint."""
- contact.setMode(self.mode)
- contact.setMu(self.mu)
- contact.setMu2(self.mu2)
- contact.setBounce(self.bounce)
- contact.setBounceVel(self.bounce_vel)
- contact.setMotion1(self.motion1)
- contact.setMotion2(self.motion2)
- contact.setSlip1(self.slip1)
- contact.setSlip2(self.slip2)
- contact.setSoftERP(self.soft_erp)
- contact.setSoftCFM(self.soft_cfm)
- contact.setFDir1(self.fdir1)
-
-
- ######################################################################
-
- # ODEBodyManipulator
- class ODEBodyManipulator(object):
- """Body manipulator class.
-
- This class can be used to apply external forces/torques to a body
- or to manipulate the position/orientation/velocities directly.
-
- You should not create instances of this class yourself. Instead, use
- the createBodyManipulator() method of the ODEDynamics component.
- """
-
- def __init__(self, body):
- """Constructor.
-
- \param body (\c ODEBody) The internal body object representing the
- rigid body to manipulate.
- """
- self._body = body
- self._odebody = body.odebody
- self._reset()
-
- # setPos
- def setPos(self, pos):
- """Set the position of the body.
-
- pos must be a 3-sequence of floats.
- """
- self._odebody.setPosition(pos)
-
- # setRot
- def setRot(self, rot):
- """Set the rotation of the body.
-
- rot must be a mat3 containing the orientation.
- """
- self._odebody.setRotation(rot)
-
- # setLinearVel
- def setLinearVel(self, vel):
- """Set the linear velocity.
-
- vel must be a 3-sequence of floats.
- """
- self._odebody.setLinearVel(vel)
-
- # setAngularVel
- def setAngularVel(self, vel):
- """Set the angular velocity.
-
- vel must be a 3-sequence of floats.
- """
- self._odebody.setAngularVel(vel)
-
- # setInitialPos
- def setInitialPos(self, pos):
- """Set the initial position.
-
- pos must be a 3-sequence of floats.
- """
- self._body.initial_pos = vec3(pos)
-
- # setInitialRot
- def setInitialRot(self, rot):
- """Set the initial orientation.
-
- rot must be a mat3.
- """
- self._body.initial_rot = mat3(rot)
-
- # setInitialLinearVel
- def setInitialLinearVel(self, vel):
- """Set the initial linear velocity.
-
- vel must be a 3-sequence of floats.
- """
- self._odebody.initial_linearvel = vec3(vel)
-
- # setInitialAngularVel
- def setInitialAngularVel(self, vel):
- """Set the initial angular velocity.
-
- vel must be a 3-sequence of floats.
- """
- self._odebody.initial_angularvel = vec3(vel)
-
- # addForce
- def addForce(self, force, relforce=False, pos=None, relpos=False):
- """Apply an external force to a rigid body.
-
- Add an external force to the current force vector. force is a
- vector containing the force to apply. If relforce is True the
- force is interpreted in local object space, otherwise it is
- assumed to be given in global world space. By default, the
- force is applied at the center of gravity. You can also pass a
- different position in the pos argument which must describe a
- point in space. relpos determines if the point is given in
- object space or world space (default).
- """
-
- R = None
- force = vec3(force)
- if relforce:
- R = mat3(self._odebody.getRotation())
- force = R*force
- # Is a position given? then add the corresponding torque
- if pos!=None:
- pos = vec3(pos)
- bodyorig = vec3(self._odebody.getPosition())
- if relpos:
- if R==None:
- R = mat3(self._odebody.getRotation())
- pos = R*pos + bodyorig
-
- self._torque += (pos-bodyorig).cross(force)
- self._torque_flag = True
-
- self._force += vec3(force)
- self._force_flag = True
-
- # addTorque
- def addTorque(self, torque, reltorque=False):
- """Apply an external torque to a rigid body.
-
- Add an external torque to the current torque vector. torque is
- a vector containing the torque to apply. reltorque determines
- if the torque vector is given in object space or world space
- (default).
- """
-
- torque = vec3(torque)
- if reltorque:
- R = mat3(self._odebody.getRotation())
- torque = R*torque
-
- self._torque += torque
- self._torque_flag = True
-
- # _apply
- def _apply(self):
- """Apply the stored force/torque.
-
- This method is called by the ODEDynamics object during the simulation
- step (once for every sub step).
- """
- if self._force_flag:
- self._odebody.addForce(self._force)
- if self._torque_flag:
- self._odebody.addTorque(self._torque)
-
- # _reset
- def _reset(self):
- """Reset the stored force/torque.
-
- This method is called by the ODEDynamics object at the end of one
- entire simulation step.
- """
- self._force = vec3(0)
- self._torque = vec3(0)
-
- self._force_flag = False
- self._torque_flag = False
-
- ## protected:
-
- # "body" property...
-
- def _getBody(self):
- """Return the current body (WorldObject).
-
- This method is used for retrieving the \a body property.
-
- \return Rigid body
- """
- return self._body.obj
-
- body = property(_getBody, None, None, "Rigid body")
-
- # "odebody" property...
-
- def _getODEBody(self):
- """Return the current ODE body.
-
- This method is used for retrieving the \a odebody property.
-
- \return ODE body
- """
- return self._odebody
-
- odebody = property(_getODEBody, None, None, "ODE body")
-
-
- ######################################################################
-
- # ODEBody
- class ODEBody:
- """This class is the interface between ODE bodies and world objects.
-
- It encapsulates the ODE body and the ODE geom objects.
-
- """
- def __init__(self, obj, dyncomp, categorybits=None, collidebits=None):
- """Constructor.
-
- \param obj (\c WorldObject) World object
- \param dyncomp (\c ODEDynamics) Dynamics component
- """
-
- # Store the world object
- self.obj = obj
-
- self.dyncomp = dyncomp
- world = self.dyncomp.world
- space = self.dyncomp.space
-
- # Store initial position/orientation
- self.initial_pos = obj.pos
- self.initial_rot = obj.rot
- self.initial_linearvel = obj.linearvel #vec3(0,0,0)
- self.initial_angularvel = obj.angularvel #vec3(0,0,0)
-
- # The ODE body
- self.odebody = None
- # A list of ODE geoms
- self.odegeoms = None
-
- # Create ODE collision geoms
- self.odegeoms = self.createGeoms(obj, space)
-
- # Set the category and collide bits
- for geom in self.odegeoms:
- if categorybits!=None:
- geom.setCategoryBits(categorybits)
- if collidebits!=None:
- geom.setCollideBits(collidebits)
-
- # Create an ODE body
- if not obj.static:
- self.odebody = self.createBody(obj, world)
- for g in self.odegeoms:
- g.setBody(self.odebody)
-
- # Store the material in the ODE geoms so that the contact
- # properties can be determined in the near callback.
- # Also store the corresponding WorldObject
- for g in self.odegeoms:
- g.material = obj.getMaterial()
- g.worldobj = obj
-
- # Initialize the ODE body/geoms
- self.reset()
-
- # Create the notification forwarders and make connections
- self.initialization = True
- # Forward changes to the "static" slot
- self._static_forwarder = NotificationForwarder(self.onStaticChanged)
- self.obj.static_slot.addDependent(self._static_forwarder)
- # Forward changes to the "mass" slot
- self._mass_forwarder = NotificationForwarder(self.onMassChanged)
- self.obj.mass_slot.addDependent(self._mass_forwarder)
- self.initialization = False
-
- # updateObj
- def updateObj(self):
- """Update the transformation of the world object.
-
- The transformation from the ODE body is copied to the corresponding
- world object. The linearvel and angularvel attributes are
- updated as well.
- """
- if self.obj.static:
- return
-
- # if type(self.odegeom)==ode.GeomTriMesh:
- # self.odegeom.clearTCCache()
-
- # Get the ODE transformation
- pos = self.odebody.getPosition()
- rot = self.odebody.getRotation()
- # Create the transformation matrix for the world object
- M = mat4().translation(vec3(pos))
- M.setMat3(mat3(rot))
- # Set the transformation on the world object
- self.obj.transform = M
-
- self.obj.linearvel = vec3(self.odebody.getLinearVel())
- self.obj.angularvel = vec3(self.odebody.getAngularVel())
-
- ## protected:
-
- # onStaticChanged
- def onStaticChanged(self):
- if self.initialization:
- return
-
- if self.obj.static:
- if self.odebody!=None:
- for g in self.odegeoms:
- g.setBody(None)
- self.odebody = None
- else:
- self.odebody = self.createBody(self.obj, self.dyncomp.world)
- for g in self.odegeoms:
- g.setBody(self.odebody)
- self.odebody.setPosition(self.obj.pos)
- self.odebody.setRotation(self.obj.rot.toList(rowmajor=True))
- self.odebody.setLinearVel((0,0,0))
- self.odebody.setAngularVel((0,0,0))
-
- # onMassChanged
- def onMassChanged(self):
- if self.initialization:
- return
-
- print "Mass changed to",self.obj.mass
-
- # reset
- def reset(self):
- """Restore the initial state of the body."""
- if self.obj.static:
- for g in self.odegeoms:
- if g.placeable():
- g.setPosition(self.initial_pos)
- g.setRotation(self.initial_rot.toList(rowmajor=True))
- else:
- self.odebody.setPosition(self.initial_pos)
- self.odebody.setRotation(self.initial_rot.toList(rowmajor=True))
- self.odebody.setLinearVel(self.initial_linearvel)
- self.odebody.setAngularVel(self.initial_angularvel)
-
- # createBody
- def createBody(self, obj, world):
- """Create an ODE body.
-
- Creates an ODE body and initializes the mass properties.
- """
- # Create an ODE Mass object
- M = ode.Mass()
- m = obj.totalmass
- if m==0:
- print 'WARNING: Object "%s" has a mass of zero.'%obj.name
- cog = obj.cog
- I = obj.inertiatensor
- # print '---Rigid body "%s"--------'%obj.name
- # print 'cog:',cog
- # print I
- I = mat3(I)
- M.setParameters(m, cog.x, cog.y, cog.z, I[0,0], I[1,1], I[2,2], I[0,1], I[0,2], I[1,2])
-
- # Create an ODE body
- body = ode.Body(world)
- body.setMass(M)
-
- return body
-
- # createGeoms
- def createGeoms(self, obj, space):
- """Create all the geoms for one world object including the children.
-
- The generated geoms will be encapsulated in GeomTransforms
- so that all of them are defined with respect to the pivot coordinate
- system of \a obj.
-
- \param obj (\c WorldObject) Top level world object
- \param space (\c ode.Space) ODE Space object
- \return List of ODE geom objects
- """
-
- # Plane? This is a special case because ODE planes are not placeable
- if isinstance(obj.geom, PlaneGeom):
- if not obj.static:
- raise RuntimeError, "Planes can only be used as static bodies"
- L = obj.localTransform()
- n = L*vec3(0,0,1) - L*vec3(0)
- n = n.normalize()
- d = n*obj.pos
- odegeom = ode.GeomPlane(space, n, d)
- return [odegeom]
-
- res = []
-
- # Create all ODE geoms in the subtree starting at obj
- # Each geom will have a position/rotation that moves it to
- # the local coordinate system of obj.
- geoms = self._createLGeoms(obj, mat4(1))
-
- # Apply the offset transform and encapsulate the geoms inside
- # geom transforms...
- P = obj.getOffsetTransform()
- Pinv = P.inverse()
- pos,rot,scale = Pinv.decompose()
- if scale!=vec3(1,1,1):
- print 'WARNING: ODEDynamics: Scaled geometries are not supported'
- res = []
- for g in geoms:
- M = mat4(1).translation(vec3(g.getPosition()))
- M.setMat3(mat3(g.getRotation()))
- M *= Pinv;
- p4 = M.getColumn(3)
- g.setPosition((p4.x, p4.y, p4.z))
- # row major or column major?
- g.setRotation(M.getMat3().toList(rowmajor=True))
-
- gt = ode.GeomTransform(space)
- gt.setGeom(g)
- res.append(gt)
-
- return res
-
- # _createLGeoms
- def _createLGeoms(self, obj, M):
- """Create ODE geom objects for a world object.
-
- Create the ODE geoms for \a obj (and its children).
- The transformation \a M is applied to each geom.
-
- The returned geoms are not assigned to a Body yet.
-
- \param obj (\c WorldObject) The world object
- \param M (\c mat4) Transformation
- \return List of ODE geoms
- """
- res = []
- # Create an ODE geom for the object itself
- geom = obj.geom
- if geom!=None:
- res.append(self._createGeom(geom, M))
-
- # Create ODE geoms for the children
- for child in obj.iterChilds():
- if not getattr(child, "dynamics", False):
- continue
- L = child.localTransform()
- res += self._createLGeoms(child, M*L)
-
- return res
-
- # _createGeom
- def _createGeom(self, geom, M):
- """Create an ODE geom object for the given cgkit geom.
-
- Create an ODE geom of the appropriate type (depening on \a geom)
- and apply the transformation \a M to it (currently, M may only
- contain a translation and a rotation, otherwise a warning is
- printed).
-
- The returned geom is not assigned to a Body yet.
-
- \param geom (\c GeomObject) cgkit geom object
- \param M (\c mat4) Transformation that's applied to the geom
- \return ODE geom
- """
-
- # Create the raw geom object that's defined in the local
- # coordinate system L of the world object this geom belongs to.
-
- # Sphere geom?
- if isinstance(geom, SphereGeom):
- odegeom = ode.GeomSphere(None, geom.radius)
- # CCylinder geom?
- elif isinstance(geom, CCylinderGeom):
- odegeom = ode.GeomCCylinder(None, geom.radius, geom.length)
- # Box geom?
- elif isinstance(geom, BoxGeom):
- odegeom = ode.GeomBox(None, (geom.lx, geom.ly, geom.lz))
- # TriMesh geom?
- elif isinstance(geom, TriMeshGeom):
- verts = []
- for i in range(geom.verts.size()):
- verts.append(geom.verts.getValue(i))
- # print "V",i,verts[i]
- faces = []
- for i in range(geom.faces.size()):
- f = geom.faces.getValue(i)
- faces.append(f)
- # print "F",i,faces[i]
- ia,ib,ic = faces[i]
- a = verts[ia]
- b = verts[ib]
- c = verts[ic]
- sa = ((b-a).cross(c-a)).length()
- # if sa<0.0001:
- # print "*****KLEIN*****",sa
-
- tmd = ode.TriMeshData()
- tmd.build(verts, faces)
- odegeom = ode.GeomTriMesh(tmd, None)
- # Plane geom?
- elif isinstance(geom, PlaneGeom):
- L = obj.localTransform()
- n = L*vec3(0,0,1) - L*vec3(0)
- n = n.normalize()
- d = n*obj.pos
- odegeom = ode.GeomPlane(space, n, d)
- # Unknown geometry
- else:
- raise ValueError, 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s".'%geom.name
- # print 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s". Using bounding box instead.'%obj.name
- # bmin, bmax = obj.boundingBox().getBounds()
- # s = bmax-bmin
- # odegeom = ode.GeomBox(None, s)
- # pos,rot,scale = P.inverse().decompose()
- # odegeom.setPosition(pos + 0.5*(bmax+bmin))
- # odegeomtransform = ode.GeomTransform(space)
- # odegeomtransform.setGeom(odegeom)
- # return odegeomtransform
-
- # Displace the geom by M
- pos,rot,scale = M.decompose()
- if scale!=vec3(1,1,1):
- print 'WARNING: ODEDynamics: Scaled geometries are not supported'
- odegeom.setPosition(pos)
- # row major or column major?
- odegeom.setRotation(rot.getMat3().toList(rowmajor=True))
-
- return odegeom
-
- ######################################################################
-
- # ODEJointBase
- class ODEJointBase(WorldObject):
-
- protocols.advise(instancesProvide=[ISceneItem])
-
- exec slotPropertyCode("lostop")
- exec slotPropertyCode("histop")
- exec slotPropertyCode("motorvel")
- exec slotPropertyCode("motorfmax")
- exec slotPropertyCode("fudgefactor")
- exec slotPropertyCode("bounce")
- exec slotPropertyCode("cfm")
- exec slotPropertyCode("stoperp")
- exec slotPropertyCode("stopcfm")
-
- def __init__(self,
- name = "",
- body1 = None,
- body2 = None,
- **params):
- WorldObject.__init__(self, name=name, **params)
-
- # ODEDynamics component
- self.odedynamics = None
-
- # The corresponding ODE joint
- self.odejoint = None
-
- self.body1 = body1
- self.body2 = body2
-
- # attach
- def attach(self, body1, body2=None):
- self.body1 = body1
- self.body2 = body2
- if self.odejoint!=None:
- if body1==None:
- b1 = ode.environment
- else:
- b1 = self.odedynamics.body_dict[body1].odebody
-
- if body2==None:
- b2 = ode.environment
- else:
- b2 = self.odedynamics.body_dict[body2].odebody
-
- self.odejoint.attach(b1, b2)
-
- # activate
- def activate(self, odedynamics):
- """
- this method is called by the ODEDynamics componenent
- """
- if odedynamics==self.odedynamics:
- return
- if self.odedynamics!=None:
- print 'Warning: Joint "%s" is already in use'%self.name
- self.odedynamics = odedynamics
- self._createODEjoint()
- self._initODEjoint()
-
- # reset
- def reset(self):
- """Reset method.
-
- This may only be called after the bodies have been reset.
- """
- pass
-
- def _createSlots(self):
-
- self._createSlot("lostop", None, "-ode.Infinity", "onLoStopChanged")
- self._createSlot("histop", None, "ode.Infinity", "onHiStopChanged")
- self._createSlot("motorvel", None, "0.0", "onMotorVelChanged")
- self._createSlot("motorfmax", None, "0.0", "onMotorFMaxChanged")
- self._createSlot("fudgefactor", None, "1.0", "onFudgeFactorChanged")
- self._createSlot("bounce", None, "0.1", "onBounceChanged")
- self._createSlot("cfm", None, 1E-5, "onCFMChanged")
- self._createSlot("stoperp", None, 0.2, "onStopERPChanged")
- self._createSlot("stopcfm", None, 1E-5, "onStopCFMChanged")
-
- def _createSlot(self, name, nr, default, callback):
- """Create one joint param slot.
-
- This method creates a DoubleSlot called \a name (with additional
- number, if \a nr is not None). \a default is the initial default
- value (as string) and \a callback the name of the callback method.
-
- \param name (\c str) Parameter name
- \param nr (\c int) Parameter set number or None
- \param default (\c str) Default value as string
- \param callback (\c str) Name of the callback method
- """
- if nr!=None:
- name += str(nr)
- # Create the slot
- s = "self.%s_slot = DoubleSlot(%s)"%(name, default)
- exec s
- # Add the slot to the component
- s = "self.addSlot('%s', self.%s_slot)"%(name, name)
- exec s
- # Create the forwarder
- s = "self._%s_forwarder = NotificationForwarder(self.%s)"%(name, callback)
- exec s
- s = "self.%s_slot.addDependent(self._%s_forwarder)"%(name, name)
- exec s
-
-
- # _createODEjoint
- def _createODEjoint(self):
- """Create the corresponding ODE joint.
-
- This may only be called if the ODEDynamics component has been set.
-
- This method has to create an ODE joint object and store it
- in self.odejoint.
- """
- pass
-
- # _initODEjoint
- def _initODEjoint(self):
- """Initialize the ODE joint.
- """
- # This will call the attach() method of the ODE joint
- self.attach(self.body1, self.body2)
-
- print "***",self.odejoint.getParam(ode.ParamStopCFM)
-
- self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
- self.odejoint.setParam(ode.ParamVel, self.motorvel)
- self.odejoint.setParam(ode.ParamLoStop, self.lostop)
- self.odejoint.setParam(ode.ParamHiStop, self.histop)
- self.odejoint.setParam(ode.ParamFudgeFactor, self.fudgefactor)
- self.odejoint.setParam(ode.ParamBounce, self.bounce)
- self.odejoint.setParam(ode.ParamCFM, self.cfm)
- self.odejoint.setParam(ode.ParamStopERP, self.stoperp)
- self.odejoint.setParam(ode.ParamStopCFM, self.stopcfm)
-
- def onLoStopChanged(self):
- # print "Lostop has been changed to",self.lostop_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamLoStop, self.lostop_slot.getValue())
-
- def onHiStopChanged(self):
- # print "Histop has been changed to",self.histop_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamHiStop, self.histop_slot.getValue())
-
- def onMotorVelChanged(self):
- # print "Motor velocity has been changed to",self.motorvel_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamVel, self.motorvel)
-
-
- def onMotorFMaxChanged(self):
- # print "Motor FMax has been changed to",self.motorfmax_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
-
- def onFudgeFactorChanged(self):
- # print "Fudgefactor has been changed to",self.fudgefactor_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamFudgeFactor, self.fudgefactor_slot.getValue())
-
- def onBounceChanged(self):
- # print "Bounce has been changed to",self.bounce_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamBounce, self.bounce_slot.getValue())
-
-
- def onMotorVel2Changed(self):
- # print "Motor2 velocity has been changed to",self.motorvel2_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamVel2, self.motorvel2)
-
-
- def onMotorFMax2Changed(self):
- # print "Motor2 FMax has been changed to",self.motorfmax2_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamFMax2, self.motorfmax2)
-
- def onCFMChanged(self):
- # print "CFM has been changed to",self.cfm_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamCFM, self.cfm)
-
- def onStopCFMChanged(self):
- # print "Stop CFM has been changed to",self.stopcfm_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamStopCFM, self.stopcfm)
-
- def onStopERPChanged(self):
- # print "Stop ERP has been changed to",self.stoperp_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamStopERP, self.stoperp)
-
-
- # BallJoint
- class ODEBallJoint(ODEJointBase):
-
- def __init__(self,
- name = "ODEBallJoint",
- body1 = None,
- body2 = None,
- **params):
- ODEJointBase.__init__(self,
- name=name, body1=body1, body2=body2,
- **params)
-
- self._createSlots()
-
- # _createODEjoint
- def _createODEjoint(self):
- # Create the ODE joint
- self.odejoint = ode.BallJoint(self.odedynamics.world)
-
- # _initODEjoint
- def _initODEjoint(self):
- ODEJointBase._initODEjoint(self)
-
- W = self.worldtransform
- p = W[3]
- self.odejoint.setAnchor((p.x, p.y, p.z))
-
-
- # HingeJoint
- class ODEHingeJoint(ODEJointBase):
- """
- the rotation axis is the local x axis
- """
-
- exec slotPropertyCode("angle")
-
- def __init__(self,
- name = "ODEHingeJoint",
- body1 = None,
- body2 = None,
- **params):
- ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
- **params)
-
- self._createSlots()
-
- self.angle_slot = ProceduralDoubleSlot(self.computeAngle)
- self.addSlot("angle", self.angle_slot)
- getScene().timer().time_slot.addDependent(self.angle_slot)
-
- #???
- # def reset(self):
- # if self.odejoint!=None:
- # print "ANGLE:",self.odejoint.getAngle(), self.angle
- # self.odejoint.setAnchor(self.odejoint.getAnchor())
-
- # computeAngle
- def computeAngle(self):
- """Return the current angle.
-
- This method is used as procedure for the angle_slot.
- """
- if self.odejoint==None:
- return 0.0
- else:
- return self.odejoint.getAngle()
-
- # _createODEjoint
- def _createODEjoint(self):
- # Create the ODE joint
- self.odejoint = ode.HingeJoint(self.odedynamics.world)
-
- # _initODEjoint
- def _initODEjoint(self):
- ODEJointBase._initODEjoint(self)
-
- W = self.worldtransform
- p = W[3]
- self.odejoint.setAnchor((p.x, p.y, p.z))
- a = -W[0]
- # print "AXIS:",a,self.name
- self.odejoint.setAxis((a.x, a.y, a.z))
- self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
- self.odejoint.setParam(ode.ParamVel, self.motorvel)
-
-
- # SliderJoint
- class ODESliderJoint(ODEJointBase):
- """
- the slider axis is the local x axis
- """
-
- exec slotPropertyCode("position")
-
- def __init__(self,
- name = "ODEHingeJoint",
- body1 = None,
- body2 = None,
- **params):
- ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
- **params)
-
- self._createSlots()
-
- self.position_slot = ProceduralDoubleSlot(self.computePosition)
- self.addSlot("position", self.position_slot)
- getScene().timer().time_slot.addDependent(self.position_slot)
-
- # computePosition
- def computePosition(self):
- """Return the current slider position.
-
- This method is used as procedure for the position_slot.
- """
- if self.odejoint==None:
- return 0.0
- else:
- return self.odejoint.getPosition()
-
- # _createODEjoint
- def _createODEjoint(self):
- # Create the ODE joint
- self.odejoint = ode.SliderJoint(self.odedynamics.world)
-
- # _initODEjoint
- def _initODEjoint(self):
- ODEJointBase._initODEjoint(self)
-
- W = self.worldtransform
- a = W[0]
- self.odejoint.setAxis((a.x, a.y, a.z))
- self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
- self.odejoint.setParam(ode.ParamVel, self.motorvel)
-
-
- # Hinge2Joint
- class ODEHinge2Joint(ODEJointBase):
- """
- axis1 is the local z axis
- """
-
- exec slotPropertyCode("motorfmax")
- exec slotPropertyCode("motorvel")
- exec slotPropertyCode("suspensionerp")
- exec slotPropertyCode("suspensioncfm")
- exec slotPropertyCode("motorfmax2")
- exec slotPropertyCode("motorvel2")
-
- def __init__(self,
- name = "ODEHingeJoint",
- body1 = None,
- body2 = None,
- **params):
- ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
- **params)
-
- self._createSlots()
- self._createSlot("suspensionerp", None, "0.2", "onSuspensionERPChanged")
- self._createSlot("suspensioncfm", None, "1E-7", "onSuspensionCFMChanged")
-
- self._createSlot("motorvel2", None, "0.0", "onMotorVel2Changed")
- self._createSlot("motorfmax2", None, "0.0", "onMotorFMax2Changed")
-
-
- # default axis2 is the local y axis
- self.axis2_local = vec4(0,1,0,0)
-
- # _createODEjoint
- def _createODEjoint(self):
- # Create the ODE joint
- self.odejoint = ode.Hinge2Joint(self.odedynamics.world)
-
- # _initODEjoint
- def _initODEjoint(self):
- ODEJointBase._initODEjoint(self)
-
- W = self.worldtransform
- p = W[3]
- self.odejoint.setAnchor((p.x, p.y, p.z))
- a = W[2]
- self.odejoint.setAxis1((a.x, a.y, a.z))
- a = W*self.axis2_local
- self.odejoint.setAxis2((a.x, a.y, a.z))
-
- self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
- self.odejoint.setParam(ode.ParamVel, self.motorvel)
-
- def onSuspensionERPChanged(self):
- print "susp. erp has been changed to",self.suspensionerp_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamSuspensionERP,
- self.suspensionerp_slot.getValue())
-
- def onSuspensionCFMChanged(self):
- print "susp. cfm has been changed to",self.suspensioncfm_slot.getValue()
- if self.odejoint!=None:
- self.odejoint.setParam(ode.ParamSuspensionCFM,
- self.suspensioncfm_slot.getValue())
-
-
- # UniversalJoint
- class ODEUniversalJoint(ODEJointBase):
- """
-
- """
-
- exec slotPropertyCode("angle")
-
- def __init__(self,
- name = "ODEUniversalJoint",
- body1 = None,
- body2 = None,
- **params):
- ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
- **params)
-
- self._createSlots()
-
- # self.angle_slot = ProceduralDoubleSlot(self.computeAngle)
- # self.addSlot("angle", self.angle_slot)
- # getScene().timer().time_slot.addDependent(self.angle_slot)
-
-
- # _createODEjoint
- def _createODEjoint(self):
- # Create the ODE joint
- self.odejoint = ode.UniversalJoint(self.odedynamics.world)
-
- # _initODEjoint
- def _initODEjoint(self):
- ODEJointBase._initODEjoint(self)
-
- W = self.worldtransform
- p = W[3]
- self.odejoint.setAnchor((p.x, p.y, p.z))
- a = W[0]
- self.odejoint.setAxis1((a.x, a.y, a.z))
- a = W[1]
- self.odejoint.setAxis2((a.x, a.y, a.z))
- self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
- self.odejoint.setParam(ode.ParamVel, self.motorvel)
-