00001
00003
00004
00005
00006
00007
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00030
00031 #define BODY
00032
00033 #include <boost/smart_ptr.hpp>
00034 #include "gmtl/Matrix.h"
00035 #include "gmtl/MatrixOps.h"
00036 #include "gmtl/Generate.h"
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
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
00161
00162
00163 mTorqueAccumulator = p.mTorqueAccumulator;
00164 mBodySpaceInertiaTensor = p.mBodySpaceInertiaTensor;
00165 mBodySpaceInertiaTensorInv = p.mBodySpaceInertiaTensorInv;
00166 }
00167
00168
00169 }
00170
00187 inline virtual void computeDerivative( const Body& currentState, const float& currentTime )
00188 {
00189
00190
00191 mPosition = currentState.linearVelocity();
00192
00193
00194
00195 mLinearMomentum = currentState.mForceAccumulator;
00196
00197 if (!mParticle)
00198 {
00199
00200
00201
00202
00203 mRotation = gmtl::makePure( (gmtl::Vec3f)(currentState.angularVelocity() * 0.5f) ) * currentState.mRotation;
00204
00205
00206
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
00353 mInvMass = 1.0f / kilograms;
00354
00355
00356 if (!mParticle) this->precalcBodyInertiaTensorForBlock();
00357 }
00358
00359 void setVolume( gmtl::Vec3f vol )
00360 {
00361 mVolume = vol;
00362
00363
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
00423
00424 gmtl::Point3f mPosition;
00425 gmtl::Quatf mRotation;
00426 gmtl::Vec3f mLinearMomentum;
00427
00428 gmtl::Vec3f mAngularMomentum;
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447 gmtl::Vec3f mForceAccumulator;
00448 gmtl::Vec3f mTorqueAccumulator;
00449
00450
00451 float mMass, mInvMass;
00452 gmtl::Vec3f mVolume;
00453
00454
00455
00456
00457
00458
00459
00460 gmtl::Matrix44f mBodySpaceInertiaTensor, mBodySpaceInertiaTensorInv;
00461
00462
00463 float mDragCoef;
00464 bool mParticle;
00465
00466
00467 private:
00468
00469
00470
00471
00472
00473
00474
00475
00476 void precalcBodyInertiaTensorForBlock()
00477 {
00478
00479
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
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
00492
00493
00494
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
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551 void calcWorldSpaceInertiaTensor( gmtl::Matrix44f& worldSpaceInertiaTensorInv ) const
00552 {
00553 gmtl::Matrix44f r;
00554
00555
00556
00557 gmtl::set( r, mRotation );
00558
00559
00560
00561 worldSpaceInertiaTensorInv = r * (mBodySpaceInertiaTensorInv * gmtl::makeTranspose( r ));
00562 }
00563 };
00564
00565 typedef boost::shared_ptr<Body> BodyPtr;
00566
00567 }
00568
00569 #endif