Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members   Related Pages  

c:/home/kevn/src/animaniac/ani/Dynamics/Body.h

Go to the documentation of this file.
00001 
00003 //
00004 //                         -=     Rigid Body     =-
00005 //
00006 // Definition: "rigid body, has position and rotation, 2nd order derivative"
00007 //
00009 //
00010 //    $RCSfile: Body.h,v $
00011 //    $Date: 2002/06/10 03:08:00 $
00012 //    $Revision: 1.19 $
00013 //    Copyright (C) 1998, 1999, 2000  Kevin Meinert, kevin@vrsource.org
00014 //
00015 //    This library is free software; you can redistribute it and/or
00016 //    modify it under the terms of the GNU Library General Public
00017 //    License as published by the Free Software Foundation; either
00018 //    version 2 of the License, or (at your option) any later version.
00019 //
00020 //    This library is distributed in the hope that it will be useful,
00021 //    but WITHOUT ANY WARRANTY; without even the implied warranty of
00022 //    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00023 //    Library General Public License for more details.
00024 //
00025 //    You should have received a copy of the GNU Library General Public
00026 //    License along with this library; if not, write to the Free
00027 //    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00028 //
00030 #ifndef BODY
00031 #define BODY
00032 
00033 #include <boost/smart_ptr.hpp>
00034 #include "gmtl/Matrix.h"
00035 #include "gmtl/MatrixOps.h" // identity
00036 #include "gmtl/Generate.h" // makePure
00037 #include "gmtl/Vec.h"
00038 #include "gmtl/Point.h"
00039 #include "gmtl/VecOps.h"
00040 #include "gmtl/Quat.h"
00041 #include "gmtl/QuatOps.h"
00042 #include "gmtl/Xforms.h"
00043 
00044 #include "ani/Dynamics/Memory.h"
00045 
00046 
00054 namespace ani
00055 {
00064    inline static void starOperator( const gmtl::Vec3f& a, gmtl::Matrix44f& aStar )
00065    {
00066       gmtl::identity( aStar );
00067       aStar(0,0) = 0.0f;
00068       aStar(0,1) = a[2];
00069       aStar(0,2) = -a[1];
00070 
00071       aStar(1,0) = -a[2];
00072       aStar(1,1) = 0.0f;
00073       aStar(1,2) = a[0];
00074 
00075       aStar(2,0) = a[1];
00076       aStar(2,1) = -a[0];
00077       aStar(2,2) = 0.0f;
00078    }     
00079 
00102    class Body : public ani::Memory
00103    {
00104    public:
00105       Body() : ani::Memory(),
00106                mPosition( 0.0f, 0.0f, 0.0f ),
00107                mRotation(),
00108 
00109                mLinearMomentum( 0.0f, 0.0f, 0.0f ),
00110                mAngularMomentum( 0.0f, 0.0f, 0.0f ),
00111 
00112                mForceAccumulator( 0.0f, 0.0f, 0.0f ),
00113                mTorqueAccumulator( 0.0f, 0.0f, 0.0f ),
00114 
00115                mMass( 1.0f ), mInvMass( 1.0f ), mVolume( 1.0f, 1.0f, 1.0f ),
00116                mBodySpaceInertiaTensor(), 
00117                mBodySpaceInertiaTensorInv(),
00118                mParticle( false )
00119                //mDragCoef(1.0f)//, mColor
00120       {
00121       }
00122 
00123       Body( const Body& p )
00124       {
00125          this->copy( p );
00126       }      
00127 
00128       virtual ~Body()
00129       {
00130       }
00131 
00132       void setParticle( bool val = false )
00133       {
00134          mParticle = val;
00135       }
00136       
00137       Body& operator=( const Body& p )
00138       {
00139          this->copy( p );
00140          return *this;
00141       }   
00142 
00146       inline virtual void copy( const Body& p )
00147       {
00148          mPosition = p.mPosition;           
00149          mLinearMomentum = p.mLinearMomentum;  
00150          mForceAccumulator = p.mForceAccumulator;  
00151          mMass = p.mMass;
00152          mInvMass = p.mInvMass; 
00153          mVolume = p.mVolume;         
00154          
00155          mParticle = p.mParticle;
00156          if (!mParticle)
00157          {
00158             mRotation = p.mRotation;          
00159             mAngularMomentum = p.mAngularMomentum;   
00160             //mWorldSpaceInertiaTensorInv = p.mWorldSpaceInertiaTensorInv; 
00161             //mVelocity = p.mVelocity;       
00162             //mAngularVelocity = p.mAngularVelocity;   
00163             mTorqueAccumulator = p.mTorqueAccumulator; 
00164             mBodySpaceInertiaTensor = p.mBodySpaceInertiaTensor;
00165             mBodySpaceInertiaTensorInv = p.mBodySpaceInertiaTensorInv; 
00166          }
00167          
00168          //mDragCoef = p.mDragCoef;
00169       }
00170 
00187       inline virtual void computeDerivative( const Body& currentState, const float& currentTime )
00188       {
00189          // change in position/rotation over time (first order)
00190             // x' = v = P(t)/M
00191             mPosition = currentState.linearVelocity();
00192 
00193          // change in momentum over time (second order)
00194             // P'(t) = F(t)
00195             mLinearMomentum = currentState.mForceAccumulator;
00196 
00197          if (!mParticle)
00198          {
00199          // change in position/rotation over time (first order)
00200             // R'(t) = w(t)*' R(t)   (matrix version)
00201             // q'(t) = 1/2 w(t) q(t) (quaternion version)
00202             //mRotation = 0.5f * currentState.angularVelocity() * currentState.mRotation;
00203             mRotation = gmtl::makePure( (gmtl::Vec3f)(currentState.angularVelocity() * 0.5f) ) * currentState.mRotation;
00204 
00205          // change in momentum over time (second order)
00206             // L'(t) = T(t)
00207             mAngularMomentum = currentState.mTorqueAccumulator;
00208          }
00209       }
00210 
00211       virtual void normalize()
00212       {
00213          if (!mParticle)
00214          {
00215             gmtl::normalize( mRotation );
00216          }
00217       }
00218 
00225       inline virtual void multiplyPhase( const Body& a, float h )
00226       {
00227          mPosition = a.mPosition * h;
00228          mLinearMomentum = a.mLinearMomentum * h;
00229          
00230          if (!mParticle)
00231          {
00232             mRotation = a.mRotation * h;
00233             mAngularMomentum = a.mAngularMomentum * h;
00234          }
00235       }
00236 
00240       inline virtual void multiplyPhase( float h )
00241       {
00242          mPosition *= h;
00243          mLinearMomentum *= h;
00244          
00245          if (!mParticle)
00246          {
00247             mRotation *= h;
00248             mAngularMomentum *= h;
00249          }
00250       }
00251 
00255       inline virtual void addPhase( const Body& a, const Body& b )
00256       {
00257          mPosition = a.mPosition + b.mPosition;
00258          mLinearMomentum = a.mLinearMomentum + b.mLinearMomentum;
00259          
00260          if (!mParticle)
00261          {
00262             mRotation = a.mRotation + b.mRotation;
00263             mAngularMomentum = a.mAngularMomentum + b.mAngularMomentum;
00264          }         
00265       }
00266 
00270       inline virtual void addPhase( const Body& a ) //
00271       {
00272          mPosition += a.mPosition;
00273          mLinearMomentum += a.mLinearMomentum;
00274          
00275          if (!mParticle)
00276          {
00277             mRotation += a.mRotation;
00278             mAngularMomentum += a.mAngularMomentum;
00279          }
00280       }
00281 
00287       void setPosition( const gmtl::Point3f& position) { mPosition = position; }
00288       void setVelocity( const gmtl::Vec3f& velocity ) { mLinearMomentum = velocity * mMass; }
00289       void setRotation( const gmtl::Quatf& rot ) { mRotation = rot; }
00290 
00295       void setAngularVelocity( gmtl::Vec3f& angularVel )
00296       {
00297          if (!mParticle)
00298          {
00299             mAngularMomentum = mBodySpaceInertiaTensor * angularVel;
00300          }
00301       }
00302 
00308       void applyForce( const gmtl::Vec3f& force ) 
00309       { 
00310          mForceAccumulator += force; 
00311       }
00312       
00316       void applyForce( const gmtl::Vec3f& force, 
00317                        const gmtl::Vec3f& location, 
00318                        const gmtl::Vec3f& center = gmtl::Vec3f( 0.0f, 0.0f, 0.0f ))
00319       {
00320          this->applyForce( force );
00321          if (!mParticle)
00322          {
00323             mTorqueAccumulator += gmtl::cross( location - center, force );
00324          }
00325       }
00326 
00331       void applyTorque( const gmtl::Vec3f& torque )
00332       {
00333          if (!mParticle)
00334          {
00335             mTorqueAccumulator += torque;
00336          }
00337       }
00338 
00339       inline virtual void zeroForce()
00340       {
00341          mForceAccumulator.set( 0.0f, 0.0f, 0.0f );
00342          mTorqueAccumulator.set( 0.0f, 0.0f, 0.0f );
00343       }
00344 
00348       void setMass( const float& kilograms )
00349       {
00350          mMass = kilograms;
00351          
00352          // precompute the inverse mass
00353          mInvMass = 1.0f / kilograms;  
00354          
00355          // precompute the body-space inertia tensor
00356          if (!mParticle) this->precalcBodyInertiaTensorForBlock();
00357       }
00358       
00359       void setVolume( gmtl::Vec3f vol )
00360       {
00361          mVolume = vol;
00362          
00363          // precompute the body inertia tensor
00364          if (!mParticle) this->precalcBodyInertiaTensorForBlock();
00365       }
00372 
00373       inline const gmtl::Point3f&       position() const { return mPosition; }
00374       inline const gmtl::Quatf&       rotation() const { return mRotation; }
00375       
00377       inline gmtl::Vec3f linearVelocity() const
00378       {
00379          return mLinearMomentum * mInvMass;
00380       }
00381       
00382 
00386       inline gmtl::Vec3f angularVelocity( const gmtl::Matrix44f& ws_tensor_inv ) const
00387       {
00388          gmtl::Vec3f temp;
00389          if (!mParticle) 
00390             temp = ws_tensor_inv * mAngularMomentum;
00391          return temp;
00392       }
00393       
00397       inline gmtl::Vec3f angularVelocity() const
00398       {
00399          gmtl::Matrix44f world_space_inertia_tensor_inv;
00400          if (!mParticle) calcWorldSpaceInertiaTensor( world_space_inertia_tensor_inv );
00401          
00402          return this->angularVelocity( world_space_inertia_tensor_inv );
00403       }
00404       
00405       inline gmtl::Vec3f linearMomentum() const
00406       {
00407          return mLinearMomentum;
00408       }
00409       
00410       inline gmtl::Vec3f angularMomentum() const
00411       {
00412          return mAngularMomentum;
00413       }
00414        
00416       inline const gmtl::Vec3f&       accumulatedForce() const { return mForceAccumulator; }
00417       inline const gmtl::Vec3f&       accumulatedTorque() const { return mTorqueAccumulator; }
00418       inline const float&             mass() const { return mMass; }
00419       inline const gmtl::Vec3f&       volume() const { return mVolume; }
00420 
00421    public:
00422    //:: "phase space" (see note above)
00423       // state variables:
00424       gmtl::Point3f mPosition;           // meters        - x(t)
00425       gmtl::Quatf mRotation;           // radians       - q(t) or R(t)
00426       gmtl::Vec3f mLinearMomentum;     // linear momentum P(t)
00427                                   // mLinearMomentum = mMass * mVelocity
00428       gmtl::Vec3f mAngularMomentum;    // angular momemtum - L(t)
00429                                   // |L| is how fast, L is the axis
00430                                   // L(t) = Ibody(t) * w(t)
00431       
00432       // derived quantities (aux variables)
00433       //gmtl::Vec3f mVelocity;         // v(t) (meters/second)
00434       //gmtl::Matrix44f mRotationM;      // R(t) (rotation matrix) 
00435       
00436       // The body may also be spinning. It spins about an axis through the 
00437       // center of mass. This axis is represented by a vector. The magnitude 
00438       // of the vector defines how fast the body is spinning and the direction 
00439       // of the vector defines the axis about which the body rotates. This it 
00440       // called the angular velocity w(t).
00441       //gmtl::Vec3f mAngularVelocity;              // w(t) or "omega"
00442       //gmtl::Matrix44f mWorldSpaceInertiaTensorInv; // tensor in world space coords.
00443       
00444    //:: other things needed by ODE to compute changes in the "phase space"
00445 
00446       // accumulation buffers:
00447       gmtl::Vec3f      mForceAccumulator;  // newtons - F(t)
00448       gmtl::Vec3f      mTorqueAccumulator; // torque - T(t)
00449 
00450       // constant (set) quantities
00451       float       mMass, mInvMass;    // kilograms
00452       gmtl::Vec3f      mVolume;            // 3D volume (a box)
00453 
00454       // The inertia tensor is a 3x3 matrix describing how the shape and mass 
00455       // distribution of the body is affected by the angular velocity. It can 
00456       // be thought of as a scaling factor between angular momentum and 
00457       // angular velocity. This matrix is computed in body space and then 
00458       // transformed as needed to world space. It is given as the matrix:
00459       // Ibody, and Ibodyinv:
00460       gmtl::Matrix44f mBodySpaceInertiaTensor, mBodySpaceInertiaTensorInv; // Ibody(t) (inertia tensor)
00461 
00462    // other attribs for the particle.
00463       float       mDragCoef;          // just a coef from 0 (no drag) to 1 (drag)
00464       bool        mParticle;          // don't calc rotations...
00465          
00466    // recalc funcs
00467    private:
00468       //: Precompute the inertia tensor in local (body) coordinates
00469       //
00470       // By using body space coordinates we can cheaply compute the inertia 
00471       // tensor for any orientation R(t) in terms of a precomputed integral 
00472       // in body space coordinates.  This is typically computed before the 
00473       // simulation begins and should be regarded as one of the input params.
00474       //
00475       // here we do a quick calc of Ibody(t) if the object is cube shaped
00476       void precalcBodyInertiaTensorForBlock()
00477       {
00478          // no need to init, since we're only ever overwriting the 3 diagonals.
00479          //mBodySpaceInertiaTensor.makeIdent();
00480          
00481          mBodySpaceInertiaTensor(0,0) = mMass * (mVolume[1]*mVolume[1] + mVolume[2]*mVolume[2]);
00482          mBodySpaceInertiaTensor(1,1) = mMass * (mVolume[0]*mVolume[0] + mVolume[2]*mVolume[2]);
00483          mBodySpaceInertiaTensor(2,2) = mMass * (mVolume[0]*mVolume[0] + mVolume[1]*mVolume[1]);
00484          
00485          // divide by a scalar 12
00486          float inv_scalar = 12.0f;
00487          mBodySpaceInertiaTensor(0,0) *= inv_scalar;
00488          mBodySpaceInertiaTensor(1,1) *= inv_scalar;
00489          mBodySpaceInertiaTensor(2,2) *= inv_scalar;
00490 
00491          // Invert sloooowly
00492          //mBodySpaceInertiaTensorInv.invert( mBodySpaceInertiaTensor );
00493          
00494          // invert very fast
00495          mBodySpaceInertiaTensorInv(0,0) = 1.0f / mBodySpaceInertiaTensor(0,0);
00496          mBodySpaceInertiaTensorInv(1,1) = 1.0f / mBodySpaceInertiaTensor(1,1);
00497          mBodySpaceInertiaTensorInv(2,2) = 1.0f / mBodySpaceInertiaTensor(2,2);
00498       }
00499       
00500       //: Precompute the inertia tensor in local (body) coordinates
00501       //
00502       // By using body space coordinates we can cheaply compute the inertia 
00503       // tensor for any orientation R(t) in terms of a precomputed integral 
00504       // in body space coordinates.  This is typically computed before the 
00505       // simulation begins and should be regarded as one of the input params.
00506       //
00507       // A simple way to compute the tensor matix is to create a bounding box 
00508       // around the polyhedron and sample inside this box. We accumulate 
00509       // the matrix as the sum of all the sample points scaled by the total 
00510       // mass and the number of points sampled. When a sample point falls 
00511       // inside the body, we add to the already accumulated matrix.  When a 
00512       // sample falls outside of the body, we do nothing. Some code to do 
00513       // this is given here: 
00514       /*
00515       void calcBodyInertiaTensorGeneral()
00516       {
00517          gmtl::Matrix44f mat;
00518          mBodySpaceInertiaTensor = mat;
00519          float low = -mVolume[0];
00520          float high = mVolume[0];
00521          float hits = 0.0f;
00522          for (int x = low; x < high; ++x)
00523          for (int y = low; y < high; ++y)
00524          for (int z = low; z < high; ++z)
00525          {
00526             if (x,y,z sample point falls within the body)
00527             {
00528                mat(0,0) = y * y + z * z;
00529                mat(0,1) = -(x * y );
00530                mat(0,2) = -(x * y );
00531 
00532                mat(1,0) = -(x * y );
00533                mat(1,1) = x * x + z * z;
00534                mat(1,2) = -(y * z );
00535 
00536                mat(2,0) = -(x * z );
00537                mat(2,1) = -(y * z );
00538                mat(2,2) = x * x + y * y;
00539 
00540                mBodySpaceInertiaTensor = mBodySpaceInertiaTensor + mat;
00541                ++hits;
00542             }
00543          }
00544          mBodySpaceInertiaTensor = (mMass/hits) * mBodySpaceInertiaTensor;
00545          mBodySpaceInertiaTensorInv.invert( mBodySpaceInertiaTensor );
00546       }
00547       */
00548 
00549       //: Calculate I^-1(t)
00550       //  I^-1(t) = R(t)  Ibody^-1(t)  R(t)^T
00551       void calcWorldSpaceInertiaTensor( gmtl::Matrix44f& worldSpaceInertiaTensorInv ) const
00552       {
00553          gmtl::Matrix44f r;
00554          
00555          // The orientation matrix R(t), which is needed to compute I^-1(t), 
00556          // will be computed as an auxiliary variable from q(t)
00557          gmtl::set( r, mRotation );
00558          
00559          // compute I^-1(t) from current rotation and the body-space 
00560          // inertia tensor
00561          worldSpaceInertiaTensorInv = r * (mBodySpaceInertiaTensorInv * gmtl::makeTranspose( r ));
00562       }
00563    };
00564 
00565    typedef boost::shared_ptr<Body> BodyPtr;
00566 
00567 } // ani namespace
00568 
00569 #endif

Generated on Wed Jun 12 01:54:01 2002 for Animaniac by doxygen1.2.15