1. Daniel K. O.
  2. KODE

Source

KODE / src / joints / AngularMotor.cpp

/*
   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/AngularMotor.hpp>

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

namespace kode {


    void
    AngularMotor::setLimits(Radian low, Radian high)
    {
        constexpr Interval valid = {-Math::Pi, Math::Pi};
        if (!valid.contains(low.radians()))
            throw std::domain_error{"lower limit must be in [-pi,+pi]"};
        if (!valid.contains(high.radians()))
            throw std::domain_error{"upper limit must be in [-pi,+pi]"};

        if (low <= high) {
            loStop = low.radians();
            hiStop = high.radians();
        } else
            throw std::invalid_argument{"lower limit must be less or equal to upper limit"};
    }


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


    bool
    AngularMotor::addConstraint(Joint& joint,
                                const Vector3& axis,
                                Radian pos,
                                Constraint& con)
    {
        if (testPowerLimit(pos.radians())) {
            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.ang1 = axis;

            if (body2)
                con.ang2 = -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->addTorque( f * axis);
                    if (body2)
                        body2->addTorque(-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->getAngularVel(), axis) : 0)
                                              -
                                              (body2 ? dot(body2->getAngularVel(), 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;

        }
    }
}