00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _JOINT_H_
00021 #define _JOINT_H_
00022
00023 #include "mbsim/link_mechanics.h"
00024 #include "mbsim/frame.h"
00025 #include "mbsim/kinematics.h"
00026
00027 namespace MBSim {
00028
00029 class GeneralizedForceLaw;
00030 class GeneralizedImpactLaw;
00031 class FrictionForceLaw;
00032 class FrictionImpactLaw;
00033 class RigidBody;
00034
00043 class Joint: public LinkMechanics {
00044 public:
00049 Joint(const std::string &name="");
00050
00054 virtual ~Joint();
00055
00056
00057 virtual void updatewb(double t, int i=0);
00058 virtual void updateW(double t, int i=0);
00059 virtual void updateh(double t, int i=0);
00060 virtual void updateg(double t);
00061 virtual void updategd(double t);
00062 virtual void updateJacobians(double t, int j=0);
00063
00064
00065
00066 virtual void updatexd(double t);
00067 virtual void updatedx(double t, double dt);
00068 virtual void calcxSize();
00069 virtual void init(InitStage stage);
00070
00071
00072
00073 virtual void calclaSize(int j);
00074 virtual void calcgSize(int j);
00075 virtual void calcgdSize(int j);
00076 virtual void calcrFactorSize(int j);
00077 virtual bool isSetValued() const;
00078 virtual bool isSingleValued() const;
00079 virtual bool isActive() const { return true; }
00080 virtual bool gActiveChanged() { return false; }
00081 virtual void solveImpactsFixpointSingle(double dt);
00082 virtual void solveConstraintsFixpointSingle();
00083 virtual void solveImpactsGaussSeidel(double dt);
00084 virtual void solveConstraintsGaussSeidel();
00085 virtual void solveImpactsRootFinding(double dt);
00086 virtual void solveConstraintsRootFinding();
00087 virtual void jacobianConstraints();
00088 virtual void jacobianImpacts();
00089 virtual void updaterFactors();
00090 virtual void checkImpactsForTermination(double dt);
00091 virtual void checkConstraintsForTermination();
00092
00093
00094
00095 virtual void plot(double t, double dt = 1);
00096
00097
00098
00103 virtual void connect(Frame *frame1, Frame* frame2);
00104
00105
00106
00107 void setForceLaw(GeneralizedForceLaw * rc) { ffl = rc; }
00108 void setMomentLaw(GeneralizedForceLaw * rc) { fml = rc; }
00109 void setImpactForceLaw(GeneralizedImpactLaw * rc) { fifl = rc; }
00110 void setImpactMomentLaw(GeneralizedImpactLaw * rc) { fiml = rc; }
00111
00112
00116 void setForceDirection(const fmatvec::Mat3xV& fd);
00117
00121 void setMomentDirection(const fmatvec::Mat3xV& md);
00122
00123 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00124 virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00125
00126 virtual std::string getType() const { return "Joint"; }
00127
00128 #ifdef HAVE_OPENMBVCPPINTERFACE
00129
00130 void setOpenMBVForceArrow(OpenMBV::Arrow *arrow) {
00131 std::vector<bool> which; which.resize(2, false);
00132 which[1]=true;
00133 LinkMechanics::setOpenMBVForceArrow(arrow, which);
00134 }
00135
00137 void setOpenMBVMomentArrow(OpenMBV::Arrow *arrow) {
00138 std::vector<bool> which; which.resize(2, false);
00139 which[1]=true;
00140 LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00141 }
00142 #endif
00143
00144 protected:
00148 fmatvec::Index IT, IR;
00149
00153 fmatvec::Mat3xV forceDir, momentDir;
00154
00158 fmatvec::Mat3xV Wf, Wm;
00159
00163 fmatvec::Mat3xV JT;
00164
00168 fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00169
00173 GeneralizedForceLaw *ffl, *fml;
00174 GeneralizedImpactLaw *fifl, *fiml;
00175
00179 fmatvec::Vec gdn, gdd;
00180
00184 Frame C;
00185
00186 private:
00187 std::string saved_ref1, saved_ref2;
00188 };
00189
00190 class InverseKineticsJoint: public Joint {
00191 public:
00192 InverseKineticsJoint(const std::string &name);
00193 virtual void updateb(double t);
00194 void calcbSize();
00195 void setBody(RigidBody* body_) { body = body_; }
00196 virtual void init(InitStage stage);
00197 virtual bool isSetValued() const {return true;}
00198
00199 protected:
00200 RigidBody* body;
00201 };
00202
00203 }
00204
00205 #endif
00206