Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PhysicsBody.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_PhysicsBody_h_
00003 #define INCLUDED_PhysicsBody_h_
00004 
00005 #include "Motion/KinematicJoint.h"
00006 #include "PhysicsWorld.h"
00007 
00008 #include <LinearMath/btTransform.h>
00009 
00010 class btCollisionShape;
00011 class btCompoundShape;
00012 class btGeneric6DofConstraint;
00013 class btRigidBody;
00014 class CollisionData;
00015 class PhysicsWorld;
00016 
00017 class ConstraintInterface;
00018 class MotorController;
00019 
00020 //! an interface to the Bullet physics engine, although parameters controlled by PhysicsBody::ComponentListener and its associated LinkComponent
00021 class PhysicsBody {
00022   friend class PhysicsWorld;
00023 public:
00024   
00025   //! This object subscribes to a LinkComponent (usually the root KinematicJoint of a chain) and creates a corresponding PhysicsBodies for it and its children.
00026   /*! When parameters change in the KinematicJoint, the ComponentListener will update fields in the PhysicsBody.  This may require
00027    *  rebuilding the chain of inter-body constraints. */
00028   class ComponentListener : virtual protected plist::CollectionListener, virtual protected KinematicJoint::BranchListener {
00029   public:
00030     //! constructor, pass the physics world, source component, a dictionary of actuator positions (from which this joint's target position will be monitored), and the initial location */
00031     ComponentListener(PhysicsWorld& world_, LinkComponent& comp_, plist::DictionaryOf<plist::Primitive<float> >& positions_, const plist::Point* location, const plist::Point* orientation)
00032     : plist::CollectionListener(), world(world_), comp(comp_), positions(positions_), parent(NULL), subComps(), bodyLocation(location), bodyOrientation(orientation), body(NULL), 
00033     centerOfMassListener(NULL), collisionModelScaleListener(NULL), collisionModelRotationListener(NULL), collisionModelOffsetListener(NULL),
00034     massListener(comp.mass,*this,&PhysicsBody::ComponentListener::updateMass, false),
00035     collisionModelListener(comp.collisionModel,*this,&PhysicsBody::ComponentListener::updateModel,false)
00036     {
00037       init();
00038     }
00039     //! destructor
00040     ~ComponentListener();
00041     
00042     std::string getPath() const; //!< returns a string identifying the component within a chain
00043     std::string getModel() const { return (comp.model.size()>0) ? comp.model : comp.collisionModel; } //!< returns the name of the graphics model in use
00044     
00045     PhysicsBody* getBody() const { return body; } //!< returns the PhysicsBody if available, otherwise NULL
00046     PhysicsBody& findBody(); //!< returns the body this link is part of; either #body or the first parent who has a non-NULL #body (if it hits the root, a body is created!)
00047     
00048   protected:
00049     //! constructor for children attached to this link
00050     ComponentListener(PhysicsWorld& world_, LinkComponent& comp_, plist::DictionaryOf<plist::Primitive<float> >& positions_, ComponentListener* parent_)
00051     : plist::CollectionListener(), world(world_), comp(comp_), positions(positions_), parent(parent_), subComps(), bodyLocation(NULL), bodyOrientation(NULL), body(NULL), 
00052     centerOfMassListener(NULL), collisionModelScaleListener(NULL), collisionModelRotationListener(NULL), collisionModelOffsetListener(NULL),
00053     massListener(comp.mass,*this,&PhysicsBody::ComponentListener::updateMass, false),
00054     collisionModelListener(comp.collisionModel,*this,&PhysicsBody::ComponentListener::updateModel,false)
00055     {
00056       init();
00057     }
00058     
00059     void init(); //!< does the setup and initialization
00060     void allocBody(); //!< allocates #body (to be called if the link has mass and shape, or its children do.
00061     void freeBody(); //!< frees #body
00062     ComponentListener& findLinkRoot(); //!< returns the ComponentListener for the KinematicJoint; either *this, or the parent.
00063     void rebuildBody() { findBody().rebuild(); } //!< called when the body has significantly changed and must rebuild its parameters
00064     void updateListeners(); //!< reinitialize individual field listeners, and those of its subcomponents
00065     
00066     virtual void plistCollectionEntryAdded(plist::Collection& col, plist::ObjectBase& entry);
00067     virtual void plistCollectionEntryRemoved(plist::Collection& col, plist::ObjectBase& entry);
00068     virtual void plistCollectionEntriesChanged(plist::Collection& col);
00069     
00070     virtual void kinematicJointBranchAdded(KinematicJoint& parent, KinematicJoint& branch);
00071     virtual void kinematicJointBranchRemoved(KinematicJoint& parent, KinematicJoint& branch);
00072     virtual void kinematicJointReconfigured(KinematicJoint& joint);
00073     
00074     void updateMass();
00075     void updateModel();
00076     static bool hasCollisionShape(const LinkComponent& c);
00077     
00078     PhysicsWorld& world;
00079     LinkComponent& comp;
00080     plist::DictionaryOf<plist::Primitive<float> >& positions;
00081     ComponentListener* parent;
00082     std::map<LinkComponent*, ComponentListener*> subComps;
00083 
00084     const plist::Point * bodyLocation;
00085     const plist::Point * bodyOrientation;
00086     PhysicsBody* body;
00087 
00088     plist::CollectionCallbackMember<PhysicsBody>* centerOfMassListener;
00089     plist::CollectionCallbackMember<PhysicsBody>* collisionModelScaleListener;
00090     plist::CollectionCallbackMember<PhysicsBody>* collisionModelRotationListener;
00091     plist::CollectionCallbackMember<PhysicsBody>* collisionModelOffsetListener;
00092     plist::PrimitiveCallbackMember<PhysicsBody::ComponentListener> massListener;
00093     plist::PrimitiveCallbackMember<PhysicsBody::ComponentListener> collisionModelListener;
00094   };
00095   
00096   KinematicJoint& getLinkComponent() { return link; }
00097   
00098   std::string getPath() const;
00099   std::string getModel() const { return (link.model.size()>0) ? link.model : link.collisionModel; }
00100   
00101   btRigidBody& getBody() const { return *body; }
00102   const btVector3& getCenterOffset() const { return centerOffset; }
00103   
00104   void getCurrent(fmat::Column<3>& pos, fmat::Quaternion& quat) const;
00105   bool hasJoint() const;
00106   float getJoint() const;
00107   fmat::Column<3> getInertia() const { return fmat::pack(inertia[0],inertia[1],inertia[2]); }
00108   
00109   void teleport(const fmat::Column<3>& pos); //!< removes from world, then re-builds at new location
00110   void teleport(const fmat::Quaternion& quat); //!< removes from world, then re-builds at new location
00111   void teleport(const fmat::Column<3>& pos, const fmat::Quaternion& quat); //!< removes from world, then re-builds at new location
00112   void teleport(const btTransform& tr); //!< removes from world, then re-builds at new location
00113   void setVelocity(const fmat::Column<3>& linear, const fmat::Column<3>& angular); //!< sets a linear and angular velocity (world coordinates), also applies to children
00114   void setVelocity(const btVector3& linear, const btVector3& angular); //!< sets a linear and angular velocity (world coordinates), also applies to children
00115   
00116 protected:
00117   PhysicsBody(PhysicsWorld& world_, KinematicJoint& link_, const plist::Primitive<float>* pos_, const btTransform& tr, PhysicsBody* parent)
00118     : world(world_), link(link_), qpos(pos_), totalMass(0), centerOffset(), inertia(),
00119     compound(NULL), body(NULL), joint(NULL), jointInterface(NULL), jointController(NULL),
00120     lastTransform(tr), parentBody(parent), children(), collisionDataName(),
00121     frictionForceListener(link.frictionForce,*this,&PhysicsBody::updateFrictionForce, false),
00122     anistropicFrictionListener(link.anistropicFrictionRatio,*this,&PhysicsBody::updateAnistropicFriction, false)
00123   {
00124     init();
00125   }
00126   ~PhysicsBody();
00127   
00128   void init();
00129   void build();
00130   void buildLink(const KinematicJoint& kj, const fmat::Transform& tr);
00131   void addComponent(const LinkComponent& comp, const fmat::Transform& tr);
00132   void resetTransform();
00133   void clear();
00134   
00135   void updateFrictionForce();
00136   void updateAnistropicFriction();
00137   
00138   static bool isChild(const LinkComponent& pb, const LinkComponent& child);
00139 
00140   void rebuild() { clear(); build(); }
00141   
00142   PhysicsWorld& world;
00143   KinematicJoint& link;
00144   const plist::Primitive<float>* qpos;
00145   
00146   float totalMass;
00147   btVector3 centerOffset;
00148   btVector3 inertia;
00149   
00150   btCompoundShape * compound;
00151   btRigidBody * body;
00152   btGeneric6DofConstraint * joint;
00153   ConstraintInterface * jointInterface;
00154   MotorController * jointController;
00155   btTransform lastTransform;
00156   PhysicsBody* parentBody;
00157   std::set<PhysicsBody*> children;
00158   
00159   std::string collisionDataName;
00160   static std::map<std::string,CollisionData*> collisionData;
00161   friend class CollisionData;
00162   
00163   plist::PrimitiveCallbackMember<PhysicsBody> frictionForceListener;
00164   plist::CollectionCallbackMember<PhysicsBody> anistropicFrictionListener;
00165   
00166 private:
00167   PhysicsBody(const PhysicsBody&); //!< do not call
00168   PhysicsBody& operator=(const PhysicsBody&); //!< do not call
00169 };
00170 
00171 /*! @file
00172  * @brief 
00173  * @author Ethan Tira-Thompson (ejt) (Creator)
00174  */
00175 
00176 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:46 2016 by Doxygen 1.6.3