Source

KODE / src / joints / Joint.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 <cassert>

#include <kode/joints/Joint.hpp>

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


namespace kode {

    Joint::Joint(Body* b1, Body* b2) :
        body1{b1},
        body2{b2}
    {
        if (!body1 && body1 == body2)
            throw std::invalid_argument{"can't constrain a body with itself (same body used twice)"};
        attachBodies();
    }


    Joint::Joint(Joint&& other) noexcept :
        body1{other.body1},
        body2{other.body2},
        numConstraints{other.numConstraints},
        constraints(std::move(other.constraints)),
        erp{other.erp},
        cfm{other.cfm},
        useERP{other.useERP},
        useCFM{other.useCFM}
    {
        other.detachBodies();
        attachBodies();
    }


    Joint::~Joint() noexcept
    {
        detachBodies();
    }


    Joint&
    Joint::operator=(Joint&& other) noexcept
    {
        if (&other != this) {
            detachBodies();
            body1 = other.body1;
            body2 = other.body2;
            numConstraints = other.numConstraints;
            constraints = std::move(other.constraints);
            erp = other.erp;
            cfm = other.cfm;
            useERP = other.useERP;
            useCFM = other.useCFM;
            other.detachBodies();
            attachBodies();
        }
        return *this;
    }


    void
    Joint::attachBodies()
    {
        if (body1)
            body1->remember(this);
        if (body2)
            body2->remember(this);
    }


    void
    Joint::attach(Body* b1, Body* b2)
    {
        detachBodies();
        rememberBody1(b1);
        rememberBody2(b2);
        attachBodies();
    }


    void
    Joint::attach(Body& b1, Body& b2)
    {
        attach(&b1, &b2);
    }


    void
    Joint::attach(Body& b)
    {
        detachBodies();
        rememberBody1(&b);
        attachBodies();
    }


    void
    Joint::detachBodies() noexcept
    {
        if (body1)
            body1->forget(this);
        body1 = nullptr;

        if (body2)
            body2->forget(this);
        body2 = nullptr;
    }


    void
    Joint::rememberBody1(Body* b) noexcept
    {
        body1 = b;
    }


    void
    Joint::rememberBody2(Body* b) noexcept
    {
        body2 = b;
    }


    void
    Joint::forgetBody(Body* b) noexcept
    {
        if (b == body1)
            body1 = nullptr;
        else if (b == body2)
            body2 = nullptr;
        else
            assert(!"BUG: forgetting a body that is not attached");
    }


    World*
    Joint::getWorld()
    {
        if ((body1 && body2 && body1->getWorld() != body2->getWorld()))
            throw std::logic_error{"can't attached bodies belong to different worlds"};
        return body1 ? body1->getWorld() :
            body2 ? body2->getWorld() : nullptr;
    }

    void
    Joint::clearConstraints() noexcept
    {
        for (unsigned i=0; i<numConstraints; ++i)
            constraints[i].clear();
    }


    void
    Joint::clearMore(unsigned m) noexcept
    {
        for (unsigned i=0; i<m; ++i)
            constraints[numConstraints+i].clear();
    }


    void
    Joint::afterStep()
    {}


    void
    Joint::fireAfterStep()
    {
        afterStep();
        onAfterStep.trigger(this);
    }


    void
    Joint::setERP(Real e)
    {
        if (e < 0)
            throw std::domain_error{"ERP can't be negative"};
        useERP = true;
        erp = e;
    }


    void
    Joint::unsetERP() noexcept
    {
        useERP = false;
    }


    Real
    Joint::getERP(const World& world) const noexcept
    {
        return useERP ? erp : world.getERP();
    }


    void
    Joint::setCFM(Real e)
    {
        if (e < 0)
            throw std::domain_error{"CFM can't be negative"};
        useCFM = true;
        cfm = e;
    }


    void
    Joint::unsetCFM() noexcept
    {
        useCFM = false;
    }


    Real
    Joint::getCFM(const World& world) const noexcept
    {
        return useCFM ? cfm : world.getCFM();
    }

}