|
mbsimflexiblebody
4.0.0
|
00001 /* Copyright (C) 2004-2015 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: martin.o.foerg@googlemail.com 00018 */ 00019 00020 #ifndef _FIXED_NODAL_FRAME_H__ 00021 #define _FIXED_NODAL_FRAME_H__ 00022 00023 #include "mbsim/frame.h" 00024 #include "mbsimFlexibleBody/utils/taylor.h" 00025 00026 namespace MBSimFlexibleBody { 00027 00032 class FixedNodalFrame : public MBSim::Frame { 00033 00034 public: 00035 FixedNodalFrame(const std::string &name = "dummy") : Frame(name), R(0), ARP(fmatvec::SqrMat3(fmatvec::EYE)), E(fmatvec::Eye()), nq(0) { } 00036 00037 FixedNodalFrame(const std::string &name, const fmatvec::Vec3 &r, const fmatvec::Mat3xV &Phi_, const fmatvec::Mat3xV &Psi_, const fmatvec::SqrMat3 &A=fmatvec::SqrMat3(fmatvec::EYE), const FixedNodalFrame *refFrame=0) : Frame(name), R(refFrame), RrRP(r), ARP(A), E(fmatvec::Eye()), Phi(Phi_), Psi(Psi_), nq(0) { } 00038 00039 std::string getType() const { return "FixedNodalFrame"; } 00040 00041 virtual void init(InitStage stage); 00042 00043 int getNumberOfModeShapes() const { return nq; } 00044 void setNumberOfModeShapes(int nq_) { nq = nq_; } 00045 00046 void updateqRef(const fmatvec::Vec& ref) { q>>ref; } 00047 void updateqdRef(const fmatvec::Vec& ref) { qd>>ref; } 00048 00049 void setRelativePosition(const fmatvec::Vec3 &r) { RrRP = r; } 00050 void setRelativeOrientation(const fmatvec::SqrMat3 &A) { ARP = A; } 00051 void setPhi(const fmatvec::Mat3xV &Phi_) { Phi = Phi_; } 00052 void setPsi(const fmatvec::Mat3xV &Psi_) { Psi = Psi_; } 00053 void setK0F(const std::vector<fmatvec::SqrMatV> &K0F_) { K0F = K0F_; } 00054 void setK0M(const std::vector<fmatvec::SqrMatV> &K0M_) { K0M = K0M_; } 00055 void setsigma0(const fmatvec::Vector<fmatvec::Fixed<6>, double > &sigma0_) { sigma0 = sigma0_; } 00056 void setsigmahel(const fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double> &sigmahel_) { sigmahel = sigmahel_; } 00057 void setsigmahen(const std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double> > &sigmahen_) { sigmahen = sigmahen_; } 00058 void setFrameOfReference(const FixedNodalFrame *frame) { R = frame; } 00059 void setFrameOfReference(const std::string &frame) { saved_frameOfReference = frame; } 00060 00061 void setPhi(const Taylor<fmatvec::Mat3xV,std::vector<fmatvec::SqrMatV> > &Phi_) { Phi = Phi_.getM0(); K0F = Phi_.getM1(); } 00062 void setPsi(const Taylor<fmatvec::Mat3xV,std::vector<fmatvec::SqrMatV> > &Psi_) { Psi = Psi_.getM0(); K0M = Psi_.getM1(); } 00063 void setsigma(const Taylor<fmatvec::Vector<fmatvec::Fixed<6>, double >,fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double>,std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double> > > &sigma_) { sigma0 = sigma_.getM0(); sigmahel = sigma_.getM1(); sigmahen = sigma_.getM2(); } 00064 00065 void setJacobianOfDeformation(const fmatvec::MatV &J, int j=0) { WJD[j] = J; } 00066 fmatvec::MatV& getJacobianOfDeformation(int j=0) { return WJD[j]; } 00067 const fmatvec::MatV& getJacobianOfDeformation(int j=0) const { return WJD[j]; } 00068 const fmatvec::Vec3& getRelativePosition() const { return RrRP; } 00069 const fmatvec::SqrMat3& getRelativeOrientation() const { return ARP; } 00070 const Frame* getFrameOfReference() const { return R; } 00071 const fmatvec::Vec3& getWrRP() const { return WrRP; } 00072 00073 void updateRelativePosition(); 00074 void updateRelativeOrientation(); 00075 void updatePosition(); 00076 void updateOrientation(); 00077 void updateVelocity(); 00078 void updateAngularVelocity(); 00079 void updateStateDependentVariables(); 00080 void updateJacobians(int j=0); 00081 void updateStateDerivativeDependentVariables(const fmatvec::Vec &ud); 00082 00083 virtual void plot(double t, double dt = 1); 00084 00085 virtual void initializeUsingXML(xercesc::DOMElement *element); 00086 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element); 00087 00088 protected: 00089 const FixedNodalFrame *R; 00090 fmatvec::Vec3 RrRP, WrRP; 00091 fmatvec::SqrMat3 ARP, APK, E; 00092 fmatvec::Mat3xV WPhi, WPsi, Phi, Psi; 00093 std::vector<fmatvec::SqrMatV> K0F, K0M; 00094 fmatvec::Vector<fmatvec::Fixed<6>, double > sigma0; 00095 fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double> sigmahel; 00096 std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double> > sigmahen; 00097 fmatvec::MatV WJD[2]; 00098 fmatvec::Vec q, qd; 00099 int nq; 00100 std::string saved_frameOfReference; 00101 }; 00102 00103 } 00104 00105 #endif /* _FIXED_NODAL_FRAME_H_ */ 00106
1.8.0