RigidBody.cpp

/***************************************************************************
 *   This file is part of OpenCAL: Open Computer Animation Library         *
 *   I created OpenCAL as my master's thesis Computer Science (multimedia) *
 *   at the tUL university in Diepenbeek, Belgium                          *
 *                                                                         *
 *   Copyright (C) 2003-2004 by Jeroen Dierckx                             *
 *   jeroen.dierckx@student.luc.ac.be                                      *
 *                                                                         *
 ***************************************************************************/

// Includes
#include "RigidBody.h"
#include <OpenCAL/PhaseSpace.h>
using namespace OpenCAL;

using namespace std;


/******************************
* Constructors and destructor *
******************************/

RigidBody::RigidBody(System *parent, float mass)
: PhysicalObject(parent, mass), Rigid()
{
#ifdef VERBOSE
    Debug::print("RigidBody constructor", 2);
#endif // VERBOSE
    //cout << "mass = " << mass << endl;
}

RigidBody::~RigidBody()
{
#ifdef VERBOSE
    Debug::print("RigidBody destructor", 2);
#endif // VERBOSE
}


/******************
* Force functions *
******************/

void RigidBody::addForce(const Vector3 &force, const Vector3 &position)
{
    m_force += force;
    //m_torque += (position - m_position).crossProduct(force);
    m_torque += position.crossProduct(force);
}


/******************
* Other functions *
******************/

Vector3 RigidBody::getAngularVelocity() const
{
    Matrix33 rot = m_rotation.toMatrix33(); // Rotation matrix
    Matrix33 invInertia = rot * m_invInertia * rot.transposed(); // Inversed inertia tensor (in world coördinates)
    return invInertia * m_angularMomentum;
}

void RigidBody::setAngularVelocity(const Vector3 &velocity)
{

    cout << "InvInertia:" << endl;
    m_invInertia.print();

    Matrix33 inertia = m_invInertia;
    inertia.set(0,0, 1.0f / inertia.get(0,0));
    inertia.set(1,1, 1.0f / inertia.get(1,1));
    inertia.set(2,2, 1.0f / inertia.get(2,2));
    cout << "Inertia:" << endl;
    inertia.print();

    Matrix33 rot = m_rotation.toMatrix33(); // Rotation matrix
    inertia = rot * inertia * rot.transposed(); // Inertia tensor (in world coördinates)
    cout << "Inertia world:" << endl;
    inertia.print();

    m_angularMomentum = inertia * velocity;
}

void RigidBody::initialize()
{
/*
#ifdef USE_OPENGL
    initializeGL();
#endif // USE_OPENGL
*/

    PhysicalObject::initialize();

    cout << "Initializing rigid body!" << endl;

    calculateVolume();
    calculateInversedInertia();
}

void RigidBody::addToState(PhaseSpace *state)
{
    state->concat(m_position); // Position (of center of mass)
    state->concat(m_rotation); // Rotation
    state->concat(m_velocity); // Linear velocity
    state->concat(m_angularMomentum); // Angular momentum
}

void RigidBody::addToDerivative(PhaseSpace *deriv)
{
    if(isFixed())
        deriv->concatZeros(13);
    else
    {
        // Calculate auxilary variables
        //m_rotation.normalize();
        Matrix33 rot = m_rotation.toMatrix33(); // Rotation matrix
        Matrix33 invInertia = rot * m_invInertia * rot.transposed(); // Inversed inertia tensor (in world coördinates)
        Vector3 omega = invInertia * m_angularMomentum;
        Quaternion qdot = 0.5f * (omega * m_rotation);
        //qdot.normalize(); //! @warning this gives very strange results!

        // Fill in the state vector
        deriv->concat(m_velocity); // Linear velocity
        deriv->concat(qdot); // Rotation derived to time
        deriv->concat(getAccelleration()); // Linear acceleration
        deriv->concat(m_torque); // Torque

/*
        cout << "Rotation: " << m_rotation << endl;
        cout << "Rotation matrix: " << endl;
        rot.print();
        cout << "Inversed inertia tensor (body co): " << endl;
        m_invInertia.print();
        cout << "Inversed inertia tensor (world co): " << endl;
        invInertia.print();
        cout << "Omega: " << omega << endl;
        cout << endl;
        cout << "Linear velocity: " << m_velocity << endl;
        cout << "Qdot: " << qdot << endl;
        cout << "Linear acceleration: " << getAccelleration() << endl;
        cout << "Torque: " << m_torque << endl;
*/
    }
}

void RigidBody::fromState(PhaseSpace *state)
{
    if(isFixed())
        state->skip(13);
    else
    {
        m_position.setX(state->getNext());
        m_position.setY(state->getNext());
        m_position.setZ(state->getNext());

        m_rotation.setScalar(state->getNext());
        m_rotation.setVector(Vector3(state->getNext(), state->getNext(), state->getNext()));

        m_velocity.setX(state->getNext());
        m_velocity.setY(state->getNext());
        m_velocity.setZ(state->getNext());

        m_angularMomentum.setX(state->getNext());
        m_angularMomentum.setY(state->getNext());
        m_angularMomentum.setZ(state->getNext());
    }

    m_force = Vector3::zero;
    m_torque = Vector3::zero;
}

void RigidBody::disable()
{
    m_force = m_torque = Vector3::zero;
    m_velocity = m_angularMomentum = Vector3::zero;
    m_enabled = false;
}

/*
#ifdef USE_OPENGL
void RigidBody::drawGL()
{
#ifdef VERBOSE
    Debug::print("RigidBody drawGL", Debug::maxLevel);
#endif // VERBOSE

    //if(!m_draw) return;

    glPushMatrix();

    translateGL();
    rotateGL();
    renderGL();

    glPopMatrix();
}
#endif // USE_OPENGL
*/


/**********************
* Protected functions *
**********************/

---