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