joints.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 "joints.h" // include first
#include "common/math/build_basis.h"
#include "common/math/catmullromcurve3.h"
#include "common/math/itl_bicgstab.h"

#include <algorithm>
#include <iostream>

using namespace sheep;

Joint::Joint(int dimension, int body_count) :
    m_dimension(dimension),
    m_body_count(body_count)
{
    assert(m_dimension > 0);
    assert(m_body_count > 0);

    m_bodies = new RigidBody *[m_body_count];

    m_j = new Matrix *[m_body_count];

    for(int i = 0; i < m_body_count; i++)
        m_j[i] = new Matrix(m_dimension, 6);

    m_zero = new Matrix(m_dimension, 6);
    *m_zero = 0;

    m_c = new Vector(m_dimension);
}

Joint::~Joint() {
    delete m_c;

    delete m_zero;

    for(int i = 0; i < m_body_count; i++)
        delete m_j[i];

    delete [] m_j;

    delete [] m_bodies;
}

SphericalJoint::SphericalJoint(RigidBody *body0, const Vector3 &anchor0,
                               RigidBody *body1, const Vector3 &anchor1) :
    Joint(3, 2) // dimension 3, 2 bodies involved
{
    assert(body0 && body1);

    m_bodies[0] = body0;
    m_bodies[1] = body1;

    m_anchor0 = anchor0;
    m_anchor1 = anchor1;
}

void SphericalJoint::Update() {
    Vector3 a = m_bodies[0]->TransformVectorToWorld(m_anchor0);
    Vector3 b = m_bodies[1]->TransformVectorToWorld(m_anchor1);

    // j_{1L} = 1_3
    (*m_j[0])(0, 0) = (*m_j[0])(1, 1) = (*m_j[0])(2, 2) = 1;
    (*m_j[0])(0, 1) = (*m_j[0])(1, 0) = 0;
    (*m_j[0])(0, 2) = (*m_j[0])(2, 0) = 0;
    (*m_j[0])(1, 2) = (*m_j[0])(2, 1) = 0;

    // j_{1A} = -a^*
    (*m_j[0])(0, 3) = 0;
    (*m_j[0])(0, 4) = a.m_z;
    (*m_j[0])(0, 5) = -a.m_y;
    (*m_j[0])(1, 3) = -a.m_z;
    (*m_j[0])(1, 4) = 0;
    (*m_j[0])(1, 5) = a.m_x;
    (*m_j[0])(2, 3) = a.m_y;
    (*m_j[0])(2, 4) = -a.m_x;
    (*m_j[0])(2, 5) = 0;

    // j_{2L} = -1_3
    (*m_j[1])(0, 0) = (*m_j[1])(1, 1) = (*m_j[1])(2, 2) = -1;
    (*m_j[1])(0, 1) = (*m_j[1])(1, 0) = 0;
    (*m_j[1])(0, 2) = (*m_j[1])(2, 0) = 0;
    (*m_j[1])(1, 2) = (*m_j[1])(2, 1) = 0;

    // j_{2A} = b^*
    (*m_j[1])(0, 3) = 0;
    (*m_j[1])(0, 4) = -b.m_z;
    (*m_j[1])(0, 5) = b.m_y;
    (*m_j[1])(1, 3) = b.m_z;
    (*m_j[1])(1, 4) = 0;
    (*m_j[1])(1, 5) = -b.m_x;
    (*m_j[1])(2, 3) = -b.m_y;
    (*m_j[1])(2, 4) = b.m_x;
    (*m_j[1])(2, 5) = 0;

    Vector3 omega0 = m_bodies[0]->GetAngularVelocity();
    Vector3 omega1 = m_bodies[1]->GetAngularVelocity();

    Vector3 t = (omega0 ^ (omega0 ^ a)) - (omega1 ^ (omega1 ^ b));

    (*m_c)[0] = t.m_x;
    (*m_c)[1] = t.m_y;
    (*m_c)[2] = t.m_z;
}

Real SphericalJoint::GetError() {
    Vector3 a = m_bodies[0]->TransformPointToWorld(m_anchor0);
    Vector3 b = m_bodies[1]->TransformPointToWorld(m_anchor1);

    return (a - b).Norm();
}

void SphericalJoint::CorrectError() {
    assert(!(m_bodies[0]->IsFrozen() && m_bodies[1]->IsFrozen()));

    Vector3 shift = m_bodies[0]->TransformPointToWorld(m_anchor0) -
        m_bodies[1]->TransformPointToWorld(m_anchor1);

    // Velocity correction.

/*  if(shift.SquareNorm() > 1e-12) {
        Vector3 u = shift.Normalized();

        std::cerr << "u.Norm()=" << u.Norm() << std::endl;

        Vector3 rb = m_body2->TransformVectorToWorld(m_anchor2);

        Vector3 k = (u / m_body2->GetMass() + m_body2->GetWorldInertiaTensor().Inverse() * (rb ^ u)) ^ rb;

        const Real H = 0.01;
        
        Real j_mag = ((shift / H - m_body2->GetPointVelocity(m_anchor2)) * k) / k.SquareNorm();

        std::cerr << "j_mag=" << j_mag << std::endl;

        Vector3 j = j_mag * u;

        m_body2->ApplyLinearImpulse(j);
        m_body2->ApplyAngularImpulse(rb ^ j);
    }*/

//  const Real H = 0.01;

//  m_body2->ApplyLinearImpulse(shift / H);

    // Position correction.

    m_bodies[1]->SetPosition(m_bodies[1]->GetPosition() + shift);
}

FixedPathJoint::FixedPathJoint(RigidBody *body, const Vector3 &anchor,
                               const Vector3 *ccp, int nccp)
: Joint(2, 1) {     // dimension 2, only 1 body involved (plus a curve).
    assert(body);

    m_bodies[0] = body;

    m_anchor = anchor;

    // Compute the number of control points and allocate them.
    m_nccp = nccp + 2;      // we need two additional control points
    m_ccp = new Vector3[m_nccp];

    // Copy the control points (this is a safety measure).
    for(int i = 0; i < m_nccp - 2; i++)
        m_ccp[i + 1] = ccp[i];

    // Compute the two additional control points.
    m_ccp[0] = 2 * m_ccp[1] - m_ccp[2];
    m_ccp[m_nccp - 1] = 2 * m_ccp[m_nccp - 2] - m_ccp[m_nccp - 3];
}

void FixedPathJoint::Update() {
    Vector3 a = m_bodies[0]->TransformVectorToWorld(m_anchor);
    Vector3 p = m_bodies[0]->TransformPointToWorld(m_anchor);

    int seg;
    Real t;

    // Compute location of the body on the curve.
    project_on_curve(p, &seg, &t);

    CatmullRomCurve3 curve(&m_ccp[seg]);

    Vector3 i = curve.GetTangentAt(t);
    Vector3 j, k;

    build_basis(i, &j, &k);

    // j_{1L} = \begin{pmatrix}
    //   \vec{j}\transpose \\
    //   \vec{k}\transpose \\
    // \end{pmatrix}

    (*m_j[0])(0, 0) = j.m_x;
    (*m_j[0])(0, 1) = j.m_y;
    (*m_j[0])(0, 2) = j.m_z;
    (*m_j[0])(1, 0) = k.m_x;
    (*m_j[0])(1, 1) = k.m_y;
    (*m_j[0])(1, 2) = k.m_z;

    // j_{1A} = \begin{pmatrix}
    //   - \vec{j}\transpose a^* \\
    //   - \vec{k}\transpose a^* \\
    // \end{pmatrix}

    (*m_j[0])(0, 3) = -j.m_y * a.m_z + j.m_z * a.m_y;
    (*m_j[0])(0, 4) = j.m_x * a.m_z - j.m_z * a.m_x;
    (*m_j[0])(0, 5) = -j.m_x * a.m_y + j.m_y * a.m_x;
    (*m_j[0])(1, 3) = -k.m_y * a.m_z + k.m_z * a.m_y;
    (*m_j[0])(1, 4) = k.m_x * a.m_z - k.m_z * a.m_x;
    (*m_j[0])(1, 5) = -k.m_x * a.m_y + k.m_y * a.m_x;

    Vector3 omega = m_bodies[0]->GetAngularVelocity();
    Vector3 tmp = omega ^ (omega ^ a);

    (*m_c)[0] = j * tmp;
    (*m_c)[1] = k * tmp;
}

Real FixedPathJoint::GetError() {
    //!\todo Implement?
    return -1;
}

void FixedPathJoint::CorrectError() {
    Vector3 p = m_bodies[0]->TransformPointToWorld(m_anchor);

    int seg;
    Real t;

    // Compute location of the body on the curve.
    project_on_curve(p, &seg, &t);

    CatmullRomCurve3 curve(&m_ccp[seg]);

    Vector3 shift = curve.GetPointAt(t) - p;

    // Position correction.

    m_bodies[0]->SetPosition(m_bodies[0]->GetPosition() + shift);
}

FixedPathJoint::DistanceFunction::DistanceFunction(const Vector3 *q, const Vector3 &p) {
    m_a = 0.5 * (-q[0] + 3 * q[1] - 3 * q[2] + q[3]);
    m_b = 0.5 * (2 * q[0] - 5 * q[1] + 4 * q[2] - q[3]);
    m_c = 0.5 * (-q[0] + q[2]);
    m_d = 0.5 * 2 * q[1];

    m_p = p;
}

Real FixedPathJoint::DistanceFunction::EvaluateAt(Real x) const {
    return -2 * (m_p - ((m_a * x + m_b) * x + m_c) * x - m_d) * ((3 * m_a * x + 2 * m_b) * x + m_c);
}

void FixedPathJoint::project_on_curve(const Vector3 &p, int *seg, Real *t) const {
    Real min_d = 1e6;

    for(int i = 0; i < m_nccp - 3; i++) {
        DistanceFunction f(&m_ccp[i], p);

        const Real EPS = 1e-6;

        Real f0 = f.EvaluateAt(0);
        Real f1 = f.EvaluateAt(1);

        Real t0;

        if(fabs(f0) < EPS)
            t0 = 0;
        else if(fabs(f1) < EPS)
            t0 = 1;
        else {
            if(f0 * f1 > 0)
                continue;

            t0 = FindRoot(f, 0, 1, EPS);
        }

        assert(t0 >= 0 && t0 <= 1);

        CatmullRomCurve3 curve(&m_ccp[i]);

        Vector3 q = curve.GetPointAt(t0);

        Real d = (p - q).Norm();

        if(d < min_d) {
            min_d = d;
            *seg = i;
            *t = t0;
        }
    }
}

JointManager::JointManager() {
//  debug = new HTMLStream("jointmanager.html");

    m_invm = 0;

    m_j = 0;
    m_c = 0;
    m_fext = 0;

    m_a = 0;
    m_b = 0;
    m_lambda = 0;
    m_f = 0;
}

void JointManager::Insert(Joint *joint) {
    assert(joint);

    m_joints.push_back(joint);

    int n = joint->GetBodyCount();

    for(int i = 0; i < n; i++) {
        RigidBody *body = joint->GetBody(i);

        if(std::find(m_bodies.begin(), m_bodies.end(), body) == m_bodies.end())
            m_bodies.push_back(body);
    }
}

void JointManager::Remove(Joint *joint) {
    assert(joint);

    m_joints.remove(joint);

    //!\todo Remove unused bodies from the 'm_bodies' list.
}

void JointManager::Setup() {
    int d = 0;

    for(joint_list::const_iterator i = m_joints.begin(); i != m_joints.end(); i++)
        d += (*i)->GetDimension();

    delete m_invm;
    delete m_j;
    delete m_c;
    delete m_fext;
    delete m_a;
    delete m_b;
    delete m_lambda;
    delete m_f;

    if(d > 0) {
        int n = m_bodies.size();

        m_invm = new Matrix(6 * n, 6 * n);
        m_j = new Matrix(d, 6 * n);
        m_c = new Vector(d);
        m_fext = new Vector(6 * n);
        m_a = new Matrix(d, d);
        m_b = new Vector(d);
        m_lambda = new Vector(d);
        m_f = new Vector(6 * n);
    }
}

void JointManager::ApplyContraints() {
    //!\todo This is a quick hack for Sheep Lab.
    if(m_joints.size() == 0)
        return;

    rigidbody_list::const_iterator bi;
    joint_list::const_iterator ji;
    int k, p;

    // Compute the M matrix.

    *m_invm = 0;

    k = 0;

    for(bi = m_bodies.begin(); bi != m_bodies.end(); bi++) {
        const RigidBody *body = *bi;

        Real inv_mass = body->IsFrozen() ? 0 : 1.0 / body->GetMass();

        // {m_k}^{-1}.
        (*m_invm)(k, k) =
        (*m_invm)(k + 1, k + 1) =
        (*m_invm)(k + 2, k + 2) = inv_mass;

        const Matrix3 inv_iw = body->IsFrozen() ? 0 : (body->GetWorldInertiaTensor()).Inverse();

        for(int r = 0; r < 3; r++) {
            for(int c = 0; c < 3; c++) {
                // {I_k}^{-1}.
                (*m_invm)(k + 3 + r, k + 3 + c) = inv_iw(r, c);
            }
        }

        k += 6;
    }

//  *debug << "M^(-1) matrix: " << *m_invm;

    for(ji = m_joints.begin(); ji != m_joints.end(); ji++) {
        (*ji)->Update();
    }

    // Compute the J matrix.

    p = 0;

    for(ji = m_joints.begin(); ji != m_joints.end(); ji++) {
        int d = (*ji)->GetDimension();

        k = 0;

        for(bi = m_bodies.begin(); bi != m_bodies.end(); bi++) {
            Matrix *j = (*ji)->GetJ(*bi);

            for(int r = 0; r < d; r++) {
                for(int c = 0; c < 6; c++) {
                    (*m_j)(p + r, k + c) = (*j)(r, c);
                }
            }

            k += 6;
        }

        p += d;
    }

//  *debug << "J matrix: " << *m_j;

    // Compute the C vector.

    p = 0;

    for(ji = m_joints.begin(); ji != m_joints.end(); ji++) {
        int d = (*ji)->GetDimension();

        Vector *c = (*ji)->GetC();

        for(int r = 0; r < d; r++)
            (*m_c)[p + r] = (*c)[r];

        p += d;
    }

//  *debug << "C vector: " << *m_c;

    // Compute the F^{ext} vector.

    k = 0;

    for(bi = m_bodies.begin(); bi != m_bodies.end(); bi++) {
        const Vector3 &f = (*bi)->GetTotalForce();

        (*m_fext)[k + 0] = f.m_x;
        (*m_fext)[k + 1] = f.m_y;
        (*m_fext)[k + 2] = f.m_z;

        const Vector3 &t = (*bi)->GetTotalTorque();

        (*m_fext)[k + 3] = t.m_x;
        (*m_fext)[k + 4] = t.m_y;
        (*m_fext)[k + 5] = t.m_z;

        k += 6;
    }

//  *debug << "Fext vector: " << *m_fext;

    Matrix j_t = m_j->Transpose();

    // Form the linear system to be solved.

    *m_a = (*m_j) * (*m_invm) * j_t;
    *m_b = - ((*m_j) * (*m_invm) * (*m_fext)) - (*m_c);

//  *debug << "A matrix: " << *m_a;
//  *debug << "B vector: " << *m_b;

    // Solve the system.
//  *m_lambda = m_a->Inverse() * (*m_b);
    itl_bicgstab(*m_a, *m_b, m_lambda);

//  *debug << "Lambda vector: " << *m_lambda << std::endl;

    *m_f = j_t * (*m_lambda);

//  *debug << "F vector: " << *m_f;

    k = 0;

    for(bi = m_bodies.begin(); bi != m_bodies.end(); bi++) {
        Vector3 f((*m_f)[k + 0], (*m_f)[k + 1], (*m_f)[k + 2]);
        Vector3 t((*m_f)[k + 3], (*m_f)[k + 4], (*m_f)[k + 5]);

        (*bi)->ApplyForce(f);
        (*bi)->ApplyTorque(t);

        k += 6;
    }

    // Error reduction step.

    for(joint_list::const_iterator ji = m_joints.begin(); ji != m_joints.end(); ji++) {
        Joint *joint = *ji;

        joint->CorrectError();
    }
}

---