Source

KODE / src / joints / LinearMotor.cpp

Full commit
/*
   KODE Physics Library
   Copyright 2013-2014 Daniel Kohler Osmari

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#include <stdexcept>

#include <kode/joints/LinearMotor.hpp>

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

namespace kode {

    void
    LinearMotor::setLimits(Real low, Real high)
    {
        if (low <= high) {
            loStop = low;
            hiStop = high;
        } else
            throw std::invalid_argument{"lower limit must be less or equal to upper limit"};
    }


    std::pair<Real,Real>
    LinearMotor::getLimits() const noexcept
    {
        return {loStop, hiStop};
    }


    bool
    LinearMotor::addConstraint(Joint& joint,
                               const Vector3& axis,
                               Real pos,
                               Constraint& con)
    {
        if (testPowerLimit(pos)) {
            joint.appendConstraints(1);

            Body* body1 = joint.getBody1();
            Body* body2 = joint.getBody2();
            World* world = joint.getWorld();
            if (!world)
                throw std::logic_error{"trying to compute constraint for a joint that is not in a world"};

            if (body1)
                con.lin1 = axis;

            if (body2)
                con.lin2 = -axis;

            // linear limot torque decoupling step:
            //
            // if this is a linear limot (e.g. from a slider), we have to be careful
            // that the linear constraint forces (+/- ax1) applied to the two bodies
            // do not create a torque couple. in other words, the points that the
            // constraint force is applied at must lie along the same ax1 axis.
            // a torque couple will result in powered or limited slider-jointed free
            // bodies from gaining angular momentum.
            // the solution used here is to apply the constraint forces at the point
            // halfway between the body centers. there is no penalty (other than an
            // extra tiny bit of computation) in doing this adjustment. note that we
            // only need to do this if the constraint connects two bodies.

            if (body1 && body2) {
                Vector3 h = (body2->getCoM() - body1->getCoM()) / 2;
                con.ang1 = cross(h, axis);
                con.ang2 = cross(h, axis);
            }


            if (powered) {
                con.cfm = normalCFM;

                if (limited == State::Unlimited) {
                    con.c = targetVel;
                    con.low = -maxForce;
                    con.high = maxForce;
                } else {
                    // powered and limited
                    Real f = maxForce;

                    // if we're powering away from the limit, apply the fudge factor
                    if ((limited == State::LowStop && targetVel > 0)
                        ||
                        (limited == State::HighStop && targetVel < 0))
                        f *= fudgeFactor;

                    if (body1)
                        body1->addForce( f * axis);
                    if (body2)
                        body2->addForce(-f * axis);
                }
            }


            if (limited != State::Unlimited) {

                const Real k = world->getFPS() * stopERP;
                con.c = -k * limitError; // note: might get overriden later
                con.cfm = stopCFM;

                if (loStop == hiStop) {
                    con.low = -Math::Infinity;
                    con.high = Math::Infinity;
                } else {
                    if (limited == Motor::State::LowStop) {
                        con.low = 0;
                        con.high = Math::Infinity;
                    } else {
                        con.low = -Math::Infinity;
                        con.high = 0;
                    }

                    // bounciness only happen when there's slack between low and high stops
                    if (bounciness > 0) {
                        const Real jointVel = (body1 ? dot(body1->getLinearVel(), axis) : 0)
                                              -
                                              (body2 ? dot(body2->getLinearVel(), axis) : 0);
                        if (limited == State::LowStop) {
                            if (jointVel < 0)
                                con.c = std::max(con.c, -bounciness*jointVel);
                        } else {
                            if (jointVel > 0)
                                con.c = std::min(con.c, -bounciness*jointVel);
                        }
                    }
                }

            }

            return true;
        } else {
            // make sure this constraint has no lambda
            con.lambda = 0;
            return false;
        }
    }
}