rigidbody.inl

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
*/

#define STATEVECTOR RigidBody::StateVector

inline
STATEVECTOR::StateVector() {
}

inline
STATEVECTOR::StateVector(const Vector3 &x, const Quaternion &q,
                         const Vector3 &P, const Vector3 &L) {
//  assert(feq(q.Norm(), 1));

    m_x = x;
    m_q = q;
    m_P = P;
    m_L = L;
}

inline
STATEVECTOR STATEVECTOR::operator+(const STATEVECTOR &s) const {
    return STATEVECTOR(m_x+s.m_x, m_q+s.m_q, m_P+s.m_P, m_L+s.m_L);
}

inline
STATEVECTOR STATEVECTOR::operator*(Real r) const {
    return STATEVECTOR(m_x*r, m_q*r, m_P*r, m_L*r);
}

inline
RigidBody::StateVectorDE::StateVectorDE(const RigidBody *parent) {
    assert(parent);

    m_parent = parent;
}

inline
Real RigidBody::GetMass() const {
//  assert(!m_is_frozen);

    return m_mass;
}

inline
const Matrix3 &RigidBody::GetInertiaTensor() const {    // body space
//  assert(!m_is_frozen);

    return m_inertia;
}

inline
Matrix3 RigidBody::GetWorldInertiaTensor() const {  // world space
//  assert(!m_is_frozen);

    assert(feq(m_state.m_q.Norm(), 1));

    Matrix3 R = Matrix3::Rotation(m_state.m_q); // rotation matrix associated to the body's orientation

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

    return R * m_inertia * R.Transpose();   // inertia tensor in world space
}

inline
void RigidBody::Freeze() {
    m_state.m_P = 0;    // no linear velocity
    m_state.m_L = 0;    // no angular velocity
    m_total_force = 0;  // no force acting
    m_total_torque = 0; // no torque acting

    m_is_frozen = true;
}

inline
void RigidBody::Unfreeze() {
    m_is_frozen = false;
}

inline
bool RigidBody::IsFrozen() const {
    return m_is_frozen;
}

inline
void RigidBody::SetPosition(const Vector3 &x) { // world space
    m_state.m_x = x;
}

inline
const Vector3 &RigidBody::GetPosition() const { // world space
    return m_state.m_x;
}

inline
void RigidBody::SetOrientation(const Quaternion &q) {   // world space
    m_state.m_q = q;
}

inline
const Quaternion &RigidBody::GetOrientation() const {   // world space
    return m_state.m_q;
}

inline
void RigidBody::SetLinearVelocity(const Vector3 &v) {   // world space
    assert(!m_is_frozen);

    if(!m_is_frozen)
        m_state.m_P = m_mass * v;
}

inline
Vector3 RigidBody::GetLinearVelocity() const {  // world space
    return m_is_frozen ? 0 : 1.0 / m_mass * m_state.m_P;
}

inline
void RigidBody::SetAngularVelocity(const Vector3 &omega) {  // world space
    assert(!m_is_frozen);

    if(!m_is_frozen)
        m_state.m_L = GetWorldInertiaTensor() * omega;
}

inline
Vector3 RigidBody::GetAngularVelocity() const { // world space
    return m_is_frozen ? 0 : GetWorldInertiaTensor().Inverse() * m_state.m_L;
}

inline
Real RigidBody::GetKineticEnergy() const {
    if(m_is_frozen)
        return 0;

    Real linear_term = 0.5 * (GetMass() * GetLinearVelocity() * GetLinearVelocity());
    Real angular_term = 0.5 * (GetWorldInertiaTensor() * GetAngularVelocity() * GetAngularVelocity());

    return linear_term + angular_term;
}

inline
Vector3 RigidBody::TransformVectorToWorld(const Vector3 &v) const { // p in body space, result in world space
    assert(feq(m_state.m_q.Norm(), 1));

    return (m_state.m_q * Quaternion(0, v) * m_state.m_q.Conjugate()).m_v;
}

inline
Vector3 RigidBody::TransformVectorToBody(const Vector3 &v) const {  // p in world space, result in body space
    assert(feq(m_state.m_q.Norm(), 1));

    return (m_state.m_q.Conjugate() * Quaternion(0, v) * m_state.m_q).m_v;
}

inline
Vector3 RigidBody::TransformPointToWorld(const Vector3 &p) const {  // p in body space, result in world space
    return TransformVectorToWorld(p) + GetPosition();
}

inline
Vector3 RigidBody::TransformPointToBody(const Vector3 &p) const {   // p in world space, result in body space
    return TransformVectorToBody(p - GetPosition());
}

inline
Vector3 RigidBody::GetPointVelocity(const Vector3 &p) const {   // p in body space, result in world space
    return GetLinearVelocity() + (GetAngularVelocity() ^ TransformVectorToWorld(p));
}

inline
void RigidBody::ApplyForce(const Vector3 &f) {  // world space
    if(!m_is_frozen)
        m_total_force += f;
}

inline
void RigidBody::ApplyForce(const Vector3 &f, const Vector3 &p) {    // f in world space, p in body space
    if(!m_is_frozen) {
        m_total_force += f;
        m_total_torque += TransformVectorToWorld(p) ^ f;
    }
}

inline
void RigidBody::ApplyTorque(const Vector3 &t) {     // t in world space
    if(!m_is_frozen) {
        m_total_torque += t;
    }
}

inline
void RigidBody::ZeroForce() {
    m_total_force = 0;
}

inline
void RigidBody::ZeroTorque() {
    m_total_torque = 0;
}

inline
Vector3 RigidBody::GetTotalForce() const {  // world space
    return m_total_force;
}

inline
Vector3 RigidBody::GetTotalTorque() const { // world space
    return m_total_torque;
}

inline
void RigidBody::ApplyLinearImpulse(const Vector3 &j) {  // world space
    if(!m_is_frozen)
        m_state.m_P += j;
}

inline
void RigidBody::ApplyAngularImpulse(const Vector3 &j) { // world space
    if(!m_is_frozen)
        m_state.m_L += j;
}

inline
void RigidBody::SetState(const STATEVECTOR &s) {
    if(!m_is_frozen)
        m_state = s;
}

inline
STATEVECTOR RigidBody::GetState() const {
    return m_state;
}

#undef STATEVECTOR

---