CIwFMat Physics::GetMatrixFromStaticdBody( const CIwFVec3 pos, const CIwFVec3 rot )
{
CIwFMat modelMatrix;
CIwFMat rotX, rotY, rotZ;
rotX.SetRotX( rot.x );
rotY.SetRotY( rot.y );
rotZ.SetRotZ( rot.z );
modelMatrix.CopyRot( rotZ * rotY * rotX );
modelMatrix.SetTrans( pos );
return modelMatrix;
}
CIwFMat Physics::GetMatrixFromRigidBody( const btRigidBody* b )
{
CIwFMat modelMatrix;
btMatrix3x3 mRot;
btTransform btTrans = b->getWorldTransform();
btVector3 pos = btTrans.getOrigin();
mRot = btTrans.getBasis();
modelMatrix.SetIdentity();
for( int i = 0, j = 0; i < 3; ++i, j = 0 )
{
modelMatrix.m[i][j] = ( mRot.getRow( i ).x() );
modelMatrix.m[i][++j] = ( mRot.getRow( i ).y() );
modelMatrix.m[i][++j] = ( mRot.getRow( i ).z() );
}
modelMatrix.SetTrans( CIwFVec3( pos[0], pos[1], pos[2] ) );
return modelMatrix;
}