rigidbody.cpp

Go to the documentation of this file.

/*
    Sheep - A Rigid Body Dynamics Engine
    Copyright (C) 2001-2004 Francois Beaune
    Contact: http://toxicengine.sourceforge.net/

    This file is part of Sheep.

    Sheep is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    Sheep is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Sheep; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "rigidbody.h"

using namespace sheep;

#define STATEVECTOR RigidBody::StateVector

STATEVECTOR RigidBody::StateVectorDE::GetDerivativeAt(const STATEVECTOR &x, Real t) const {
    // Rotation matrix associated to the body's orientation.
    Matrix3 R = Matrix3::Rotation(x.m_q.Normalized());  // world space

    assert(feq(R.Determinant(), 1));

    // Inertia tensor in world space.
    Matrix3 I = R * m_parent->m_inertia * R.Transpose();    // world space

    // Angular velocity.
    Vector3 omega = I.Inverse() * x.m_L;    // world space

    STATEVECTOR d;

    // Derivative of the location of the center of mass.
    d.m_x = 1.0 / m_parent->m_mass * x.m_P; // world space

    // Derivative of the orientation.
    d.m_q = 0.5 * Quaternion(0, omega) * x.m_q; // world space

    // Derivative of the linear momentum.
    d.m_P = m_parent->m_total_force;    // world space

    // Derivative of the angular momentum.
    d.m_L = m_parent->m_total_torque;   // world space

    return d;
}

#undef STATEVECTOR

RigidBody::RigidBody(Real mass, const Matrix3 &inertia) {
    const Real EPSILON = 1e-6;

    assert(mass > EPSILON);
    assert(fnz(inertia.Determinant()));

    m_mass = mass;
    m_inertia = inertia;

    // Define the initial state of the body.
    m_state.m_x = 0;
    m_state.m_q = Quaternion::Identity();
    m_state.m_P = 0;
    m_state.m_L = 0;

    // The state differential equation needs to access the body
    // to which it is attached.
    m_state_de = new StateVectorDE(this);

    m_is_frozen = false;

    m_total_force = 0;
    m_total_torque = 0;
}

RigidBody::~RigidBody() {
    delete m_state_de;
}

void RigidBody::EvolveOneStep(const StateDESolver &solver, Real t, Real h) {
    const Real EPSILON = 1e-6;

    assert(h > EPSILON);

    m_state = solver.Integrate(*m_state_de, m_state, t, h);

    //!\todo Calculate the system energy.

    // Numerical integration can result in a non-normalized quaternion
    // for the orientation, so we ensure that this quaternion has unit
    // length before continuing.
    m_state.m_q = m_state.m_q.Normalized();
}

---