mbsim 4.0.0 MBSim Kernel

two_dimensional_tabular_function.h
1 /* Copyright (C) 2004-2009 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: markus.ms.schneider@gmail.com
18 */
19
20 #ifndef _TWO_DIMENSIONAL_TABULAR_FUNCTION_H_
21 #define _TWO_DIMENSIONAL_TABULAR_FUNCTION_H_
22
23 #include "mbsim/functions/function.h"
24 #include "mbsim/utils/utils.h"
25
26 namespace MBSim {
27
28 template<typename Sig> class TwoDimensionalTabularFunction;
29
30 template<typename Ret, typename Arg1, typename Arg2>
31 class TwoDimensionalTabularFunction<Ret(Arg1, Arg2)> : public Function<Ret(Arg1, Arg2)> {
32 public:
33 TwoDimensionalTabularFunction() : zVal(4,fmatvec::INIT,1), zInd(4,fmatvec::INIT,0), zFac(4, 4,fmatvec::INIT,0) { }
34 /* INHERITED INTERFACE OF FUNCTION2 */
35 int getArg1Size() const override { return 1; }
36 int getArg2Size() const override { return 1; }
37 std::pair<int, int> getRetSize() const override { return std::make_pair(1,1); }
38 void initializeUsingXML(xercesc::DOMElement *element) override {
39 xercesc::DOMElement * e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"x");
40 if(e) {
41 setx(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
42 e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"y");
43 sety(MBXMLUtils::E(e)->getText<fmatvec::Vec>());
44 e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"z");
45 setz(MBXMLUtils::E(e)->getText<fmatvec::Mat>(y.size(), x.size()));
46 }
47 e = MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"xyz");
48 if(e) setxyz(MBXMLUtils::E(e)->getText<fmatvec::Mat>());
49 }
50 Ret operator()(const Arg1& xVal_, const Arg2& yVal_) override {
51 double xVal = ToDouble<Arg1>::cast(xVal_);
52 double yVal = ToDouble<Arg2>::cast(yVal_);
53 calcIndex(xVal, x, x.size(), x0Index, x1Index);
54 calcIndex(yVal, y, y.size(), y0Index, y1Index);
55
56 zVal(1) = xVal;
57 zVal(2) = yVal;
58 zVal(3) = xVal * yVal;
59 const double x0 = x(x0Index);
60 const double x1 = x(x1Index);
61 const double y0 = y(y0Index);
62 const double y1 = y(y1Index);
63 const double nenner = (x0 - x1) * (y0 - y1);
64 zInd(0) = z(y0Index, x0Index);
65 zInd(1) = z(y0Index, x1Index);
66 zInd(2) = z(y1Index, x0Index);
67 zInd(3) = z(y1Index, x1Index);
68 zFac(0, 0) = x1 * y1;
69 zFac(0, 1) = -x0 * y1;
70 zFac(0, 2) = -x1 * y0;
71 zFac(0, 3) = x0 * y0;
72 zFac(1, 0) = -y1;
73 zFac(1, 1) = y1;
74 zFac(1, 2) = y0;
75 zFac(1, 3) = -y0;
76 zFac(2, 0) = -x1;
77 zFac(2, 1) = x0;
78 zFac(2, 2) = x1;
79 zFac(2, 3) = -x0;
80 zFac(3, 0) = 1.;
81 zFac(3, 1) = -1.;
82 zFac(3, 2) = -1.;
83 zFac(3, 3) = 1.;
84
85 return FromDouble<Ret>::cast((trans(zFac*zInd)*zVal)/nenner);
86 }
87 /***************************************************/
88 /* GETTER / SETTER */
89 void setx(const fmatvec::VecV &x_) { x <<= x_; }
90 void sety(const fmatvec::VecV &y_) { y <<= y_; }
91 void setz(const fmatvec::MatV &z_) { z <<= z_; }
92 void setxyz(const fmatvec::MatV &xyz) {
93 if(xyz.rows() <= 1 or xyz.cols() <= 1)
94 this->throwError("Dimension missmatch in size of xyz");
95 x <<= xyz.row(0)(fmatvec::RangeV(1,xyz.cols()-1)).T();
96 y <<= xyz.col(0)(fmatvec::RangeV(1,xyz.rows()-1));
97 z <<= xyz(fmatvec::RangeV(1,xyz.rows()-1),fmatvec::RangeV(1,xyz.cols()-1));
98 }
99
100 double getxMin() { return x(0); }
101 double getxMax() { return x(x.size() - 1); }
102 double getyMin() { return y(0); }
103 double getyMax() { return y(y.size() - 1); }
104 /***************************************************/
105
106 void init(Element::InitStage stage, const InitConfigSet &config) override {
107 Function<Ret(Arg1, Arg2)>::init(stage, config);
108 if(stage==Element::preInit) {
109 if (z.cols() != x.size())
110 this->throwError("Dimension missmatch in size of x");
111 if (z.rows() != y.size())
112 this->throwError("Dimension missmatch in size of y");
113 for (int i = 1; i < x.size(); i++)
114 if (x(i - 1) >= x(i))
115 this->throwError("x values must be strictly monotonic increasing!");
116 for (int i = 1; i < y.size(); i++)
117 if (y(i - 1) >= y(i))
118 this->throwError("y values must be strictly monotonic increasing!");
119 }
120 }
121 protected:
122 fmatvec::VecV x;
123 fmatvec::VecV y;
124 fmatvec::MatV z;
125
126 int x0Index{0}, x1Index{0};
127 int y0Index{0}, y1Index{0};
128
129 fmatvec::VecV zVal;
130 fmatvec::VecV zInd;
131 fmatvec::MatV zFac;
132
133 void calcIndex(double x, const fmatvec::VecV &X, int xSize, int &xIndexMinus, int &xIndexPlus) {
134 if (x <= X(0)) {
135 xIndexPlus = 1;
136 xIndexMinus = 0;
137 if(x < X(0))
138 fmatvec::Atom::msg(fmatvec::Atom::Warn) << "TwoDimensionalTabularFunction: Value (" << x << ") is smaller than the smallest table value(" << X(0) << ")!" << std::endl;
139 }
140 else if (x >= X(xSize - 1)) {
141 xIndexPlus = xSize - 1;
142 xIndexMinus = xSize - 2;
143 if(x > X(xSize - 1))
144 fmatvec::Atom::msg(fmatvec::Atom::Warn) << "TwoDimensionalTabularFunction: Value (" << x << ") is greater than the greatest table value(" << X(xSize - 1) << ")!" << std::endl;
145 }
146 else {
147 if (x < X(xIndexPlus))
148 while (x < X(xIndexPlus - 1) && xIndexPlus > 1)
149 xIndexPlus--;
150 else if (x > X(xIndexPlus))
151 while (x > X(xIndexPlus) && xIndexPlus < xSize - 1)
152 xIndexPlus++;
153 xIndexMinus = xIndexPlus - 1;
154 }
155 }
156 };
157 }
158
159 #endif
MBSim::TwoDimensionalTabularFunction< Ret(Arg1, Arg2)>::init
void init(Element::InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: two_dimensional_tabular_function.h:106
MBSim::ToDouble
Definition: utils.h:61
MBSim::Element::InitStage
InitStage
The stages of the initialization.
Definition: element.h:62
MBSim::FromDouble
Definition: utils.h:89
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::Element::preInit
Definition: element.h:64
MBSim::TwoDimensionalTabularFunction
Definition: two_dimensional_tabular_function.h:28
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30