mbsim 4.0.0 MBSim Kernel

two_dimensional_piecewise_polynom_function.h
1 /* Copyright (C) 2004-2016 MBSim Development Team
2 *
3 * This library is free software; you can redistribute it and/or
4 * modify it under the terms of the GNU Lesser General Public
5 * License as published by the Free Software Foundation; either
6 * version 2.1 of the License, or (at your option) any later version.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public
14 * License along with this library; if not, write to the Free Software
15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16 *
17 * Contact: martin.o.foerg@gmail.com
18 */
19
20 #ifndef _TWO_DIMENSIONAL_PIECEWISE_POLYNOM_FUNCTION_H_
21 #define _TWO_DIMENSIONAL_PIECEWISE_POLYNOM_FUNCTION_H_
22
23 #include "mbsim/functions/piecewise_polynom_function.h"
24
25 namespace MBSim {
26
27 template<typename Sig> class TwoDimensionalPiecewisePolynomFunction;
28
29 template<typename Ret, typename Arg1, typename Arg2>
30 class TwoDimensionalPiecewisePolynomFunction<Ret(Arg1, Arg2)> : public Function<Ret(Arg1, Arg2)> {
31 using B = fmatvec::Function<Ret(Arg1, Arg2)>;
32 public:
33 enum InterpolationMethod {
34 cSplinePeriodic,
35 cSplineNatural,
36 piecewiseLinear,
37 unknown
38 };
39
40 TwoDimensionalPiecewisePolynomFunction() : method1(cSplineNatural), method2(cSplineNatural) {
41 f1.setParent(this);
42 f2.setParent(this);
43 }
44
45 virtual void initializeUsingXML(xercesc::DOMElement *element) {
46 xercesc::DOMElement * e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
47 if(e) {
48 setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
49 e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
50 sety(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
51 e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"z");
52 setz(MBXMLUtils::E(e)->getText<fmatvec::Mat>(y.size(), x.size()));
53 }
54 e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xyz");
55 if(e) setxyz(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
56 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethodFirstDimension");
57 if(e) {
58 std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
59 str=str.substr(1,str.length()-2);
60 if(str=="cSplinePeriodic") method1=cSplinePeriodic;
61 else if(str=="cSplineNatural") method1=cSplineNatural;
62 else if(str=="piecewiseLinear") method1=piecewiseLinear;
63 else method1=unknown;
64 }
65 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"interpolationMethodSecondDimension");
66 if(e) {
67 std::string str=MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData();
68 str=str.substr(1,str.length()-2);
69 if(str=="cSplinePeriodic") method2=cSplinePeriodic;
70 else if(str=="cSplineNatural") method2=cSplineNatural;
71 else if(str=="piecewiseLinear") method2=piecewiseLinear;
72 else method2=unknown;
73 }
74 }
75
76 int getArgSize() const { return 1; }
77
78 std::pair<int, int> getRetSize() const { return std::make_pair(1,1); }
79
80 virtual Ret operator()(const Arg1& xVal, const Arg2& yVal) {
81 f2.sety(f1(xVal));
82 f2.reset();
83 f2.calculateSpline();
84 return f2(yVal);
85 }
86
87 typename B::DRetDArg1 parDer1(const Arg1 &xVal, const Arg2 &yVal) {
88 f2.sety(f1.parDer(xVal));
89 f2.reset();
90 f2.calculateSpline();
91 return f2(yVal);
92 }
93
94 typename B::DRetDArg2 parDer2(const Arg1 &xVal, const Arg2 &yVal) {
95 f2.sety(f1(xVal));
96 f2.reset();
97 f2.calculateSpline();
98 return f2.parDer(yVal);
99 }
100
101 typename B::DRetDArg1 parDer1DirDer1(const Arg1 &xdVal, const Arg1 &xVal, const Arg2 &yVal) {
102 f2.sety(f1.parDerDirDer(xdVal,xVal));
103 f2.reset();
104 f2.calculateSpline();
105 return f2(yVal);
106 }
107
108 typename B::DRetDArg1 parDer1DirDer2(const Arg2 &ydVal, const Arg1 &xVal, const Arg2 &yVal) {
109 f2.sety(f1.parDer(xVal));
110 f2.reset();
111 f2.calculateSpline();
112 return f2.parDer(yVal)*ydVal;
113 }
114
115 typename B::DRetDArg2 parDer2DirDer1(const Arg1 &xdVal, const Arg1 &xVal, const Arg2 &yVal) {
116 f2.sety(f1.parDer(xVal)*xdVal);
117 f2.reset();
118 f2.calculateSpline();
119 return f2.parDer(yVal);
120 }
121
122 typename B::DRetDArg2 parDer2DirDer2(const Arg2 &ydVal, const Arg1 &xVal, const Arg2 &yVal) {
123 f2.sety(f1(xVal));
124 f2.reset();
125 f2.calculateSpline();
126 return f2.parDerDirDer(ydVal,yVal);
127 }
128
129 void setx(const fmatvec::VecV &x_) { x <<= x_; }
130
131 void sety(const fmatvec::VecV &y_) { y <<= y_; }
132
133 void setz(const fmatvec::MatV &z_) { z <<= z_; }
134
135 void setxyz(const fmatvec::MatV &xyz) {
136 if(xyz.rows() <= 1 or xyz.cols() <= 1)
137 this->throwError("Dimension missmatch in size of xyz");
138 x <<= xyz.row(0)(fmatvec::RangeV(1,xyz.cols()-1)).T();
139 y <<= xyz.col(0)(fmatvec::RangeV(1,xyz.rows()-1));
140 z <<= xyz(fmatvec::RangeV(1,xyz.rows()-1),fmatvec::RangeV(1,xyz.cols()-1));
141 }
142
143 void setInterpolationMethodFirstDimension(InterpolationMethod method1_) { method1 = method1_; }
144 void setInterpolationMethodSecondDimension(InterpolationMethod method2_) { method2 = method2_; }
145
146 void init(Element::InitStage stage, const InitConfigSet &config) {
147 Function<Ret(Arg1, Arg2)>::init(stage, config);
148 if(stage==Element::preInit) {
149 if(method1==unknown)
150 Element::throwError("(TwoDimensionalPiecewisePolynomFunction::init): interpolation method first dimension unknown");
151 if(method2==unknown)
152 Element::throwError("(TwoDimensionalPiecewisePolynomFunction::init): interpolation method second dimension unknown");
153 if (z.cols() != x.size())
154 this->throwError("Dimension missmatch in xSize");
155 if (z.rows() != y.size())
156 this->throwError("Dimension missmatch in ySize");
157 for (int i = 1; i < x.size(); i++)
158 if (x(i - 1) >= x(i))
159 this->throwError("x values must be strictly monotonic increasing!");
160 for (int i = 1; i < y.size(); i++)
161 if (y(i - 1) >= y(i))
162 this->throwError("y values must be strictly monotonic increasing!");
163 f1.setx(x);
164 f1.sety(z.T());
165 f1.setInterpolationMethod(static_cast<typename PiecewisePolynomFunction<fmatvec::VecV(Arg1)>::InterpolationMethod>(method1));
166 f2.setx(y);
167 f2.sety(y);
168 f2.setInterpolationMethod(static_cast<typename PiecewisePolynomFunction<Ret(Arg2)>::InterpolationMethod>(method2));
169 }
170 f1.init(stage, config);
171 f2.init(stage, config);
172 }
173
174 protected:
175 fmatvec::VecV x;
176 fmatvec::VecV y;
177 fmatvec::MatV z;
178 PiecewisePolynomFunction<fmatvec::VecV(Arg1)> f1;
179 PiecewisePolynomFunction<Ret(Arg2)> f2;
180 InterpolationMethod method1, method2;
181 };
182 }
183
184 #endif
MBSim::TwoDimensionalPiecewisePolynomFunction< Ret(Arg1, Arg2)>::init
void init(Element::InitStage stage, const InitConfigSet &config)
plots time series header
Definition: two_dimensional_piecewise_polynom_function.h:146
MBXMLUtils::X
MBSim::TwoDimensionalPiecewisePolynomFunction
Definition: two_dimensional_piecewise_polynom_function.h:27
MBSim::Element::InitStage
InitStage
The stages of the initialization.
Definition: element.h:62
MBSim::PiecewisePolynomFunction
Definition: piecewise_polynom_function.h:31
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::Element::preInit
Definition: element.h:64
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30