1 #ifndef _fmatvec_symbolic_h_ 2 #define _fmatvec_symbolic_h_ 14 template<
class Dep,
class ATIndep>
15 Dep
parDer(
const Dep &dep,
const ATIndep &indep) {
17 if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
19 else if constexpr (Dep::isVector)
20 ret.resize(dep.size());
22 ret.resize(dep.rows(), dep.cols());
25 for(; d!=dep.end(); ++d, ++r)
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)
36 auto retTilde=Rs*
trans(R);
37 Vector<Fixed<3>, ATDep> ret;
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));
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));
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)));
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;
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;
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) {
82 if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
85 else if constexpr (Dep::isVector)
86 ret<<=Dep(dep.size(), INIT, 0.0);
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);
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;
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());
105 num.resize(sym.rows(), sym.cols());
108 for(; s!=sym.end(); ++s, ++n)
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.");
121 for(
int i=0; i<a.size(); ++i)
122 ret=
subst(ret, a(i), b(i));
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.");
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));
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);
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.");
153 for(
int i=0; i<a.size(); ++i)
154 ret=
subst(ret, a(i), b(i));
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.");
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));
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);
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.");
186 for(
int i=0; i<a.size(); ++i)
187 ret=
subst(ret, a(i), b(i));
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.");
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));
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)
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)
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)
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)
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)
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