function.h

#ifndef _FMATVEC_FUNCTION_H_
#define _FMATVEC_FUNCTION_H_
 
#include <fmatvec/fmatvec.h>
#include <fmatvec/atom.h>
#include <stdexcept>
#include <boost/mpl/list.hpp>
 
namespace fmatvec {
 
class ErrorType {
  public:
#if !defined(SWIG) && !defined(MBSIM_COMPILE_SWIG)
    ErrorType() = delete;
#else
  // SWIG instantiats everything. Hence, we allow this for SWIG but throw a runtime error
    ErrorType() {
      throw std::runtime_error("Impossible type.");
    }
#endif
};
 
template<typename T>
struct StaticSize;
 
template<>
struct StaticSize<int> {
  enum { size1=1, size2=1 };
};
 
template<>
struct StaticSize<double> {
  enum { size1=1, size2=1 };
};
 
template<typename Shape, typename AT>
struct StaticSize<Vector<Shape, AT>> {
  enum { size1=0, size2=1 };
};
 
template<typename Shape, typename AT>
struct StaticSize<RowVector<Shape, AT>> {
  enum { size1=1, size2=0 };
};
 
template<int N, typename AT>
struct StaticSize<Vector<Fixed<N>, AT>> {
  enum { size1=N, size2=1 };
};
 
template<int N, typename AT>
struct StaticSize<RowVector<Fixed<N>, AT>> {
  enum { size1=1, size2=N };
};
 
template<typename Storage, typename RowShape, typename ColShape, typename AT>
struct StaticSize<Matrix<Storage, RowShape, ColShape, AT>> {
  enum { size1=0, size2=0 };
};
 
template<typename Storage, int N, int M, typename AT>
struct StaticSize<Matrix<Storage, Fixed<N>, Fixed<M>, AT>> {
  enum { size1=N, size2=M };
};
 
template<typename Storage, int N, typename ColShape, typename AT>
struct StaticSize<Matrix<Storage, Fixed<N>, ColShape, AT>> {
  enum { size1=N, size2=0 };
};
 
template<typename Storage, typename RowShape, int M, typename AT>
struct StaticSize<Matrix<Storage, RowShape, Fixed<M>, AT>> {
  enum { size1=0, size2=M };
};
 
template<int N, typename AT>
struct StaticSize<SquareMatrix<Fixed<N>, AT>> {
  enum { size1=N, size2=N };
};
 
template<typename Shape, typename AT>
struct StaticSize<SquareMatrix<Shape, AT>> {
  enum { size1=0, size2=0 };
};
 
template<typename Dep, typename Indep>
struct Der {
  using type = ErrorType;
};
 
template<typename Dep>
struct Der<Dep, double> {
  using type = Dep;
};
 
template<typename IndepVecShape>
struct Der<double, Vector<IndepVecShape, double>> {
  using type = RowVector<IndepVecShape, double>;
};
 
template<typename DepVecShape, typename IndepVecShape>
struct Der<Vector<DepVecShape, double>, Vector<IndepVecShape, double>> {
  using type = Matrix<General, DepVecShape, IndepVecShape, double>;
};
 
template<>
struct Der<Matrix<Rotation, Fixed<3>, Fixed<3>, double>, double> {
  using type = Vector<Fixed<3>, double>;
};
 
template<typename IndepVecShape>
struct Der<Matrix<Rotation, Fixed<3>, Fixed<3>, double>, Vector<IndepVecShape, double>> {
  using type = Matrix<General, Fixed<3>, IndepVecShape, double>;
};
 
template<typename Dep, typename Indep>
struct DirDer {
  using type = Dep;
};
 
template<typename Indep>
struct DirDer<Matrix<Rotation, Fixed<3>, Fixed<3>, double>, Indep> {
  using type = Vector<Fixed<3>, double>;
};
 
template<typename Sig>
class Function;
 
template<typename Ret, typename Arg>
class Function<Ret(Arg)> : virtual public Atom {
 
  public:
 
    using DRetDArg = typename Der<Ret, Arg>::type;
    using DRetDDir = typename DirDer<Ret, Arg>::type;
    using DDRetDDArg = typename Der<DRetDArg, Arg>::type;
    using RetType = Ret;
    using ArgType = boost::mpl::list<Arg>;
 
    enum { retSize1 = StaticSize<Ret>::size1, retSize2 = StaticSize<Ret>::size2 };
 
    static constexpr int argSize = StaticSize<Arg>::size1;
 
    virtual std::pair<int, int> getRetSize() const { return std::make_pair(retSize1, retSize2); }
 
    virtual int getArgSize() const { return argSize; }
 
    virtual Ret operator()(const Arg &arg)=0;
 
    virtual DRetDArg parDer(const Arg &arg) {
      throw std::runtime_error("parDer must be overloaded by derived class.");
    }
 
    virtual DRetDDir dirDer(const Arg &argDir, const Arg &arg) {
      if constexpr (std::is_same_v<DRetDArg, ErrorType>)
        throw std::runtime_error("dirDer must be overloaded by derived class.");
      else
        return parDer(arg) * argDir;
    }
 
    virtual DDRetDDArg parDerParDer(const Arg &arg) {
      throw std::runtime_error("parDerParDer must be overloaded by derived class.");
    }
 
    virtual DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) {
      if constexpr (std::is_same_v<DDRetDDArg, ErrorType>)
        throw std::runtime_error("parDerDirDer must be overloaded by derived class.");
      else
        return parDerParDer(arg) * argDir;
    }
 
    virtual DRetDDir dirDerDirDer(const Arg &argDir_1, const Arg &argDir_2, const Arg &arg) {
      if constexpr (std::is_same_v<DRetDArg, ErrorType>)
        throw std::runtime_error("dirDerDirDer must be overloaded by derived class.");
      else
        return parDerDirDer(argDir_2, arg) * argDir_1;
    }
 
    //  is constant.
    virtual bool constParDer() const { return false; }
};
 
template<typename Ret, typename Arg1, typename Arg2>
class Function<Ret(Arg1, Arg2)> : virtual public Atom {
 
  public:
    using DRetDArg1 = typename Der<Ret, Arg1>::type;
    using DRetDArg2 = typename Der<Ret, Arg2>::type;
    using DRetDDir1 = typename DirDer<Ret, Arg1>::type;
    using DRetDDir2 = typename DirDer<Ret, Arg2>::type;
    using DDRetDDArg1 = typename Der<DRetDArg1, Arg1>::type;
    using DDRetDDArg2 = typename Der<DRetDArg2, Arg2>::type;
    using DDRetDArg1DArg2 = typename Der<DRetDArg1, Arg2>::type;
    using RetType = Ret;
    using ArgType = boost::mpl::list<Arg1, Arg2>;
 
    enum { retSize1 = StaticSize<Ret>::size1, retSize2 = StaticSize<Ret>::size2 };
 
    static constexpr int arg1Size = StaticSize<Arg1>::size1;
 
    static constexpr int arg2Size = StaticSize<Arg2>::size1;
 
    virtual std::pair<int, int> getRetSize() const { return std::make_pair(retSize1, retSize2); }
 
    virtual int getArg1Size() const { return arg1Size; }
    virtual int getArg2Size() const { return arg2Size; }
 
    virtual Ret operator()(const Arg1 &arg1, const Arg2 &arg2)=0;
 
    virtual DRetDArg1 parDer1(const Arg1 &arg1, const Arg2 &arg2) {
      throw std::runtime_error("parDer1 must be overloaded by derived class.");
    }
 
    virtual DRetDDir1 dirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DRetDArg1, ErrorType>)
        throw std::runtime_error("dirDer1 must be overloaded by derived class.");
      else
        return parDer1(arg1, arg2) * arg1Dir;
    }
 
    virtual DRetDArg2 parDer2(const Arg1 &arg1, const Arg2 &arg2) {
      throw std::runtime_error("parDer2 must be overloaded by derived class.");
    }
 
    virtual DRetDDir2 dirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DRetDArg2, ErrorType>)
        throw std::runtime_error("dirDer2 must be overloaded by derived class.");
      else
        return parDer2(arg1, arg2) * arg2Dir;
    }
 
    virtual DDRetDDArg1 parDer1ParDer1(const Arg1 &arg1, const Arg2 &arg2) {
      throw std::runtime_error("parDer1ParDer1 must be overloaded by derived class.");
    }
 
    virtual DRetDArg1 parDer1DirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDDArg1, ErrorType>)
        throw std::runtime_error("parDer1DirDer1 must be overloaded by derived class.");
      else
        return parDer1ParDer1(arg1, arg2) * arg1Dir;
    }
 
    virtual DRetDDir1 dirDer1DirDer1(const Arg1 &arg1Dir_1, const Arg1 &arg1Dir_2, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDDArg1, ErrorType>)
        throw std::runtime_error("dirDer1DirDer1 must be overloaded by derived class.");
      else
        return parDer1ParDer1(arg1, arg2) * arg1Dir_1 * arg1Dir_2;
    }
 
    virtual DDRetDDArg2 parDer2ParDer2(const Arg1 &arg1, const Arg2 &arg2) {
      throw std::runtime_error("parDer2ParDer2 must be overloaded by derived class.");
    }
 
    virtual DRetDArg2 parDer2DirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDDArg2, ErrorType>)
        throw std::runtime_error("parDer2DirDer2 must be overloaded by derived class.");
      else
        return parDer2ParDer2(arg1, arg2) * arg2Dir;
    }
 
    virtual DRetDDir2 dirDer2DirDer2(const Arg2 &arg2Dir_1, const Arg2 &arg2Dir_2, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDDArg2, ErrorType>)
        throw std::runtime_error("dirDer2DirDer2 must be overloaded by derived class.");
      else
        return parDer2ParDer2(arg1, arg2) * arg2Dir_1 * arg2Dir_2;
    }
 
    virtual DDRetDArg1DArg2 parDer1ParDer2(const Arg1 &arg1, const Arg2 &arg2) {
      throw std::runtime_error("parDer1ParDer2 must be overloaded by derived class.");
    }
 
    virtual DRetDArg1 parDer1DirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDArg1DArg2, ErrorType>)
        throw std::runtime_error("parDer1DirDer2 must be overloaded by derived class.");
      else
        return parDer1ParDer2(arg1, arg2) * arg2Dir;
    }
 
    virtual DRetDDir2 dirDer2DirDer1(const Arg2 &arg2Dir, const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDArg1DArg2, ErrorType>)
        throw std::runtime_error("dirDer2DirDer1 must be overloaded by derived class.");
      else
        return parDer1ParDer2(arg1, arg2) * arg2Dir * arg1Dir;
    }
 
    virtual DRetDArg2 parDer2DirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
      if constexpr (std::is_same_v<DDRetDArg1DArg2, ErrorType>)
        throw std::runtime_error("parDer2DirDer1 must be overloaded by derived class.");
      else
        return parDer1ParDer2(arg1, arg2) * arg1Dir;
    }
 
    //  is constant.
    virtual bool constParDer1() const { return false; }
 
    //  is constant.
    virtual bool constParDer2() const { return false; }
};
 
// A function object with 3, 4, 5, ... arguments
// Not required till now!
 
}
 
#endif