1 #ifndef _fmatvec_symbolic_h_ 2 #define _fmatvec_symbolic_h_ 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);
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);
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));
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));
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)
52 auto retTilde=Rs*
trans(R);
53 Vector<Fixed<3>, ATDep> ret;
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)));
68 template<
class Dep,
class ATIndep>
69 Dep dirDer(
const Dep &dep,
const ATIndep &indepdir,
const ATIndep &indep) {
70 return parDer(dep, indep)*indepdir;
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;
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;
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;
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;
103 template<
class Dep,
class ShapeIndep,
class ATIndep>
104 Dep dirDer(
const Dep &dep,
const Vector<ShapeIndep, ATIndep> &indepdir,
const Vector<ShapeIndep, ATIndep> &indep) {
106 for(
int i=0; i<indep.size(); ++i)
107 ret+=
parDer(dep, indep(i))*indepdir(i);
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);
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);
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);
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;
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)
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)
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)
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)
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));
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));
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));
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));
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.");
217 for(
int i=0; i<a.size(); ++i)
218 ret=
subst(ret, a(i), b(i));
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);
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);
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)
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)
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)
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)
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