btBody.cpp

Engine/source/T3D/physics/bullet/btBody.cpp
Detailed Description

  1
  2//-----------------------------------------------------------------------------
  3// Copyright (c) 2012 GarageGames, LLC
  4//
  5// Permission is hereby granted, free of charge, to any person obtaining a copy
  6// of this software and associated documentation files (the "Software"), to
  7// deal in the Software without restriction, including without limitation the
  8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  9// sell copies of the Software, and to permit persons to whom the Software is
 10// furnished to do so, subject to the following conditions:
 11//
 12// The above copyright notice and this permission notice shall be included in
 13// all copies or substantial portions of the Software.
 14//
 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 21// IN THE SOFTWARE.
 22//-----------------------------------------------------------------------------
 23
 24#include "platform/platform.h"
 25#include "T3D/physics/bullet/btBody.h"
 26
 27#include "T3D/physics/bullet/bt.h"
 28#include "T3D/physics/bullet/btCasts.h"
 29#include "T3D/physics/bullet/btWorld.h"
 30#include "T3D/physics/bullet/btCollision.h"
 31#include "math/mBox.h"
 32#include "console/console.h"
 33
 34
 35BtBody::BtBody() :
 36   mActor( NULL ),
 37   mWorld( NULL ),
 38   mMass( 0.0f ),
 39   mCompound( NULL ),
 40   mCenterOfMass( NULL ),
 41   mInvCenterOfMass( NULL ),
 42   mIsDynamic( false ),
 43   mIsEnabled( false )
 44{
 45}
 46
 47BtBody::~BtBody()
 48{
 49   _releaseActor();
 50}
 51
 52void BtBody::_releaseActor()
 53{
 54   if ( mActor )
 55   {
 56      mWorld->getDynamicsWorld()->removeRigidBody( mActor );
 57      mActor->setUserPointer( NULL );
 58      SAFE_DELETE( mActor );
 59   }
 60
 61   SAFE_DELETE( mCompound );
 62   SAFE_DELETE( mCenterOfMass );
 63   SAFE_DELETE( mInvCenterOfMass );
 64   
 65   mColShape = NULL;
 66}
 67
 68bool BtBody::init(   PhysicsCollision *shape, 
 69                        F32 mass,
 70                        U32 bodyFlags,
 71                        SceneObject *obj, 
 72                        PhysicsWorld *world )
 73{
 74   AssertFatal( obj, "BtBody::init - Got a null scene object!" );
 75   AssertFatal( world, "BtBody::init - Got a null world!" );
 76   AssertFatal( dynamic_cast<BtWorld*>( world ), "BtBody::init - The world is the wrong type!" );
 77   AssertFatal( shape, "BtBody::init - Got a null collision shape!" );
 78   AssertFatal( dynamic_cast<BtCollision*>( shape ), "BtBody::init - The collision shape is the wrong type!" );
 79   AssertFatal( ((BtCollision*)shape)->getShape(), "BtBody::init - Got empty collision shape!" );
 80    
 81   // Cleanup any previous actor.
 82   _releaseActor();
 83
 84   mWorld = (BtWorld*)world;
 85
 86   mColShape = (BtCollision*)shape;
 87   btCollisionShape *btColShape = mColShape->getShape();
 88   MatrixF localXfm = mColShape->getLocalTransform();
 89   btVector3 localInertia( 0, 0, 0 );   
 90
 91   // If we have a mass then we're dynamic.
 92   mIsDynamic = mass > 0.0f;
 93   if ( mIsDynamic )
 94   {
 95      if ( btColShape->isCompound() )
 96      {
 97         btCompoundShape *btCompound = (btCompoundShape*)btColShape;
 98
 99         btScalar *masses = new btScalar[ btCompound->getNumChildShapes() ];
100         for ( U32 j=0; j < btCompound->getNumChildShapes(); j++ )
101            masses[j] = mass / btCompound->getNumChildShapes();
102
103         btVector3 principalInertia;
104         btTransform principal;
105         btCompound->calculatePrincipalAxisTransform( masses, principal, principalInertia );
106         delete [] masses;
107
108         // Create a new compound with the shifted children.
109         btColShape = mCompound = new btCompoundShape();
110         for ( U32 i=0; i < btCompound->getNumChildShapes(); i++ )
111         {
112            btTransform newChildTransform = principal.inverse() * btCompound->getChildTransform(i);
113            mCompound->addChildShape( newChildTransform, btCompound->getChildShape(i) );
114         }
115
116         localXfm = btCast<MatrixF>( principal );
117      }
118
119      // Note... this looks like we're changing the shape, but 
120      // we're not.  All this does is ask the shape to calculate the
121      // local inertia vector from the mass... the shape doesn't change.
122      btColShape->calculateLocalInertia( mass, localInertia );
123   }
124
125   // If we have a local transform then we need to
126   // store it and the inverse to offset the center
127   // of mass from the graphics origin.
128   if ( !localXfm.isIdentity() )
129   {
130      mCenterOfMass = new MatrixF( localXfm );
131      mInvCenterOfMass = new MatrixF( *mCenterOfMass );
132      mInvCenterOfMass->inverse();
133   }
134
135   mMass = mass;
136   mActor = new btRigidBody( mass, NULL, btColShape, localInertia );
137   
138   int btFlags = mActor->getCollisionFlags();
139
140   if ( bodyFlags & BF_TRIGGER )
141      btFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
142   if ( bodyFlags & BF_KINEMATIC )
143   {
144      btFlags &= ~btCollisionObject::CF_STATIC_OBJECT;
145      btFlags |= btCollisionObject::CF_KINEMATIC_OBJECT;
146   }
147
148   mActor->setCollisionFlags( btFlags );
149
150   mWorld->getDynamicsWorld()->addRigidBody( mActor );
151   mIsEnabled = true;
152
153   mUserData.setObject( obj );
154   mUserData.setBody( this );
155   mActor->setUserPointer( &mUserData );
156
157   return true;
158}
159
160void BtBody::setMaterial(  F32 restitution,
161                           F32 friction, 
162                           F32 staticFriction )
163{
164   AssertFatal( mActor, "BtBody::setMaterial - The actor is null!" );
165
166   mActor->setRestitution( restitution );
167
168   // TODO: Weird.. Bullet doesn't have seperate dynamic 
169   // and static friction.
170   //
171   // Either add it and submit it as an official patch
172   // or hack it via contact reporting or something
173   // like that.
174
175   mActor->setFriction( friction );
176
177   // Wake it up... it may need to move.
178   mActor->activate();
179}
180
181void BtBody::setSleepThreshold( F32 linear, F32 angular )
182{
183   AssertFatal( mActor, "BtBody::setSleepThreshold - The actor is null!" );
184   mActor->setSleepingThresholds( linear, angular );
185}
186
187void BtBody::setDamping( F32 linear, F32 angular )
188{
189   AssertFatal( mActor, "BtBody::setDamping - The actor is null!" );
190   mActor->setDamping( linear, angular );
191}
192
193void BtBody::getState( PhysicsState *outState )
194{
195   AssertFatal( isDynamic(), "BtBody::getState - This call is only for dynamics!" );
196
197   // TODO: Fix this to do what we intended... to return
198   // false so that the caller can early out of the state
199   // hasn't changed since the last tick.
200
201   MatrixF trans;
202   if ( mInvCenterOfMass )
203      trans.mul( btCast<MatrixF>( mActor->getCenterOfMassTransform() ), *mInvCenterOfMass );
204   else
205      trans = btCast<MatrixF>( mActor->getCenterOfMassTransform() );
206
207   outState->position = trans.getPosition();
208   outState->orientation.set( trans );
209   outState->linVelocity = btCast<Point3F>( mActor->getLinearVelocity() ); 
210   outState->angVelocity = btCast<Point3F>( mActor->getAngularVelocity() ); 
211   outState->sleeping = !mActor->isActive();
212
213   // Bullet doesn't keep the momentum... recalc it.
214   outState->momentum = ( 1.0f / mActor->getInvMass() ) * outState->linVelocity;
215}
216
217Point3F BtBody::getCMassPosition() const
218{
219   AssertFatal( mActor, "BtBody::getCMassPosition - The actor is null!" );
220   return btCast<Point3F>( mActor->getCenterOfMassTransform().getOrigin() );
221}
222
223void BtBody::setLinVelocity( const Point3F &vel )
224{
225   AssertFatal( mActor, "BtBody::setLinVelocity - The actor is null!" );
226   AssertFatal( isDynamic(), "BtBody::setLinVelocity - This call is only for dynamics!" );
227
228   mActor->setLinearVelocity( btCast<btVector3>( vel ) );
229}
230
231void BtBody::setAngVelocity( const Point3F &vel )
232{
233   AssertFatal( mActor, "BtBody::setAngVelocity - The actor is null!" );
234   AssertFatal( isDynamic(), "BtBody::setAngVelocity - This call is only for dynamics!" );
235
236   mActor->setAngularVelocity( btCast<btVector3>( vel ) );
237}
238
239Point3F BtBody::getLinVelocity() const
240{
241   AssertFatal( mActor, "BtBody::getLinVelocity - The actor is null!" );
242   AssertFatal( isDynamic(), "BtBody::getLinVelocity - This call is only for dynamics!" );
243
244   return btCast<Point3F>( mActor->getLinearVelocity() );
245}
246
247Point3F BtBody::getAngVelocity() const
248{
249   AssertFatal( mActor, "BtBody::getAngVelocity - The actor is null!" );
250   AssertFatal( isDynamic(), "BtBody::getAngVelocity - This call is only for dynamics!" );
251
252   return btCast<Point3F>( mActor->getAngularVelocity() );
253}
254
255void BtBody::setSleeping( bool sleeping )
256{
257   AssertFatal( mActor, "BtBody::setSleeping - The actor is null!" );
258   AssertFatal( isDynamic(), "BtBody::setSleeping - This call is only for dynamics!" );
259
260   if ( sleeping )
261   {
262      //mActor->setCollisionFlags( mActor->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT );
263      mActor->setActivationState( WANTS_DEACTIVATION );
264      mActor->setDeactivationTime( 0.0f );
265   }
266   else
267   {
268      //mActor->setCollisionFlags( mActor->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT );
269      mActor->activate();
270   }
271}
272
273PhysicsWorld* BtBody::getWorld() 
274{
275   return mWorld; 
276}
277
278PhysicsCollision* BtBody::getColShape() 
279{
280   return mColShape;
281}
282
283MatrixF& BtBody::getTransform( MatrixF *outMatrix )
284{
285   AssertFatal( mActor, "BtBody::getTransform - The actor is null!" );
286
287   if ( mInvCenterOfMass )
288      outMatrix->mul( *mInvCenterOfMass, btCast<MatrixF>( mActor->getCenterOfMassTransform() ) );
289   else
290      *outMatrix = btCast<MatrixF>( mActor->getCenterOfMassTransform() );
291
292   return *outMatrix;
293}
294
295void BtBody::setTransform( const MatrixF &transform )
296{
297   AssertFatal( mActor, "BtBody::setTransform - The actor is null!" );
298
299   if ( mCenterOfMass )
300   {
301      MatrixF xfm;
302      xfm.mul( transform, *mCenterOfMass );
303      mActor->setCenterOfMassTransform( btCast<btTransform>( xfm ) ); 
304   }
305   else
306      mActor->setCenterOfMassTransform( btCast<btTransform>( transform ) ); 
307
308   // If its dynamic we have more to do.
309   if ( isDynamic() )
310   {
311      // Clear any velocity and forces... this is a warp.
312      mActor->clearForces();
313      mActor->setLinearVelocity( btVector3( 0, 0, 0 ) );
314      mActor->setAngularVelocity( btVector3( 0, 0, 0 ) );
315      mActor->activate();
316   }
317}
318
319void BtBody::applyCorrection( const MatrixF &transform )
320{
321   AssertFatal( mActor, "BtBody::applyCorrection - The actor is null!" );
322   AssertFatal( isDynamic(), "BtBody::applyCorrection - This call is only for dynamics!" );
323
324   if ( mCenterOfMass )
325   {
326      MatrixF xfm;
327      xfm.mul( transform, *mCenterOfMass );
328      mActor->setCenterOfMassTransform( btCast<btTransform>( xfm ) ); 
329   }
330   else
331      mActor->setCenterOfMassTransform( btCast<btTransform>( transform ) ); 
332}
333
334void BtBody::applyImpulse( const Point3F &origin, const Point3F &force )
335{
336   AssertFatal( mActor, "BtBody::applyImpulse - The actor is null!" );
337   AssertFatal( isDynamic(), "BtBody::applyImpulse - This call is only for dynamics!" );
338
339   // Convert the world position to local
340   MatrixF trans = btCast<MatrixF>( mActor->getCenterOfMassTransform() );
341   trans.inverse();
342   Point3F localOrigin( origin );
343   trans.mulP( localOrigin );
344
345   if ( mCenterOfMass )
346   {
347      Point3F relOrigin( localOrigin );
348      mCenterOfMass->mulP( relOrigin );
349      Point3F relForce( force );
350      mCenterOfMass->mulV( relForce );
351      mActor->applyImpulse( btCast<btVector3>( relForce ), btCast<btVector3>( relOrigin ) );
352   }
353   else
354      mActor->applyImpulse( btCast<btVector3>( force ), btCast<btVector3>( localOrigin ) );
355
356   if ( !mActor->isActive() )
357      mActor->activate();
358}
359
360void BtBody::applyTorque( const Point3F &torque )
361{
362   AssertFatal(mActor, "BtBody::applyTorque - The actor is null!");
363   AssertFatal(isDynamic(), "BtBody::applyTorque - This call is only for dynamics!");
364
365   mActor->applyTorque( btCast<btVector3>(torque) );
366
367   if (!mActor->isActive())
368      mActor->activate();
369}
370
371void BtBody::applyForce( const Point3F &force )
372{
373   AssertFatal(mActor, "BtBody::applyForce - The actor is null!");
374   AssertFatal(isDynamic(), "BtBody::applyForce - This call is only for dynamics!");
375
376   if (mCenterOfMass)
377   {
378      Point3F relForce(force);
379      mCenterOfMass->mulV(relForce);
380      mActor->applyCentralForce(btCast<btVector3>(relForce));
381   }
382   else
383      mActor->applyCentralForce(btCast<btVector3>(force));
384
385   if (!mActor->isActive())
386      mActor->activate();
387}
388
389Box3F BtBody::getWorldBounds()
390{   
391   btVector3 min, max;
392   mActor->getAabb( min, max );
393
394   Box3F bounds( btCast<Point3F>( min ), btCast<Point3F>( max ) );
395
396   return bounds;
397}
398
399void BtBody::setSimulationEnabled( bool enabled )
400{
401   if ( mIsEnabled == enabled )
402      return;
403
404   if ( !enabled )
405      mWorld->getDynamicsWorld()->removeRigidBody( mActor );
406   else
407      mWorld->getDynamicsWorld()->addRigidBody( mActor );
408
409   mIsEnabled = enabled;
410}
411
412void BtBody::findContact(SceneObject **contactObject,
413   VectorF *contactNormal,
414   Vector<SceneObject*> *outOverlapObjects) const
415{
416}
417
418void BtBody::moveKinematicTo(const MatrixF &transform)
419{
420   AssertFatal(mActor, "BtBody::moveKinematicTo - The actor is null!");
421
422   U32 bodyflags = mActor->getCollisionFlags();
423   const bool isKinematic = bodyflags & BF_KINEMATIC;
424   if (!isKinematic)
425   {
426      Con::errorf("BtBody::moveKinematicTo is only for kinematic bodies.");
427      return;
428   }
429
430   if (mCenterOfMass)
431   {
432      MatrixF xfm;
433      xfm.mul(transform, *mCenterOfMass);
434      mActor->setCenterOfMassTransform(btCast<btTransform>(xfm));
435   }
436   else
437      mActor->setCenterOfMassTransform(btCast<btTransform>(transform));
438}
439