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-12 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"
/*
  Update:
    This is a port of the original code so that it will work with
    the multibody code RBDL written by Martin Felis.
  
  Author:
    Matthew Millard
  
  Date:
    Nov 2015
 
*/
 
#include <cassert>
#include <rbdl/rbdl_math.h>
#include <vector>
#include <cmath>
#include <cstdio>    
namespace RigidBodyDynamics {
  namespace Addons {
    namespace Geometry{
 
 
template <class T>
class Function_ {
public:
  class Constant;
  class Linear;
  class Polynomial;
  class Sinusoid;
  class Step;
  virtual ~Function_() {
  }
  virtual T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const = 0;
  virtual T calcDerivative(const std::vector<int>& derivComponents, 
               const RigidBodyDynamics::Math::VectorNd& x) const = 0;
 
  /*
  T calcDerivative(const std::vector<int>& derivComponents, 
           const RigidBodyDynamics::Math::VectorNd& x) const 
  {   return calcDerivative(std::vector<int>(derivComponents),x); }
  */
 
  virtual int getArgumentSize() const = 0;
  virtual int getMaxDerivativeOrder() const = 0;
};
 
typedef Function_<double> 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 RigidBodyDynamics::Math::VectorNd& x) const {
    assert(x.size() == argumentSize);
    return value;
  }
  T calcDerivative(const std::vector<int>& derivComponents, 
           const RigidBodyDynamics::Math::VectorNd& 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 RigidBodyDynamics::Math::VectorNd& x) const 
  {   return calcDerivative(std::vector<int>(derivComponents),x); }
  */
private:
  const int argumentSize;
  const T value;
};
 
template <class T>
class Function_<T>::Linear : public Function_<T> {
public:
  explicit Linear(
    const RigidBodyDynamics::Math::VectorNd& coefficients) 
    : coefficients(coefficients) {
  }
  T calcValue(const RigidBodyDynamics::Math::VectorNd& 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 std::vector<int>& derivComponents, 
           const RigidBodyDynamics::Math::VectorNd& 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 RigidBodyDynamics::Math::VectorNd& x) const 
  {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
  */
private:
  const RigidBodyDynamics::Math::VectorNd coefficients;
};
 
 
template <class T>
class Function_<T>::Polynomial : public Function_<T> {
public:
  Polynomial(const RigidBodyDynamics::Math::VectorNd& coefficients) 
        : coefficients(coefficients) {
  }
  T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const {
    assert(x.size() == 1);
    double 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 std::vector<int>& derivComponents, 
           const RigidBodyDynamics::Math::VectorNd& x) const {
    assert(x.size() == 1);
    assert(derivComponents.size() > 0);
    double 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 RigidBodyDynamics::Math::VectorNd& x) const 
  {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
  */
private:
  const RigidBodyDynamics::Math::VectorNd coefficients;
};
 
 
template <>
class Function_<double>::Sinusoid : public Function_<double> {
public:
  Sinusoid(double amplitude, double frequency, double phase=0) 
  :   a(amplitude), w(frequency), p(phase) {}
 
  void setAmplitude(double amplitude) {a=amplitude;}
  void setFrequency(double frequency) {w=frequency;}
  void setPhase  (double phase)   {p=phase;}
 
  double getAmplitude() const {return a;}
  double getFrequency() const {return w;}
  double getPhase  () const {return p;}
 
  // Implementation of Function_<T> virtuals.
 
  virtual double calcValue(
    const RigidBodyDynamics::Math::VectorNd& x) const {
    
    const double t = x[0]; // we expect just one argument
    return a*std::sin(w*t + p);
  }
 
  virtual double calcDerivative(
      const std::vector<int>& derivComponents,
      const RigidBodyDynamics::Math::VectorNd&    x) const {
 
    const double 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 double sign = double(((order/2) & 0x1) ? -1 : 1);
      const double sc   = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p);
      const double wn   = std::pow(w, order);
      return sign*a*wn*sc;
    }
  }
 
  virtual int getArgumentSize() const {return 1;} // just time
  virtual int getMaxDerivativeOrder() const {
    return std::numeric_limits<int>::max();
  }
 
  /*
  double calcDerivative(const std::vector<int>& derivComponents, 
            const RigidBodyDynamics::Math::VectorNd& x) const 
  {   return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
  */
private:
  double a, w, p;
};
 
template <class T>
class Function_<T>::Step : public Function_<T> {
public:
  Step(const T& y0, const T& y1, double x0, double x1) 
  :   m_y0(y0), m_y1(y1), m_yr(y1-y0), m_zero(double(0)*y0),
    m_x0(x0), m_x1(x1), m_ooxr(1/(x1-x0)), m_sign(copysign(1,m_ooxr)) 
  {   
    /*
    SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::ctor()",
    "A zero-length switching interval is illegal; both ends were %g.", x0);
    */
    assert(x0 != x1);
    std::printf( "Function_<T>::Step::ctor():"
        "A zero-length switching interval "
        "is illegal; both ends were %g.", x0);
 
  }
 
  T calcValue(const RigidBodyDynamics::Math::VectorNd& xin) const {
    /*
    SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
      "Function_<T>::Step::calcValue()", 
      "Expected just one input argument but got %d.", xin.size());
    */      
    assert(xin.size() == 1);
    std::printf( "Function_<T>::Step::calcValue() "
        "Expected just one input argument but got %d.",
         xin.size());
 
 
    const double 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 double f = step01(m_x0,m_ooxr, x);
    return m_y0 + f*m_yr;
  }
 
  T calcDerivative(const std::vector<int>& derivComponents, 
                   const RigidBodyDynamics::Math::VectorNd& xin) const {
    /*
    SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
      "Function_<T>::Step::calcDerivative()", 
      "Expected just one input argument but got %d.", xin.size());
    */
    assert(xin.size() == 1);
    std::printf( "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);
    */
    assert(1 <= derivOrder && derivOrder <= 3);
    std::printf("Function_<T>::Step::calcDerivative() "
      "Only 1st, 2nd, and 3rd derivatives of the step are available,"
      " but derivative %d was requested.", derivOrder);
 
    const double 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 dstep01(m_x0,m_ooxr, x) * m_yr;
      case 2: return d2step01(m_x0,m_ooxr, x) * m_yr;
      case 3: return d3step01(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 RigidBodyDynamics::Math::VectorNd& 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 double m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0)
  const double m_sign;       // sign(x1-x0) is 1 or -1
 
  double step01(double x0, double x1, double x){
    double u = (x-x0)/(x1-x0);
    double u2 = u*u;
    double u3 = u2*u;
    return (3*u2 - 2*u3);
  }
 
  double dstep01(double x0, double x1, double x){
    double u  = (x-x0)/(x1-x0);
    double du = (1)/(x1-x0);
    double du2 = 2*u*du;
    double du3 = 3*u*u*du;
    return (3*du2 - 2*du3);
  }
 
  double d2step01(double x0, double x1, double x){
    double u  = (x-x0)/(x1-x0);
    double du = (1)/(x1-x0);
    //double ddu = 0;
    double ddu2 = 2*du*du;// + 2*u*ddu since ddu=0;
    double ddu3 = 3*du*u*du + 3*u*du*du;// + 3*u*u*du; ditto
    return (3*ddu2 - 2*ddu3);
  }
 
  double d3step01(double x0, double x1, double x){
    double u  = (x-x0)/(x1-x0);
    double du = (1)/(x1-x0);
    //double ddu = 0;
    //double dddu = 0;
    double dddu2 = 0; //2*du*du;// since ddu=0;
    double dddu3 = 3*du*du*du + 3*du*du*du;// ditto
    return (3*dddu2 - 2*dddu3);
  }
};
 
}
}
}
 
#endif // SimTK_SimTKCOMMON_FUNCTION_H_