Source

KODE / src / joints / Fixed.cpp

/*
  This file is part of the KODE.

    KODE Physics Library
    Copyright (C) 2013-2014  Daniel Kohler Osmari

    KODE is free software: you can redistribute it and/or modify it
    under the terms of EITHER:

        * the GNU Lesser General Public License as published by the
          Free Software Foundation, either version 3 of the License,
          or (at your option) any later version.

        * the Apache License, Version 2.0.

    This program 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 Lesser General Public License and the Apache License for more
    details.

    You should have received a copy of the GNU Lesser General Public
    License along with this program.  If not, see
    <http://www.gnu.org/licenses/>.

    You may obtain a copy of the Apache License at
       http://www.apache.org/licenses/LICENSE-2.0
*/

#include <kode/joints/Fixed.hpp>

#include <kode/Body.hpp>
#include <kode/World.hpp>

namespace kode {

    Fixed::Fixed(Body* b1, Body* b2) :
        Joint{b1, b2}
    {
        reset();
    }


    Fixed::Fixed(Body& b) :
        Fixed{&b, nullptr}
    {}


    Fixed::Fixed(Body& b1, Body& b2) :
        Fixed{&b1, &b2}
    {}


    void
    Fixed::reset()
    {
        //const Vector3 center1 = body1 ? body1->getPosition() : Vector3::Zero();
        //const Vector3 center2 = body2 ? body2->getPosition() : Vector3::Zero();
        const Matrix3 r1 = body1 ? body1->getLocalAxes() : Matrix3::Identity();
        const Matrix3 r2 = body2 ? body2->getLocalAxes() : Matrix3::Identity();

        offsetRot = r1.transposed() * r2;
        if (body1 && body2) {
            offsetPos = body1->worldPointToLocal(body2->getPosition());
        }
    }


    void
    Fixed::updateConstraints(World& world) noexcept
    {
        numConstraints = 6;
        clearConstraints();

        const Vector3 worldAnchor1 = body1 ? body1->localPointToWorld(offsetPos) : Vector3::Zero();
        const Vector3 worldAnchor2 = body2 ? body2->getPosition() : Vector3::Zero();

        // constrain position
        if (body1) {
            constraints[0].lin1 = {1,0,0};
            constraints[1].lin1 = {0,1,0};
            constraints[2].lin1 = {0,0,1};
        }
        if (body2) {
            constraints[0].lin2 = {-1,0,0};
            constraints[1].lin2 = {0,-1,0};
            constraints[2].lin2 = {0,0,-1};
        }
        if (body1 && body2) {
            // only needed with 2 bodies, see that it's very similar to the Ball joint
            const Vector3 h = (body2->getCoM() - body1->getCoM())/2;

            constraints[0].ang1 = cross(h, {1,0,0});
            constraints[0].ang2 = cross(h, {1,0,0});

            constraints[1].ang1 = cross(h, {0,1,0});
            constraints[1].ang2 = cross(h, {0,1,0});

            constraints[2].ang1 = cross(h, {0,0,1});
            constraints[2].ang2 = cross(h, {0,0,1});
        }

        const Real c = getCFM(world);
        constraints[0].cfm = c;
        constraints[1].cfm = c;
        constraints[2].cfm = c;

        const Real k = world.getFPS() * getERP(world);
        const Vector3 error = worldAnchor2 - worldAnchor1;
        constraints[0].c = k * error.x;
        constraints[1].c = k * error.y;
        constraints[2].c = k * error.z;

        // angular constraints
        if (body1) {
            constraints[3].ang1 = {1,0,0};
            constraints[4].ang1 = {0,1,0};
            constraints[5].ang1 = {0,0,1};
        }
        if (body2) {
            constraints[3].ang2 = {-1,0,0};
            constraints[4].ang2 = {0,-1,0};
            constraints[5].ang2 = {0,0,-1};
        }
        constraints[3].cfm = c;
        constraints[4].cfm = c;
        constraints[5].cfm = c;

        // use small angle approximation: sin(theta) ~= theta
        const Matrix3 r1 = body1 ? body1->getLocalAxes() : Matrix3::Identity();
        const Matrix3 r2 = body2 ? body2->getLocalAxes() : Matrix3::Identity();
        const Matrix3 rotDiff = r1.transposed() * r2;
        constraints[3].c = k * dot({1,0,0}, cross(offsetRot.col(0), rotDiff.col(0)));
        constraints[4].c = k * dot({0,1,0}, cross(offsetRot.col(1), rotDiff.col(1)));
        constraints[5].c = k * dot({0,0,1}, cross(offsetRot.col(2), rotDiff.col(2)));
    }
}