C++ MatrixF::isIdentity方法代码示例

bool BtBody::init(   PhysicsCollision *shape, 
                        F32 mass,
                        U32 bodyFlags,
                        SceneObject *obj, 
                        PhysicsWorld *world )
{
   AssertFatal( obj, "BtBody::init - Got a null scene object!" );
   AssertFatal( world, "BtBody::init - Got a null world!" );
   AssertFatal( dynamic_cast<BtWorld*>( world ), "BtBody::init - The world is the wrong type!" );
   AssertFatal( shape, "BtBody::init - Got a null collision shape!" );
   AssertFatal( dynamic_cast<BtCollision*>( shape ), "BtBody::init - The collision shape is the wrong type!" );
   AssertFatal( ((BtCollision*)shape)->getShape(), "BtBody::init - Got empty collision shape!" );
	 
   // Cleanup any previous actor.
   _releaseActor();

   mWorld = (BtWorld*)world;

   mColShape = (BtCollision*)shape;
   btCollisionShape *btColShape = mColShape->getShape();
   MatrixF localXfm = mColShape->getLocalTransform();
   btVector3 localInertia( 0, 0, 0 );   

   // If we have a mass then we're dynamic.
   mIsDynamic = mass > 0.0f;
   if ( mIsDynamic )
   {
      if ( btColShape->isCompound() )
      {
         btCompoundShape *btCompound = (btCompoundShape*)btColShape;

         btScalar *masses = new btScalar[ btCompound->getNumChildShapes() ];
         for ( U32 j=0; j < btCompound->getNumChildShapes(); j++ )
	         masses[j] = mass / btCompound->getNumChildShapes();

         btVector3 principalInertia;
         btTransform principal;
         btCompound->calculatePrincipalAxisTransform( masses, principal, principalInertia );
         delete [] masses;

	      // Create a new compound with the shifted children.
	      btColShape = mCompound = new btCompoundShape();
	      for ( U32 i=0; i < btCompound->getNumChildShapes(); i++ )
	      {
		      btTransform newChildTransform = principal.inverse() * btCompound->getChildTransform(i);
		      mCompound->addChildShape( newChildTransform, btCompound->getChildShape(i) );
	      }

         localXfm = btCast<MatrixF>( principal );
      }

      // Note... this looks like we're changing the shape, but 
      // we're not.  All this does is ask the shape to calculate the
      // local inertia vector from the mass... the shape doesn't change.
      btColShape->calculateLocalInertia( mass, localInertia );
   }

   // If we have a local transform then we need to
   // store it and the inverse to offset the center
   // of mass from the graphics origin.
   if ( !localXfm.isIdentity() )
   {
      mCenterOfMass = new MatrixF( localXfm );
      mInvCenterOfMass = new MatrixF( *mCenterOfMass );
      mInvCenterOfMass->inverse();
   }

   mMass = mass;
   mActor = new btRigidBody( mass, NULL, btColShape, localInertia );
   
   int btFlags = mActor->getCollisionFlags();

   if ( bodyFlags & BF_TRIGGER )
      btFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
   if ( bodyFlags & BF_KINEMATIC )
   {
      btFlags &= ~btCollisionObject::CF_STATIC_OBJECT;
      btFlags |= btCollisionObject::CF_KINEMATIC_OBJECT;
   }

   mActor->setCollisionFlags( btFlags );

   mWorld->getDynamicsWorld()->addRigidBody( mActor );
   mIsEnabled = true;

   mUserData.setObject( obj );
   mUserData.setBody( this );
   mActor->setUserPointer( &mUserData );

   return true;
}