home *** CD-ROM | disk | FTP | other *** search
/ Hackers Magazine 57 / CdHackersMagazineNr57.iso / Software / Multimedia / k3d-setup-0.7.11.0.exe / lib / site-packages / cgkit / odedynamics.py < prev    next >
Encoding:
Python Source  |  2007-01-11  |  47.0 KB  |  1,489 lines

  1. # ***** BEGIN LICENSE BLOCK *****
  2. # Version: MPL 1.1/GPL 2.0/LGPL 2.1
  3. #
  4. # The contents of this file are subject to the Mozilla Public License Version
  5. # 1.1 (the "License"); you may not use this file except in compliance with
  6. # the License. You may obtain a copy of the License at
  7. # http://www.mozilla.org/MPL/
  8. #
  9. # Software distributed under the License is distributed on an "AS IS" basis,
  10. # WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
  11. # for the specific language governing rights and limitations under the
  12. # License.
  13. #
  14. # The Original Code is the Python Computer Graphics Kit.
  15. #
  16. # The Initial Developer of the Original Code is Matthias Baas.
  17. # Portions created by the Initial Developer are Copyright (C) 2004
  18. # the Initial Developer. All Rights Reserved.
  19. #
  20. # Contributor(s):
  21. #
  22. # Alternatively, the contents of this file may be used under the terms of
  23. # either the GNU General Public License Version 2 or later (the "GPL"), or
  24. # the GNU Lesser General Public License Version 2.1 or later (the "LGPL"),
  25. # in which case the provisions of the GPL or the LGPL are applicable instead
  26. # of those above. If you wish to allow use of your version of this file only
  27. # under the terms of either the GPL or the LGPL, and not to allow others to
  28. # use your version of this file under the terms of the MPL, indicate your
  29. # decision by deleting the provisions above and replace them with the notice
  30. # and other provisions required by the GPL or the LGPL. If you do not delete
  31. # the provisions above, a recipient may use your version of this file under
  32. # the terms of any one of the MPL, the GPL or the LGPL.
  33. #
  34. # ***** END LICENSE BLOCK *****
  35. # $Id: odedynamics.py,v 1.8 2006/05/26 17:08:04 mbaas Exp $
  36.  
  37. ## \file odedynamics.py
  38. ## Contains the ODEDynamics class.
  39.  
  40. """This module contains a Dynamics component using the ODE rigid body
  41. dynamics package."""
  42.  
  43. import protocols, weakref
  44. from Interfaces import *
  45. from component import Component
  46. from scene import getScene
  47. from eventmanager import eventManager
  48. from cgtypes import *
  49. from worldobject import WorldObject
  50. from spheregeom import SphereGeom
  51. from ccylindergeom import CCylinderGeom
  52. from boxgeom import BoxGeom
  53. from trimeshgeom import TriMeshGeom
  54. from planegeom import PlaneGeom
  55. from joints import *
  56. from events import *
  57. from slots import *
  58. import cmds
  59. import _core
  60. try:
  61.     import ode
  62.     has_ode = True
  63. except:
  64.     has_ode = False
  65.  
  66. #import numarray
  67. #import numarray.linear_algebra
  68.  
  69. ODE_COLLISION = "ODECollision"
  70.  
  71. # ODECollisionEvent
  72. class ODECollisionEvent:
  73.     """Event object that is used as argument for collision events.
  74.     """
  75.     
  76.     def __init__(self, obj1, obj2, contacts, contactproperties):
  77.         self.obj1 = obj1
  78.         self.obj2 = obj2
  79.         self.contacts = contacts
  80.         self.contactproperties = contactproperties
  81.  
  82.     def __str__(self):
  83.         if self.obj1==None:
  84.             n1 = "None"
  85.         else:
  86.             n1 = '"%s"'%self.obj1.name
  87.         if self.obj2==None:
  88.             n2 = "None"
  89.         else:
  90.             n2 = '"%s"'%self.obj2.name
  91.         return '<ODECollisionEvent: obj1:%s obj2:%s #Contacts:%d>'%(n1, n2, len(self.contacts))
  92.  
  93.     # averageContactGeom
  94.     def averageContactGeom(self):
  95.         """Return the average contact position, normal and depth.
  96.         """
  97.         pos = vec3(0)
  98.         normal = vec3(0)
  99.         depth = 0.0
  100.         for c in self.contacts:
  101.             p,n,d,g1,g2 = c.getContactGeomParams()
  102.             pos += vec3(p)
  103.             normal += vec3(n)
  104.             depth += d
  105.  
  106.         # n is not 0 (the event wouldn't have been generated if it was)
  107.         n = len(self.contacts)
  108.         pos /= n
  109.         normal /= n
  110.         depth /= n
  111.  
  112.         return pos,normal,depth
  113.     
  114.  
  115. # ODEDynamics
  116. class ODEDynamics(Component):
  117.     """Dynamics component using the ODE rigid body dynamics package.
  118.     """
  119.  
  120.     def __init__(self,
  121.                  name="ODEDynamics",
  122.                  gravity = 9.81,
  123.                  substeps = 1,
  124.                  cfm = None,
  125.                  erp = None,
  126.                  defaultcontactproperties = None,
  127.                  enabled = True,
  128.                  show_contacts = False,
  129.                  contactmarkersize = 0.1,
  130.                  contactnormalsize = 1.0,
  131.                  collision_events = False,
  132.                  auto_add = False,
  133.                  auto_insert=True):
  134.         """Constructor.
  135.  
  136.         \param name (\c str) Component name
  137.         \param auto_add (\c bool) Automatically add the world objects to the simulation
  138.         \param auto_insert (\c bool) Automatically add the component to the scene
  139.         """
  140.         Component.__init__(self, name=name, auto_insert=auto_insert)
  141.  
  142.         scene = getScene()
  143.  
  144.         self.substeps = substeps
  145.         self.collision_events = collision_events
  146.  
  147.         self.world = ode.World()
  148.         g = -gravity*scene.up
  149.         self.world.setGravity(g)
  150.         if cfm!=None:
  151.             self.world.setCFM(cfm)
  152.         if erp!=None:
  153.             self.world.setERP(erp)
  154.         
  155. #        self.world.setAutoDisableFlag(True)
  156.  
  157.         self.enabled = enabled
  158.  
  159.         self.eventmanager = eventManager()
  160.         self.eventmanager.connect(STEP_FRAME, self)
  161.         self.eventmanager.connect(RESET, self.reset)
  162.  
  163.         # Space object
  164.         self.space = ode.Space()
  165.  
  166.         # Joint group for the contact joints
  167.         self.contactgroup = ode.JointGroup()
  168.  
  169.         # A dictionary with WorldObjects as keys and ODEBodies as values
  170.         self.body_dict = {}
  171.         # A list of all bodies
  172.         self.bodies = []
  173.         # A list of all joints
  174.         self.joints = []
  175.  
  176.         # A dictionary with contact properties
  177.         # Key is a 2-tuple (material1, material2). Value is a
  178.         # ODEContactProperties object.
  179.         # The value of (mat1, mat2) should always be the same than
  180.         # the value of (mat2, mat1).
  181.         self.contactprops = {}
  182.  
  183.         # Default contact properties
  184.         if defaultcontactproperties==None:
  185.             defaultcontactproperties = ODEContactProperties()
  186.         self.defaultcontactprops = defaultcontactproperties
  187.  
  188.         self.show_contacts = show_contacts
  189.         self.contactmarkersize = contactmarkersize
  190.         self.contactnormalsize = contactnormalsize
  191.  
  192.         # A list of weakrefs to body manipulators
  193.         self.body_manips = []
  194.  
  195.         # Debug statistics (the number of contacts per simulation step)
  196.         self.numcontacts = 0
  197.  
  198.         # Automatically add world objects
  199.         if auto_add:
  200.             # Add all rigid bodies first...
  201.             for obj in scene.worldRoot().iterChilds():
  202.                 try:
  203.                     obj = protocols.adapt(obj, IRigidBody)
  204.                     self.add(obj)
  205.                 except NotImplementedError:
  206.                     pass
  207.  
  208.             # Then add all joints...
  209.             for obj in scene.worldRoot().iterChilds():
  210.                 if isinstance(obj, ODEJointBase):
  211.                     self.add(obj)
  212.  
  213.     # add
  214.     def add(self, objs, categorybits=None, collidebits=None):
  215.         """Add a world object to the simulation.
  216.  
  217.         \param objs World objects to be simulated (given as names or objects)
  218.         """
  219.         objs = cmds.worldObjects(objs)
  220.         for obj in objs:
  221.             self._add(obj, categorybits, collidebits)
  222.  
  223.     def _add(self, obj, categorybits, collidebits):
  224.         
  225.         if isinstance(obj, ODEJointBase):
  226.             obj.activate(self)
  227.             self.joints.append(obj)
  228.             return
  229.  
  230.         try:
  231.             obj = protocols.adapt(obj, IRigidBody)
  232.         except NotImplementedError:
  233.             print 'Object "%s" is not a rigid body.'%obj.name
  234.             return
  235.  
  236.         # Should the object be ignored?
  237.         if not obj.dynamics:
  238.             return
  239.  
  240. #       print "#####################",obj.name,"#########################"
  241. #       print "Old center of gravity:",obj.cog
  242. #       print "Old inertiatensor:"
  243. #        print obj.inertiatensor
  244. #       print "Old Offset transform:"
  245.         P = obj.getOffsetTransform()
  246. #       print P
  247.         obj.pivot = P*obj.cog
  248. #       print "Setting pivot to", obj.pivot
  249. #        print "New center of gravity:", obj.cog
  250. #        print "Intermediate inertia tensor:"
  251. #        print obj.inertiatensor
  252. #        I = obj.inertiatensor
  253. #        a = numarray.array([I.getRow(0), I.getRow(1), I.getRow(2)], type=numarray.Float64)
  254. #        vals, vecs = numarray.linear_algebra.eigenvectors(a)
  255. #        print "Eigenvalues:",vals
  256.         # Eigenvektoren sind ungenau! (bei cube_base_cog grosse Abweichungen)
  257. #        b1 = vec3(vecs[0]).normalize()
  258. #        b2 = vec3(vecs[1]).normalize()
  259. #        b3 = vec3(vecs[2]).normalize()
  260. #        P = obj.getOffsetTransform()        
  261. #        Pnull = P*vec3(0,0,0)
  262. #        b1 = P*b1 - Pnull
  263. #        b2 = P*b2 - Pnull
  264. #        b3 = P*b3 - Pnull        
  265. #        I2 = mat3(b1,b2,b3)
  266. #        print I2
  267. #        if I2.determinant()<0:
  268. #            I2.setColumn(0, -I2.getColumn(0))
  269. #        print "Det of new basis",I2.determinant()
  270. #        P = obj.getOffsetTransform()
  271. #        P.setMat3(I2)
  272. #        obj.setOffsetTransform(P)
  273. #        print "New offset transform:"
  274. #        print P
  275. #        print "New center of gravity:",obj.cog
  276. #        print "New inertia tensor:"
  277. #        print obj.inertiatensor
  278.  
  279.         body = ODEBody(obj, self, categorybits=categorybits, collidebits=collidebits)
  280.         self.bodies.append(body)
  281.         self.body_dict[obj] = body
  282.  
  283.     # reset
  284.     def reset(self):
  285.         """Reset the body states.
  286.  
  287.         All bodies are reset to their position and velocity at the time
  288.         they were added.
  289.         
  290.         This method is also called when the RESET event is issued.
  291.         """
  292.         for b in self.bodies:
  293.             b.reset()
  294.             b.updateObj()
  295.  
  296.         for j in self.joints:
  297.             j.reset()
  298.  
  299.     # setContactProperties
  300.     def setContactProperties(self, (mat1, mat2), props):
  301.         """Set the contact properties of a pair of materials.
  302.  
  303.         The contact properties \a props are applied whenever an object
  304.         with material \a mat1 collides with an object with material \a mat2.
  305.  
  306.         \param mat1 (\c Material) Material 1
  307.         \param mat2 (\c Material) Material 2
  308.         \param props (\c ODEContactProperties) Contact properties
  309.         """
  310.         self.contactprops[(mat1,mat2)] = props
  311.         self.contactprops[(mat2,mat1)] = props
  312.  
  313.     # getContactProperties
  314.     def getContactProperties(self, matpair):
  315.         """Return the contact properties for a material pair.
  316.  
  317.         \param matpair A 2-tuple of two Material objects
  318.         \return Contact properties (\c ODEContactProperties)
  319.         """
  320.         cp = self.contactprops.get(matpair)
  321.         if cp==None:
  322. #            print 'ODEDynamics: Warning: No contact properties defined for material "%s" and "%s"'%(matpair[0].name, matpair[1].name)
  323.             cp = self.defaultcontactprops
  324.         return cp
  325.  
  326.     # createBodyManipulator
  327.     def createBodyManipulator(self, obj):
  328. #        return self.body_dict[obj].odebody
  329.         bm = ODEBodyManipulator(self.body_dict[obj])
  330.         self.body_manips.append(weakref.ref(bm))
  331.         return bm
  332.  
  333.     # nearCallback
  334.     def nearCallback(self, args, geom1, geom2):
  335.         # Check if the objects do collide
  336.         contacts = ode.collide(geom1, geom2)
  337.  
  338.         # No contacts? then return immediately
  339.         if len(contacts)==0:
  340.             return
  341.         
  342. #        print len(contacts),"contacts"
  343.         self.numcontacts += len(contacts)
  344.  
  345.         # Get the contact properties
  346.         cp = self.getContactProperties((geom1.material, geom2.material))
  347.  
  348.         # Create a collision event?
  349.         if self.collision_events:
  350.             obj1 = getattr(geom1, "worldobj", None)
  351.             obj2 = getattr(geom2, "worldobj", None)
  352.             evt = ODECollisionEvent(obj1, obj2, contacts, cp)
  353.             self.eventmanager.event(ODE_COLLISION, evt)
  354.  
  355.         # Create contact joints
  356.         for c in contacts:
  357.             if self.show_contacts:
  358.                 p,n,d,g1,g2 = c.getContactGeomParams()
  359.                 cmds.drawMarker(p, size=self.contactmarkersize, color=(1,0,0))
  360.                 cmds.drawLine(p, vec3(p)+self.contactnormalsize*vec3(n), color=(1,0.5,0.5))
  361. #                print p,n,d
  362.             
  363.             # Set the properties
  364.             cp.apply(c)
  365.             # Create the contact joint
  366.             j = ode.ContactJoint(self.world, self.contactgroup, c)
  367.             b1 = geom1.getBody()
  368.             b2 = geom2.getBody()
  369. #            if b1==None:
  370. #                b1=ode.environment
  371. #            if b2==None:
  372. #                b2=ode.environment
  373.             j.attach(b1, b2)
  374.  
  375.     # onStepFrame
  376.     def onStepFrame(self):
  377.         """Callback for the StepFrame event.
  378.         """
  379.         if self.substeps==0 or not self.enabled:
  380.             return
  381.  
  382.         if self.show_contacts:
  383.             cmds.drawClear()
  384.  
  385.         # Remove dead body manipulators...
  386.         self.body_manips = filter(lambda x: x() is not None, self.body_manips)
  387.  
  388.         # Sim loop...
  389.         subdt = getScene().timer().timestep/self.substeps
  390.         for i in range(self.substeps):
  391.             self.numcontacts = 0
  392.             
  393.             # Apply body manipulators
  394.             for bmref in self.body_manips:
  395.                 bm = bmref()
  396.                 if bm is not None:
  397.                     bm._apply()
  398.             
  399.             # Detect collisions and create contact joints
  400.             self.space.collide(None, self.nearCallback)
  401. #            print "#Contacts:",self.numcontacts
  402.             # Simulation step
  403. #            self.world.step(subdt)
  404.             self.world.quickStep(subdt)
  405.             # Remove all contact joints
  406.             self.contactgroup.empty()
  407.             
  408.         # Update the world objects
  409.         for body in self.bodies:
  410.             body.updateObj()
  411.  
  412.         # Reset body manipulators
  413.         for bmref in self.body_manips:
  414.             bm = bmref()
  415.             if bm is not None:
  416.                 bm._reset()
  417.  
  418.  
  419. ######################################################################
  420.  
  421. # ODEContactProperties
  422. class ODEContactProperties:
  423.  
  424.     """Contact properties.
  425.  
  426.     This class stores contact properties that are used whenever
  427.     two objects collide. The attributes are used to initialize
  428.     ODE contact joints.
  429.     """    
  430.     
  431.     def __init__(self,
  432.                  mode = 0,
  433.                  mu = 0.3,
  434.                  mu2 = None,
  435.                  bounce = None,
  436.                  bounce_vel = None,
  437.                  soft_erp = None,
  438.                  soft_cfm = None,
  439.                  motion1 = None,
  440.                  motion2 = None,
  441.                  slip1 = None,
  442.                  slip2 = None,
  443.                  fdir1 = None):
  444.         """Constructor.
  445.  
  446.         See the ODE manual for an explanation of the parameters.
  447.  
  448.         The flags for the mode parameter are automatically set.
  449.         However, you can initially set the ContactApprox flags.
  450.         """
  451.  
  452.         if mu2!=None:
  453.             mode |= ode.ContactMu2
  454.         else:
  455.             mu2 = 0.0
  456.  
  457.         if bounce!=None:
  458.             mode |= ode.ContactBounce
  459.         else:
  460.             bounce = 0.0
  461.  
  462.         if bounce_vel==None:
  463.             bounce_vel = 0.0
  464.  
  465.         if soft_erp!=None:
  466.             mode |= ode.ContactSoftERP
  467.         else:
  468.             soft_erp = 0.0
  469.  
  470.         if soft_cfm!=None:
  471.             mode |= ode.ContactSoftCFM
  472.         else:
  473.             soft_cfm = 0.0
  474.  
  475.         if motion1!=None:
  476.             mode |= ode.ContactMotion1
  477.         else:
  478.             motion1 = 0.0
  479.  
  480.         if motion2!=None:
  481.             mode |= ode.ContactMotion2
  482.         else:
  483.             motion2 = 0.0
  484.  
  485.         if slip1!=None:
  486.             mode |= ode.ContactSlip1
  487.         else:
  488.             slip1 = 0.0
  489.  
  490.         if slip2!=None:
  491.             mode |= ode.ContactSlip2
  492.         else:
  493.             slip2 = 0.0
  494.  
  495.         if fdir1!=None:
  496.             mode |= ode.ContactFDir1
  497.         else:
  498.             fdir1 = (0,0,0)
  499.  
  500.         self.mode = mode
  501. #        print "ODEContactProps: mode =",mode
  502.         self.mu = mu
  503.         self.mu2 = mu2
  504.         self.bounce = bounce
  505.         self.bounce_vel = bounce_vel
  506.         self.soft_erp = soft_erp
  507.         self.soft_cfm = soft_cfm
  508.         self.motion1 = motion1
  509.         self.motion2 = motion2
  510.         self.slip1 = slip1
  511.         self.slip2 = slip2
  512.         self.fdir1 = fdir1
  513.  
  514.     # apply
  515.     def apply(self, contact):
  516.         """Apply the contact properties to a contact joint."""
  517.         contact.setMode(self.mode)
  518.         contact.setMu(self.mu)
  519.         contact.setMu2(self.mu2)
  520.         contact.setBounce(self.bounce)
  521.         contact.setBounceVel(self.bounce_vel)
  522.         contact.setMotion1(self.motion1)
  523.         contact.setMotion2(self.motion2)
  524.         contact.setSlip1(self.slip1)
  525.         contact.setSlip2(self.slip2)
  526.         contact.setSoftERP(self.soft_erp)
  527.         contact.setSoftCFM(self.soft_cfm)
  528.         contact.setFDir1(self.fdir1)
  529.                          
  530.  
  531. ######################################################################
  532.  
  533. # ODEBodyManipulator
  534. class ODEBodyManipulator(object):
  535.     """Body manipulator class.
  536.  
  537.     This class can be used to apply external forces/torques to a body
  538.     or to manipulate the position/orientation/velocities directly.
  539.  
  540.     You should not create instances of this class yourself. Instead, use
  541.     the createBodyManipulator() method of the ODEDynamics component.
  542.     """
  543.     
  544.     def __init__(self, body):
  545.         """Constructor.
  546.  
  547.         \param body (\c ODEBody) The internal body object representing the
  548.                rigid body to manipulate.
  549.         """
  550.         self._body = body
  551.         self._odebody = body.odebody
  552.         self._reset()
  553.  
  554.     # setPos
  555.     def setPos(self, pos):
  556.         """Set the position of the body.
  557.  
  558.         pos must be a 3-sequence of floats.
  559.         """
  560.         self._odebody.setPosition(pos)
  561.  
  562.     # setRot
  563.     def setRot(self, rot):
  564.         """Set the rotation of the body.
  565.  
  566.         rot must be a mat3 containing the orientation.
  567.         """
  568.         self._odebody.setRotation(rot)
  569.  
  570.     # setLinearVel
  571.     def setLinearVel(self, vel):
  572.         """Set the linear velocity.
  573.  
  574.         vel must be a 3-sequence of floats.
  575.         """
  576.         self._odebody.setLinearVel(vel)
  577.  
  578.     # setAngularVel
  579.     def setAngularVel(self, vel):
  580.         """Set the angular velocity.
  581.  
  582.         vel must be a 3-sequence of floats.
  583.         """
  584.         self._odebody.setAngularVel(vel)
  585.  
  586.     # setInitialPos
  587.     def setInitialPos(self, pos):
  588.         """Set the initial position.
  589.  
  590.         pos must be a 3-sequence of floats.
  591.         """
  592.         self._body.initial_pos = vec3(pos)
  593.  
  594.     # setInitialRot
  595.     def setInitialRot(self, rot):
  596.         """Set the initial orientation.
  597.  
  598.         rot must be a mat3.
  599.         """
  600.         self._body.initial_rot = mat3(rot)
  601.  
  602.     # setInitialLinearVel
  603.     def setInitialLinearVel(self, vel):
  604.         """Set the initial linear velocity.
  605.  
  606.         vel must be a 3-sequence of floats.
  607.         """
  608.         self._odebody.initial_linearvel = vec3(vel)
  609.  
  610.     # setInitialAngularVel
  611.     def setInitialAngularVel(self, vel):
  612.         """Set the initial angular velocity.
  613.  
  614.         vel must be a 3-sequence of floats.
  615.         """
  616.         self._odebody.initial_angularvel = vec3(vel)
  617.  
  618.     # addForce
  619.     def addForce(self, force, relforce=False, pos=None, relpos=False):
  620.         """Apply an external force to a rigid body.
  621.         
  622.         Add an external force to the current force vector. force is a
  623.         vector containing the force to apply. If relforce is True the
  624.         force is interpreted in local object space, otherwise it is
  625.         assumed to be given in global world space. By default, the
  626.         force is applied at the center of gravity. You can also pass a
  627.         different position in the pos argument which must describe a
  628.         point in space. relpos determines if the point is given in
  629.         object space or world space (default).
  630.         """
  631.         
  632.         R = None
  633.         force = vec3(force)
  634.         if relforce:
  635.             R = mat3(self._odebody.getRotation())
  636.             force = R*force
  637.         # Is a position given? then add the corresponding torque
  638.         if pos!=None:
  639.             pos = vec3(pos)
  640.             bodyorig = vec3(self._odebody.getPosition())
  641.             if relpos:
  642.                 if R==None:
  643.                     R = mat3(self._odebody.getRotation())
  644.                 pos = R*pos + bodyorig
  645.  
  646.             self._torque += (pos-bodyorig).cross(force)
  647.             self._torque_flag = True
  648.             
  649.         self._force += vec3(force)
  650.         self._force_flag = True
  651.                     
  652.     # addTorque
  653.     def addTorque(self, torque, reltorque=False):
  654.         """Apply an external torque to a rigid body.
  655.         
  656.         Add an external torque to the current torque vector. torque is
  657.         a vector containing the torque to apply. reltorque determines
  658.         if the torque vector is given in object space or world space
  659.         (default).
  660.         """
  661.         
  662.         torque = vec3(torque)
  663.         if reltorque:
  664.             R = mat3(self._odebody.getRotation())
  665.             torque = R*torque
  666.             
  667.         self._torque += torque
  668.         self._torque_flag = True
  669.  
  670.     # _apply
  671.     def _apply(self):
  672.         """Apply the stored force/torque.
  673.  
  674.         This method is called by the ODEDynamics object during the simulation
  675.         step (once for every sub step).
  676.         """
  677.         if self._force_flag:
  678.             self._odebody.addForce(self._force)
  679.         if self._torque_flag:
  680.             self._odebody.addTorque(self._torque)
  681.  
  682.     # _reset
  683.     def _reset(self):
  684.         """Reset the stored force/torque.
  685.  
  686.         This method is called by the ODEDynamics object at the end of one
  687.         entire simulation step.
  688.         """
  689.         self._force = vec3(0)
  690.         self._torque = vec3(0)
  691.         
  692.         self._force_flag = False
  693.         self._torque_flag = False
  694.  
  695.     ## protected:
  696.         
  697.     # "body" property...
  698.     
  699.     def _getBody(self):
  700.         """Return the current body (WorldObject).
  701.  
  702.         This method is used for retrieving the \a body property.
  703.  
  704.         \return Rigid body
  705.         """
  706.         return self._body.obj
  707.  
  708.     body = property(_getBody, None, None, "Rigid body")
  709.  
  710.     # "odebody" property...
  711.     
  712.     def _getODEBody(self):
  713.         """Return the current ODE body.
  714.  
  715.         This method is used for retrieving the \a odebody property.
  716.  
  717.         \return ODE body
  718.         """
  719.         return self._odebody
  720.  
  721.     odebody = property(_getODEBody, None, None, "ODE body")
  722.  
  723.     
  724. ######################################################################
  725.  
  726. # ODEBody
  727. class ODEBody:
  728.     """This class is the interface between ODE bodies and world objects.
  729.  
  730.     It encapsulates the ODE body and the ODE geom objects.
  731.  
  732.     """
  733.     def __init__(self, obj, dyncomp, categorybits=None, collidebits=None):
  734.         """Constructor.
  735.  
  736.         \param obj (\c WorldObject) World object
  737.         \param dyncomp (\c ODEDynamics) Dynamics component
  738.         """
  739.         
  740.         # Store the world object
  741.         self.obj = obj
  742.  
  743.         self.dyncomp = dyncomp
  744.         world = self.dyncomp.world
  745.         space = self.dyncomp.space
  746.  
  747.         # Store initial position/orientation
  748.         self.initial_pos = obj.pos
  749.         self.initial_rot = obj.rot
  750.         self.initial_linearvel = obj.linearvel #vec3(0,0,0)
  751.         self.initial_angularvel = obj.angularvel #vec3(0,0,0)
  752.  
  753.         # The ODE body
  754.         self.odebody = None
  755.         # A list of ODE geoms 
  756.         self.odegeoms = None
  757.     
  758.         # Create ODE collision geoms
  759.         self.odegeoms = self.createGeoms(obj, space)
  760.  
  761.         # Set the category and collide bits
  762.         for geom in self.odegeoms:
  763.             if categorybits!=None:
  764.                 geom.setCategoryBits(categorybits)
  765.             if collidebits!=None:
  766.                 geom.setCollideBits(collidebits)
  767.  
  768.         # Create an ODE body
  769.         if not obj.static:
  770.             self.odebody = self.createBody(obj, world)
  771.             for g in self.odegeoms:
  772.                 g.setBody(self.odebody)
  773.  
  774.         # Store the material in the ODE geoms so that the contact
  775.         # properties can be determined in the near callback.
  776.         # Also store the corresponding WorldObject
  777.         for g in self.odegeoms:
  778.             g.material = obj.getMaterial()
  779.             g.worldobj = obj
  780.  
  781.         # Initialize the ODE body/geoms
  782.         self.reset()
  783.  
  784.         # Create the notification forwarders and make connections
  785.         self.initialization = True
  786.         # Forward changes to the "static" slot
  787.         self._static_forwarder = NotificationForwarder(self.onStaticChanged)
  788.         self.obj.static_slot.addDependent(self._static_forwarder)
  789.         # Forward changes to the "mass" slot
  790.         self._mass_forwarder = NotificationForwarder(self.onMassChanged)
  791.         self.obj.mass_slot.addDependent(self._mass_forwarder)
  792.         self.initialization = False
  793.  
  794.     # updateObj
  795.     def updateObj(self):
  796.         """Update the transformation of the world object.
  797.  
  798.         The transformation from the ODE body is copied to the corresponding
  799.         world object. The linearvel and angularvel attributes are
  800.         updated as well.
  801.         """
  802.         if self.obj.static:
  803.             return
  804.  
  805. #        if type(self.odegeom)==ode.GeomTriMesh:
  806. #            self.odegeom.clearTCCache()
  807.  
  808.         # Get the ODE transformation
  809.         pos = self.odebody.getPosition()
  810.         rot = self.odebody.getRotation()
  811.         # Create the transformation matrix for the world object
  812.         M = mat4().translation(vec3(pos))
  813.         M.setMat3(mat3(rot))
  814.         # Set the transformation on the world object
  815.         self.obj.transform = M
  816.  
  817.         self.obj.linearvel = vec3(self.odebody.getLinearVel())
  818.         self.obj.angularvel = vec3(self.odebody.getAngularVel())
  819.  
  820.     ## protected:
  821.  
  822.     # onStaticChanged
  823.     def onStaticChanged(self):
  824.         if self.initialization:
  825.             return
  826.         
  827.         if self.obj.static:
  828.             if self.odebody!=None:
  829.                 for g in self.odegeoms:
  830.                     g.setBody(None)
  831.             self.odebody = None
  832.         else:
  833.             self.odebody = self.createBody(self.obj, self.dyncomp.world)
  834.             for g in self.odegeoms:
  835.                 g.setBody(self.odebody)
  836.             self.odebody.setPosition(self.obj.pos)
  837.             self.odebody.setRotation(self.obj.rot.toList(rowmajor=True))
  838.             self.odebody.setLinearVel((0,0,0))
  839.             self.odebody.setAngularVel((0,0,0))            
  840.             
  841.     # onMassChanged
  842.     def onMassChanged(self):
  843.         if self.initialization:
  844.             return
  845.         
  846.         print "Mass changed to",self.obj.mass
  847.  
  848.     # reset
  849.     def reset(self):
  850.         """Restore the initial state of the body."""
  851.         if self.obj.static:
  852.             for g in self.odegeoms:
  853.                 if g.placeable():
  854.                     g.setPosition(self.initial_pos)
  855.                     g.setRotation(self.initial_rot.toList(rowmajor=True))
  856.         else:
  857.             self.odebody.setPosition(self.initial_pos)
  858.             self.odebody.setRotation(self.initial_rot.toList(rowmajor=True))
  859.             self.odebody.setLinearVel(self.initial_linearvel)
  860.             self.odebody.setAngularVel(self.initial_angularvel)
  861.  
  862.     # createBody
  863.     def createBody(self, obj, world):
  864.         """Create an ODE body.
  865.  
  866.         Creates an ODE body and initializes the mass properties.
  867.         """
  868.         # Create an ODE Mass object
  869.         M = ode.Mass()
  870.         m = obj.totalmass
  871.         if m==0:
  872.             print 'WARNING: Object "%s" has a mass of zero.'%obj.name
  873.         cog = obj.cog
  874.         I = obj.inertiatensor
  875. #        print '---Rigid body "%s"--------'%obj.name
  876. #        print 'cog:',cog
  877. #        print I
  878.         I = mat3(I)
  879.         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])
  880.  
  881.         # Create an ODE body
  882.         body = ode.Body(world)
  883.         body.setMass(M)
  884.  
  885.         return body
  886.  
  887.     # createGeoms
  888.     def createGeoms(self, obj, space):
  889.         """Create all the geoms for one world object including the children.
  890.  
  891.         The generated geoms will be encapsulated in GeomTransforms
  892.         so that all of them are defined with respect to the pivot coordinate
  893.         system of \a obj.
  894.  
  895.         \param obj (\c WorldObject) Top level world object
  896.         \param space (\c ode.Space) ODE Space object
  897.         \return List of ODE geom objects
  898.         """
  899.  
  900.         # Plane? This is a special case because ODE planes are not placeable
  901.         if isinstance(obj.geom, PlaneGeom):
  902.             if not obj.static:
  903.                 raise RuntimeError, "Planes can only be used as static bodies"
  904.             L = obj.localTransform()
  905.             n = L*vec3(0,0,1) - L*vec3(0)
  906.             n = n.normalize()
  907.             d = n*obj.pos
  908.             odegeom = ode.GeomPlane(space, n, d)
  909.             return [odegeom]
  910.  
  911.         res = []
  912.  
  913.         # Create all ODE geoms in the subtree starting at obj
  914.         # Each geom will have a position/rotation that moves it to
  915.         # the local coordinate system of obj.
  916.         geoms = self._createLGeoms(obj, mat4(1))
  917.  
  918.         # Apply the offset transform and encapsulate the geoms inside
  919.         # geom transforms...
  920.         P = obj.getOffsetTransform()
  921.         Pinv = P.inverse()
  922.         pos,rot,scale = Pinv.decompose()
  923.         if scale!=vec3(1,1,1):
  924.             print 'WARNING: ODEDynamics: Scaled geometries are not supported'
  925.         res = []
  926.         for g in geoms:
  927.             M = mat4(1).translation(vec3(g.getPosition()))
  928.             M.setMat3(mat3(g.getRotation()))
  929.             M *= Pinv;
  930.             p4 = M.getColumn(3)
  931.             g.setPosition((p4.x, p4.y, p4.z))
  932.             # row major or column major?
  933.             g.setRotation(M.getMat3().toList(rowmajor=True))
  934.         
  935.             gt = ode.GeomTransform(space)
  936.             gt.setGeom(g)
  937.             res.append(gt)
  938.  
  939.         return res
  940.         
  941.     # _createLGeoms
  942.     def _createLGeoms(self, obj, M):
  943.         """Create ODE geom objects for a world object.
  944.  
  945.         Create the ODE geoms for \a obj (and its children).
  946.         The transformation \a M is applied to each geom.
  947.  
  948.         The returned geoms are not assigned to a Body yet.
  949.  
  950.         \param obj (\c WorldObject) The world object
  951.         \param M (\c mat4) Transformation
  952.         \return List of ODE geoms
  953.         """
  954.         res = []
  955.         # Create an ODE geom for the object itself
  956.         geom = obj.geom
  957.         if geom!=None:
  958.             res.append(self._createGeom(geom, M))
  959.  
  960.         # Create ODE geoms for the children
  961.         for child in obj.iterChilds():
  962.             if not getattr(child, "dynamics", False):
  963.                 continue
  964.             L = child.localTransform()
  965.             res += self._createLGeoms(child, M*L)
  966.  
  967.         return res
  968.         
  969.     # _createGeom
  970.     def _createGeom(self, geom, M):
  971.         """Create an ODE geom object for the given cgkit geom.
  972.  
  973.         Create an ODE geom of the appropriate type (depening on \a geom)
  974.         and apply the transformation \a M to it (currently, M may only
  975.         contain a translation and a rotation, otherwise a warning is
  976.         printed).
  977.  
  978.         The returned geom is not assigned to a Body yet.
  979.  
  980.         \param geom (\c GeomObject) cgkit geom object
  981.         \param M (\c mat4) Transformation that's applied to the geom
  982.         \return ODE geom
  983.         """
  984.  
  985.         # Create the raw geom object that's defined in the local
  986.         # coordinate system L of the world object this geom belongs to.
  987.  
  988.         # Sphere geom?
  989.         if isinstance(geom, SphereGeom):
  990.             odegeom = ode.GeomSphere(None, geom.radius)
  991.         # CCylinder geom?
  992.         elif isinstance(geom, CCylinderGeom):
  993.             odegeom = ode.GeomCCylinder(None, geom.radius, geom.length)
  994.         # Box geom?
  995.         elif isinstance(geom, BoxGeom):
  996.             odegeom = ode.GeomBox(None, (geom.lx, geom.ly, geom.lz))
  997.         # TriMesh geom?
  998.         elif isinstance(geom, TriMeshGeom):
  999.             verts = []
  1000.             for i in range(geom.verts.size()):
  1001.                 verts.append(geom.verts.getValue(i))
  1002. #                print "V",i,verts[i]
  1003.             faces = []
  1004.             for i in range(geom.faces.size()):
  1005.                 f = geom.faces.getValue(i)
  1006.                 faces.append(f)
  1007. #                print "F",i,faces[i]
  1008.                 ia,ib,ic = faces[i]
  1009.                 a = verts[ia]
  1010.                 b = verts[ib]
  1011.                 c = verts[ic]
  1012.                 sa = ((b-a).cross(c-a)).length()
  1013. #                if sa<0.0001:
  1014. #                    print "*****KLEIN*****",sa
  1015.  
  1016.             tmd = ode.TriMeshData()
  1017.             tmd.build(verts, faces)            
  1018.             odegeom = ode.GeomTriMesh(tmd, None)
  1019.         # Plane geom?
  1020.         elif isinstance(geom, PlaneGeom):
  1021.             L = obj.localTransform()
  1022.             n = L*vec3(0,0,1) - L*vec3(0)
  1023.             n = n.normalize()
  1024.             d = n*obj.pos
  1025.             odegeom = ode.GeomPlane(space, n, d)
  1026.         # Unknown geometry
  1027.         else:
  1028.             raise ValueError, 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s".'%geom.name
  1029. #            print 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s". Using bounding box instead.'%obj.name
  1030. #            bmin, bmax = obj.boundingBox().getBounds()
  1031. #            s = bmax-bmin
  1032. #            odegeom = ode.GeomBox(None, s)
  1033. #            pos,rot,scale = P.inverse().decompose()
  1034. #            odegeom.setPosition(pos + 0.5*(bmax+bmin))
  1035. #            odegeomtransform = ode.GeomTransform(space)
  1036. #            odegeomtransform.setGeom(odegeom)
  1037. #            return odegeomtransform
  1038.  
  1039.         # Displace the geom by M
  1040.         pos,rot,scale = M.decompose()
  1041.         if scale!=vec3(1,1,1):
  1042.             print 'WARNING: ODEDynamics: Scaled geometries are not supported'
  1043.         odegeom.setPosition(pos)
  1044.         # row major or column major?
  1045.         odegeom.setRotation(rot.getMat3().toList(rowmajor=True))
  1046.         
  1047.         return odegeom
  1048.  
  1049. ######################################################################
  1050.  
  1051. # ODEJointBase
  1052. class ODEJointBase(WorldObject):
  1053.  
  1054.     protocols.advise(instancesProvide=[ISceneItem])
  1055.  
  1056.     exec slotPropertyCode("lostop")
  1057.     exec slotPropertyCode("histop")
  1058.     exec slotPropertyCode("motorvel")
  1059.     exec slotPropertyCode("motorfmax")
  1060.     exec slotPropertyCode("fudgefactor")
  1061.     exec slotPropertyCode("bounce")
  1062.     exec slotPropertyCode("cfm")
  1063.     exec slotPropertyCode("stoperp")
  1064.     exec slotPropertyCode("stopcfm")
  1065.  
  1066.     def __init__(self,
  1067.                  name = "",
  1068.                  body1 = None,
  1069.                  body2 = None,
  1070.                  **params):
  1071.         WorldObject.__init__(self, name=name, **params)
  1072.         
  1073.         # ODEDynamics component
  1074.         self.odedynamics = None
  1075.  
  1076.         # The corresponding ODE joint
  1077.         self.odejoint = None
  1078.  
  1079.         self.body1 = body1
  1080.         self.body2 = body2
  1081.  
  1082.     # attach
  1083.     def attach(self, body1, body2=None):
  1084.         self.body1 = body1
  1085.         self.body2 = body2
  1086.         if self.odejoint!=None:
  1087.             if body1==None:
  1088.                 b1 = ode.environment
  1089.             else:
  1090.                 b1 = self.odedynamics.body_dict[body1].odebody
  1091.             
  1092.             if body2==None:
  1093.                 b2 = ode.environment
  1094.             else:
  1095.                 b2 = self.odedynamics.body_dict[body2].odebody
  1096.  
  1097.             self.odejoint.attach(b1, b2)
  1098.  
  1099.     # activate
  1100.     def activate(self, odedynamics):
  1101.         """
  1102.         this method is called by the ODEDynamics componenent
  1103.         """
  1104.         if odedynamics==self.odedynamics:
  1105.             return
  1106.         if self.odedynamics!=None:
  1107.             print 'Warning: Joint "%s" is already in use'%self.name
  1108.         self.odedynamics = odedynamics
  1109.         self._createODEjoint()
  1110.         self._initODEjoint()
  1111.  
  1112.     # reset
  1113.     def reset(self):
  1114.         """Reset method.
  1115.  
  1116.         This may only be called after the bodies have been reset.
  1117.         """
  1118.         pass
  1119.  
  1120.     def _createSlots(self):
  1121.         
  1122.         self._createSlot("lostop", None, "-ode.Infinity", "onLoStopChanged")
  1123.         self._createSlot("histop", None, "ode.Infinity", "onHiStopChanged")
  1124.         self._createSlot("motorvel", None, "0.0", "onMotorVelChanged")
  1125.         self._createSlot("motorfmax", None, "0.0", "onMotorFMaxChanged")
  1126.         self._createSlot("fudgefactor", None, "1.0", "onFudgeFactorChanged")
  1127.         self._createSlot("bounce", None, "0.1", "onBounceChanged")
  1128.         self._createSlot("cfm", None, 1E-5, "onCFMChanged")
  1129.         self._createSlot("stoperp", None, 0.2, "onStopERPChanged")
  1130.         self._createSlot("stopcfm", None, 1E-5, "onStopCFMChanged")
  1131.         
  1132.     def _createSlot(self, name, nr, default, callback):
  1133.         """Create one joint param slot.
  1134.  
  1135.         This method creates a DoubleSlot called \a name (with additional
  1136.         number, if \a nr is not None). \a default is the initial default
  1137.         value (as string) and \a callback the name of the callback method.
  1138.  
  1139.         \param name (\c str) Parameter name
  1140.         \param nr (\c int) Parameter set number or None
  1141.         \param default (\c str) Default value as string
  1142.         \param callback (\c str) Name of the callback method
  1143.         """
  1144.         if nr!=None:
  1145.             name += str(nr)
  1146.         # Create the slot
  1147.         s = "self.%s_slot = DoubleSlot(%s)"%(name, default)
  1148.         exec s
  1149.         # Add the slot to the component
  1150.         s = "self.addSlot('%s', self.%s_slot)"%(name, name)
  1151.         exec s
  1152.         # Create the forwarder
  1153.         s = "self._%s_forwarder = NotificationForwarder(self.%s)"%(name, callback)
  1154.         exec s
  1155.         s = "self.%s_slot.addDependent(self._%s_forwarder)"%(name, name)
  1156.         exec s
  1157.        
  1158.  
  1159.     # _createODEjoint
  1160.     def _createODEjoint(self):
  1161.         """Create the corresponding ODE joint.
  1162.  
  1163.         This may only be called if the ODEDynamics component has been set.
  1164.  
  1165.         This method has to create an ODE joint object and store it
  1166.         in self.odejoint.
  1167.         """
  1168.         pass
  1169.  
  1170.     # _initODEjoint
  1171.     def _initODEjoint(self):
  1172.         """Initialize the ODE joint.
  1173.         """
  1174.         # This will call the attach() method of the ODE joint
  1175.         self.attach(self.body1, self.body2)
  1176.  
  1177.         print "***",self.odejoint.getParam(ode.ParamStopCFM)
  1178.  
  1179.         self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
  1180.         self.odejoint.setParam(ode.ParamVel, self.motorvel)
  1181.         self.odejoint.setParam(ode.ParamLoStop, self.lostop)
  1182.         self.odejoint.setParam(ode.ParamHiStop, self.histop)
  1183.         self.odejoint.setParam(ode.ParamFudgeFactor, self.fudgefactor)
  1184.         self.odejoint.setParam(ode.ParamBounce, self.bounce)
  1185.         self.odejoint.setParam(ode.ParamCFM, self.cfm)
  1186.         self.odejoint.setParam(ode.ParamStopERP, self.stoperp)
  1187.         self.odejoint.setParam(ode.ParamStopCFM, self.stopcfm)
  1188.  
  1189.     def onLoStopChanged(self):
  1190. #        print "Lostop has been changed to",self.lostop_slot.getValue()
  1191.         if self.odejoint!=None:
  1192.             self.odejoint.setParam(ode.ParamLoStop, self.lostop_slot.getValue())
  1193.  
  1194.     def onHiStopChanged(self):
  1195. #        print "Histop has been changed to",self.histop_slot.getValue()
  1196.         if self.odejoint!=None:
  1197.             self.odejoint.setParam(ode.ParamHiStop, self.histop_slot.getValue())
  1198.  
  1199.     def onMotorVelChanged(self):
  1200. #        print "Motor velocity has been changed to",self.motorvel_slot.getValue()
  1201.         if self.odejoint!=None:
  1202.             self.odejoint.setParam(ode.ParamVel, self.motorvel)
  1203.  
  1204.  
  1205.     def onMotorFMaxChanged(self):
  1206. #        print "Motor FMax has been changed to",self.motorfmax_slot.getValue()
  1207.         if self.odejoint!=None:
  1208.             self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
  1209.  
  1210.     def onFudgeFactorChanged(self):
  1211. #        print "Fudgefactor has been changed to",self.fudgefactor_slot.getValue()
  1212.         if self.odejoint!=None:
  1213.             self.odejoint.setParam(ode.ParamFudgeFactor, self.fudgefactor_slot.getValue())
  1214.  
  1215.     def onBounceChanged(self):
  1216. #        print "Bounce has been changed to",self.bounce_slot.getValue()
  1217.         if self.odejoint!=None:
  1218.             self.odejoint.setParam(ode.ParamBounce, self.bounce_slot.getValue())
  1219.  
  1220.  
  1221.     def onMotorVel2Changed(self):
  1222. #        print "Motor2 velocity has been changed to",self.motorvel2_slot.getValue()
  1223.         if self.odejoint!=None:
  1224.             self.odejoint.setParam(ode.ParamVel2, self.motorvel2)
  1225.  
  1226.  
  1227.     def onMotorFMax2Changed(self):
  1228. #        print "Motor2 FMax has been changed to",self.motorfmax2_slot.getValue()
  1229.         if self.odejoint!=None:
  1230.             self.odejoint.setParam(ode.ParamFMax2, self.motorfmax2)
  1231.  
  1232.     def onCFMChanged(self):
  1233. #        print "CFM has been changed to",self.cfm_slot.getValue()
  1234.         if self.odejoint!=None:
  1235.             self.odejoint.setParam(ode.ParamCFM, self.cfm)
  1236.  
  1237.     def onStopCFMChanged(self):
  1238. #        print "Stop CFM has been changed to",self.stopcfm_slot.getValue()
  1239.         if self.odejoint!=None:
  1240.             self.odejoint.setParam(ode.ParamStopCFM, self.stopcfm)
  1241.  
  1242.     def onStopERPChanged(self):
  1243. #        print "Stop ERP has been changed to",self.stoperp_slot.getValue()
  1244.         if self.odejoint!=None:
  1245.             self.odejoint.setParam(ode.ParamStopERP, self.stoperp)
  1246.  
  1247.  
  1248. # BallJoint
  1249. class ODEBallJoint(ODEJointBase):
  1250.  
  1251.     def __init__(self,
  1252.                  name = "ODEBallJoint",
  1253.                  body1 = None,
  1254.                  body2 = None,
  1255.                  **params):
  1256.         ODEJointBase.__init__(self,
  1257.                               name=name, body1=body1, body2=body2,
  1258.                               **params)
  1259.  
  1260.         self._createSlots()
  1261.  
  1262.     # _createODEjoint
  1263.     def _createODEjoint(self):
  1264.         # Create the ODE joint
  1265.         self.odejoint = ode.BallJoint(self.odedynamics.world)
  1266.  
  1267.     # _initODEjoint
  1268.     def _initODEjoint(self):
  1269.         ODEJointBase._initODEjoint(self)
  1270.  
  1271.         W = self.worldtransform
  1272.         p = W[3]
  1273.         self.odejoint.setAnchor((p.x, p.y, p.z))
  1274.  
  1275.  
  1276. # HingeJoint
  1277. class ODEHingeJoint(ODEJointBase):
  1278.     """
  1279.     the rotation axis is the local x axis
  1280.     """
  1281.  
  1282.     exec slotPropertyCode("angle")
  1283.  
  1284.     def __init__(self,
  1285.                  name = "ODEHingeJoint",
  1286.                  body1 = None,
  1287.                  body2 = None,
  1288.                  **params):
  1289.         ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
  1290.                               **params)
  1291.  
  1292.         self._createSlots()
  1293.  
  1294.         self.angle_slot = ProceduralDoubleSlot(self.computeAngle)
  1295.         self.addSlot("angle", self.angle_slot)
  1296.         getScene().timer().time_slot.addDependent(self.angle_slot)
  1297.  
  1298. #???
  1299. #    def reset(self):
  1300. #        if self.odejoint!=None:
  1301. #            print "ANGLE:",self.odejoint.getAngle(), self.angle
  1302. #            self.odejoint.setAnchor(self.odejoint.getAnchor())
  1303.         
  1304.     # computeAngle
  1305.     def computeAngle(self):
  1306.         """Return the current angle.
  1307.  
  1308.         This method is used as procedure for the angle_slot.
  1309.         """
  1310.         if self.odejoint==None:
  1311.             return 0.0
  1312.         else:
  1313.             return self.odejoint.getAngle()            
  1314.  
  1315.     # _createODEjoint
  1316.     def _createODEjoint(self):
  1317.         # Create the ODE joint
  1318.         self.odejoint = ode.HingeJoint(self.odedynamics.world)
  1319.  
  1320.     # _initODEjoint
  1321.     def _initODEjoint(self):
  1322.         ODEJointBase._initODEjoint(self)
  1323.  
  1324.         W = self.worldtransform
  1325.         p = W[3]
  1326.         self.odejoint.setAnchor((p.x, p.y, p.z))
  1327.         a = -W[0]
  1328. #        print "AXIS:",a,self.name
  1329.         self.odejoint.setAxis((a.x, a.y, a.z))
  1330.         self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
  1331.         self.odejoint.setParam(ode.ParamVel, self.motorvel)
  1332.  
  1333.  
  1334. # SliderJoint
  1335. class ODESliderJoint(ODEJointBase):
  1336.     """
  1337.     the slider axis is the local x axis
  1338.     """
  1339.  
  1340.     exec slotPropertyCode("position")
  1341.  
  1342.     def __init__(self,
  1343.                  name = "ODEHingeJoint",
  1344.                  body1 = None,
  1345.                  body2 = None,
  1346.                  **params):
  1347.         ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
  1348.                               **params)
  1349.  
  1350.         self._createSlots()
  1351.  
  1352.         self.position_slot = ProceduralDoubleSlot(self.computePosition)
  1353.         self.addSlot("position", self.position_slot)
  1354.         getScene().timer().time_slot.addDependent(self.position_slot)
  1355.  
  1356.     # computePosition
  1357.     def computePosition(self):
  1358.         """Return the current slider position.
  1359.  
  1360.         This method is used as procedure for the position_slot.
  1361.         """
  1362.         if self.odejoint==None:
  1363.             return 0.0
  1364.         else:
  1365.             return self.odejoint.getPosition()            
  1366.  
  1367.     # _createODEjoint
  1368.     def _createODEjoint(self):
  1369.         # Create the ODE joint
  1370.         self.odejoint = ode.SliderJoint(self.odedynamics.world)
  1371.  
  1372.     # _initODEjoint
  1373.     def _initODEjoint(self):
  1374.         ODEJointBase._initODEjoint(self)
  1375.  
  1376.         W = self.worldtransform
  1377.         a = W[0]
  1378.         self.odejoint.setAxis((a.x, a.y, a.z))
  1379.         self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
  1380.         self.odejoint.setParam(ode.ParamVel, self.motorvel)
  1381.  
  1382.  
  1383. # Hinge2Joint
  1384. class ODEHinge2Joint(ODEJointBase):
  1385.     """
  1386.     axis1 is the local z axis
  1387.     """
  1388.  
  1389.     exec slotPropertyCode("motorfmax")
  1390.     exec slotPropertyCode("motorvel")
  1391.     exec slotPropertyCode("suspensionerp")
  1392.     exec slotPropertyCode("suspensioncfm")
  1393.     exec slotPropertyCode("motorfmax2")
  1394.     exec slotPropertyCode("motorvel2")
  1395.  
  1396.     def __init__(self,
  1397.                  name = "ODEHingeJoint",
  1398.                  body1 = None,
  1399.                  body2 = None,
  1400.                  **params):
  1401.         ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
  1402.                               **params)
  1403.  
  1404.         self._createSlots()
  1405.         self._createSlot("suspensionerp", None, "0.2", "onSuspensionERPChanged")
  1406.         self._createSlot("suspensioncfm", None, "1E-7", "onSuspensionCFMChanged")
  1407.  
  1408.         self._createSlot("motorvel2", None, "0.0", "onMotorVel2Changed")
  1409.         self._createSlot("motorfmax2", None, "0.0", "onMotorFMax2Changed")
  1410.  
  1411.  
  1412.         # default axis2 is the local y axis
  1413.         self.axis2_local = vec4(0,1,0,0)
  1414.  
  1415.     # _createODEjoint
  1416.     def _createODEjoint(self):
  1417.         # Create the ODE joint
  1418.         self.odejoint = ode.Hinge2Joint(self.odedynamics.world)
  1419.  
  1420.     # _initODEjoint
  1421.     def _initODEjoint(self):
  1422.         ODEJointBase._initODEjoint(self)
  1423.  
  1424.         W = self.worldtransform
  1425.         p = W[3]
  1426.         self.odejoint.setAnchor((p.x, p.y, p.z))
  1427.         a = W[2]
  1428.         self.odejoint.setAxis1((a.x, a.y, a.z))
  1429.         a = W*self.axis2_local
  1430.         self.odejoint.setAxis2((a.x, a.y, a.z))
  1431.         
  1432.         self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
  1433.         self.odejoint.setParam(ode.ParamVel, self.motorvel)
  1434.  
  1435.     def onSuspensionERPChanged(self):
  1436.         print "susp. erp has been changed to",self.suspensionerp_slot.getValue()
  1437.         if self.odejoint!=None:
  1438.             self.odejoint.setParam(ode.ParamSuspensionERP,
  1439.                                    self.suspensionerp_slot.getValue())
  1440.  
  1441.     def onSuspensionCFMChanged(self):
  1442.         print "susp. cfm has been changed to",self.suspensioncfm_slot.getValue()
  1443.         if self.odejoint!=None:
  1444.             self.odejoint.setParam(ode.ParamSuspensionCFM,
  1445.                                    self.suspensioncfm_slot.getValue())
  1446.  
  1447.  
  1448. # UniversalJoint
  1449. class ODEUniversalJoint(ODEJointBase):
  1450.     """
  1451.  
  1452.     """
  1453.  
  1454.     exec slotPropertyCode("angle")
  1455.  
  1456.     def __init__(self,
  1457.                  name = "ODEUniversalJoint",
  1458.                  body1 = None,
  1459.                  body2 = None,
  1460.                  **params):
  1461.         ODEJointBase.__init__(self, name=name, body1=body1, body2=body2,
  1462.                               **params)
  1463.  
  1464.         self._createSlots()
  1465.  
  1466. #        self.angle_slot = ProceduralDoubleSlot(self.computeAngle)
  1467. #        self.addSlot("angle", self.angle_slot)
  1468. #        getScene().timer().time_slot.addDependent(self.angle_slot)
  1469.  
  1470.  
  1471.     # _createODEjoint
  1472.     def _createODEjoint(self):
  1473.         # Create the ODE joint
  1474.         self.odejoint = ode.UniversalJoint(self.odedynamics.world)
  1475.  
  1476.     # _initODEjoint
  1477.     def _initODEjoint(self):
  1478.         ODEJointBase._initODEjoint(self)
  1479.  
  1480.         W = self.worldtransform
  1481.         p = W[3]
  1482.         self.odejoint.setAnchor((p.x, p.y, p.z))
  1483.         a = W[0]
  1484.         self.odejoint.setAxis1((a.x, a.y, a.z))
  1485.         a = W[1]
  1486.         self.odejoint.setAxis2((a.x, a.y, a.z))
  1487.         self.odejoint.setParam(ode.ParamFMax, self.motorfmax)
  1488.         self.odejoint.setParam(ode.ParamVel, self.motorvel)
  1489.