fmatvec  0.0.0
Fast Matrix/Vector Library
symbolic.h
1 #ifndef _fmatvec_symbolic_h_
2 #define _fmatvec_symbolic_h_
3 
4 #include "types.h"
5 #include "ast.h"
6 #include "function.h"
7 #include <type_traits>
8 
9 namespace fmatvec {
10 
11 // definitions of matrix/vector operations/functions only usefull for symbolic calculations
12 // like building partial derivatives, evaluation of symbolic expressions to double values, ...
13 
14 template<class Dep, class ATIndep>
15 Dep parDer(const Dep &dep, const ATIndep &indep) {
16  Dep ret;
17  if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
18  }
19  else if constexpr (Dep::isVector)
20  ret.resize(dep.size());
21  else
22  ret.resize(dep.rows(), dep.cols());
23  auto d=dep.begin();
24  auto r=ret.begin();
25  for(; d!=dep.end(); ++d, ++r)
26  *r=parDer(*d, indep);
27  return ret;
28 }
29 
30 template<class ATDep, class ATIndep>
31 Vector<Fixed<3>, ATDep> parDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const ATIndep &x) {
32  Matrix<General, Fixed<3>, Fixed<3>, ATDep> Rs;
33  for(int r=0; r<3; ++r)
34  for(int c=0; c<3; ++c)
35  Rs(r,c)=parDer(R(r,c), x);
36  auto retTilde=Rs*trans(R);
37  Vector<Fixed<3>, ATDep> ret;
38  ret(0)=retTilde(2,1);
39  ret(1)=retTilde(0,2);
40  ret(2)=retTilde(1,0);
41  return ret;
42 }
43 
44 template<class DepShape, class IndepShape, class ATDep, class ATIndep>
45 Matrix<General, DepShape, IndepShape, ATDep> parDer(const Vector<DepShape, ATDep> &dep, const Vector<IndepShape, ATIndep> &indep) {
46  Matrix<General, DepShape, IndepShape, ATDep> ret(dep.size(), indep.size());
47  for(int r=0; r<dep.size(); ++r)
48  for(int c=0; c<indep.size(); ++c)
49  ret(r,c)=parDer(dep(r), indep(c));
50  return ret;
51 }
52 
53 template<class IndepShape, class ATDep, class ATIndep>
54 RowVector<IndepShape, ATDep> parDer(const ATDep &dep, const Vector<IndepShape, ATIndep> &indep) {
55  RowVector<IndepShape, ATDep> ret(indep.size());
56  for(int r=0; r<indep.size(); ++r)
57  ret(r)=parDer(dep, indep(r));
58  return ret;
59 }
60 
61 template<class ATDep, class Shape, class ATIndep>
62 Matrix<General, Fixed<3>, Fixed<3>, ATDep> parDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const Vector<Shape, ATIndep> &x) {
63  Matrix<General, Fixed<3>, Shape, ATDep> ret;
64  for(int i=0; i<x.size(); ++i)
65  ret.set(i, parDer(R, x(i)));
66  return ret;
67 }
68 
69 template<class Dep, class ATDep, class ATIndep>
70 Dep dirDer(const Dep &dep, const ATDep &indepdir, const ATIndep &indep) {
71  return parDer(dep, indep)*indepdir;
72 }
73 
74 template<class ATDep, class ATIndep>
75 Vector<Fixed<3>, ATDep> dirDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const ATDep &indepdir, const ATIndep &indep) {
76  return parDer(R, indep)*indepdir;
77 }
78 
79 template<class Dep, class ShapeIndep, class ATDep, class ATIndep>
80 Dep dirDer(const Dep &dep, const Vector<ShapeIndep, ATDep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
81  Dep ret;
82  if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
83  ret=0.0;
84  }
85  else if constexpr (Dep::isVector)
86  ret<<=Dep(dep.size(), INIT, 0.0);
87  else
88  ret<<=Dep(dep.rows(), dep.cols(), INIT, 0.0);
89  for(int i=0; i<indep.size(); ++i)
90  ret+=parDer(dep, indep(i))*indepdir(i);
91  return ret;
92 }
93 
94 template<class ATDep, class ShapeIndep, class ATIndep>
95 Vector<Fixed<3>, ATDep> dirDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &dep, const Vector<ShapeIndep, ATDep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
96  return parDer(dep, indep)*indepdir;
97 }
98 
99 template<class Sym>
100 typename ReplaceAT<Sym, double>::Type eval(const Sym &sym) {
101  typename ReplaceAT<Sym, double>::Type num;
102  if constexpr (Sym::isVector)
103  num.resize(sym.size());
104  else
105  num.resize(sym.rows(), sym.cols());
106  auto s=sym.begin();
107  auto n=num.begin();
108  for(; s!=sym.end(); ++s, ++n)
109  *n=eval(*s);
110  return num;
111 }
112 
113 // substitute scalar in scalar expression -> defined in ast.h
114 
115 // substitute vector in scalar expression
116 template<class DepR, class ShapeA, class IndepA, class ShapeB, class DepB>
117 DepR subst(const DepR &se, const Vector<ShapeA, IndepA>& a, const Vector<ShapeB, DepB> &b) {
118  if(a.size()!=b.size())
119  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
120  auto ret=se;
121  for(int i=0; i<a.size(); ++i)
122  ret=subst(ret, a(i), b(i));
123  return ret;
124 }
125 
126 // substitute matrix in scalar expression
127 template<class DepR, class TypeA, class RowA, class ColA, class IndepA, class TypeB, class RowB, class ColB, class DepB>
128 DepR subst(const DepR &se, const Matrix<TypeA, RowA, ColA, IndepA>& A, const Matrix<TypeB, RowB, ColB, DepB> &B) {
129  if(A.rows()!=B.rows() || A.cols()!=B.cols())
130  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
131  auto ret=se;
132  for(int r=0; r<A.rows(); ++r)
133  for(int c=0; c<A.cols(); ++c)
134  ret=subst(ret, A(r,c), B(r,c));
135  return ret;
136 }
137 
138 // substitute scalar in vector expression
139 template<class ShapeR, class DepR, class IndepA, class DepB>
140 Vector<ShapeR, DepR> subst(const Vector<ShapeR, DepR> &se, const IndepA& a, const DepB &b) {
141  Vector<ShapeR, DepR> ret(se.size());
142  for(int i=0; i<se.size(); ++i)
143  ret(i)=subst(se(i), a, b);
144  return ret;
145 }
146 
147 // substitute vector in vector expression
148 template<class ShapeR, class DepR, class ShapeA, class IndepA, class ShapeB, class DepB>
149 Vector<ShapeR, DepR> subst(const Vector<ShapeR, DepR> &se, const Vector<ShapeA, IndepA>& a, const Vector<ShapeB, DepB> &b) {
150  if(a.size()!=b.size())
151  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
152  auto ret=se;
153  for(int i=0; i<a.size(); ++i)
154  ret=subst(ret, a(i), b(i));
155  return ret;
156 }
157 
158 // substitute matrix in vector expression
159 template<class ShapeR, class DepR, class TypeA, class RowA, class ColA, class IndepA, class TypeB, class RowB, class ColB, class DepB>
160 Vector<ShapeR, DepR> subst(const Vector<ShapeR, DepR> &se, const Matrix<TypeA, RowA, ColA, IndepA>& A, const Matrix<TypeB, RowB, ColB, DepB> &B) {
161  if(A.rows()!=B.rows() || A.cols()!=B.cols())
162  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
163  auto ret=se;
164  for(int r=0; r<A.rows(); ++r)
165  for(int c=0; c<A.cols(); ++c)
166  ret=subst(ret, A(r,c), B(r,c));
167  return ret;
168 }
169 
170 // substitute scalar in matrix expression
171 template<class TypeR, class RowR, class ColR, class DepR, class IndepA, class DepB>
172 Matrix<TypeR, RowR, ColR, DepR> subst(const Matrix<TypeR, RowR, ColR, DepR> &SE, const IndepA& a, const DepB &b) {
173  Matrix<TypeR, RowR, ColR, DepR> ret(SE.rows(), SE.cols());
174  for(int r=0; r<SE.rows(); ++r)
175  for(int c=0; c<SE.cols(); ++c)
176  ret(r,c)=subst(SE(r,c), a, b);
177  return ret;
178 }
179 
180 // substitute vector in matrix expression
181 template<class TypeR, class RowR, class ColR, class DepR, class ShapeA, class IndepA, class ShapeB, class DepB>
182 Matrix<TypeR, RowR, ColR, DepR> subst(const Matrix<TypeR, RowR, ColR, DepR> &SE, const Vector<ShapeA, IndepA>& a, const Vector<ShapeB, DepB> &b) {
183  if(a.size()!=b.size())
184  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
185  auto ret=SE;
186  for(int i=0; i<a.size(); ++i)
187  ret=subst(ret, a(i), b(i));
188  return ret;
189 }
190 
191 // substitute matrix in matrix expression
192 template<class TypeR, class RowR, class ColR, class DepR, class TypeA, class RowA, class ColA, class IndepA, class TypeB, class RowB, class ColB, class DepB>
193 Matrix<TypeR, RowR, ColR, DepR> subst(const Matrix<TypeR, RowR, ColR, DepR> &SE, const Matrix<TypeA, RowA, ColA, IndepA>& A, const Matrix<TypeB, RowB, ColB, DepB> &B) {
194  if(A.rows()!=B.rows() || A.cols()!=B.cols())
195  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
196  auto ret=SE;
197  for(int r=0; r<A.rows(); ++r)
198  for(int c=0; c<A.cols(); ++c)
199  ret=subst(ret, A(r,c), B(r,c));
200  return ret;
201 }
202 
203 
204 
205 template<class ShapeDst, class ShapeSrc, class ATIndep>
206 Vector<ShapeDst, ATIndep>& operator^=(Vector<ShapeDst, ATIndep> &dst, const Vector<ShapeSrc, double> &src) {
207  FMATVEC_ASSERT(dst.size()==src.size(), ATIndep);
208  for(int i=0; i<dst.size(); ++i)
209  dst(i)^=src(i);
210  return dst;
211 }
212 
213 template<int N, class ATIndep>
214 Vector<Fixed<N>, ATIndep>& operator^=(Vector<Fixed<N>, ATIndep> &dst, const Vector<Fixed<N>, double> &src) {
215  for(int i=0; i<N; ++i)
216  dst(i)^=src(i);
217  return dst;
218 }
219 
220 template<int N, class ShapeSrc, class ATIndep>
221 Vector<Fixed<N>, ATIndep>& operator^=(Vector<Fixed<N>, ATIndep> &dst, const Vector<ShapeSrc, double> &src) {
222  FMATVEC_ASSERT(N==src.size(), ATIndep);
223  for(int i=0; i<N; ++i)
224  dst(i)^=src(i);
225  return dst;
226 }
227 
228 template<class ShapeDst, int N, class ATIndep>
229 Vector<ShapeDst, ATIndep>& operator^=(Vector<ShapeDst, ATIndep> &dst, const Vector<Fixed<N>, double> &src) {
230  FMATVEC_ASSERT(N==src.size(), ATIndep);
231  for(int i=0; i<N; ++i)
232  dst(i)^=src(i);
233  return dst;
234 }
235 
236 template<class TypeDst, class TypeSrc, class ShapeRowDst, class ShapeRowSrc, class ShapeColDst, class ShapeColSrc, class ATIndep>
237 Matrix<TypeDst, ShapeRowDst, ShapeColDst, ATIndep>& operator^=(Matrix<TypeDst, ShapeRowDst, ShapeColDst, ATIndep> &dst,
238  const Matrix<TypeSrc, ShapeRowSrc, ShapeColSrc, double> &src) {
239  FMATVEC_ASSERT(dst.rows()==src.rows(), ATIndep);
240  FMATVEC_ASSERT(dst.cols()==src.cols(), ATIndep);
241  for(int r=0; r<dst.rows(); ++r)
242  for(int c=0; c<dst.cols(); ++c)
243  dst(r,c)^=src(r,c);
244  return dst;
245 }
246 
247 }
248 
249 #endif
RowVector< Row, AT > trans(const Vector< Row, AT > &x)
Transpose of a vector.
Definition: linear_algebra.h:1370
Namespace fmatvec.
Definition: _memory.cc:28
SymbolicExpression subst(const SymbolicExpression &se, const IndependentVariable &a, const SymbolicExpression &b)
Substitutes in se the independent variable a with the expression b and returns the new expression.
Definition: ast.cc:234
double eval(const SymbolicExpression &x)
Definition: ast.h:436
SymbolicExpression parDer(const SymbolicExpression &dep, const IndependentVariable &indep)
Definition: ast.cc:282