piecewise_polynom_function.h

/* Copyright (C) 2004-2009 MBSim Development Team
 *
 * This library is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation; either 
 * version 2.1 of the License, or (at your option) any later version. 
 *  
 * This library is distributed in the hope that it will be useful, 
 * but WITHOUT ANY WARRANTY; without even the implied warranty of 
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
 * Lesser General Public License for more details. 
 *  
 * You should have received a copy of the GNU Lesser General Public 
 * License along with this library; if not, write to the Free Software 
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
 *
 * Contact: martin.o.foerg@googlemail.com
 */
 
#ifndef PIECEWISE_POLYNOM
#define PIECEWISE_POLYNOM
 
#include "fmatvec/fmatvec.h"
#include "mbsim/functions/function.h"
#include "mbsim/utils/utils.h"
#include "mbsim/utils/eps.h"
#include "mbsim/mbsim_event.h"
 
namespace MBSim {
 
  template<typename Sig> class PiecewisePolynomFunction; 
 
  template<typename Ret, typename Arg>
  class PiecewisePolynomFunction<Ret(Arg)> : public MBSim::Function<Ret(Arg)> {
    protected:
    using B = fmatvec::Function<Ret(Arg)>; 
 
    public:
      enum InterpolationMethod {
        cSplinePeriodic,
        cSplineNatural,
        piecewiseLinear,
        useBreaksAndCoefs
      };
 
      enum ExtrapolationMethod {
        error,
        continuePolynom,
        linear,
      };
 
      PiecewisePolynomFunction() : index(0), interpolationMethod(cSplineNatural), extrapolationMethod(error), f(this), fd(this), fdd(this) { }
      virtual ~PiecewisePolynomFunction() = default;
 
      int getArgSize() const { return 1; }
 
      std::pair<int, int> getRetSize() const { return std::make_pair(coefs[0].cols(),1); }
 
      void init(Element::InitStage stage, const InitConfigSet &config=InitConfigSet()) {
        Function<Ret(Arg)>::init(stage, config);
        if(stage==Element::preInit) {
          if(interpolationMethod!=useBreaksAndCoefs) {
            if(y.rows() != x.size())
              this->throwError("Dimension missmatch in size of x");
            for(int i=1; i<x.size(); i++)
              if(x(i) <= x(i-1))
                this->throwError("Values of x must be strictly monotonic increasing!");
            calculateSpline();
          }
          else {
            for(int i=1; i<breaks.size(); i++)
              if(breaks(i) < breaks(i-1))
                this->throwError("Values of breaks must be monotonic increasing!");
            for(int j=0; j<static_cast<int>(coefs.size()); j++) {
              if(coefs[j].rows()!=breaks.size()-1)
                this->throwError("Dimension missmatch in size of breaks and rows of coefficients!");
              if(coefs[0].cols()!=coefs[j].cols())
                this->throwError("Dimension missmatch in columns of coefficients!");
            }
 
            // remove duplicate entries from breaks
            std::set<int> removeIdx;
            for(int i=0; i<breaks.size()-1; ++i)
              if(breaks(i)==breaks(i+1))
                removeIdx.emplace(i);
            if(!removeIdx.empty()) {
              auto breaksOld(std::move(breaks));
              auto coefsOld(std::move(coefs));
              breaks.resize(breaksOld.size()-removeIdx.size());
              coefs.resize(coefsOld.size());
              for(auto &c : coefs)
                c.resize(coefsOld[0].rows()-removeIdx.size(),coefsOld[0].cols());
              int ii=0, i=0;
              for(i=0; i<breaksOld.size()-1; ++i) {
                if(removeIdx.find(i)!=removeIdx.end())
                  continue;
                breaks(ii)=breaksOld(i);
                for(int o=0; o<static_cast<int>(coefs.size()); o++)
                  coefs[o].set(ii,coefsOld[o].row(i));
                ++ii;
              }
              breaks(ii)=breaksOld(i);
            }
 
          }
          nPoly = (coefs[0]).rows();
          order = coefs.size()-1;
        }
      }
 
      void calculateSpline() {
        if(interpolationMethod == cSplinePeriodic) calculateSplinePeriodic();
        else if(interpolationMethod == cSplineNatural) calculateSplineNatural();
        else if(interpolationMethod == piecewiseLinear) calculatePLinear();
        else this->throwError("(PiecewisePolynomFunction::init): No valid method to calculate pp-form");
      }
 
      void reset() {
        index = 0;
        f.reset();
        fd.reset();
        fdd.reset();
        coefs.clear();
      }
 
      Ret operator()(const Arg &x) { return f(ToDouble<Arg>::cast(x)); }
 
      // we do not use B::DRetDArg as return type here since SWIG cannot wrap the propably
      // -> hence we resolve B::DRetDArg manually here to fmatvec::Der<Ret, Arg>::type
      typename fmatvec::Der<Ret, Arg>::type parDer(const Arg &x) { return fd(ToDouble<Arg>::cast(x)); }
      typename fmatvec::Der<Ret, Arg>::type parDerDirDer(const Arg &argDir, const Arg &arg) { return fdd(ToDouble<Arg>::cast(arg))*ToDouble<Arg>::cast(argDir); }
 
      fmatvec::VecV getBreaks() { return breaks; }
 
      void setInterpolationMethod(InterpolationMethod method_) { interpolationMethod = method_; }
 
      void setExtrapolationMethod(ExtrapolationMethod method_) { extrapolationMethod = method_; }
 
      void setx(const fmatvec::VecV &x_) { x <<= x_; }
      void sety(const fmatvec::MatV &y_) { y <<= y_; }
      void setxy(const fmatvec::MatV &xy) {
        if(xy.cols() <= 1)
          this->throwError("Dimension missmatch in size of xy");
        x <<= xy.col(0);
        y <<= xy(fmatvec::RangeV(0, xy.rows() - 1), fmatvec::RangeV(1, xy.cols() - 1));
      }
 
      void setCoefficientsArray(const std::vector<fmatvec::MatV> &allCoefs);
 
      void addCoefficients(const fmatvec::MatV &coef);
 
      void setBreaks(const fmatvec::VecV &breaks_u) {
        interpolationMethod=useBreaksAndCoefs;
        breaks <<= breaks_u;
      }
 
      virtual void initializeUsingXML(xercesc::DOMElement *element);
 
    protected:
      std::vector<fmatvec::MatV> coefs;
 
      fmatvec::VecV breaks;
 
      int nPoly;
 
      int order;
 
      int index;
 
      fmatvec::VecV x;
      fmatvec::MatV y;
 
      InterpolationMethod interpolationMethod;
 
      ExtrapolationMethod extrapolationMethod;
 
      void calculateSplinePeriodic();
 
      void calculateSplineNatural();
 
      /* 
       * \brief calculation of piecewise linear interpolation
       * \param interpolated arguments
       * \param interpolated function values
       *
       * the first derivative is weak and the second derivative is zero elsewhere although it should be distributionally at the corners
       */
      void calculatePLinear();
 
#ifndef SWIG
      class ZerothDerivative {
        public:
          ZerothDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
 
          void reset() { firstCall = true; }
 
          Ret operator()(const double &x);
 
        private:
          PiecewisePolynomFunction<Ret(Arg)> *parent;
          double xSave;
          fmatvec::VecV ySave;
          bool firstCall;
      };
 
      class FirstDerivative {
        public:
          FirstDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
 
          void reset() { firstCall = true; }
 
          Ret operator()(const double& x);
 
        private:
          PiecewisePolynomFunction<Ret(Arg)> *parent;
          double xSave;
          fmatvec::VecV ySave;
          bool firstCall;
      };
 
      class SecondDerivative {
        public:
          SecondDerivative(PiecewisePolynomFunction<Ret(Arg)> *polynom) : parent(polynom), xSave(0), ySave(), firstCall(true) {}
 
          void reset() { firstCall = true; }
 
          Ret operator()(const double& x);
 
        private:
          PiecewisePolynomFunction<Ret(Arg)> *parent;
          double xSave;
          fmatvec::VecV ySave;
          bool firstCall;
      };
#endif
 
    private:
      ZerothDerivative f;
      FirstDerivative fd;
      SecondDerivative fdd;
  };
 
  // the code is moved to piecewise_polynom_function_impl to avoid SWIG parsing errors
  #include "piecewise_polynom_function_impl.h"
 
}
 
#endif /* PPOLYNOM */