TAPsRigidBodyDynamics.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsRigidBodyDynamics.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (03/25/2010)
UPDATE          (12/03/2010)
******************************************************************************/
#include "TAPsRigidBodyDynamics.hpp"
// Using Inclusion Model (i.e. definitions are included in declarations)
//                       (this name.cpp is included in name.hpp)
// Each friend is defined directly inside its declaration.

BEGIN_NAMESPACE_TAPs
//=============================================================================
// Constructors
//-----------------------------------------------------------------------------
template <typename T>
RigidBodyDynamics<T>::RigidBodyDynamics () : m_bPositionLocked( false )
{}
//-----------------------------------------------------------------------------
template <typename T>
RigidBodyDynamics<T>::RigidBodyDynamics ( Vector3<T> const & Origin, Quaternion<T> const & Orientation )
    : RigidBodyBase<T>( Origin, Orientation )
    , m_bPositionLocked( false )
{}
//-----------------------------------------------------------------------------
//template <typename T>
//RigidBodyDynamics<T>::RigidBodyDynamics ( RigidBodyDynamics<T> const &orig )
//  : 
//{}
//-----------------------------------------------------------------------------
template <typename T>
RigidBodyDynamics<T>::~RigidBodyDynamics ()
{}
//-----------------------------------------------------------------------------
template <typename T>
std::string RigidBodyDynamics<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "RigidBodyDynamics<" << typeid(T).name() << ">";
    ss << " is " << RigidBodyBase<T>::StrInfo();
    //ss << " with physical properties " << m_PhysicalProperties;
    return ss.str();
}
//-----------------------------------------------------------------------------
template <typename T>
bool RigidBodyDynamics<T>::CreateInteractionPointGroupBasedOnBVHTree (
    int level   
)
{
    if ( level < 0 )    return false;
    //-----------------------------------------------------
    m_IPGroup.GetTheSetOfInteractionPoints().clear();   // clear the interaction point group
    //-----------------------------------------------------
    // Currently only HalfEdgeModel supports BVH tree
    OpenGL::HalfEdgeModel<T> * pHFModel = dynamic_cast< OpenGL::HalfEdgeModel<T> * >( m_pModel );
    if ( !pHFModel )    return false;
    //-----------------------------------------------------
    // if BVH tree is note created yet,
    // then create it and delete it later
    bool bDelBVH = false;
    if ( pHFModel->GetBVHTree() == NULL ) {
        pHFModel->BuildBVHTree( TAPs::Enum::BVH_TREE_BINARY_SPHERE );
        bDelBVH = true;
    }
    //-----------------------------------------------------
    bool result = false;
    if ( pHFModel->GetBVHTree() != NULL ) {
        // Use a recursive function
        BVHNode<T> ** L = new BVHNode<T>*[1];
        L[0] = pHFModel->GetBVHTree()->Root();
        result = CreateInteractionPointGroupBasedOnBVHTree_Recursive( L, 1, 0, level );
        delete [] L;
    }
    //-----------------------------------------------------
    // Delete the BVH tree if the BVH tree was not created before this function call
    if ( bDelBVH ) {
        pHFModel->ClearBVHTree();
    }
    //-----------------------------------------------------
    return result;
}
//-----------------------------------------------------------------------------
template <typename T>
std::vector< Vector3<T> > RigidBodyDynamics<T>::GetAllInteractionPointPositionsInWorldSpace () const
{
    OpenGL::HalfEdgeModel<T> const *    m_pHalfEdgeModel = dynamic_cast< OpenGL::HalfEdgeModel<T> const * >( GetPtrToModel() );
    if ( !m_pHalfEdgeModel )    {
        std::vector< Vector3<T> > nothing;
        return nothing;
    };

    Matrix4x4<T> Trx( 1, 0, 0, GetOriginPoint()[0], 0, 1, 0, GetOriginPoint()[1], 0, 0, 1, GetOriginPoint()[2], 0, 0, 0, 1 );
    Trx *= GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHalfEdgeModel->GetTransform().RefToMatrixTransform();

    std::vector< Vector3<T> > positions;
    for ( unsigned int i = 0; i < RefToInteractionPointGroup().GetNumOfInteractionPoints(); ++i ) {
        positions.push_back( Trx * RefToInteractionPointGroup().GetInteractionPointAtIndex(i).GetPosition() );
    }

    return positions;
}
//-----------------------------------------------------------------------------
template <typename T>
bool RigidBodyDynamics<T>::CreateInteractionPointGroupBasedOnBVHTree_Recursive (
    BVHNode<T> ** node,
    int numOfNodes,
    int currentLevel,
    int finalLevel
)
{
    bool result = false;
    if ( currentLevel == finalLevel ) {
        for ( int i = 0; i < numOfNodes; ++i ) {
            m_IPGroup.AddInteractionPoint( node[i]->GetCenter() );
        }
        return true;
    }
    else {
        int size = numOfNodes * 2;
        BVHNode<T> ** L = new BVHNode<T>*[ size ];
        for ( int i = 0, j = 0; i < numOfNodes; ++i, j+=2 ) {
            L[j  ] = node[i]->Child(0);
            L[j+1] = node[i]->Child(1);
        }
        bool result = CreateInteractionPointGroupBasedOnBVHTree_Recursive( L, size, ++currentLevel, finalLevel );
        delete [] L;
    }
    return result;
}
//-----------------------------------------------------------------------------
template <typename T>
bool RigidBodyDynamics<T>::CalAndSetVolumeAndCentroid_BasedOnTetrahedra ()
{
    if ( !GetPtrToModel() ) return false;

    TAPs::CGAlgo<T>::Physics::CalVolumeAndCentroid_BasedOnTetrahedron( GetPtrToModel(), mPhyProp_tVolume, mPhyProp_vCenterOfMass );
    m_OrientationCenter = mPhyProp_vCenterOfMass;
    m_OriginPt = mPhyProp_vCenterOfMass;

    return true;
}
//-----------------------------------------------------------------------------
template <typename T>
bool RigidBodyDynamics<T>::CalAndSetVolumeAndCentroid_BasedOnInteractionPointGroup ()
{
    if ( !GetPtrToModel() ) return false;

    m_IPGroup.CalVolumeAndCentroid( mPhyProp_tVolume, mPhyProp_vCenterOfMass );
    m_OrientationCenter = mPhyProp_vCenterOfMass;
    m_OriginPt = mPhyProp_vCenterOfMass;

    return true;
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::ClearAllForcesAndTorques ()
{
    ClearForceAndTorque();
    RefToInteractionPointGroup().RefToCDPointForces().clear();
}
//-----------------------------------------------------------------------------
//=============================================================================




//=============================================================================
// Override the member functions from PhysicsSupport
//-----------------------------------------------------------------------------
template <typename T>
Vector3<T> const & RigidBodyDynamics<T>::GetCenterOfMass () const
{
    return mPhyProp_vCenterOfMass;
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::SetCenterOfMass ( Vector3<T> const & COM )
{
    mPhyProp_vCenterOfMass = COM;
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::SetCenterOfMass ( T x, T y, T z )
{
    mPhyProp_vCenterOfMass.SetXYZ( x, y, z );
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::AdjustCenterOfMass ( Vector3<T> const & COM )
{
    m_OriginPt -= mPhyProp_vCenterOfMass;
    m_OrientationCenter -= mPhyProp_vCenterOfMass;
    mPhyProp_vCenterOfMass = COM;
    m_OrientationCenter += mPhyProp_vCenterOfMass;
    m_OriginPt += mPhyProp_vCenterOfMass;
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::AdjustCenterOfMass ( T x, T y, T z )
{
    m_OriginPt -= mPhyProp_vCenterOfMass;
    m_OrientationCenter -= mPhyProp_vCenterOfMass;
    mPhyProp_vCenterOfMass.SetXYZ( x, y, z );
    m_OrientationCenter += mPhyProp_vCenterOfMass;
    m_OriginPt += mPhyProp_vCenterOfMass;
}
//-----------------------------------------------------------------------------
// Override the member functions from PhysicsSupport
//=============================================================================




//=============================================================================
// Override the member functions from AdvanceSimulationInterface
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::AdvanceSimulation ( T timeStep )
{
    StepSimulation( timeStep );
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::AdvanceSimulation ( T tCurrent, T tNext )
{
    StepSimulation( tNext - tCurrent );
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::AdvanceSimulation ( Simulation::SimClock<T> & simClock )
{
    if ( simClock.Current >= simClock.End ) return;
    StepSimulation( simClock.Step );
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::StepSimulation ( T dt )
{
    if ( IsPositionLocked() )   return;

    // Calculate all of the forces and moments on the object
    // ...

    //
    // Translation (Explicit Euler Integration)
    //
    // Calculate the linear acceleration (in world space) -- Acc = F/mass
    mPhyProp_vLinearAcceleration = mPhyProp_vTotalForce / mPhyProp_tMass;

#ifdef  TAPs_ADD_RESTING_LEVEL_Y
    if ( m_OriginPt[1] > mPhyProp_tRestingLevel ) {
        mPhyProp_vLinearAcceleration[1] -= GetPhyPropGravitationalConstant();
    }
#else //TAPs_ADD_RESTING_LEVEL_Y
    mPhyProp_vLinearAcceleration[1] -= GetPhyPropGravitationalConstant();
#endif//TAPs_ADD_RESTING_LEVEL_Y
    // Calculate the linear velocity (in wolrd space) -- Vel = Acc * dt
    mPhyProp_vLinearVelocity -= mPhyProp_vLinearVelocity.ComponentWiseMult( mPhyProp_vDampingLinearVelocity );
    mPhyProp_vLinearVelocity += mPhyProp_vLinearAcceleration * dt;
    // Calculate the position of the object (in world space)
    m_OriginPt += mPhyProp_vLinearVelocity * dt;

    //
    // Rotation (Explicit Euler Integration)
    //
    // Calculate the angular acceleration (in body space)
    //m_vAngularAcceleration = m_vTotalTorque - ( m_vAngularVelocity ^ ( m_mMomentOfInertia * m_vAngularVelocity ));
    // Calculate the angular velocity (in body space)
    mPhyProp_vAngularVelocity -= mPhyProp_vAngularVelocity.ComponentWiseMult( mPhyProp_vDampingAngularVelocity );
    mPhyProp_vAngularVelocity += mPhyProp_mInvMomentOfInertiaTensor * mPhyProp_vTotalTorque -
        ( mPhyProp_vAngularVelocity ^ ( mPhyProp_mMomentOfInertiaTensor * mPhyProp_vAngularVelocity ) );
    // Calculate the new orientation represented by quaternion 
    // (in body space) -- (1/2 * quaternion * quaternion(0,angular_velocity))*dt
    // If angular_velocity is expressed in world space, 
    // then the equation is (1/2 * quaternion(0,angular_velocity) * quaternion)*dt
    m_Orientation += m_Orientation * mPhyProp_vAngularVelocity * (T(0.5)*dt);
    m_Orientation.Normalized();
}
//-----------------------------------------------------------------------------
// Override the member functions from AdvanceSimulationInterface
//=============================================================================




//=============================================================================
// Override the member functions from RigidBodyBase
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::SetBVOfType ( Enum::CD type )
{
    RigidBodyBase::SetBVOfType( type );
    if ( !m_pBV )   return;
    m_pBV->SetPhysicsSupport( this );
}
//-----------------------------------------------------------------------------
// Override the member functions from RigidBodyBase
//=============================================================================




//=============================================================================
// OpenGL
#if defined(__gl_h_) || defined(__GL_H__)
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::Draw () const
{
    //std::cout << "m_pModel: " << m_pModel << "\n";
    if ( m_pModel ) {

        glPushMatrix();

        /*
        // In body space transformation sequence
        // 1) Translate to the center of mass in world space ( origin pt (in world space) + center of rotation (in body space) )
        // 2) Then rotate by the orientation
        // 3) Then translate back by the center of rotation in body space
        Matrix4x4<T> Trx(
            CGMath<T>::MatrixTranslation( GetOriginPoint() + GetOrientationCenter() ) *
            GetOrientation().GetRotationMatrix4x4() *
            CGMath<T>::MatrixTranslation( -GetOrientationCenter() )
        );
        //*/

        Matrix4x4<T> Trx( CalMatrixTransform() );
        GLfloat m[16] = {
            static_cast<GLfloat>(Trx[ 0]), static_cast<GLfloat>(Trx[ 4]), static_cast<GLfloat>(Trx[ 8]), static_cast<GLfloat>(Trx[ 12]), 
            static_cast<GLfloat>(Trx[ 1]), static_cast<GLfloat>(Trx[ 5]), static_cast<GLfloat>(Trx[ 9]), static_cast<GLfloat>(Trx[ 13]), 
            static_cast<GLfloat>(Trx[ 2]), static_cast<GLfloat>(Trx[ 6]), static_cast<GLfloat>(Trx[10]), static_cast<GLfloat>(Trx[ 14]), 
            static_cast<GLfloat>(Trx[ 3]), static_cast<GLfloat>(Trx[ 7]), static_cast<GLfloat>(Trx[11]), static_cast<GLfloat>(Trx[ 15])
        };
        glMultMatrixf( m );

        m_pModel->Draw();

        /*
        // Draw BVH Tree
        if ( m_pModel->GetBVHTree() ) {
            m_pModel->GetBVHTree()->Draw();
            //m_pModel->GetBVHTree()->DrawByOpenGLOnlyEndLevel();
        }
        //*/

        glPopMatrix();
    }
    else {
        glPushMatrix();
        glPushAttrib( GL_ALL_ATTRIB_BITS );
        glPointSize( 5 );
        glTranslatef( m_OriginPt[0], m_OriginPt[1], m_OriginPt[2] );
        glBegin( GL_POINTS );
            glVertex3f( 0, 0, 0 );
        glEnd();
        glPopAttrib();
        glPopMatrix();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::DrawCenterOfMass () const
{
    Matrix4x4<T> Trx( CalMatrixTransform() );
    Vector3<T> COM( Trx * GetCenterOfMass() );
    glPushMatrix();
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glDisable( GL_DEPTH_TEST );
    glDisable( GL_LIGHTING );
    glColor3f( 0.25, 0.25, 0.25 );
    glPointSize( 5 );
    glBegin( GL_POINTS );
    glVertex3f( COM[0], COM[1], COM[2] );
    glEnd();
    glPopAttrib();
    glPopMatrix();
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::DrawInteractionPointGroup () const
{
    glPushMatrix();
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glDisable( GL_DEPTH_TEST );
    Matrix4x4<T> Trx = CalMatrixTransform();
    GLfloat m[16] = {
        static_cast<GLfloat>(Trx[ 0]), static_cast<GLfloat>(Trx[ 4]), static_cast<GLfloat>(Trx[ 8]), static_cast<GLfloat>(Trx[ 12]), 
        static_cast<GLfloat>(Trx[ 1]), static_cast<GLfloat>(Trx[ 5]), static_cast<GLfloat>(Trx[ 9]), static_cast<GLfloat>(Trx[ 13]), 
        static_cast<GLfloat>(Trx[ 2]), static_cast<GLfloat>(Trx[ 6]), static_cast<GLfloat>(Trx[10]), static_cast<GLfloat>(Trx[ 14]), 
        static_cast<GLfloat>(Trx[ 3]), static_cast<GLfloat>(Trx[ 7]), static_cast<GLfloat>(Trx[11]), static_cast<GLfloat>(Trx[ 15])
    };
    glMultMatrixf( m );
    m_IPGroup.Draw();

    //OpenGL::HalfEdgeModel<T> * pHFModel = dynamic_cast< OpenGL::HalfEdgeModel<T> * >( m_pModel );
    //pHFModel->GetBVHTree()->Draw();

    glPopAttrib();
    glPopMatrix();
}
//-----------------------------------------------------------------------------
template <typename T>
void RigidBodyDynamics<T>::DrawForDebugging () const
{
    Draw();
    DrawInteractionPointGroup();
    DrawCenterOfMass();
    DrawOrigin();
    DrawOrientation();

    DrawBV();
}
//-----------------------------------------------------------------------------
#endif  // OpenGL
//=============================================================================


//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----