mbsimflexiblebody  3.0.0
trafo33RCM.h
1 /* Copyright (C) 2004-2011 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: thorsten.schindler@mytum.de
18  */
19 
20 #ifndef TRAFO33RCM_H_
21 #define TRAFO33RCM_H_
22 
23 #include "fmatvec.h"
24 #include "mbsim/utils/function.h"
25 #include "mbsimFlexibleBody/pointer.h"
26 
27 namespace MBSimFlexibleBody {
28 
29  /***
30  * \brief computation of FiniteElement1s33RCM position coordinates (cp-angles and bending)
31  * \author Thorsten Schindler
32  * \date 2009-04-24 initial commit (Thorsten Schindler)
33  */
34  class PositionFunction : public MBSim::Function1<fmatvec::Vec,fmatvec::Vec> {
35  public:
44  PositionFunction(RevCardanPtr angle_,double l0_,const fmatvec::Vec& pL_,const fmatvec::Vec& pR_,
45  double cL1_,double cR1_,double cL2_,double cR2_,const fmatvec::RowVec& rRrLmH_);
46 
50  virtual ~PositionFunction();
51 
52  /* INHERITED INTERFACE */
53  fmatvec::Vec operator()(const fmatvec::Vec& pos, const void * =NULL);
54  /***************************************************/
55 
56  private:
60  RevCardanPtr angle;
61 
65  double l0;
66 
70  fmatvec::Vec pL, pR;
71 
75  double cL1, cR1, cL2, cR2;
76 
80  fmatvec::RowVec rRrLmH;
81  };
82 
88  class PositionJacobian : public MBSim::Function1<fmatvec::SqrMat,fmatvec::Vec> {
89  public:
97  PositionJacobian(RevCardanPtr angle_,double l0_,const fmatvec::RowVec &rRrLmH_,const fmatvec::Mat &pSbE_);
98 
102  virtual ~PositionJacobian();
103 
104  /* INHERITED INTERFACE */
105  fmatvec::SqrMat operator()(const fmatvec::Vec &pos, const void * =NULL);
106  /***************************************************/
107 
108  private:
112  RevCardanPtr angle;
113 
117  double l0;
118 
122  fmatvec::RowVec rRrLmH;
123 
127  fmatvec::Mat pSbE;
128  };
129  /*******************************************************************/
130 
136  class Trafo33RCM {
137  public:
141  Trafo33RCM(RevCardanPtr angle_,double l0_);
142 
146  virtual ~Trafo33RCM();
147 
148  /* GETTER / SETTER */
149  const fmatvec::SqrMat& getJIG() const;
150  const fmatvec::SqrMat& getJIGt() const;
151  const fmatvec::Vec& getpS() const;
152  const fmatvec::Vec& getpSt() const;
153  const fmatvec::Vec& gettS() const;
154  const fmatvec::Vec& getnS() const;
155  const fmatvec::Vec& getbS() const;
156  const fmatvec::RowVec& getnSH() const;
157  const fmatvec::RowVec& getbSH() const;
158  const fmatvec::Vec& getntilS() const;
159  const fmatvec::Vec& getbtilS() const;
160  const fmatvec::RowVec& getntilSH() const;
161  const fmatvec::RowVec& getbtilSH() const;
162  const fmatvec::Vec& gettSt() const;
163  const fmatvec::Vec& getnSt() const;
164  const fmatvec::Vec& getbSt() const;
165  const fmatvec::RowVec& gettStH() const;
166  const fmatvec::RowVec& getnStH() const;
167  const fmatvec::RowVec& getbStH() const;
168  const fmatvec::RowVec& getntilStH() const;
169  const fmatvec::RowVec& getbtilStH() const;
170  const fmatvec::SqrMat& gettSpS() const;
171  const fmatvec::SqrMat& getnSpS() const;
172  const fmatvec::SqrMat& getbSpS() const;
173  const fmatvec::SqrMat& getntilSpS() const;
174  const fmatvec::SqrMat& getbtilSpS() const;
175  const fmatvec::SqrMat& gettSpSt() const;
176  const fmatvec::SqrMat& getnSpSt() const;
177  const fmatvec::SqrMat& getbSpSt() const;
178  const fmatvec::SqrMat& getntilSpSt() const;
179  const fmatvec::SqrMat& getbtilSpSt() const;
180 
181  const fmatvec::Vec& getqIt() const;
182  const double& getepstil() const;
183  const double& getepstilt() const;
184  const fmatvec::Vec& getrS() const;
185  const fmatvec::Vec& getrSt() const;
186  const double& getk0() const;
187  const double& getk0t() const;
188  const fmatvec::Vec& getbe() const;
189  const fmatvec::Vec& getbet() const;
190 
191  const double& getxintil() const;
192  const double& getxibtil() const;
193  const double& getetantil() const;
194  const double& getetabtil() const;
195  const double& getxintilt() const;
196  const double& getxibtilt() const;
197  const double& getetantilt() const;
198  const double& getetabtilt() const;
199  const fmatvec::SqrMat& getV() const;
200  /***************************************************/
201 
206  void computeqI(const fmatvec::Vec& qG);
207 
212  void computeJIG(const fmatvec::Vec& qG);
213 
219  void computezI(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
220 
226  void computeCOSYt(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
227 
233  void computeTrafo(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
234 
235  private:
239  RevCardanPtr angle;
240 
244  double l0, l0h2, l0h3, l0h4, l0h5;
245  double xstar, xstarh2, xstarh3;
246 
250  double epstil, k0;
251  fmatvec::Vec rS, pS;
252 
256  fmatvec::Vec rRrLp;
257  fmatvec::RowVec rRrLmH;
258 
262  fmatvec::Vec be;
263 
267  fmatvec::Vec tS, nS, bS, ntilS, btilS;
268  fmatvec::RowVec nSH, bSH, ntilSH, btilSH;
269  fmatvec::SqrMat tSpS, nSpS, bSpS, ntilSpS, btilSpS;
270  double xibtil, xintil, etabtil, etantil;
271 
275  fmatvec::Mat SMRHS_Jac;
276 
280  fmatvec::SqrMat V;
281 
285  fmatvec::Mat drRdrLp, drRdrLm, pSbE;
286 
290  fmatvec::Mat SMRHS, nSbE, bSbE, ntilSbE, btilSbE;
291  fmatvec::RowVec xibtilbE, xintilbE, etabtilbE, etantilbE;
292 
296  fmatvec::Mat beqG, tSqG, nSqG, bSqG, ntilSqG, btilSqG;
297  fmatvec::RowVec xintilqG, xibtilqG, etantilqG, etabtilqG;
298 
302  fmatvec::SqrMat JIG, JIGt;
303 
307  double k0t, epstilt;
308  fmatvec::Vec qIt, rSt, bet, pSt, tSt, nSt, bSt;
309  fmatvec::RowVec tStH, nStH, bStH, ntilStH, btilStH;
310  fmatvec::SqrMat tSpSt, nSpSt, bSpSt, ntilSpSt, btilSpSt;
311  double xibtilt, xintilt, etabtilt, etantilt;
312 
317  void computeprelim(const fmatvec::Vec& qG);
318 
323  fmatvec::Vec computes0(const fmatvec::Vec& qG);
324 
329  void computebe(const fmatvec::Vec& qG);
330 
334  void computeCOSY();
335 
340  void computerSepstk0(const fmatvec::Vec& qG);
341 
345  void computedrRdrL();
346 
350  void computeV();
351 
355  void computebeqG();
356 
360  void computeCOSYqG();
361 
365  void computeCOSYt();
366 
371  void computeJIGt(const fmatvec::Vec& qGt);
372  };
373 
374  inline const fmatvec::SqrMat& Trafo33RCM::getJIG() const { return JIG; }
375  inline const fmatvec::SqrMat& Trafo33RCM::getJIGt() const { return JIGt; }
376  inline const fmatvec::Vec& Trafo33RCM::getpS() const { return pS; }
377  inline const fmatvec::Vec& Trafo33RCM::getpSt() const { return pSt; }
378  inline const fmatvec::Vec& Trafo33RCM::gettS() const { return tS; }
379  inline const fmatvec::Vec& Trafo33RCM::getnS() const { return nS; }
380  inline const fmatvec::Vec& Trafo33RCM::getbS() const { return bS; }
381  inline const fmatvec::RowVec& Trafo33RCM::getnSH() const { return nSH; }
382  inline const fmatvec::RowVec& Trafo33RCM::getbSH() const { return bSH; }
383  inline const fmatvec::Vec& Trafo33RCM::getntilS() const { return ntilS; }
384  inline const fmatvec::Vec& Trafo33RCM::getbtilS() const { return btilS; }
385  inline const fmatvec::RowVec& Trafo33RCM::getntilSH() const { return ntilSH; }
386  inline const fmatvec::RowVec& Trafo33RCM::getbtilSH() const { return btilSH; }
387  inline const fmatvec::Vec& Trafo33RCM::gettSt() const { return tSt; }
388  inline const fmatvec::Vec& Trafo33RCM::getnSt() const { return nSt; }
389  inline const fmatvec::Vec& Trafo33RCM::getbSt() const { return bSt; }
390  inline const fmatvec::RowVec& Trafo33RCM::gettStH() const { return tStH; }
391  inline const fmatvec::RowVec& Trafo33RCM::getnStH() const { return nStH; }
392  inline const fmatvec::RowVec& Trafo33RCM::getbStH() const { return bStH; }
393  inline const fmatvec::RowVec& Trafo33RCM::getntilStH() const { return ntilStH; }
394  inline const fmatvec::RowVec& Trafo33RCM::getbtilStH() const { return btilStH; }
395  inline const fmatvec::SqrMat& Trafo33RCM::gettSpS() const { return tSpS; }
396  inline const fmatvec::SqrMat& Trafo33RCM::getnSpS() const { return nSpS; }
397  inline const fmatvec::SqrMat& Trafo33RCM::getbSpS() const { return bSpS; }
398  inline const fmatvec::SqrMat& Trafo33RCM::getntilSpS() const { return ntilSpS; }
399  inline const fmatvec::SqrMat& Trafo33RCM::getbtilSpS() const { return btilSpS; }
400  inline const fmatvec::SqrMat& Trafo33RCM::gettSpSt() const { return tSpSt; }
401  inline const fmatvec::SqrMat& Trafo33RCM::getnSpSt() const { return nSpSt; }
402  inline const fmatvec::SqrMat& Trafo33RCM::getbSpSt() const { return bSpSt; }
403  inline const fmatvec::SqrMat& Trafo33RCM::getntilSpSt() const { return ntilSpSt; }
404  inline const fmatvec::SqrMat& Trafo33RCM::getbtilSpSt() const { return btilSpSt; }
405  inline const fmatvec::Vec& Trafo33RCM::getqIt() const { return qIt; }
406  inline const double& Trafo33RCM::getepstil() const { return epstil; }
407  inline const double& Trafo33RCM::getepstilt() const { return epstilt; }
408  inline const fmatvec::Vec& Trafo33RCM::getrS() const { return rS; }
409  inline const fmatvec::Vec& Trafo33RCM::getrSt() const { return rSt; }
410  inline const double& Trafo33RCM::getk0() const { return k0; }
411  inline const double& Trafo33RCM::getk0t() const { return k0t; }
412  inline const fmatvec::Vec& Trafo33RCM::getbe() const { return be; }
413  inline const fmatvec::Vec& Trafo33RCM::getbet() const { return bet; }
414  inline const double& Trafo33RCM::getxintil() const { return xintil; }
415  inline const double& Trafo33RCM::getxibtil() const { return xibtil; }
416  inline const double& Trafo33RCM::getetantil() const { return etantil; }
417  inline const double& Trafo33RCM::getetabtil() const { return etabtil; }
418  inline const double& Trafo33RCM::getxintilt() const { return xintilt; }
419  inline const double& Trafo33RCM::getxibtilt() const { return xibtilt; }
420  inline const double& Trafo33RCM::getetantilt() const { return etantilt; }
421  inline const double& Trafo33RCM::getetabtilt() const { return etabtilt; }
422  inline const fmatvec::SqrMat& Trafo33RCM::getV() const { return V; }
423  /*******************************************************************/
424 
425 }
426 
427 #endif /*TRAFO33RCM_H_*/
428