Function.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_FUNCTION_H_
#define SimTK_SimTKCOMMON_FUNCTION_H_

/* -------------------------------------------------------------------------- *
 *                         SimTK Core: SimTKcommon                            *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2008-10 Stanford University and the Authors.        *
 * Authors: Peter Eastman                                                     *
 * Contributors: Michael Sherman                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

// 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 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;
};

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 {
        assert(x.size() == argumentSize);
        return value;
    }
    T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
        return static_cast<T>(0);
    }
    virtual int getArgumentSize() const {
        return argumentSize;
    }
    int getMaxDerivativeOrder() const {
        return std::numeric_limits<int>::max();
    }

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

private:
    const int argumentSize;
    const 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 {
        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 {
        assert(x.size() == coefficients.size()-1);
        assert(derivComponents.size() > 0);
        if (derivComponents.size() == 1)
            return coefficients(derivComponents[0]);
        return static_cast<T>(0);
    }
    virtual int getArgumentSize() const {
        return coefficients.size()-1;
    }
    int getMaxDerivativeOrder() const {
        return std::numeric_limits<int>::max();
    }

    T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const 
    {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
private:
    const 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 {
        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 {
        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;
    }
    virtual int getArgumentSize() const {
        return 1;
    }
    int getMaxDerivativeOrder() const {
        return std::numeric_limits<int>::max();
    }

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

template <class T>
class Function_<T>::Step : public Function_<T> {
public:
    Step(const T& y0, const T& y1, Real x0, Real x1) 
    :   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)) 
    {   SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::ctor()",
        "A zero-length switching interval is illegal; both ends were %g.", x0);
    }

    T calcValue(const Vector& xin) const {
        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 {
        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*/
    }

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

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

} // namespace SimTK

#endif // SimTK_SimTKCOMMON_FUNCTION_H_