mbsimflexiblebody  4.0.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 <mbsim/functions/function.h>
00024 #include "mbsimFlexibleBody/pointer.h"
00025 
00026 namespace MBSimFlexibleBody {
00027 
00028   /***
00029    * \brief computation of FiniteElement1s33RCM position coordinates (cp-angles and bending)
00030    * \author Thorsten Schindler
00031    * \date 2009-04-24 initial commit (Thorsten Schindler)
00032    */
00033   class PositionFunction : public MBSim::Function<fmatvec::Vec(fmatvec::Vec)> {    
00034     public:
00043       PositionFunction(RevCardanPtr angle_,double l0_,const fmatvec::Vec& pL_,const fmatvec::Vec& pR_,
00044           double cL1_,double cR1_,double cL2_,double cR2_,const fmatvec::RowVec& rRrLmH_);
00045       
00049       virtual ~PositionFunction();
00050       
00051       /* INHERITED INTERFACE */
00052       fmatvec::Vec operator()(const fmatvec::Vec& pos);
00053       /***************************************************/
00054 
00055     private:
00059        RevCardanPtr angle;
00060 
00064       double l0;
00065 
00069       fmatvec::Vec pL, pR;
00070 
00074       double cL1, cR1, cL2, cR2;
00075 
00079       fmatvec::RowVec rRrLmH;  
00080   };
00081 
00087   class PositionJacobian : public MBSim::Function<fmatvec::SqrMat(fmatvec::Vec)> {
00088     public:
00096       PositionJacobian(RevCardanPtr angle_,double l0_,const fmatvec::RowVec &rRrLmH_,const fmatvec::Mat &pSbE_);
00097       
00101       virtual ~PositionJacobian();
00102       
00103       /* INHERITED INTERFACE */
00104       fmatvec::SqrMat operator()(const fmatvec::Vec &pos);
00105       /***************************************************/
00106 
00107     private:
00111       RevCardanPtr angle;
00112 
00116       double l0;
00117 
00121       fmatvec::RowVec rRrLmH;
00122 
00126       fmatvec::Mat pSbE;
00127   };
00128   /*******************************************************************/
00129 
00135   class Trafo33RCM {            
00136     public:
00140       Trafo33RCM(RevCardanPtr angle_,double l0_);
00141       
00145       virtual ~Trafo33RCM();
00146 
00147       /* GETTER / SETTER */
00148       const fmatvec::SqrMat& getJIG() const;            
00149       const fmatvec::SqrMat& getJIGt() const;
00150       const fmatvec::Vec& getpS() const;
00151       const fmatvec::Vec& getpSt() const;
00152       const fmatvec::Vec& gettS() const;
00153       const fmatvec::Vec& getnS() const;
00154       const fmatvec::Vec& getbS() const;
00155       const fmatvec::RowVec& getnSH() const;
00156       const fmatvec::RowVec& getbSH() const;
00157       const fmatvec::Vec& getntilS() const;
00158       const fmatvec::Vec& getbtilS() const;
00159       const fmatvec::RowVec& getntilSH() const;
00160       const fmatvec::RowVec& getbtilSH() const;
00161       const fmatvec::Vec& gettSt() const;
00162       const fmatvec::Vec& getnSt() const;
00163       const fmatvec::Vec& getbSt() const;
00164       const fmatvec::RowVec& gettStH() const;
00165       const fmatvec::RowVec& getnStH() const;
00166       const fmatvec::RowVec& getbStH() const;
00167       const fmatvec::RowVec& getntilStH() const;
00168       const fmatvec::RowVec& getbtilStH() const;
00169       const fmatvec::SqrMat& gettSpS() const;
00170       const fmatvec::SqrMat& getnSpS() const;
00171       const fmatvec::SqrMat& getbSpS() const;
00172       const fmatvec::SqrMat& getntilSpS() const;
00173       const fmatvec::SqrMat& getbtilSpS() const;
00174       const fmatvec::SqrMat& gettSpSt() const;
00175       const fmatvec::SqrMat& getnSpSt() const;
00176       const fmatvec::SqrMat& getbSpSt() const;
00177       const fmatvec::SqrMat& getntilSpSt() const;
00178       const fmatvec::SqrMat& getbtilSpSt() const;
00179 
00180       const fmatvec::Vec& getqIt() const;
00181       const double& getepstil() const;
00182       const double& getepstilt() const;
00183       const fmatvec::Vec& getrS() const;
00184       const fmatvec::Vec& getrSt() const;
00185       const double& getk0() const;
00186       const double& getk0t() const;
00187       const fmatvec::Vec& getbe() const;
00188       const fmatvec::Vec& getbet() const;
00189 
00190       const double& getxintil() const;
00191       const double& getxibtil() const;
00192       const double& getetantil() const;
00193       const double& getetabtil() const;
00194       const double& getxintilt() const;
00195       const double& getxibtilt() const;
00196       const double& getetantilt() const;
00197       const double& getetabtilt() const;
00198       const fmatvec::SqrMat& getV() const;
00199       /***************************************************/
00200 
00205       void computeqI(const fmatvec::Vec& qG);
00206       
00211       void computeJIG(const fmatvec::Vec& qG);
00212       
00218       void computezI(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
00219       
00225       void computeCOSYt(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
00226       
00232       void computeTrafo(const fmatvec::Vec& qG,const fmatvec::Vec& qGt);
00233 
00234     private:
00238       RevCardanPtr angle;
00239 
00243       double l0, l0h2, l0h3, l0h4, l0h5;
00244       double xstar, xstarh2, xstarh3;
00245 
00249       double epstil, k0;
00250       fmatvec::Vec rS, pS;
00251 
00255       fmatvec::Vec rRrLp;
00256       fmatvec::RowVec rRrLmH;
00257 
00261       fmatvec::Vec be;
00262 
00266       fmatvec::Vec tS, nS, bS, ntilS, btilS;
00267       fmatvec::RowVec nSH, bSH, ntilSH, btilSH;
00268       fmatvec::SqrMat tSpS, nSpS, bSpS, ntilSpS, btilSpS;
00269       double xibtil, xintil, etabtil, etantil;
00270 
00274       fmatvec::Mat SMRHS_Jac;
00275 
00279       fmatvec::SqrMat V;
00280 
00284       fmatvec::Mat drRdrLp, drRdrLm, pSbE;              
00285 
00289       fmatvec::Mat SMRHS, nSbE, bSbE, ntilSbE, btilSbE;
00290       fmatvec::RowVec xibtilbE, xintilbE, etabtilbE, etantilbE;
00291 
00295       fmatvec::Mat beqG, tSqG, nSqG, bSqG, ntilSqG, btilSqG;
00296       fmatvec::RowVec xintilqG, xibtilqG, etantilqG, etabtilqG;
00297 
00301       fmatvec::SqrMat JIG, JIGt;
00302 
00306       double k0t, epstilt;
00307       fmatvec::Vec qIt, rSt, bet, pSt, tSt, nSt, bSt;
00308       fmatvec::RowVec tStH, nStH, bStH, ntilStH, btilStH;
00309       fmatvec::SqrMat tSpSt, nSpSt, bSpSt, ntilSpSt, btilSpSt;
00310       double xibtilt, xintilt, etabtilt, etantilt;
00311 
00316       void computeprelim(const fmatvec::Vec& qG);
00317       
00322       fmatvec::Vec computes0(const fmatvec::Vec& qG);
00323       
00328       void computebe(const fmatvec::Vec& qG);
00329       
00333       void computeCOSY();
00334       
00339       void computerSepstk0(const fmatvec::Vec& qG);
00340       
00344       void computedrRdrL();
00345       
00349       void computeV();
00350       
00354       void computebeqG();
00355       
00359       void computeCOSYqG();
00360       
00364       void computeCOSYt();
00365       
00370       void computeJIGt(const fmatvec::Vec& qGt);        
00371   };
00372 
00373   inline const fmatvec::SqrMat& Trafo33RCM::getJIG() const { return JIG; }
00374   inline const fmatvec::SqrMat& Trafo33RCM::getJIGt() const { return JIGt; }
00375   inline const fmatvec::Vec& Trafo33RCM::getpS() const { return pS; }
00376   inline const fmatvec::Vec& Trafo33RCM::getpSt() const { return pSt; }
00377   inline const fmatvec::Vec& Trafo33RCM::gettS() const { return tS; }
00378   inline const fmatvec::Vec& Trafo33RCM::getnS() const { return nS; }
00379   inline const fmatvec::Vec& Trafo33RCM::getbS() const { return bS; }
00380   inline const fmatvec::RowVec& Trafo33RCM::getnSH() const { return nSH; }
00381   inline const fmatvec::RowVec& Trafo33RCM::getbSH() const { return bSH; }
00382   inline const fmatvec::Vec& Trafo33RCM::getntilS() const { return ntilS; }
00383   inline const fmatvec::Vec& Trafo33RCM::getbtilS() const { return btilS; }
00384   inline const fmatvec::RowVec& Trafo33RCM::getntilSH() const { return ntilSH; }
00385   inline const fmatvec::RowVec& Trafo33RCM::getbtilSH() const { return btilSH; }
00386   inline const fmatvec::Vec& Trafo33RCM::gettSt() const { return tSt; }
00387   inline const fmatvec::Vec& Trafo33RCM::getnSt() const { return nSt; }
00388   inline const fmatvec::Vec& Trafo33RCM::getbSt() const { return bSt; }
00389   inline const fmatvec::RowVec& Trafo33RCM::gettStH() const { return tStH; }
00390   inline const fmatvec::RowVec& Trafo33RCM::getnStH() const { return nStH; }
00391   inline const fmatvec::RowVec& Trafo33RCM::getbStH() const { return bStH; }
00392   inline const fmatvec::RowVec& Trafo33RCM::getntilStH() const { return ntilStH; }
00393   inline const fmatvec::RowVec& Trafo33RCM::getbtilStH() const { return btilStH; }
00394   inline const fmatvec::SqrMat& Trafo33RCM::gettSpS() const { return tSpS; }
00395   inline const fmatvec::SqrMat& Trafo33RCM::getnSpS() const { return nSpS; }
00396   inline const fmatvec::SqrMat& Trafo33RCM::getbSpS() const { return bSpS; }
00397   inline const fmatvec::SqrMat& Trafo33RCM::getntilSpS() const { return ntilSpS; }
00398   inline const fmatvec::SqrMat& Trafo33RCM::getbtilSpS() const { return btilSpS; }
00399   inline const fmatvec::SqrMat& Trafo33RCM::gettSpSt() const { return tSpSt; }
00400   inline const fmatvec::SqrMat& Trafo33RCM::getnSpSt() const { return nSpSt; }
00401   inline const fmatvec::SqrMat& Trafo33RCM::getbSpSt() const { return bSpSt; }
00402   inline const fmatvec::SqrMat& Trafo33RCM::getntilSpSt() const { return ntilSpSt; }
00403   inline const fmatvec::SqrMat& Trafo33RCM::getbtilSpSt() const { return btilSpSt; }
00404   inline const fmatvec::Vec& Trafo33RCM::getqIt() const { return qIt; }
00405   inline const double& Trafo33RCM::getepstil() const { return epstil; }
00406   inline const double& Trafo33RCM::getepstilt() const { return epstilt; }
00407   inline const fmatvec::Vec& Trafo33RCM::getrS() const { return rS; }
00408   inline const fmatvec::Vec& Trafo33RCM::getrSt() const { return rSt; }
00409   inline const double& Trafo33RCM::getk0() const { return k0; }
00410   inline const double& Trafo33RCM::getk0t() const { return k0t; }
00411   inline const fmatvec::Vec& Trafo33RCM::getbe() const { return be; }
00412   inline const fmatvec::Vec& Trafo33RCM::getbet() const { return bet; }
00413   inline const double& Trafo33RCM::getxintil() const { return xintil; }
00414   inline const double& Trafo33RCM::getxibtil() const { return xibtil; }
00415   inline const double& Trafo33RCM::getetantil() const { return etantil; }
00416   inline const double& Trafo33RCM::getetabtil() const { return etabtil; }
00417   inline const double& Trafo33RCM::getxintilt() const { return xintilt; }
00418   inline const double& Trafo33RCM::getxibtilt() const { return xibtilt; }
00419   inline const double& Trafo33RCM::getetantilt() const { return etantilt; }
00420   inline const double& Trafo33RCM::getetabtilt() const { return etabtilt; }
00421   inline const fmatvec::SqrMat& Trafo33RCM::getV() const { return V; } 
00422   /*******************************************************************/
00423 
00424 }
00425 
00426 #endif /*TRAFO33RCM_H_*/
00427