fmatvec  0.0.0
symbolic.h
1 #ifndef _fmatvec_symbolic_h_
2 #define _fmatvec_symbolic_h_
3 
4 #include "ast.h"
5 #include <set>
6 
7 namespace fmatvec {
8 
9 // definitions of matrix/vector operations/functions only usefull for symbolic calculations
10 // like building partial derivatives, evaluation of symbolic expressions to double values, ...
11 
12 template<class Dep, class ATIndep>
13 Dep parDer(const Dep &dep, const ATIndep &indep) {
14  Dep ret;
15  if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
16  }
17  else if constexpr (Dep::isVector)
18  ret.resize(dep.size());
19  else
20  ret.resize(dep.rows(), dep.cols());
21  auto d=dep.begin();
22  auto r=ret.begin();
23  for(; d!=dep.end(); ++d, ++r)
24  *r=parDer(*d, indep);
25  return ret;
26 }
27 
28 template<class ATDep, class ATIndep>
29 Vector<Fixed<3>, ATDep> parDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const ATIndep &x) {
30  Matrix<General, Fixed<3>, Fixed<3>, ATDep> Rs;
31  for(int r=0; r<3; ++r)
32  for(int c=0; c<3; ++c)
33  Rs(r,c)=parDer(R(r,c), x);
34  auto retTilde=Rs*trans(R);
35  Vector<Fixed<3>, ATDep> ret;
36  ret(0)=retTilde(2,1);
37  ret(1)=retTilde(0,2);
38  ret(2)=retTilde(1,0);
39  return ret;
40 }
41 
42 template<class DepShape, class IndepShape, class ATDep, class ATIndep>
43 Matrix<General, DepShape, IndepShape, ATDep> parDer(const Vector<DepShape, ATDep> &dep, const Vector<IndepShape, ATIndep> &indep) {
44  Matrix<General, DepShape, IndepShape, ATDep> ret(dep.size(), indep.size());
45  for(int r=0; r<dep.size(); ++r)
46  for(int c=0; c<indep.size(); ++c)
47  ret(r,c)=parDer(dep(r), indep(c));
48  return ret;
49 }
50 
51 template<class IndepShape, class ATDep, class ATIndep>
52 RowVector<IndepShape, ATDep> parDer(const ATDep &dep, const Vector<IndepShape, ATIndep> &indep) {
53  RowVector<IndepShape, ATDep> ret(indep.size());
54  for(int r=0; r<indep.size(); ++r)
55  ret(r)=parDer(dep, indep(r));
56  return ret;
57 }
58 
59 template<class ATDep, class Shape, class ATIndep>
60 Matrix<General, Fixed<3>, Shape, ATDep> parDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const Vector<Shape, ATIndep> &x) {
61  Matrix<General, Fixed<3>, Shape, ATDep> ret(3,x.size());
62  for(int i=0; i<x.size(); ++i)
63  ret.set(i, parDer(R, x(i)));
64  return ret;
65 }
66 
67 template<class Dep, class ATDep, class ATIndep>
68 Dep dirDer(const Dep &dep, const ATDep &indepdir, const ATIndep &indep) {
69  return parDer(dep, indep)*indepdir;
70 }
71 
72 template<class ATDep, class ATIndep>
73 Vector<Fixed<3>, ATDep> dirDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const ATDep &indepdir, const ATIndep &indep) {
74  return parDer(R, indep)*indepdir;
75 }
76 
77 template<class Dep, class ShapeIndep, class ATDep, class ATIndep>
78 Dep dirDer(const Dep &dep, const Vector<ShapeIndep, ATDep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
79  Dep ret;
80  if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
81  ret=0.0;
82  }
83  else if constexpr (Dep::isVector)
84  ret<<=Dep(dep.size(), INIT, 0.0);
85  else
86  ret<<=Dep(dep.rows(), dep.cols(), INIT, 0.0);
87  for(int i=0; i<indep.size(); ++i)
88  ret+=parDer(dep, indep(i))*indepdir(i);
89  return ret;
90 }
91 
92 template<class ATDep, class ShapeIndep, class ATIndep>
93 Vector<Fixed<3>, ATDep> dirDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &dep, const Vector<ShapeIndep, ATDep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
94  return parDer(dep, indep)*indepdir;
95 }
96 
97 // substitute scalar in scalar expression -> defined in ast.h
98 template<class Ret, class A, class B>
99 Ret subst(const Ret &src, const A &a, const B &b) {
100  if constexpr (!std::is_same_v<A, IndependentVariable>) {
101  if constexpr (A::isVector) {
102  if(a.size()!=b.size())
103  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
104  }
105  else {
106  if(a.rows()!=b.rows() || a.cols()!=b.cols())
107  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
108  }
109  }
110 
111  if constexpr (std::is_same_v<Ret, SymbolicExpression> && std::is_same_v<A, IndependentVariable>) {
112  return AST::substScalar(src, a, b);
113  }
114  if constexpr (std::is_same_v<Ret, SymbolicExpression> && !std::is_same_v<A, IndependentVariable>) {
115  auto ret=src;
116  auto aIt=a.begin();
117  auto bIt=b.begin();
118  for(; aIt<a.end(); ++aIt, ++bIt)
119  ret=subst(ret, *aIt, *bIt);
120  return ret;
121  }
122  if constexpr (!std::is_same_v<Ret, SymbolicExpression> && std::is_same_v<A, IndependentVariable>) {
123  auto ret=src;
124  for(auto retIt=ret.begin(); retIt<ret.end(); ++retIt)
125  *retIt=subst(*retIt, a, b);
126  return ret;
127  }
128  if constexpr (!std::is_same_v<Ret, SymbolicExpression> && !std::is_same_v<A, IndependentVariable>) {
129  auto ret=src;
130  for(auto retIt=ret.begin(); retIt<ret.end(); ++retIt) {
131  auto aIt=a.begin();
132  auto bIt=b.begin();
133  for(; aIt<a.end(); ++aIt, ++bIt)
134  *retIt=subst(*retIt, *aIt, *bIt);
135  }
136  return ret;
137  }
138 }
139 
140 
141 
142 template<class Dst, class Src>
143 Dst& operator^=(Dst &dst, const Src &src) {
144  auto dstIt=dst.begin();
145  auto srcIt=src.begin();
146  for(; dstIt<dst.end(); ++dstIt, ++srcIt)
147  *dstIt ^= *srcIt;
148  return dst;
149 }
150 
151 
152 
153 /* Class for evaluating a symbolic expression
154  * This class is not copy/move-able to since it uses ByteCode which itself uses internal raw pointers (for
155  * performance reasons) which cannot be copied/moved.
156 */
157 template<class... Arg>
158 class Eval {
159  private:
160  // Type for all symbolic arguments as a tuple
161  using SymTuple = std::tuple<Arg...>;
162  // Type for all numeric return values as a tuple
163  using NumTuple = std::tuple<typename ReplaceAT<Arg, double>::Type...>;
164  // The return type: equals NumTuple if more than 1 arg is given in the ctor; for only 1 arg the tuple is skipped
165  using NumRetType = std::conditional_t<std::tuple_size_v<NumTuple> == 1, std::tuple_element_t<0,NumTuple>, NumTuple>;
166  public:
167  ~Eval();
168  Eval(const Eval &src) = delete;
169  Eval(Eval &&src) = delete;
170  Eval<Arg...>& operator=(const Eval &src) = delete;
171  Eval<Arg...>& operator=(Eval &&src) = delete;
172 
173  // construct an evaluation object for all symbolic args
174  // An arg can be a symbolic scalar, vector or matrix
175  Eval(const Arg&... arg);
176  // evaluate all symbolic args given by the ctor and return a tuple of corrsponding evaluated numeric values.
177  // Note that the return values can be get easily using "structured binding".
178  inline const NumRetType& operator()() const;
179  private:
180  // the numeric values as a tuple
181  NumTuple numTuple;
182  // hold a reference to Symbols used by the evaluation to avoid deleting these symbols (since the code has pointers to these)
183  std::set<std::shared_ptr<const AST::Symbol>> symbolStore;
184  // helper function to walk over all SymbolicExpression (AT=atomic type) and corresponding double value in parallel.
185  // For each pair the function is called.
186  template<int I=0>
187  void walkAT(const SymTuple &symTuple, NumTuple &numTuple,
188  const std::function<void(const SymbolicExpression&, double&)> &func);
189 
190  // members for bytecode evaluation
191 
192  std::vector<AST::ByteCode> byteCode;
193 
194  // the constructor and operator() for runtime evaluation
195  void ctorByteCode(const Arg&... arg);
196  inline void callByteCode() const;
197 };
198 
199 template<class... Arg>
200 Eval<Arg...>::~Eval() = default;
201 
202 template<class... Arg>
203 Eval<Arg...>::Eval(const Arg&... arg) {
204  ctorByteCode(arg...);
205 }
206 
207 template<class... Arg>
208 auto Eval<Arg...>::operator()() const -> const NumRetType& {
209  callByteCode();
210  // return the numeric values: the above call has written to its addresses
211  if constexpr (std::tuple_size_v<NumTuple> == 1)
212  return std::get<0>(numTuple);
213  else
214  return numTuple;
215 }
216 
217 template<class... Arg>
218 template<int I>
219 void Eval<Arg...>::walkAT(const SymTuple &symTuple, NumTuple &numTuple,
220  const std::function<void(const SymbolicExpression&, double&)> &func) {
221  // get the I-th symbolic and numeric variable form the tuples (this function handles the I-th tuple entry)
222  // (this function is called with I=0 from the external caller)
223  auto &sym = std::get<I>(symTuple);
224  auto &num = std::get<I>(numTuple);
225  // get the type of the I-th tuple entry
226  using Sym = std::tuple_element_t<I,SymTuple>;
227 
228  if constexpr (std::is_same_v<Sym, SymbolicExpression>)
229  func(sym, num);
230  else {
231  // fill bytecode for vector or matrix value
232  // resize the vector or matrix
233  if constexpr (Sym::isVector)
234  num.resize(sym.size());
235  else
236  num.resize(sym.rows(), sym.cols());
237  // get begin iterator of the vector/matrix and iterator both iterator simultaniously
238  auto symIt=sym.begin();
239  auto numIt=num.begin();
240  for(; symIt!=sym.end(); ++symIt, ++numIt)
241  func(*symIt, *numIt);
242  }
243 
244  // call this function recursively for the next (I+1)-th entry in the tuple (only if there is a next entry)
245  if constexpr (I+1<std::tuple_size_v<SymTuple>)
246  walkAT<I+1>(symTuple, numTuple, func);
247 }
248 
249 template<class... Arg>
250 void Eval<Arg...>::ctorByteCode(const Arg&... arg) {
251  // first walk through all vertices in all args and count the number of operations needed
252  size_t byteCodeCount=0;
253  std::set<const AST::Vertex*> existingVertex1;
254  walkAT(SymTuple(arg...), numTuple, [&byteCodeCount, &existingVertex1](auto &sym, auto &num) {
255  sym->walkVertex([&byteCodeCount, &existingVertex1](const std::shared_ptr<const AST::Vertex>& v) {
256  if(!existingVertex1.insert(v.get()).second) return;
257 
258  if(auto s=std::dynamic_pointer_cast<const AST::Symbol>(v); s)
259  byteCodeCount++;
260  byteCodeCount++;
261  });
262  byteCodeCount++;
263  });
264  // pre-allocate byteCode (ByteCode has not move-ctor)
265  byteCode.reserve(byteCodeCount);
266  std::map<const AST::Vertex*, std::vector<AST::ByteCode>::iterator> existingVertex;
267  // than save all symbols in the symbolStore (the code uses a pointer to Symbol::x)
268  // and add code to copy from &Symbol::x to byteCode. This copy is a "slow" operation since &Symbol::x may be far away.
269  walkAT(SymTuple(arg...), numTuple, [this, &existingVertex](auto &sym, auto &num) {
270  sym->walkVertex([this, &existingVertex](const std::shared_ptr<const AST::Vertex>& v) {
271  if(auto s=std::dynamic_pointer_cast<const AST::Symbol>(v); s) {
272  symbolStore.insert(s);
273  s->dumpByteCode(byteCode, existingVertex);
274  }
275  });
276  });
277  // now add the code of the symbolic expression: this is a "fast" operation since only addresses inside of byteCode are used
278  // Also store a interator to each AT (atomic type) result of the symbolic expression
279  std::vector< std::vector<AST::ByteCode>::iterator > exprRet;
280  walkAT(SymTuple(arg...), numTuple, [this, &existingVertex, &exprRet](auto &sym, auto &num) {
281  exprRet.emplace_back(sym->dumpByteCode(byteCode, existingVertex));
282  });
283  // lastly add code to copy the result of each AT (the iterators from above) to the address of the return value.
284  // This is again a "slow" operation since the address of the return value may be far away fromo byteCode.
285  auto exprRetIt = exprRet.begin();
286  walkAT(SymTuple(arg...), numTuple, [this, &exprRet, &exprRetIt](auto &sym, auto &num) {
287  byteCode.emplace_back();
288  auto it = --byteCode.end();
289  it->func = [](double* r, const std::array<double*,AST::ByteCode::N>& a) { *r = *a[0]; };
290  it->argsPtr = { (*(exprRetIt++))->retPtr };
291  it->retPtr = &num;
292  });
293 }
294 
295 template<class... Arg>
296 void Eval<Arg...>::callByteCode() const {
297 #if !defined(NDEBUG) && !defined(SWIG)
298  SymbolicExpression::evalOperationsCount = byteCode.size();
299 #endif
300  std::for_each(byteCode.begin(), byteCode.end(), [](const AST::ByteCode &bc){
301  bc.func(bc.retPtr, bc.argsPtr);
302  });
303 }
304 
305 }
306 
307 #endif
RowVector< Row, AT > trans(const Vector< Row, AT > &x)
Transpose of a vector.
Definition: linear_algebra.h:1716
Namespace fmatvec.
Definition: _memory.cc:28
SymbolicExpression parDer(const SymbolicExpression &dep, const IndependentVariable &indep)
Definition: ast.cc:286