fmatvec  2.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 "function.h"
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 DepVec, class ATIndep>
13 DepVec parDer(const DepVec &dep, const ATIndep &indep) {
14  DepVec ret(dep.size());
15  for(int i=0; i<dep.size(); ++i)
16  ret(i)=parDer(dep(i), indep);
17  return ret;
18 }
19 
20 template<class Type, class RowShape, class ColShape, class ATDep, class ATIndep>
21 Matrix<Type, RowShape, ColShape, ATDep> parDer(const Matrix<Type, RowShape, ColShape, ATDep> &dep, const ATIndep &indep) {
22  Matrix<Type, RowShape, ColShape, ATDep> ret(dep.rows(), dep.cols());
23  for(int r=0; r<dep.rows(); ++r)
24  for(int c=0; c<dep.cols(); ++c)
25  ret(r,c)=parDer(dep(r,c), indep);
26  return ret;
27 }
28 
29 template<class DepShape, class IndepShape, class ATDep, class ATIndep>
30 Matrix<General, DepShape, IndepShape, ATDep> parDer(const Vector<DepShape, ATDep> &dep, const Vector<IndepShape, ATIndep> &indep) {
31  Matrix<General, DepShape, IndepShape, ATDep> ret(dep.size(), indep.size());
32  for(int r=0; r<dep.size(); ++r)
33  for(int c=0; c<indep.size(); ++c)
34  ret(r,c)=parDer(dep(r), indep(c));
35  return ret;
36 }
37 
38 template<class IndepShape, class ATDep, class ATIndep>
39 RowVector<IndepShape, ATDep> parDer(const ATDep &dep, const Vector<IndepShape, ATIndep> &indep) {
40  RowVector<IndepShape, ATDep> ret(indep.size());
41  for(int r=0; r<indep.size(); ++r)
42  ret(r)=parDer(dep, indep(r));
43  return ret;
44 }
45 
46 template<class ATDep, class ATIndep>
47 Vector<Fixed<3>, ATDep> parDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const ATIndep &x) {
48  Matrix<General, Fixed<3>, Fixed<3>, ATDep> Rs;
49  for(int r=0; r<3; ++r)
50  for(int c=0; c<3; ++c)
51  Rs(r,c)=parDer(R(r,c), x);
52  auto retTilde=Rs*trans(R);
53  Vector<Fixed<3>, ATDep> ret;
54  ret(0)=retTilde(2,1);
55  ret(1)=retTilde(0,2);
56  ret(2)=retTilde(1,0);
57  return ret;
58 }
59 
60 template<class ATDep, class Shape, class ATIndep>
61 Matrix<General, Fixed<3>, Fixed<3>, ATDep> parDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R, const Vector<Shape, ATIndep> &x) {
62  Matrix<General, Fixed<3>, Shape, ATDep> ret;
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 ATIndep>
69 Dep dirDer(const Dep &dep, const ATIndep &indepdir, const ATIndep &indep) {
70  return parDer(dep, indep)*indepdir;
71 }
72 
73 template<class ShapeDep, class ATDep, class ATIndep>
74 Vector<ShapeDep, ATDep> dirDer(const Vector<ShapeDep, ATDep> &dep, const ATIndep &indepdir, const ATIndep &indep) {
75  Vector<ShapeDep, ATDep> ret(dep.size());
76  for(int i=0; i<dep.size(); ++i)
77  ret(i)=parDer(dep(i), indep)*indepdir;
78  return ret;
79 }
80 
81 template<class ShapeDep, class ATDep, class ATIndep>
82 RowVector<ShapeDep, ATDep> dirDer(const RowVector<ShapeDep, ATDep> &dep, const ATIndep &indepdir, const ATIndep &indep) {
83  RowVector<ShapeDep, ATDep> ret(dep.size());
84  for(int i=0; i<dep.size(); ++i)
85  ret(i)=parDer(dep(i), indep)*indepdir;
86  return ret;
87 }
88 
89 template<class Type, class RowShape, class ColShape, class ATDep, class ATIndep>
90 Matrix<Type, RowShape, ColShape, ATDep> dirDer(const Matrix<Type, RowShape, ColShape, ATDep> &dep, const ATIndep &indepdir, const ATIndep &indep) {
91  Matrix<Type, RowShape, ColShape, ATDep> ret(dep.rows(), dep.cols());
92  for(int r=0; r<dep.rows(); ++r)
93  for(int c=0; c<dep.cols(); ++c)
94  ret(r,c)=parDer(dep(r,c), indep)*indepdir;
95  return ret;
96 }
97 
98 template<class ATDep, class ATIndep>
99 Vector<Fixed<3>, ATDep> dirDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &dep, const ATIndep &indepdir, const ATIndep &indep) {
100  return parDer(dep, indep)*indepdir;
101 }
102 
103 template<class Dep, class ShapeIndep, class ATIndep>
104 Dep dirDer(const Dep &dep, const Vector<ShapeIndep, ATIndep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
105  Dep ret=0;
106  for(int i=0; i<indep.size(); ++i)
107  ret+=parDer(dep, indep(i))*indepdir(i);
108  return ret;
109 }
110 
111 template<class DepShape, class ATDep, class ShapeIndep, class ATIndep>
112 Vector<DepShape, ATDep> dirDer(const Vector<DepShape, ATDep> &dep, const Vector<ShapeIndep, ATIndep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
113  Vector<DepShape, ATDep> ret(dep.size());
114  for(int d=0; d<dep.size(); ++d)
115  for(int i=0; i<indep.size(); ++i)
116  ret(d)+=parDer(dep(d), indep(i))*indepdir(i);
117  return ret;
118 }
119 
120 template<class DepShape, class ATDep, class ShapeIndep, class ATIndep>
121 RowVector<DepShape, ATDep> dirDer(const RowVector<DepShape, ATDep> &dep, const Vector<ShapeIndep, ATIndep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
122  RowVector<DepShape, ATDep> ret(dep.size());
123  for(int d=0; d<dep.size(); ++d)
124  for(int i=0; i<indep.size(); ++i)
125  ret(d)+=parDer(dep(d), indep(i))*indepdir(i);
126  return ret;
127 }
128 
129 template<class Type, class RowShape, class ColShape, class ATDep, class ShapeIndep, class ATIndep>
130 Matrix<Type, RowShape, ColShape, ATDep> dirDer(const Matrix<Type, RowShape, ColShape, ATDep> &dep, const Vector<ShapeIndep, ATIndep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
131  Matrix<Type, RowShape, ColShape, ATDep> ret(dep.rows(), dep.cols());
132  for(int r=0; r<dep.rows(); ++r)
133  for(int c=0; c<dep.cols(); ++c)
134  for(int i=0; i<indep.size(); ++i)
135  ret(r,c)+=parDer(dep(r,c), indep(i))*indepdir(i);
136  return ret;
137 }
138 
139 template<class ATDep, class ShapeIndep, class ATIndep>
140 Vector<Fixed<3>, ATDep> dirDer(const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &dep, const Vector<ShapeIndep, ATIndep> &indepdir, const Vector<ShapeIndep, ATIndep> &indep) {
141  return parDer(dep, indep)*indepdir;
142 }
143 
144 template<class Shape, class AT>
145 Vector<Shape, double> eval(const Vector<Shape, AT> &x) {
146  Vector<Shape, double> ret(x.size());
147  for(int i=0; i<x.size(); ++i)
148  ret(i)=eval(x(i));
149  return ret;
150 }
151 
152 template<class Shape, class AT>
153 RowVector<Shape, double> eval(const RowVector<Shape, AT> &x) {
154  RowVector<Shape, double> ret(x.size());
155  for(int i=0; i<x.size(); ++i)
156  ret(i)=eval(x(i));
157  return ret;
158 }
159 
160 template<int N, class AT>
161 Vector<Fixed<N>, double> eval(const Vector<Fixed<N>, AT> &x) {
162  Vector<Fixed<N>, double> ret;
163  for(int i=0; i<N; ++i)
164  ret(i)=eval(x(i));
165  return ret;
166 }
167 
168 template<int N, class AT>
169 RowVector<Fixed<N>, double> eval(const RowVector<Fixed<N>, AT> &x) {
170  RowVector<Fixed<N>, double> ret;
171  for(int i=0; i<N; ++i)
172  ret(i)=eval(x(i));
173  return ret;
174 }
175 
176 template<class Type, class RowShape, class ColShape, class AT>
177 Matrix<Type, RowShape, ColShape, double> eval(const Matrix<Type, RowShape, ColShape, AT> &x) {
178  Matrix<Type, RowShape, ColShape, double> ret(x.rows(), x.cols());
179  for(int r=0; r<x.rows(); ++r)
180  for(int c=0; c<x.cols(); ++c)
181  ret(r,c)=eval(x(r,c));
182  return ret;
183 }
184 
185 template<class Type, int N, class ColShape, class AT>
186 Matrix<Type, Fixed<N>, ColShape, double> eval(const Matrix<Type, Fixed<N>, ColShape, AT> &x) {
187  Matrix<Type, Fixed<N>, ColShape, double> ret(x.cols());
188  for(int r=0; r<N; ++r)
189  for(int c=0; c<x.cols(); ++c)
190  ret(r,c)=eval(x(r,c));
191  return ret;
192 }
193 
194 template<class Type, class RowShape, int M, class AT>
195 Matrix<Type, RowShape, Fixed<M>, double> eval(const Matrix<Type, RowShape, Fixed<M>, AT> &x) {
196  Matrix<Type, RowShape, Fixed<M>, double> ret(x.rows());
197  for(int r=0; r<x.rows(); ++r)
198  for(int c=0; c<M; ++c)
199  ret(r,c)=eval(x(r,c));
200  return ret;
201 }
202 
203 template<class Type, int N, int M, class AT>
204 Matrix<Type, Fixed<N>, Fixed<M>, double> eval(const Matrix<Type, Fixed<N>, Fixed<M>, AT> &x) {
205  Matrix<Type, Fixed<N>, Fixed<M>, double> ret;
206  for(int r=0; r<N; ++r)
207  for(int c=0; c<M; ++c)
208  ret(r,c)=eval(x(r,c));
209  return ret;
210 }
211 
212 template<class DepR, class ShapeA, class IndepA, class ShapeB, class DepB>
213 DepR subst(const DepR &se, const Vector<ShapeA, IndepA>& a, const Vector<ShapeB, DepB> &b) {
214  if(a.size()!=b.size())
215  throw std::runtime_error("The size of the independent and the dependent substitution variable does not match.");
216  DepR ret=se;
217  for(int i=0; i<a.size(); ++i)
218  ret=subst(ret, a(i), b(i));
219  return ret;
220 }
221 
222 template<class ShapeR, class DepR, class ShapeA, class IndepA, class ShapeB, class DepB>
223 Vector<ShapeR, DepR> subst(const Vector<ShapeR, DepR> &se, const IndepA& a, const DepB &b) {
224  Vector<ShapeR, DepR> ret(se.size());
225  for(int i=0; i<se.size(); ++i)
226  ret(i)=subst(se(i), a, b);
227  return ret;
228 }
229 
230 template<class TypeR, class RowShapeR, class ColShapeR, class DepR, class ShapeA, class IndepA, class ShapeB, class DepB>
231 Matrix<TypeR, RowShapeR, ColShapeR, DepR> subst(const Matrix<TypeR, RowShapeR, ColShapeR, DepR> &se,
232  const IndepA& a, const DepB &b) {
233  Matrix<TypeR, RowShapeR, ColShapeR, DepR> ret(se.rows(), se.cols());
234  for(int r=0; r<se.rows(); ++r)
235  for(int c=0; c<se.cols(); ++c)
236  ret(r,c)=subst(se(r,c), a, b);
237  return ret;
238 }
239 
240 //mfmf change operator &= to something better
241 template<class ShapeDst, class ShapeSrc, class ATIndep>
242 Vector<ShapeDst, ATIndep>& operator&=(Vector<ShapeDst, ATIndep> &dst, const Vector<ShapeSrc, double> &src) {
243  FMATVEC_ASSERT(dst.size()==src.size(), ATIndep);
244  for(int i=0; i<dst.size(); ++i)
245  dst(i)&=src(i);
246  return dst;
247 }
248 
249 template<int N, class ATIndep>
250 Vector<Fixed<N>, ATIndep>& operator&=(Vector<Fixed<N>, ATIndep> &dst, const Vector<Fixed<N>, double> &src) {
251  for(int i=0; i<N; ++i)
252  dst(i)&=src(i);
253  return dst;
254 }
255 
256 template<int N, class ShapeSrc, class ATIndep>
257 Vector<Fixed<N>, ATIndep>& operator&=(Vector<Fixed<N>, ATIndep> &dst, const Vector<ShapeSrc, double> &src) {
258  FMATVEC_ASSERT(N==src.size(), ATIndep);
259  for(int i=0; i<N; ++i)
260  dst(i)&=src(i);
261  return dst;
262 }
263 
264 template<class ShapeDst, int N, class ATIndep>
265 Vector<ShapeDst, ATIndep>& operator&=(Vector<ShapeDst, ATIndep> &dst, const Vector<Fixed<N>, double> &src) {
266  FMATVEC_ASSERT(N==src.size(), ATIndep);
267  for(int i=0; i<N; ++i)
268  dst(i)&=src(i);
269  return dst;
270 }
271 
272 }
273 
274 #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:435
SymbolicExpression parDer(const SymbolicExpression &dep, const IndependentVariable &indep)
Definition: ast.cc:282