Source

KODE / src / joints / BallSocket.cpp

Full commit
/*
  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 <stdexcept>

#include <kode/joints/BallSocket.hpp>

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


namespace kode {

    BallSocket::BallSocket(Body* b1, Body* b2, const Vector3& anchor) :
        Joint{b1, b2}
    {
        setAnchor(anchor);
    }


    BallSocket::BallSocket(Body& b, const Vector3& anchor) :
        BallSocket{&b, nullptr, anchor}
    {}


    BallSocket::BallSocket(Body& b1, Body& b2, const Vector3& anchor) :
        BallSocket{&b1, &b2, anchor}
    {}


    void
    BallSocket::setAnchor1(const Vector3& anchor) noexcept
    {
        anchor1 = body1 ? body1->worldPointToLocal(anchor) : anchor;
    }


    void
    BallSocket::setAnchor2(const Vector3& anchor) noexcept
    {
        anchor2 = body2 ? body2->worldPointToLocal(anchor) : anchor;
    }


    void
    BallSocket::setAnchor(const Vector3& anchor) noexcept
    {
        setAnchor1(anchor);
        setAnchor2(anchor);
    }


    void
    BallSocket::setAnchor(Real ax, Real ay, Real az) noexcept
    {
        setAnchor({ax, ay, az});
    }


    Vector3
    BallSocket::getAnchor1() const noexcept
    {
        return body1 ? body1->localPointToWorld(anchor1) : anchor1;
    }


    Vector3
    BallSocket::getAnchor2() const noexcept
    {
        return body2 ? body2->localPointToWorld(anchor2) : anchor2;
    }


    void
    BallSocket::updateConstraints(World& world) noexcept
    {
        numConstraints = 3;

        clearConstraints();

        const Vector3 worldAnchor1 = getAnchor1();
        const Vector3 worldAnchor2 = getAnchor2();

        // Jacobian
        if (body1) {
            constraints[0].lin1 = {1,0,0};
            constraints[1].lin1 = {0,1,0};
            constraints[2].lin1 = {0,0,1};

            const Vector3 relAnchor1 = worldAnchor1 - body1->getCoM();
            constraints[0].ang1 = cross(relAnchor1, {1,0,0});
            constraints[1].ang1 = cross(relAnchor1, {0,1,0});
            constraints[2].ang1 = cross(relAnchor1, {0,0,1});;
        }

        if (body2) {
            constraints[0].lin2 = {-1,0,0};
            constraints[1].lin2 = {0,-1,0};
            constraints[2].lin2 = {0,0,-1};

            const Vector3 relAnchor2 = worldAnchor2 - body2->getCoM();
            constraints[0].ang2 = cross(relAnchor2, {-1,0,0});
            constraints[1].ang2 = cross(relAnchor2, {0,-1,0});
            constraints[2].ang2 = cross(relAnchor2, {0,0,-1});
        }

        // right hand side
        const Real k = world.getFPS() * (useERP ? erp : world.getERP());
        const Vector3 error = worldAnchor2 - worldAnchor1;

        constraints[0].c = k * error.x;
        constraints[1].c = k * error.y;
        constraints[2].c = k * error.z;

        // cfm
        const Real q = useCFM ? cfm : world.getCFM();
        constraints[0].cfm = q;
        constraints[1].cfm = q;
        constraints[2].cfm = q;
    }
}