fmatvec  0.0.0
function.h
1 #ifndef _FMATVEC_FUNCTION_H_
2 #define _FMATVEC_FUNCTION_H_
3 
4 #include <fmatvec/fmatvec.h>
5 #include <fmatvec/atom.h>
6 #include <stdexcept>
7 #include <boost/mpl/list.hpp>
8 
9 namespace fmatvec {
10 
14 class ErrorType {
15  public:
16 #if !defined(SWIG) && !defined(MBSIM_COMPILE_SWIG)
17  ErrorType() = delete;
18 #else
19  // SWIG instantiats everything. Hence, we allow this for SWIG but throw a runtime error
20  ErrorType() {
21  throw std::runtime_error("Impossible type.");
22  }
23 #endif
24 };
25 
30 template<typename T>
31 struct StaticSize;
32 
34 template<>
35 struct StaticSize<int> {
36  enum { size1=1, size2=1 };
37 };
38 
40 template<>
41 struct StaticSize<double> {
42  enum { size1=1, size2=1 };
43 };
44 
46 template<typename Shape, typename AT>
47 struct StaticSize<Vector<Shape, AT>> {
48  enum { size1=0, size2=1 };
49 };
50 
52 template<typename Shape, typename AT>
53 struct StaticSize<RowVector<Shape, AT>> {
54  enum { size1=1, size2=0 };
55 };
56 
58 template<int N, typename AT>
59 struct StaticSize<Vector<Fixed<N>, AT>> {
60  enum { size1=N, size2=1 };
61 };
62 
64 template<int N, typename AT>
65 struct StaticSize<RowVector<Fixed<N>, AT>> {
66  enum { size1=1, size2=N };
67 };
68 
70 template<typename Storage, typename RowShape, typename ColShape, typename AT>
71 struct StaticSize<Matrix<Storage, RowShape, ColShape, AT>> {
72  enum { size1=0, size2=0 };
73 };
74 
76 template<typename Storage, int N, int M, typename AT>
77 struct StaticSize<Matrix<Storage, Fixed<N>, Fixed<M>, AT>> {
78  enum { size1=N, size2=M };
79 };
80 
82 template<typename Storage, int N, typename ColShape, typename AT>
83 struct StaticSize<Matrix<Storage, Fixed<N>, ColShape, AT>> {
84  enum { size1=N, size2=0 };
85 };
86 
88 template<typename Storage, typename RowShape, int M, typename AT>
89 struct StaticSize<Matrix<Storage, RowShape, Fixed<M>, AT>> {
90  enum { size1=0, size2=M };
91 };
92 
94 template<int N, typename AT>
95 struct StaticSize<SquareMatrix<Fixed<N>, AT>> {
96  enum { size1=N, size2=N };
97 };
98 
100 template<typename Shape, typename AT>
101 struct StaticSize<SquareMatrix<Shape, AT>> {
102  enum { size1=0, size2=0 };
103 };
104 
108 template<typename Dep, typename Indep>
109 struct Der {
110  using type = ErrorType;
111 };
112 
116 template<typename Dep>
117 struct Der<Dep, double> {
118  using type = Dep;
119 };
120 
126 template<typename IndepVecShape>
127 struct Der<double, Vector<IndepVecShape, double>> {
129 };
130 
136 template<typename DepVecShape, typename IndepVecShape>
137 struct Der<Vector<DepVecShape, double>, Vector<IndepVecShape, double>> {
139 };
140 
151 template<>
152 struct Der<Matrix<Rotation, Fixed<3>, Fixed<3>, double>, double> {
153  using type = Vector<Fixed<3>, double>;
154 };
155 
170 template<typename IndepVecShape>
171 struct Der<Matrix<Rotation, Fixed<3>, Fixed<3>, double>, Vector<IndepVecShape, double>> {
172  using type = Matrix<General, Fixed<3>, IndepVecShape, double>;
173 };
174 
178 template<typename Dep, typename Indep>
179 struct DirDer {
180  using type = Dep;
181 };
182 
186 template<typename Indep>
187 struct DirDer<Matrix<Rotation, Fixed<3>, Fixed<3>, double>, Indep> {
188  using type = Vector<Fixed<3>, double>;
189 };
190 
201 template<typename Sig>
202 class Function;
203 
205 template<typename Ret, typename Arg>
206 class Function<Ret(Arg)> : virtual public Atom {
207 
208  public:
209 
210  using DRetDArg = typename Der<Ret, Arg>::type;
211  using DRetDDir = typename DirDer<Ret, Arg>::type;
212  using DDRetDDArg = typename Der<DRetDArg, Arg>::type;
213  using RetType = Ret;
214  using ArgType = boost::mpl::list<Arg>;
215 
217  enum { retSize1 = StaticSize<Ret>::size1, retSize2 = StaticSize<Ret>::size2 };
218 
220  static constexpr int argSize = StaticSize<Arg>::size1;
221 
223  virtual std::pair<int, int> getRetSize() const { return std::make_pair(retSize1, retSize2); }
224 
226  virtual int getArgSize() const { return argSize; }
227 
229  virtual Ret operator()(const Arg &arg)=0;
230 
232  virtual DRetDArg parDer(const Arg &arg) {
233  throw std::runtime_error("parDer must be overloaded by derived class.");
234  }
235 
237  virtual DRetDDir dirDer(const Arg &argDir, const Arg &arg) {
238  if constexpr (std::is_same_v<DRetDArg, ErrorType>)
239  throw std::runtime_error("dirDer must be overloaded by derived class.");
240  else
241  return parDer(arg) * argDir;
242  }
243 
245  virtual DDRetDDArg parDerParDer(const Arg &arg) {
246  throw std::runtime_error("parDerParDer must be overloaded by derived class.");
247  }
248 
250  virtual DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg) {
251  if constexpr (std::is_same_v<DDRetDDArg, ErrorType>)
252  throw std::runtime_error("parDerDirDer must be overloaded by derived class.");
253  else
254  return parDerParDer(arg) * argDir;
255  }
256 
258  virtual DRetDDir dirDerDirDer(const Arg &argDir_1, const Arg &argDir_2, const Arg &arg) {
259  if constexpr (std::is_same_v<DRetDArg, ErrorType>)
260  throw std::runtime_error("dirDerDirDer must be overloaded by derived class.");
261  else
262  return parDerDirDer(argDir_2, arg) * argDir_1;
263  }
264 
266  // is constant.
267  virtual bool constParDer() const { return false; }
268 };
269 
271 template<typename Ret, typename Arg1, typename Arg2>
272 class Function<Ret(Arg1, Arg2)> : virtual public Atom {
273 
274  public:
275  using DRetDArg1 = typename Der<Ret, Arg1>::type;
276  using DRetDArg2 = typename Der<Ret, Arg2>::type;
277  using DRetDDir1 = typename DirDer<Ret, Arg1>::type;
278  using DRetDDir2 = typename DirDer<Ret, Arg2>::type;
279  using DDRetDDArg1 = typename Der<DRetDArg1, Arg1>::type;
280  using DDRetDDArg2 = typename Der<DRetDArg2, Arg2>::type;
281  using DDRetDArg1DArg2 = typename Der<DRetDArg1, Arg2>::type;
282  using RetType = Ret;
283  using ArgType = boost::mpl::list<Arg1, Arg2>;
284 
286  enum { retSize1 = StaticSize<Ret>::size1, retSize2 = StaticSize<Ret>::size2 };
287 
289  static constexpr int arg1Size = StaticSize<Arg1>::size1;
290 
292  static constexpr int arg2Size = StaticSize<Arg2>::size1;
293 
295  virtual std::pair<int, int> getRetSize() const { return std::make_pair(retSize1, retSize2); }
296 
298  virtual int getArg1Size() const { return arg1Size; }
300  virtual int getArg2Size() const { return arg2Size; }
301 
303  virtual Ret operator()(const Arg1 &arg1, const Arg2 &arg2)=0;
304 
306  virtual DRetDArg1 parDer1(const Arg1 &arg1, const Arg2 &arg2) {
307  throw std::runtime_error("parDer1 must be overloaded by derived class.");
308  }
309 
311  virtual DRetDDir1 dirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
312  if constexpr (std::is_same_v<DRetDArg1, ErrorType>)
313  throw std::runtime_error("dirDer1 must be overloaded by derived class.");
314  else
315  return parDer1(arg1, arg2) * arg1Dir;
316  }
317 
319  virtual DRetDArg2 parDer2(const Arg1 &arg1, const Arg2 &arg2) {
320  throw std::runtime_error("parDer2 must be overloaded by derived class.");
321  }
322 
324  virtual DRetDDir2 dirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) {
325  if constexpr (std::is_same_v<DRetDArg2, ErrorType>)
326  throw std::runtime_error("dirDer2 must be overloaded by derived class.");
327  else
328  return parDer2(arg1, arg2) * arg2Dir;
329  }
330 
332  virtual DDRetDDArg1 parDer1ParDer1(const Arg1 &arg1, const Arg2 &arg2) {
333  throw std::runtime_error("parDer1ParDer1 must be overloaded by derived class.");
334  }
335 
337  virtual DRetDArg1 parDer1DirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
338  if constexpr (std::is_same_v<DDRetDDArg1, ErrorType>)
339  throw std::runtime_error("parDer1DirDer1 must be overloaded by derived class.");
340  else
341  return parDer1ParDer1(arg1, arg2) * arg1Dir;
342  }
343 
345  virtual DRetDDir1 dirDer1DirDer1(const Arg1 &arg1Dir_1, const Arg1 &arg1Dir_2, const Arg1 &arg1, const Arg2 &arg2) {
346  if constexpr (std::is_same_v<DDRetDDArg1, ErrorType>)
347  throw std::runtime_error("dirDer1DirDer1 must be overloaded by derived class.");
348  else
349  return parDer1ParDer1(arg1, arg2) * arg1Dir_1 * arg1Dir_2;
350  }
351 
353  virtual DDRetDDArg2 parDer2ParDer2(const Arg1 &arg1, const Arg2 &arg2) {
354  throw std::runtime_error("parDer2ParDer2 must be overloaded by derived class.");
355  }
356 
358  virtual DRetDArg2 parDer2DirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) {
359  if constexpr (std::is_same_v<DDRetDDArg2, ErrorType>)
360  throw std::runtime_error("parDer2DirDer2 must be overloaded by derived class.");
361  else
362  return parDer2ParDer2(arg1, arg2) * arg2Dir;
363  }
364 
366  virtual DRetDDir2 dirDer2DirDer2(const Arg2 &arg2Dir_1, const Arg2 &arg2Dir_2, const Arg1 &arg1, const Arg2 &arg2) {
367  if constexpr (std::is_same_v<DDRetDDArg2, ErrorType>)
368  throw std::runtime_error("dirDer2DirDer2 must be overloaded by derived class.");
369  else
370  return parDer2ParDer2(arg1, arg2) * arg2Dir_1 * arg2Dir_2;
371  }
372 
374  virtual DDRetDArg1DArg2 parDer1ParDer2(const Arg1 &arg1, const Arg2 &arg2) {
375  throw std::runtime_error("parDer1ParDer2 must be overloaded by derived class.");
376  }
377 
379  virtual DRetDArg1 parDer1DirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) {
380  if constexpr (std::is_same_v<DDRetDArg1DArg2, ErrorType>)
381  throw std::runtime_error("parDer1DirDer2 must be overloaded by derived class.");
382  else
383  return parDer1ParDer2(arg1, arg2) * arg2Dir;
384  }
385 
387  virtual DRetDDir2 dirDer2DirDer1(const Arg2 &arg2Dir, const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
388  if constexpr (std::is_same_v<DDRetDArg1DArg2, ErrorType>)
389  throw std::runtime_error("dirDer2DirDer1 must be overloaded by derived class.");
390  else
391  return parDer1ParDer2(arg1, arg2) * arg2Dir * arg1Dir;
392  }
393 
395  virtual DRetDArg2 parDer2DirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) {
396  if constexpr (std::is_same_v<DDRetDArg1DArg2, ErrorType>)
397  throw std::runtime_error("parDer2DirDer1 must be overloaded by derived class.");
398  else
399  return parDer1ParDer2(arg1, arg2) * arg1Dir;
400  }
401 
403  // is constant.
404  virtual bool constParDer1() const { return false; }
405 
407  // is constant.
408  virtual bool constParDer2() const { return false; }
409 };
410 
411 // A function object with 3, 4, 5, ... arguments
412 // Not required till now!
413 
414 }
415 
416 #endif
virtual DDRetDArg1DArg2 parDer1ParDer2(const Arg1 &arg1, const Arg2 &arg2)
Second mixed derivative: partial derivative of parDer1 with respect to the second argument.
Definition: function.h:374
virtual DRetDDir2 dirDer2DirDer2(const Arg2 &arg2Dir_1, const Arg2 &arg2Dir_2, const Arg1 &arg1, const Arg2 &arg2)
Second derivative: directional derivative of dirDer2 with respect to the first argument.
Definition: function.h:366
Definition: fmatvec.h:39
virtual std::pair< int, int > getRetSize() const
Return the size of the return value: =0 == unknown size.
Definition: function.h:223
virtual DDRetDDArg2 parDer2ParDer2(const Arg1 &arg1, const Arg2 &arg2)
Second derivative: partial derivative of parDer2 with respect to the first argument.
Definition: function.h:353
virtual DRetDArg parDer(const Arg &arg)
First derivative: partial derivative of the function value with respect to the argument.
Definition: function.h:232
This is the basic matrix class for arbitrary matrices.
Definition: fmatvec.h:42
virtual DRetDDir1 dirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2)
First derivative: directional derivative of the function value with respect to the first argument.
Definition: function.h:311
virtual bool constParDer() const
Returns true, if the partial derivative of the function value with respect to the argument.
Definition: function.h:267
virtual DRetDArg1 parDer1DirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2)
Second derivative: directional derivative of parDer1 with respect to the first argument.
Definition: function.h:337
Definition: atom.h:23
virtual DRetDDir2 dirDer2DirDer1(const Arg2 &arg2Dir, const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2)
Second mixed derivative: directional derivative of dirDer2 with respect to the first argument.
Definition: function.h:387
Definition: fmatvec.h:51
virtual DRetDDir dirDer(const Arg &argDir, const Arg &arg)
First derivative: directional derivative of the function value with respect to the argument.
Definition: function.h:237
Shape class for rotation matrices.
Definition: types.h:136
Definition: function.h:202
Definition: function.h:14
Definition: function.h:109
virtual int getArgSize() const
Return the size of the argument: =1 == scalar; >1 == vector; =0 == unknown vector size.
Definition: function.h:226
virtual DRetDDir dirDerDirDer(const Arg &argDir_1, const Arg &argDir_2, const Arg &arg)
Second derivative: directional derivative of dirDer with respect to the argument.
Definition: function.h:258
virtual DRetDDir1 dirDer1DirDer1(const Arg1 &arg1Dir_1, const Arg1 &arg1Dir_2, const Arg1 &arg1, const Arg2 &arg2)
Second derivative: directional derivative of dirDer1 with respect to the first argument.
Definition: function.h:345
virtual int getArg2Size() const
Return the size of the second argument: =1 == scalar; >1 == vector; =0 == unknown vector size.
Definition: function.h:300
Definition: fmatvec.h:45
Definition: fmatvec.h:48
virtual DRetDArg1 parDer1DirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2)
Second mixed derivative: directional derivative of parDer1 with respect to the second argument.
Definition: function.h:379
virtual DRetDArg2 parDer2DirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2)
Second derivative: directional derivative of parDer2 with respect to the first argument.
Definition: function.h:358
virtual int getArg1Size() const
Return the size of the first argument: =1 == scalar; >1 == vector; =0 == unknown vector size.
Definition: function.h:298
Namespace fmatvec.
Definition: _memory.cc:28
virtual std::pair< int, int > getRetSize() const
Return the size of the return value: =0 == unknown size.
Definition: function.h:295
virtual DDRetDDArg1 parDer1ParDer1(const Arg1 &arg1, const Arg2 &arg2)
Second derivative: partial derivative of parDer1 with respect to the first argument.
Definition: function.h:332
Definition: function.h:31
virtual bool constParDer1() const
Returns true, if the partial derivative of the function value with respect to the first argument.
Definition: function.h:404
virtual DRetDArg parDerDirDer(const Arg &argDir, const Arg &arg)
Second derivative: directional derivative of parDer with respect to the argument.
Definition: function.h:250
virtual DRetDArg2 parDer2(const Arg1 &arg1, const Arg2 &arg2)
First derivative: partial derivative of the function value with respect to the second argument.
Definition: function.h:319
virtual DRetDDir2 dirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2)
First derivative: directional derivative of the function value with respect to the second argument.
Definition: function.h:324
virtual DDRetDDArg parDerParDer(const Arg &arg)
Second derivative: partial derivative of parDer with respect to the argument.
Definition: function.h:245
virtual bool constParDer2() const
Returns true, if the partial derivative of the function value with respect to the second argument.
Definition: function.h:408
virtual DRetDArg1 parDer1(const Arg1 &arg1, const Arg2 &arg2)
First derivative: partial derivative of the function value with respect to the first argument.
Definition: function.h:306
Definition: function.h:179
SymbolicExpression parDer(const SymbolicExpression &dep, const IndependentVariable &indep)
Definition: ast.cc:286
virtual DRetDArg2 parDer2DirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2)
Second mixed derivative: partial derivative of dirDer1 with respect to the second argument.
Definition: function.h:395