Function.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_FUNCTION_H_
#define SimTK_SimTKCOMMON_FUNCTION_H_

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2008-15 Stanford University and the Authors.        *
 * Authors: Peter Eastman                                                     *
 * Contributors: Michael Sherman                                              *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

// Note: this file was moved from Simmath to SimTKcommon 20100601; see the
// Simmath repository for earlier history.

#include "SimTKcommon/basics.h"
#include "SimTKcommon/Simmatrix.h"
#include <cassert>

namespace SimTK {

template <class T>
class Function_ {
public:
    class Constant;
    class Linear;
    class Polynomial;
    class Sinusoid;
    class Step;
    virtual ~Function_() {
    }
    virtual T calcValue(const Vector& x) const = 0;
    virtual T calcDerivative(const Array_<int>& derivComponents, 
                             const Vector&      x) const = 0;

    T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }

    virtual int getArgumentSize() const = 0;
    virtual int getMaxDerivativeOrder() const = 0;

    virtual Function_* clone() const {
        SimTK_THROW2(Exception::UnimplementedVirtualMethod, "Function_", 
                     "clone");
    }
};

typedef Function_<Real> Function;



template <class T>
class Function_<T>::Constant : public Function_<T> {
public:
    explicit Constant(T value, int argumentSize=1) 
    :   argumentSize(argumentSize), value(value) {
    }
    T calcValue(const Vector& x) const override {
        assert(x.size() == argumentSize);
        return value;
    }
    T calcDerivative(const Array_<int>& derivComponents, 
                     const Vector& x) const override {
        return static_cast<T>(0);
    }
    int getArgumentSize() const override {
        return argumentSize;
    }
    int getMaxDerivativeOrder() const override {
        return std::numeric_limits<int>::max();
    }

    Constant* clone() const override {return new Constant(*this);}

    T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }

private:
    int argumentSize;
    T   value;
};

template <class T>
class Function_<T>::Linear : public Function_<T> {
public:
    explicit Linear(const Vector_<T>& coefficients) : coefficients(coefficients) {
    }
    T calcValue(const Vector& x) const override {
        assert(x.size() == coefficients.size()-1);
        T value = static_cast<T>(0);
        for (int i = 0; i < x.size(); ++i)
            value += x[i]*coefficients[i];
        value += coefficients[x.size()];
        return value;
    }
    T calcDerivative(const Array_<int>& derivComponents, 
                     const Vector& x) const override {
        assert(x.size() == coefficients.size()-1);
        assert(derivComponents.size() > 0);
        if (derivComponents.size() == 1)
            return coefficients(derivComponents[0]);
        return static_cast<T>(0);
    }
    int getArgumentSize() const override {
        return coefficients.size()-1;
    }
    int getMaxDerivativeOrder() const override {
        return std::numeric_limits<int>::max();
    }

    Linear* clone() const override {return new Linear(*this);}

    T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
private:
    Vector_<T> coefficients;
};


template <class T>
class Function_<T>::Polynomial : public Function_<T> {
public:
    Polynomial(const Vector_<T>& coefficients) : coefficients(coefficients) {
    }
    T calcValue(const Vector& x) const override {
        assert(x.size() == 1);
        Real arg = x[0];
        T value = static_cast<T>(0);
        for (int i = 0; i < coefficients.size(); ++i)
            value = value*arg + coefficients[i];
        return value;
    }
    T calcDerivative(const Array_<int>& derivComponents, 
                     const Vector& x) const override {
        assert(x.size() == 1);
        assert(derivComponents.size() > 0);
        Real arg = x[0];
        T value = static_cast<T>(0);
        const int derivOrder = (int)derivComponents.size();
        const int polyOrder = coefficients.size()-1;
        for (int i = 0; i <= polyOrder-derivOrder; ++i) {
            T coeff = coefficients[i];
            for (int j = 0; j < derivOrder; ++j)
                coeff *= polyOrder-i-j;
            value = value*arg + coeff;
        }
        return value;
    }
    int getArgumentSize() const override {
        return 1;
    }
    int getMaxDerivativeOrder() const override {
        return std::numeric_limits<int>::max();
    }

    Polynomial* clone() const override {return new Polynomial(*this);}

    T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
private:
    Vector_<T> coefficients;
};


template <>
class Function_<Real>::Sinusoid : public Function_<Real> {
public:
    Sinusoid(Real amplitude, Real frequency, Real phase=0) 
    :   a(amplitude), w(frequency), p(phase) {}

    void setAmplitude(Real amplitude) {a=amplitude;}
    void setFrequency(Real frequency) {w=frequency;}
    void setPhase    (Real phase)     {p=phase;}

    Real getAmplitude() const {return a;}
    Real getFrequency() const {return w;}
    Real getPhase    () const {return p;}

    // Implementation of Function_<T> virtuals.

    virtual Real calcValue(const Vector& x) const override {
        const Real t = x[0]; // we expect just one argument
        return a*std::sin(w*t + p);
    }

    virtual Real calcDerivative(const Array_<int>& derivComponents,
                                const Vector&      x) const override {
        const Real t = x[0]; // time is the only argument
        const int order = derivComponents.size();
        // The n'th derivative is
        //    sign * a * w^n * sc
        // where sign is -1 if floor(order/2) is odd, else 1
        // and   sc is cos(w*t+p) if order is odd, else sin(w*t+p)
        switch(order) {
        case 0: return  a*      std::sin(w*t + p);
        case 1: return  a*w*    std::cos(w*t + p);
        case 2: return -a*w*w*  std::sin(w*t + p);
        case 3: return -a*w*w*w*std::cos(w*t + p);
        default:
            const Real sign = Real(((order/2) & 0x1) ? -1 : 1);
            const Real sc   = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p);
            const Real wn   = std::pow(w, order);
            return sign*a*wn*sc;
        }
    }

    int getArgumentSize() const override {return 1;}
    int getMaxDerivativeOrder() const override {
        return std::numeric_limits<int>::max();
    }

    Sinusoid* clone() const override {return new Sinusoid(*this);}

    Real calcDerivative(const std::vector<int>& derivComponents, 
                        const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
private:
    Real a, w, p;
};

template <class T>
class Function_<T>::Step : public Function_<T> {
public:
    Step(const T& y0, const T& y1, Real x0, Real x1) 
    {   setParameters(y0,y1,x0,x1); }

    void setParameters(const T& y0, const T& y1, Real x0, Real x1) {
        SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::setParameters()",
        "A zero-length switching interval is illegal; both ends were %g.", x0);
        m_y0 = y0; m_y1 = y1; m_yr = y1-y0; m_zero = Real(0)*y0;
        m_x0 = x0; m_x1 = x1; m_ooxr = 1/(x1-x0); m_sign = sign(m_ooxr); 
    }

    T calcValue(const Vector& xin) const override {
        SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
            "Function_<T>::Step::calcValue()", 
            "Expected just one input argument but got %d.", xin.size());

        const Real x = xin[0];
        if ((x-m_x0)*m_sign <= 0) return m_y0;
        if ((x-m_x1)*m_sign >= 0) return m_y1;
        // f goes from 0 to 1 as x goes from x0 to x1.
        const Real f = stepAny(0,1,m_x0,m_ooxr, x);
        return m_y0 + f*m_yr;
    }

    T calcDerivative(const Array_<int>& derivComponents, 
                     const Vector& xin) const override {
        SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
            "Function_<T>::Step::calcDerivative()", 
            "Expected just one input argument but got %d.", xin.size());

        const int derivOrder = (int)derivComponents.size();
        SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3,
            "Function_<T>::Step::calcDerivative()",
            "Only 1st, 2nd, and 3rd derivatives of the step are available,"
            " but derivative %d was requested.", derivOrder);
        const Real x = xin[0];
        if ((x-m_x0)*m_sign <= 0) return m_zero;
        if ((x-m_x1)*m_sign >= 0) return m_zero;
        switch(derivOrder) {
          case 1: return dstepAny (1,m_x0,m_ooxr, x) * m_yr;
          case 2: return d2stepAny(1,m_x0,m_ooxr, x) * m_yr;
          case 3: return d3stepAny(1,m_x0,m_ooxr, x) * m_yr;
          default: assert(!"impossible derivOrder");
        }
        return NaN*m_yr; /*NOTREACHED*/
    }

    int getArgumentSize() const override {return 1;}
    int getMaxDerivativeOrder() const override {return 3;}

    Step* clone() const override {return new Step(*this);}

    T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
private:
    T    m_y0, m_y1, m_yr;   // precalculate yr=(y1-y0)
    T    m_zero;             // precalculate T(0)
    Real m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0)
    Real m_sign;             // sign(x1-x0) is 1 or -1
};

} // namespace SimTK

#endif // SimTK_SimTKCOMMON_FUNCTION_H_