|
mbsimflexiblebody
3.2.0
|
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
1.8.0