mbsimflexiblebody  4.0.0
fixed_nodal_frame.h
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