Source

KODE / src / joints / DoubleBall.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/DoubleBall.hpp>

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

#include <iostream>
using std::clog;
using std::endl;

namespace kode {

    DoubleBall::DoubleBall(Body* b1, Body* b2,
                           const Vector3& a1, const Vector3& a2) :
        Joint{b1, b2}
    {
        setAnchors(a1, a2);
    }


    DoubleBall::DoubleBall(Body& b,
                           const Vector3& a1, const Vector3& a2) :
        DoubleBall{&b, nullptr, a1, a2}
    {}


    DoubleBall::DoubleBall(Body& b1, Body& b2,
                           const Vector3& a1, const Vector3& a2) :
        DoubleBall{&b1, &b2, a1, a2}
    {}


    void
    DoubleBall::setAnchor1(const Vector3& a1) noexcept
    {
        anchor1 = body1 ? body1->worldPointToLocal(a1) : a1;
        setTargetDistance(distance(a1, getAnchor2()));
    }


    void
    DoubleBall::setAnchor2(const Vector3& a2) noexcept
    {
        anchor2 = body2 ? body2->worldPointToLocal(a2) : a2;
        setTargetDistance(distance(getAnchor1(), a2));
    }


    void
    DoubleBall::setAnchors(const Vector3& a1, const Vector3& a2) noexcept
    {
        anchor1 = body1 ? body1->worldPointToLocal(a1) : a1;
        anchor2 = body2 ? body2->worldPointToLocal(a2) : a2;
        setTargetDistance(distance(a1, a2));
    }


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


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


    void
    DoubleBall::setTargetDistance(Real d)
    {
        if (d < 0)
            throw std::domain_error{"distance can't be negative"};
        targetDistance = d;
    }


    void
    DoubleBall::updateConstraints(World& world) noexcept
    {
        numConstraints = 1;
        clearConstraints();

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

        // constraint will act around axis q
        Vector3 q = worldAnchor1 - worldAnchor2;

        constexpr Real tol = Epsilon;
        if (q.squaredLength() < tol) {
            // too small, let's choose an arbitrary direction
            // heuristic: difference in velocities at anchors
            Vector3 v1 = body1 ? body1->velAtLocalPoint(anchor1) : Vector3::Zero();
            Vector3 v2 = body2 ? body2->velAtLocalPoint(anchor2) : Vector3::Zero();

            q = v1 - v2;

            if (q.squaredLength() < tol)
                q = {1, 0, 0}; // this direction is as good as any
        }

        q.normalize();

        Constraint& con = constraints[0];

        if (body1) {
            const Vector3 relAnchor1 = worldAnchor1 - body1->getCoM();
            con.lin1 = q;
            con.ang1 = cross(relAnchor1, q);
        }
        if (body2) {
            const Vector3 relAnchor2 = worldAnchor2 - body2->getCoM();
            con.lin2 = -q;
            con.ang2 = cross(relAnchor2, -q);
        }

        const Real k = world.getFPS() * getERP(world);
        const Real dist = dot(worldAnchor1 - worldAnchor2, q);
        const Real error = targetDistance - dist;

        con.c = k * error;
        con.cfm = getCFM(world);
    }
}