1 #ifndef _fmatvec_symbolic_h_ 2 #define _fmatvec_symbolic_h_ 12 template<
class Dep,
class ATIndep>
13 Dep
parDer(
const Dep &dep,
const ATIndep &indep) {
15 if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
17 else if constexpr (Dep::isVector)
18 ret.resize(dep.size());
20 ret.resize(dep.rows(), dep.cols());
23 for(; d!=dep.end(); ++d, ++r)
28 template<
class ATDep,
class ATIndep>
29 Vector<Fixed<3>, ATDep>
parDer(
const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R,
const ATIndep &x) {
30 Matrix<General, Fixed<3>, Fixed<3>, ATDep> Rs;
31 for(
int r=0; r<3; ++r)
32 for(
int c=0; c<3; ++c)
34 auto retTilde=Rs*
trans(R);
35 Vector<Fixed<3>, ATDep> ret;
42 template<
class DepShape,
class IndepShape,
class ATDep,
class ATIndep>
43 Matrix<General, DepShape, IndepShape, ATDep>
parDer(
const Vector<DepShape, ATDep> &dep,
const Vector<IndepShape, ATIndep> &indep) {
44 Matrix<General, DepShape, IndepShape, ATDep> ret(dep.size(), indep.size());
45 for(
int r=0; r<dep.size(); ++r)
46 for(
int c=0; c<indep.size(); ++c)
47 ret(r,c)=
parDer(dep(r), indep(c));
51 template<
class IndepShape,
class ATDep,
class ATIndep>
52 RowVector<IndepShape, ATDep>
parDer(
const ATDep &dep,
const Vector<IndepShape, ATIndep> &indep) {
53 RowVector<IndepShape, ATDep> ret(indep.size());
54 for(
int r=0; r<indep.size(); ++r)
55 ret(r)=
parDer(dep, indep(r));
59 template<
class ATDep,
class Shape,
class ATIndep>
60 Matrix<General, Fixed<3>, Shape, ATDep>
parDer(
const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R,
const Vector<Shape, ATIndep> &x) {
61 Matrix<General, Fixed<3>, Shape, ATDep> ret(3,x.size());
62 for(
int i=0; i<x.size(); ++i)
63 ret.set(i,
parDer(R, x(i)));
67 template<
class Dep,
class ATDep,
class ATIndep>
68 Dep dirDer(
const Dep &dep,
const ATDep &indepdir,
const ATIndep &indep) {
69 return parDer(dep, indep)*indepdir;
72 template<
class ATDep,
class ATIndep>
73 Vector<Fixed<3>, ATDep> dirDer(
const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &R,
const ATDep &indepdir,
const ATIndep &indep) {
74 return parDer(R, indep)*indepdir;
77 template<
class Dep,
class ShapeIndep,
class ATDep,
class ATIndep>
78 Dep dirDer(
const Dep &dep,
const Vector<ShapeIndep, ATDep> &indepdir,
const Vector<ShapeIndep, ATIndep> &indep) {
80 if constexpr (std::is_same_v<Dep, SymbolicExpression>) {
83 else if constexpr (Dep::isVector)
84 ret<<=Dep(dep.size(), INIT, 0.0);
86 ret<<=Dep(dep.rows(), dep.cols(), INIT, 0.0);
87 for(
int i=0; i<indep.size(); ++i)
88 ret+=
parDer(dep, indep(i))*indepdir(i);
92 template<
class ATDep,
class ShapeIndep,
class ATIndep>
93 Vector<Fixed<3>, ATDep> dirDer(
const Matrix<Rotation, Fixed<3>, Fixed<3>, ATDep> &dep,
const Vector<ShapeIndep, ATDep> &indepdir,
const Vector<ShapeIndep, ATIndep> &indep) {
94 return parDer(dep, indep)*indepdir;
98 template<
class Ret,
class A,
class B>
99 Ret subst(
const Ret &src,
const A &a,
const B &b) {
100 if constexpr (!std::is_same_v<A, IndependentVariable>) {
101 if constexpr (A::isVector) {
102 if(a.size()!=b.size())
103 throw std::runtime_error(
"The size of the independent and the dependent substitution variable does not match.");
106 if(a.rows()!=b.rows() || a.cols()!=b.cols())
107 throw std::runtime_error(
"The size of the independent and the dependent substitution variable does not match.");
111 if constexpr (std::is_same_v<Ret, SymbolicExpression> && std::is_same_v<A, IndependentVariable>) {
112 return AST::substScalar(src, a, b);
114 if constexpr (std::is_same_v<Ret, SymbolicExpression> && !std::is_same_v<A, IndependentVariable>) {
118 for(; aIt<a.end(); ++aIt, ++bIt)
119 ret=subst(ret, *aIt, *bIt);
122 if constexpr (!std::is_same_v<Ret, SymbolicExpression> && std::is_same_v<A, IndependentVariable>) {
124 for(
auto retIt=ret.begin(); retIt<ret.end(); ++retIt)
125 *retIt=subst(*retIt, a, b);
128 if constexpr (!std::is_same_v<Ret, SymbolicExpression> && !std::is_same_v<A, IndependentVariable>) {
130 for(
auto retIt=ret.begin(); retIt<ret.end(); ++retIt) {
133 for(; aIt<a.end(); ++aIt, ++bIt)
134 *retIt=subst(*retIt, *aIt, *bIt);
142 template<
class Dst,
class Src>
143 Dst& operator^=(Dst &dst,
const Src &src) {
144 auto dstIt=dst.begin();
145 auto srcIt=src.begin();
146 for(; dstIt<dst.end(); ++dstIt, ++srcIt)
157 template<
class... Arg>
161 using SymTuple = std::tuple<Arg...>;
163 using NumTuple = std::tuple<typename ReplaceAT<Arg, double>::Type...>;
165 using NumRetType = std::conditional_t<std::tuple_size_v<NumTuple> == 1, std::tuple_element_t<0,NumTuple>, NumTuple>;
168 Eval(
const Eval &src) =
delete;
169 Eval(Eval &&src) =
delete;
170 Eval<Arg...>& operator=(
const Eval &src) =
delete;
171 Eval<Arg...>& operator=(Eval &&src) =
delete;
175 Eval(
const Arg&... arg);
178 inline const NumRetType& operator()()
const;
183 std::set<std::shared_ptr<const AST::Symbol>> symbolStore;
187 void walkAT(
const SymTuple &symTuple, NumTuple &numTuple,
188 const std::function<
void(
const SymbolicExpression&,
double&)> &func);
192 std::vector<AST::ByteCode> byteCode;
195 void ctorByteCode(
const Arg&... arg);
196 inline void callByteCode()
const;
199 template<
class... Arg>
200 Eval<Arg...>::~Eval() =
default;
202 template<
class... Arg>
203 Eval<Arg...>::Eval(
const Arg&... arg) {
204 ctorByteCode(arg...);
207 template<
class... Arg>
208 auto Eval<Arg...>::operator()() const -> const NumRetType& {
211 if constexpr (std::tuple_size_v<NumTuple> == 1)
212 return std::get<0>(numTuple);
217 template<class... Arg>
219 void Eval<Arg...>::walkAT(const SymTuple &symTuple, NumTuple &numTuple,
220 const std::function<
void(const SymbolicExpression&,
double&)> &func) {
223 auto &sym = std::get<I>(symTuple);
224 auto &num = std::get<I>(numTuple);
226 using Sym = std::tuple_element_t<I,SymTuple>;
228 if constexpr (std::is_same_v<Sym, SymbolicExpression>)
233 if constexpr (Sym::isVector)
234 num.resize(sym.size());
236 num.resize(sym.rows(), sym.cols());
238 auto symIt=sym.begin();
239 auto numIt=num.begin();
240 for(; symIt!=sym.end(); ++symIt, ++numIt)
241 func(*symIt, *numIt);
245 if constexpr (I+1<std::tuple_size_v<SymTuple>)
246 walkAT<I+1>(symTuple, numTuple, func);
249 template<
class... Arg>
250 void Eval<Arg...>::ctorByteCode(
const Arg&... arg) {
252 size_t byteCodeCount=0;
253 std::set<const AST::Vertex*> existingVertex1;
254 walkAT(SymTuple(arg...), numTuple, [&byteCodeCount, &existingVertex1](
auto &sym,
auto &num) {
255 sym->walkVertex([&byteCodeCount, &existingVertex1](const std::shared_ptr<const AST::Vertex>& v) {
256 if(!existingVertex1.insert(v.get()).second) return;
258 if(auto s=std::dynamic_pointer_cast<const AST::Symbol>(v); s)
265 byteCode.reserve(byteCodeCount);
266 std::map<const AST::Vertex*, std::vector<AST::ByteCode>::iterator> existingVertex;
269 walkAT(SymTuple(arg...), numTuple, [
this, &existingVertex](
auto &sym,
auto &num) {
270 sym->walkVertex([this, &existingVertex](const std::shared_ptr<const AST::Vertex>& v) {
271 if(auto s=std::dynamic_pointer_cast<const AST::Symbol>(v); s) {
272 symbolStore.insert(s);
273 s->dumpByteCode(byteCode, existingVertex);
279 std::vector< std::vector<AST::ByteCode>::iterator > exprRet;
280 walkAT(SymTuple(arg...), numTuple, [
this, &existingVertex, &exprRet](
auto &sym,
auto &num) {
281 exprRet.emplace_back(sym->dumpByteCode(byteCode, existingVertex));
285 auto exprRetIt = exprRet.begin();
286 walkAT(SymTuple(arg...), numTuple, [
this, &exprRet, &exprRetIt](
auto &sym,
auto &num) {
287 byteCode.emplace_back();
288 auto it = --byteCode.end();
289 it->func = [](double* r, const std::array<double*,AST::ByteCode::N>& a) { *r = *a[0]; };
290 it->argsPtr = { (*(exprRetIt++))->retPtr };
295 template<
class... Arg>
296 void Eval<Arg...>::callByteCode()
const {
297 #if defined(FMATVEC_DEBUG) && !defined(SWIG) 298 SymbolicExpression::evalOperationsCount = byteCode.size();
300 std::for_each(byteCode.begin(), byteCode.end(), [](
const AST::ByteCode &bc){
301 bc.func(bc.retPtr, bc.argsPtr);
RowVector< Row, AT > trans(const Vector< Row, AT > &x)
Transpose of a vector.
Definition: linear_algebra.h:1723
Namespace fmatvec.
Definition: _memory.cc:28
SymbolicExpression parDer(const SymbolicExpression &dep, const IndependentVariable &indep)
Definition: ast.cc:286