mbsimflexiblebody  3.2.0
trafo33RCM.h
00001 /* Copyright (C) 2004-2011 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: thorsten.schindler@mytum.de
00018  */
00019 
00020 #ifndef TRAFO33RCM_H_
00021 #define TRAFO33RCM_H_
00022 
00023 #include "fmatvec/fmatvec.h"
00024 #include "mbsim/utils/function.h"
00025 #include "mbsimFlexibleBody/pointer.h"
00026 
00027 namespace MBSimFlexibleBody {
00028 
00029   /***
00030    * \brief computation of FiniteElement1s33RCM position coordinates (cp-angles and bending)
00031    * \author Thorsten Schindler
00032    * \date 2009-04-24 initial commit (Thorsten Schindler)
00033    */
00034   class PositionFunction : public MBSim::Function1<fmatvec::Vec,fmatvec::Vec> {    
00035     public:
00044       PositionFunction(RevCardanPtr angle_,double l0_,const fmatvec::Vec& pL_,const fmatvec::Vec& pR_,
00045           double cL1_,double cR1_,double cL2_,double cR2_,const fmatvec::RowVec& rRrLmH_);
00046       
00050       virtual ~PositionFunction();
00051       
00052       /* INHERITED INTERFACE */
00053       fmatvec::Vec operator()(const fmatvec::Vec& pos, const void * =NULL);
00054       /***************************************************/
00055 
00056     private:
00060        RevCardanPtr angle;
00061 
00065       double l0;
00066 
00070       fmatvec::Vec pL, pR;
00071 
00075       double cL1, cR1, cL2, cR2;
00076 
00080       fmatvec::RowVec rRrLmH;  
00081   };
00082 
00088   class PositionJacobian : public MBSim::Function1<fmatvec::SqrMat,fmatvec::Vec> {
00089     public:
00097       PositionJacobian(RevCardanPtr angle_,double l0_,const fmatvec::RowVec &rRrLmH_,const fmatvec::Mat &pSbE_);
00098       
00102       virtual ~PositionJacobian();
00103       
00104       /* INHERITED INTERFACE */
00105       fmatvec::SqrMat operator()(const fmatvec::Vec &pos, const void * =NULL);
00106       /***************************************************/
00107 
00108     private:
00112       RevCardanPtr angle;
00113 
00117       double l0;
00118 
00122       fmatvec::RowVec rRrLmH;
00123 
00127       fmatvec::Mat pSbE;
00128   };
00129   /*******************************************************************/
00130 
00136   class Trafo33RCM {            
00137     public:
00141       Trafo33RCM(RevCardanPtr angle_,double l0_);
00142       
00146       virtual ~Trafo33RCM();
00147 
00148       /* GETTER / SETTER */
00149       const fmatvec::SqrMat& getJIG() const;            
00150       const fmatvec::SqrMat& getJIGt() const;
00151       const fmatvec::Vec& getpS() const;
00152       const fmatvec::Vec& getpSt() const;
00153       const fmatvec::Vec& gettS() const;
00154       const fmatvec::Vec& getnS() const;
00155       const fmatvec::Vec& getbS() const;
00156       const fmatvec::RowVec& getnSH() const;
00157       const fmatvec::RowVec& getbSH() const;
00158       const fmatvec::Vec& getntilS() const;
00159       const fmatvec::Vec& getbtilS() const;
00160       const fmatvec::RowVec& getntilSH() const;
00161       const fmatvec::RowVec& getbtilSH() const;
00162       const fmatvec::Vec& gettSt() const;
00163       const fmatvec::Vec& getnSt() const;
00164       const fmatvec::Vec& getbSt() const;
00165       const fmatvec::RowVec& gettStH() const;
00166       const fmatvec::RowVec& getnStH() const;
00167       const fmatvec::RowVec& getbStH() const;
00168       const fmatvec::RowVec& getntilStH() const;
00169       const fmatvec::RowVec& getbtilStH() const;
00170       const fmatvec::SqrMat& gettSpS() const;
00171       const fmatvec::SqrMat& getnSpS() const;
00172       const fmatvec::SqrMat& getbSpS() const;
00173       const fmatvec::SqrMat& getntilSpS() const;
00174       const fmatvec::SqrMat& getbtilSpS() const;
00175       const fmatvec::SqrMat& gettSpSt() const;
00176       const fmatvec::SqrMat& getnSpSt() const;
00177       const fmatvec::SqrMat& getbSpSt() const;
00178       const fmatvec::SqrMat& getntilSpSt() const;
00179       const fmatvec::SqrMat& getbtilSpSt() const;
00180 
00181       const fmatvec::Vec& getqIt() const;
00182       const double& getepstil() const;
00183       const double& getepstilt() const;
00184       const fmatvec::Vec& getrS() const;
00185       const fmatvec::Vec& getrSt() const;
00186       const double& getk0() const;
00187       const double& getk0t() const;
00188       const fmatvec::Vec& getbe() const;
00189       const fmatvec::Vec& getbet() const;
00190 
00191       const double& getxintil() const;
00192       const double& getxibtil() const;
00193       const double& getetantil() const;
00194       const double& getetabtil() const;
00195       const double& getxintilt() const;
00196       const double& getxibtilt() const;
00197       const double& getetantilt() const;
00198       const double& getetabtilt() const;
00199       const fmatvec::SqrMat& getV() const;
00200       /***************************************************/
00201 
00206       void computeqI(const fmatvec::Vec& qG);
00207       
00212       void computeJIG(const fmatvec::Vec& qG);
00213       
00219       void computezI(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
00220       
00226       void computeCOSYt(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
00227       
00233       void computeTrafo(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
00234 
00235     private:
00239       RevCardanPtr angle;
00240 
00244       double l0, l0h2, l0h3, l0h4, l0h5;
00245       double xstar, xstarh2, xstarh3;
00246 
00250       double epstil, k0;
00251       fmatvec::Vec rS, pS;
00252 
00256       fmatvec::Vec rRrLp;
00257       fmatvec::RowVec rRrLmH;
00258 
00262       fmatvec::Vec be;
00263 
00267       fmatvec::Vec tS, nS, bS, ntilS, btilS;
00268       fmatvec::RowVec nSH, bSH, ntilSH, btilSH;
00269       fmatvec::SqrMat tSpS, nSpS, bSpS, ntilSpS, btilSpS;
00270       double xibtil, xintil, etabtil, etantil;
00271 
00275       fmatvec::Mat SMRHS_Jac;
00276 
00280       fmatvec::SqrMat V;
00281 
00285       fmatvec::Mat drRdrLp, drRdrLm, pSbE;              
00286 
00290       fmatvec::Mat SMRHS, nSbE, bSbE, ntilSbE, btilSbE;
00291       fmatvec::RowVec xibtilbE, xintilbE, etabtilbE, etantilbE;
00292 
00296       fmatvec::Mat beqG, tSqG, nSqG, bSqG, ntilSqG, btilSqG;
00297       fmatvec::RowVec xintilqG, xibtilqG, etantilqG, etabtilqG;
00298 
00302       fmatvec::SqrMat JIG, JIGt;
00303 
00307       double k0t, epstilt;
00308       fmatvec::Vec qIt, rSt, bet, pSt, tSt, nSt, bSt;
00309       fmatvec::RowVec tStH, nStH, bStH, ntilStH, btilStH;
00310       fmatvec::SqrMat tSpSt, nSpSt, bSpSt, ntilSpSt, btilSpSt;
00311       double xibtilt, xintilt, etabtilt, etantilt;
00312 
00317       void computeprelim(const fmatvec::Vec& qG);
00318       
00323       fmatvec::Vec computes0(const fmatvec::Vec& qG);
00324       
00329       void computebe(const fmatvec::Vec& qG);
00330       
00334       void computeCOSY();
00335       
00340       void computerSepstk0(const fmatvec::Vec& qG);
00341       
00345       void computedrRdrL();
00346       
00350       void computeV();
00351       
00355       void computebeqG();
00356       
00360       void computeCOSYqG();
00361       
00365       void computeCOSYt();
00366       
00371       void computeJIGt(const fmatvec::Vec& qGt);        
00372   };
00373 
00374   inline const fmatvec::SqrMat& Trafo33RCM::getJIG() const { return JIG; }
00375   inline const fmatvec::SqrMat& Trafo33RCM::getJIGt() const { return JIGt; }
00376   inline const fmatvec::Vec& Trafo33RCM::getpS() const { return pS; }
00377   inline const fmatvec::Vec& Trafo33RCM::getpSt() const { return pSt; }
00378   inline const fmatvec::Vec& Trafo33RCM::gettS() const { return tS; }
00379   inline const fmatvec::Vec& Trafo33RCM::getnS() const { return nS; }
00380   inline const fmatvec::Vec& Trafo33RCM::getbS() const { return bS; }
00381   inline const fmatvec::RowVec& Trafo33RCM::getnSH() const { return nSH; }
00382   inline const fmatvec::RowVec& Trafo33RCM::getbSH() const { return bSH; }
00383   inline const fmatvec::Vec& Trafo33RCM::getntilS() const { return ntilS; }
00384   inline const fmatvec::Vec& Trafo33RCM::getbtilS() const { return btilS; }
00385   inline const fmatvec::RowVec& Trafo33RCM::getntilSH() const { return ntilSH; }
00386   inline const fmatvec::RowVec& Trafo33RCM::getbtilSH() const { return btilSH; }
00387   inline const fmatvec::Vec& Trafo33RCM::gettSt() const { return tSt; }
00388   inline const fmatvec::Vec& Trafo33RCM::getnSt() const { return nSt; }
00389   inline const fmatvec::Vec& Trafo33RCM::getbSt() const { return bSt; }
00390   inline const fmatvec::RowVec& Trafo33RCM::gettStH() const { return tStH; }
00391   inline const fmatvec::RowVec& Trafo33RCM::getnStH() const { return nStH; }
00392   inline const fmatvec::RowVec& Trafo33RCM::getbStH() const { return bStH; }
00393   inline const fmatvec::RowVec& Trafo33RCM::getntilStH() const { return ntilStH; }
00394   inline const fmatvec::RowVec& Trafo33RCM::getbtilStH() const { return btilStH; }
00395   inline const fmatvec::SqrMat& Trafo33RCM::gettSpS() const { return tSpS; }
00396   inline const fmatvec::SqrMat& Trafo33RCM::getnSpS() const { return nSpS; }
00397   inline const fmatvec::SqrMat& Trafo33RCM::getbSpS() const { return bSpS; }
00398   inline const fmatvec::SqrMat& Trafo33RCM::getntilSpS() const { return ntilSpS; }
00399   inline const fmatvec::SqrMat& Trafo33RCM::getbtilSpS() const { return btilSpS; }
00400   inline const fmatvec::SqrMat& Trafo33RCM::gettSpSt() const { return tSpSt; }
00401   inline const fmatvec::SqrMat& Trafo33RCM::getnSpSt() const { return nSpSt; }
00402   inline const fmatvec::SqrMat& Trafo33RCM::getbSpSt() const { return bSpSt; }
00403   inline const fmatvec::SqrMat& Trafo33RCM::getntilSpSt() const { return ntilSpSt; }
00404   inline const fmatvec::SqrMat& Trafo33RCM::getbtilSpSt() const { return btilSpSt; }
00405   inline const fmatvec::Vec& Trafo33RCM::getqIt() const { return qIt; }
00406   inline const double& Trafo33RCM::getepstil() const { return epstil; }
00407   inline const double& Trafo33RCM::getepstilt() const { return epstilt; }
00408   inline const fmatvec::Vec& Trafo33RCM::getrS() const { return rS; }
00409   inline const fmatvec::Vec& Trafo33RCM::getrSt() const { return rSt; }
00410   inline const double& Trafo33RCM::getk0() const { return k0; }
00411   inline const double& Trafo33RCM::getk0t() const { return k0t; }
00412   inline const fmatvec::Vec& Trafo33RCM::getbe() const { return be; }
00413   inline const fmatvec::Vec& Trafo33RCM::getbet() const { return bet; }
00414   inline const double& Trafo33RCM::getxintil() const { return xintil; }
00415   inline const double& Trafo33RCM::getxibtil() const { return xibtil; }
00416   inline const double& Trafo33RCM::getetantil() const { return etantil; }
00417   inline const double& Trafo33RCM::getetabtil() const { return etabtil; }
00418   inline const double& Trafo33RCM::getxintilt() const { return xintilt; }
00419   inline const double& Trafo33RCM::getxibtilt() const { return xibtilt; }
00420   inline const double& Trafo33RCM::getetantilt() const { return etantilt; }
00421   inline const double& Trafo33RCM::getetabtilt() const { return etabtilt; }
00422   inline const fmatvec::SqrMat& Trafo33RCM::getV() const { return V; } 
00423   /*******************************************************************/
00424 
00425 }
00426 
00427 #endif /*TRAFO33RCM_H_*/
00428