rctracker.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 "engine/geometry/icollisiondetector.h"     // sheep::HUCK_THICKNESS
#include "engine/simulation/rigidobject.h"
#include "rctracker.h"
#include "rigidbody.h"

#include <cmath>        // for fabs()
#include <limits>

using namespace sheep;

const Real HULL_THICKNESS = 0.05 / 2.0;     // duplicated because of lazyness

Matrix RCTracker::compute_A_matrix() {
    Matrix A(m_resting_contacts.size(), m_resting_contacts.size());

    for(int i=0; i<m_resting_contacts.size(); i++)      // rows
        for(int j=0; j<m_resting_contacts.size(); j++) {    // columns
            Contact ci = m_resting_contacts[i];
            Contact cj = m_resting_contacts[j];

            RigidBody *a = ci.m_a->GetBody();
            RigidBody *b = ci.m_b->GetBody();

            // If the bodies involved in the ith and jth contacts
            // are distinct, then a[i,j] is zero.
            if((ci.m_a!=cj.m_a)&&(ci.m_b!=cj.m_b)&&
                (ci.m_a!=cj.m_b)&&(ci.m_b!=cj.m_a)) {
                A(i,j) = 0;
                continue;
            }

            Vector3 ria = ci.m_a->GetBody()->TransformVectorToWorld(ci.m_pa);
            Vector3 rib = ci.m_b->GetBody()->TransformVectorToWorld(ci.m_pb);
            Vector3 rja = cj.m_a->GetBody()->TransformVectorToWorld(cj.m_pa);
            Vector3 rjb = cj.m_b->GetBody()->TransformVectorToWorld(cj.m_pb);

            // Compute force and torque the jth contact exerts on body A.

            Vector3 force_on_a = 0, torque_on_a = 0;

            if(cj.m_a==ci.m_a) {
                force_on_a = cj.m_n;
                torque_on_a = rja ^ cj.m_n;
            }
            else if(cj.m_b==ci.m_a) {
                force_on_a = - cj.m_n;
                torque_on_a = rja ^ cj.m_n;
            }

            // Compute force and torque the jth contact exerts on body B.

            Vector3 force_on_b = 0, torque_on_b = 0;

            if(cj.m_a==ci.m_b) {
                force_on_b = cj.m_n;
                torque_on_b = rjb ^ cj.m_n;
            }
            else if(cj.m_b==ci.m_b) {
                force_on_b = - cj.m_n;
                torque_on_b = rjb ^ cj.m_n;
            }

            // Now compute how the jth contact force affects the linear
            // and angular accelerations of the contact point on each body.

            Vector3 a_part = a->IsFrozen() ? 0 : force_on_a / a->GetMass() +
                ((a->GetWorldInertiaTensor().Inverse() * torque_on_a) ^ ria);
            
            Vector3 b_part = b->IsFrozen() ? 0 : force_on_b / b->GetMass() +
                ((b->GetWorldInertiaTensor().Inverse() * torque_on_b) ^ rib);

            A(i,j) = ci.m_n * (a_part - b_part);
        }

    return A;
}

Vector RCTracker::compute_b_vector() {
    Vector bv(m_resting_contacts.size());

    for(int i=0; i<m_resting_contacts.size(); i++) {
        Contact c = m_resting_contacts[i];

        RigidBody *a = c.m_a->GetBody();
        RigidBody *b = c.m_b->GetBody();

        Vector3 ra = a->TransformVectorToWorld(c.m_pa);
        Vector3 rb = b->TransformVectorToWorld(c.m_pb);

        // Compute the part of padotdot(t0) due to the external
        // force and torque, and similarly for pbdotdot(t0).

        Vector3 a_ext_part = a->IsFrozen() ? 0 : a->GetTotalForce() / a->GetMass() +
            ((a->GetWorldInertiaTensor().Inverse() * a->GetTotalTorque()) ^ ra);

        Vector3 b_ext_part = b->IsFrozen() ? 0 : b->GetTotalForce() / b->GetMass() +
            ((b->GetWorldInertiaTensor().Inverse() * b->GetTotalTorque()) ^ rb);

        // Compute the part of padotdot(t0) due to velocity, and
        // similarly for pbdotdot(t0).

        Vector3 a_vel_part = 0, b_vel_part = 0;

        if(!a->IsFrozen()) {
            Vector3 a_L = a->GetWorldInertiaTensor() * a->GetAngularVelocity();
            a_vel_part = (a->GetAngularVelocity() ^ (a->GetAngularVelocity() ^ ra)) +
                ((a->GetWorldInertiaTensor().Inverse() * (a_L ^ a->GetAngularVelocity())) ^ ra);
        }

        if(!b->IsFrozen()) {
            Vector3 b_L = b->GetWorldInertiaTensor() * b->GetAngularVelocity();
            b_vel_part = (b->GetAngularVelocity() ^ (b->GetAngularVelocity() ^ rb)) +
                ((b->GetWorldInertiaTensor().Inverse() * (b_L ^ b->GetAngularVelocity())) ^ rb);
        }

        // Combine the above results, and dot with the contact normal.

        Real k1 = c.m_n * ((a_ext_part + a_vel_part) - (b_ext_part + b_vel_part));
        Real k2 = 2 * c.GetNormalDerivative() * (c.m_va - c.m_vb);

        bv[i] = k1 + k2;
    }

    return bv;
}

Vector RCTracker::fdirection(const Matrix &A, int d) {
    Vector delta_f(m_resting_contacts.size(), 0);

    delta_f[d] = 1;

    if(C.size()<1)
        return delta_f;

    int i, j, k;

    // Compute A11.

    Matrix A11(C.size(), C.size());

    k = 0;      // linear index in A11 (which is row-major)

    for(i=0; i<m_resting_contacts.size(); i++) {    // rows
        if(!C.Contains(i))
            continue;       // skip this row

        for(j=0; j<m_resting_contacts.size(); j++) {    // columns
            if(C.Contains(j))
                A11[k++] = A(i,j);
        }
    }

    assert(k==C.size()*C.size());

    // Compute v1.

    Vector v1(C.size());

    k = 0;      // index in v1

    for(i=0; i<m_resting_contacts.size(); i++) {
        if(C.Contains(i))
            v1[k++] = A(i,d);
    }

    assert(k==C.size());

//  *debug << "A11:" << A11 << std::endl;

    Vector x = - A11.Inverse() * v1;

    // Transfer x into delta_f.

    k = 0;      // index in x

    for(i=0; i<m_resting_contacts.size(); i++) {
        if(C.Contains(i))
            delta_f[i] = x[k++];
    }

    return delta_f;
}

// Output to s and j.
void RCTracker::maxstep(const Vector &a,
                        const Vector &f,
                        const Vector &delta_a,
                        const Vector &delta_f,
                        int d, Real *s, int *j) {
    *s = std::numeric_limits<Real>::max();      // fake infinity
    *j = -1;

    if(delta_a[d]>0) {
        //!\todo Resolve properly the case where delta_a[d] is very small (1e-16).
        //! Solution: do not add a resting contact if it does not accelerate toward
        //! its neighbor more than EPSILON (along the contact normal).

//      assert(fabs(delta_a[d])>EPSILON);

        *s = - a[d] / delta_a[d];
        *j = d;
    }

    index_set::iterator i;

    for(i=C.begin(); i!=C.end(); i++)
        if(delta_f[*i]<0) {
            assert(fnz(delta_f[*i]));

            Real s1 = -f[*i] / delta_f[*i];

            if(s1 < *s) {
                *s = s1;
                *j = *i;
            }
        }

    for(i=NC.begin(); i!=NC.end(); i++)
        if(delta_a[*i]<0) {
            assert(fnz(delta_a[*i]));

            Real s1 = -a[*i] / delta_a[*i];

            if(s1 < *s) {
                *s = s1;
                *j = *i;
            }
        }
}

void RCTracker::drive_to_zero(const Matrix &A, Vector &f, Vector &a, int d) {
L1:
//  *debug << "At that point, we have C = {";

//  index_set::const_iterator i;

//  for(i=C.begin(); i!=C.end(); i++)
//      *debug << ' ' << *i;

//  *debug << " } and NC = {";

//  for(i=NC.begin(); i!=NC.end(); i++)
//      *debug << ' ' << *i;

//  *debug << " }." << br << std::endl;

    Vector delta_f = fdirection(A, d);
    Vector delta_a = A * delta_f;

    Real s;
    int j;

    maxstep(a, f, delta_a, delta_f, d, &s, &j);     // output to s and j

    assert(j!=-1);      // make sure that max_step < infinity

    f += s * delta_f;
    a += s * delta_a;

    if(C.Contains(j)) {
//      *debug << "Moving " << j << " from C to NC." << br << std::endl;
        C.remove(j);
        NC.push_back(j);
        goto L1;
    }
    else if(NC.Contains(j)) {
//      *debug << "Moving " << j << " from NC to C." << br << std::endl;
        NC.remove(j);
        C.push_back(j);
        goto L1;
    }
    else {      // j must be d, implying a[d] = 0
//      *debug << "Pushing " << j << " to C." << br << std::endl;
        C.push_back(j);
    }
}

Vector RCTracker::compute_forces(const Matrix &A, const Vector &b) {
    Vector f(m_resting_contacts.size(), 0);

    Vector a = b;

    C.clear();
    NC.clear();

//  *debug << "<p>Starting with C = {";

//  index_set::const_iterator i;

//  for(i=C.begin(); i!=C.end(); i++)
//      *debug << ' ' << *i;

//  *debug << " } and NC = {";

//  for(i=NC.begin(); i!=NC.end(); i++)
//      *debug << ' ' << *i;

//  *debug << " }." << br << std::endl;

    int d = 0;

    const Real EPSILON = 1e-6;

    while(d<a.GetDimension()) {
        if(a[d] < -EPSILON) {
//          Real temp = a[d];
            drive_to_zero(A, f, a, d);
//          *debug << "Drove a[" << d << "] from " << temp << " to " << a[d] << "." << br << std::endl;
            d = 0;
        }
        else d++;
    }

//  *debug << "Ending with C = {";

//  for(i=C.begin(); i!=C.end(); i++)
//      *debug << ' ' << *i;

//  *debug << " } and NC = {";

//  for(i=NC.begin(); i!=NC.end(); i++)
//      *debug << ' ' << *i;

//  *debug << " }.</p>" << std::endl;

    return f;
}

bool RCTracker::is_inactivated_rc::operator()(Contact &c) const {
    c.Update();

//  *debug << "is_inactivated_rc: " << c.m_inactivated << ", " << (c.m_pa - c.m_pb).Norm() << br;

//  if(c.m_inactivated) {
//      std::cerr << "- " << c.m_fa << "-" << c.m_fb << std::endl;
//  }

    return c.m_inactivated /*|| (c.m_pa - c.m_pb).Norm() > HULL_THICKNESS -- warning: HULL_THICKNESS definition changed! */;
}

void RCTracker::remove_inactivated_rc() {
    m_resting_contacts.erase(
        std::remove_if(m_resting_contacts.begin(), m_resting_contacts.end(), is_inactivated_rc()),
        m_resting_contacts.end());
}

void RCTracker::ResolveAll() {
    // Remove inactivated resting contacts.
    remove_inactivated_rc();

    if(m_resting_contacts.size()==0)
        return;

    // Compute and apply contact forces that keep the contact
    // points from accelerating toward each other.

    Vector f = compute_forces(compute_A_matrix(), compute_b_vector());

//  *debug << "Acceleration before:" << compute_b_vector();

    Vector v(m_resting_contacts.size());
    int i;

    for(i=0; i<m_resting_contacts.size(); i++) {
        Contact c = m_resting_contacts[i];
        c.Update();
        v[i] = c.m_n * (c.m_va - c.m_vb);
    }

//  *debug << "Normal velocity before: " << v;

    // Apply the contact forces to resting bodies.
    for(i=0; i<m_resting_contacts.size(); i++) {
//      *debug << "Handling resting contact #" << i << ":" << br;

        Contact c = m_resting_contacts[i];

        RigidBody *a = c.m_a->GetBody();
        RigidBody *b = c.m_b->GetBody();

//      *debug << "  Applying force +/-" << f[i] * c.m_n;

        a->ApplyForce(f[i] * c.m_n, c.m_pa);
        b->ApplyForce(-f[i] * c.m_n, c.m_pb);
    }

//  *debug << "Acceleration after:" << compute_b_vector();

    for(i=0; i<m_resting_contacts.size(); i++) {
        Contact c = m_resting_contacts[i];
        c.Update();
        c.CancelNormalVelocity();
        v[i] = c.m_n * (c.m_va - c.m_vb);
    }

//  *debug << "Normal velocity after: " << v;
}

---