subspace.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 "rigidobject.h"
#include "subspace.h"
#include "world.h"

using namespace sheep;

//#define DEBUG_INFO
/*
void Subspace::Step(Real start_time, Real end_time) {
    assert(end_time - start_time > EPSILON);

    m_cd.RefreshTransformations();

    while(true) {
        if(end_time - start_time <= EPSILON)
            break;

        Real step_end = compute_step_end(start_time, end_time);

        assert(step_end <= end_time);
        assert(step_end - start_time > EPSILON);

#ifdef DEBUG_INFO
        *m_debug << "Step by " << step_end - start_time << br;
#endif  // DEBUG_INFO

        step_bodies(start_time, step_end - start_time);

        start_time = step_end;

        for(rigidobject_list::iterator i = m_objects.begin(); i != m_objects.end(); i++) {
            RigidBody *body = (*i)->GetBody();

            body->ZeroForce();
            body->ZeroTorque();
            body->ApplyForce(Vector3(0,-9.8,0));        //!\todo This is a quick hack
        }

        m_jm.ApplyContraints();
        m_jm.ApplyCorrections();

        ContactList contacts;

        compute_contacts(&contacts);

        for(ContactList::iterator i = contacts.begin(); i != contacts.end(); i++) {
#ifdef DEBUG_INFO
//          print_contact(*i);
#endif  // DEBUG_INFO

            m_ch.Insert(*i);
        }

#ifdef DEBUG_INFO
        *m_debug << "Resting contacts so far: "
            << m_ch.GetRestingContactTracker()->GetRestingContacts().size() << br;
#endif  // DEBUG_INFO

        m_ch.ResolveAll();
    }
}
*/
void Subspace::Step(Real start_time, Real end_time) {
    const Real EPSILON = 1e-6;

    //!\todo Is this really necessary?
    assert(end_time - start_time > EPSILON);

    m_cd.RefreshTransformations();

    while(end_time - start_time > EPSILON) {
        find_contacts();

#ifdef DEBUG_INFO
        *m_debug << "Resting contacts so far: "
            << m_ch.GetRestingContactTracker()->GetRestingContacts().size() << br;
#endif  // DEBUG_INFO

        for(rigidobject_list::iterator i = m_objects.begin(); i != m_objects.end(); i++) {
            RigidBody *body = (*i)->GetBody();

            body->ZeroForce();
            body->ZeroTorque();

            //!\todo This is a quick hack.

            // Gravity.
            body->ApplyForce(Vector3(0, -9.8, 0));

            // Damping.
            body->ApplyForce(-0.2 * body->GetLinearVelocity());
            body->ApplyTorque(-0.2 * body->GetAngularVelocity());
        }

        //!\todo I guess joints and resting contacts should be resolved
        //! together at the same time, which is obviously not the case at
        //! the moment. I believe that making resting contacts unilateral
        //! constraints (thus a particular type of joint) will help to
        //! achieve simultaneous resolution of resting contacts and joints.

        m_jm.ApplyContraints();
        m_ch.ResolveAll();

        Real step_end = compute_step_end(start_time, end_time);

        assert(step_end <= end_time);
        assert(step_end - start_time > EPSILON);

#ifdef DEBUG_INFO
        *m_debug << "Step by " << step_end - start_time << br;
#endif  // DEBUG_INFO

        step_bodies(start_time, step_end - start_time);

        start_time = step_end;
    }
}

void Subspace::save_state(state_snapshot *s) {
    assert(s);

    s->m_states = new RigidBody::StateVector[m_objects.size()];

    RigidBody::StateVector *p = s->m_states;

    for(rigidobject_list::iterator i = m_objects.begin(); i != m_objects.end(); i++) {
        *p++ = (*i)->GetBody()->GetState();
    }
}

void Subspace::restore_state(const state_snapshot &s) {
    RigidBody::StateVector *p = s.m_states;

    for(rigidobject_list::iterator i = m_objects.begin(); i != m_objects.end(); i++) {
        (*i)->GetBody()->SetState(*p++);
    }

    m_cd.RefreshTransformations();
}

void Subspace::find_contacts() {
#ifdef DEBUG_INFO
    *m_debug << "Let's now compute contacts:" << br;
#endif  // DEBUG_INFO

    ContactList contacts;

    m_cd.FindContacts(&contacts);

    for(ContactList::iterator i = contacts.begin(); i != contacts.end(); i++) {
        Contact &c = *i;

        if((c.m_fa.m_type == Feature::Type::FACE && c.m_fb.m_type == Feature::Type::VERTEX) ||
           (c.m_fa.m_type == Feature::Type::EDGE && c.m_fb.m_type == Feature::Type::VERTEX)) {
            std::swap(c.m_a, c.m_b);
            std::swap(c.m_fa, c.m_fb);
        }

        c.Update();

        if(c.GetType() == Contact::Type::SEPARATING_CONTACT) {
#ifdef DEBUG_INFO
            *m_debug << "It is a separating contact. Skipping it." << br;
#endif  // DEBUG_INFO
            continue;
        }

        if(c.GetType() == Contact::Type::RESTING_CONTACT) {
            const ContactVector &rc = m_ch.GetRCTracker().GetRestingContacts();
            bool known_rc = false;

            for(ContactVector::const_iterator i = rc.begin(); i != rc.end(); i++) {
                if(c == *i) {
                    known_rc = true;
                    break;
                }
            }

            if(known_rc) {
#ifdef DEBUG_INFO
                *m_debug << "It is an already known resting contact. Skipping it." << br;
#endif  // DEBUG_INFO
                continue;
            }
        }

#ifdef DEBUG_INFO
        *m_debug << "Adding the following contact to the contact list:" << br;
        print_contact(c);
#endif  // DEBUG_INFO

        m_ch.Insert(c);
    }
}

Real Subspace::compute_step_end(Real start_time, Real end_time) {
    //!\todo Much work can be done here to speed things up, like
    //! time-of-intersection prediction, and so on...

    const Real EPSILON = 1e-6;

    assert(end_time - start_time > EPSILON);
    assert(m_cd.AreObjectsDisjoint());

    state_snapshot orig_state;
    save_state(&orig_state);

    Real step_end = start_time;
    Real step = end_time - start_time;

    while(true) {
        assert(end_time - step_end >= 0);

        if(end_time - step_end < EPSILON) {
#ifdef DEBUG_INFO
            *m_debug << "Maximum step reached." << br;
#endif  // DEBUG_INFO
            break;
        }

        assert(m_cd.AreObjectsDisjoint());

        state_snapshot last_state;
        save_state(&last_state);

        assert(step > EPSILON);

        step_bodies(step_end, step);

        if(!m_cd.AreObjectsDisjoint()) {
            restore_state(last_state);

            step /= 2;

            if(step <= EPSILON) {
#ifdef DEBUG_INFO
                *m_debug << "Giving up time bisection." << br;
                *m_debug << "Hint: one or several objects may be moving too fast." << br;
#endif  // DEBUG_INFO
                break;
            }
        } else {
            step_end += step;

            if(!m_cd.AreObjectHullsDisjoint()) {
#ifdef DEBUG_INFO
                *m_debug << "Interference detected." << br;
#endif  // DEBUG_INFO
                break;
            }
        }
    }

    restore_state(orig_state);

    assert(m_cd.AreObjectsDisjoint());

    assert(step_end <= end_time);
    assert(step_end - start_time > EPSILON);

    return step_end;
}

void Subspace::step_bodies(Real time, Real step) {
    const Real EPSILON = 1e-6;

    //!\todo Is this really necessary?
    assert(step > EPSILON);

    for(rigidobject_list::iterator i = m_objects.begin(); i != m_objects.end(); i++) {
        RigidBody *body = (*i)->GetBody();

        if(body->IsFrozen())
            continue;

        body->EvolveOneStep(m_solver, time, step);
    }

    m_cd.RefreshTransformations();
}

---