00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _SINGLE_CONTACT_H_
00021 #define _SINGLE_CONTACT_H_
00022
00023 #include <mbsim/link_mechanics.h>
00024
00025 #include <map>
00026
00027 #ifdef HAVE_OPENMBVCPPINTERFACE
00028 namespace OpenMBV {
00029 class Frame;
00030 class Arrow;
00031 }
00032 #endif
00033
00034 namespace MBSim {
00035
00036 class ContactKinematics;
00037 class GeneralizedForceLaw;
00038 class GeneralizedImpactLaw;
00039 class FrictionForceLaw;
00040 class FrictionImpactLaw;
00041 class ContourPointData;
00042
00056 class SingleContact: public LinkMechanics {
00057 public:
00062 SingleContact(const std::string &name="");
00063
00067 virtual ~SingleContact();
00068
00069
00070 virtual void updatewb(double t, int i=0);
00071 virtual void updateW(double t, int i=0);
00072 virtual void updateV(double t, int i=0);
00073 virtual void updateh(double t, int i=0);
00074 virtual void updateg(double t);
00075 virtual void updategd(double t);
00076 virtual void updateStopVector(double t);
00077 virtual void updateJacobians(double t, int j=0);
00078
00079
00080
00081 virtual void updateWRef(const fmatvec::Mat &ref, int j=0);
00082 virtual void updateVRef(const fmatvec::Mat &ref, int j=0);
00083 virtual void updatehRef(const fmatvec::Vec &hRef, int j=0);
00084 virtual void updatelaRef(const fmatvec::Vec& ref);
00085 virtual void updategdRef(const fmatvec::Vec& ref);
00086 virtual void calcxSize();
00087 virtual void calclaSize(int j);
00088 virtual void calcgSize(int j);
00089 virtual void calcgdSize(int j);
00090 virtual void calcrFactorSize(int j);
00091 virtual void calcsvSize();
00092 virtual void calcLinkStatusSize();
00093 virtual void calcLinkStatusRegSize();
00094 virtual void init(InitStage stage);
00095 virtual bool isSetValued() const;
00096 virtual bool isSingleValued() const;
00097 virtual void updateLinkStatus(double dt);
00098 virtual void updateLinkStatusReg(double dt);
00099 virtual bool isActive() const;
00100 virtual bool gActiveChanged();
00101 virtual void solveImpactsFixpointSingle(double dt);
00102 virtual void solveConstraintsFixpointSingle();
00103 virtual void solveImpactsGaussSeidel(double dt);
00104 virtual void solveConstraintsGaussSeidel();
00105 virtual void solveImpactsRootFinding(double dt);
00106 virtual void solveConstraintsRootFinding();
00107 virtual void jacobianConstraints();
00108 virtual void jacobianImpacts();
00109 virtual void updaterFactors();
00110 virtual void checkConstraintsForTermination();
00111 virtual void checkImpactsForTermination(double dt);
00112 using LinkMechanics::connect;
00113 virtual void checkActive(int j);
00114 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_,fmatvec::Vec &gdInActive_,int *IndInActive_,fmatvec::Vec &gAct_,int *IndActive_);
00115 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00116
00117
00118
00119
00120 virtual std::string getType() const { return "Contact"; }
00121 virtual void plot(double t, double dt = 1);
00122 virtual void closePlot();
00123
00124
00125 #ifdef HAVE_OPENMBVCPPINTERFACE
00126
00132 void enableOpenMBVContactPoints(double size=1.,bool enable=true) { openMBVContactFrameSize=size; openMBVContactFrameEnabled=enable; }
00133
00139 void setOpenMBVNormalForceArrow(OpenMBV::Arrow *arrow) { contactArrow=arrow; }
00140
00148 void setOpenMBVFrictionArrow(OpenMBV::Arrow *arrow) { frictionArrow=arrow; }
00149 #endif
00150
00151
00152 void setContactForceLaw(GeneralizedForceLaw *fcl_) { fcl = fcl_; }
00153 GeneralizedForceLaw * getContactForceLaw() const {return fcl; }
00154 void setContactImpactLaw(GeneralizedImpactLaw *fnil_) { fnil = fnil_; }
00155 void setFrictionForceLaw(FrictionForceLaw *fdf_) { fdf = fdf_; }
00156 void setFrictionImpactLaw(FrictionImpactLaw *ftil_) { ftil = ftil_; }
00157 void setContactKinematics(ContactKinematics* ck) { contactKinematics = ck; }
00158 ContactKinematics* getContactKinematics() const { return contactKinematics; }
00159 ContourPointData* & getcpData() { return cpData; }
00160 ContourPointData* getcpData() const { return cpData; }
00161 fmatvec::Vec & getlaN() { return laN; }
00162 fmatvec::Vec getlaN() const { return laN; }
00163 fmatvec::Vec & getlaT() { return laT; }
00164 fmatvec::Vec getlaT() const { return laT; }
00165 fmatvec::Vec & getgdN() { return gdN; }
00166 fmatvec::Vec getgdN() const { return gdN; }
00167 fmatvec::Vec & getgdT() { return gdT; }
00168 fmatvec::Vec getgdT() const { return gdT; }
00169
00170
00174 virtual int getFrictionDirections();
00175 virtual int getFrictionDirections() const;
00176
00184 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0);
00185
00191 void applyh(int t, int j);
00192
00193 void computeCurvatures(fmatvec::Vec & r) const;
00194
00195 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00196
00197 void calccorrSize(int j);
00198 void updatecorr(int j);
00199
00200 void checkRoot();
00201
00202 protected:
00206 ContactKinematics *contactKinematics;
00207
00211 GeneralizedForceLaw *fcl;
00212
00216 FrictionForceLaw *fdf;
00217
00221 GeneralizedImpactLaw *fnil;
00222
00226 FrictionImpactLaw *ftil;
00227
00231 ContourPointData* cpData;
00232
00238 fmatvec::Vec laN;
00239
00245 fmatvec::Vec laT;
00246
00252 fmatvec::Vec gdN;
00253
00259 fmatvec::Vec gdT;
00260
00264 unsigned int gActive, gActive0;
00265
00271 unsigned int* gdActive;
00272
00278 unsigned int* gddActive;
00279
00283 fmatvec::Index iT;
00284
00288 fmatvec::Vec gdnN, gdnT, gddN, gddT;
00289
00293 fmatvec::Vec gddNBuf, gddTBuf;
00294
00295 #ifdef HAVE_OPENMBVCPPINTERFACE
00296
00299 OpenMBV::Group* openMBVContactGrp;
00300
00304 std::vector<OpenMBV::Frame*> openMBVContactFrame;
00305
00309 OpenMBV::Arrow * openMBVNormalForceArrow;
00310
00314 OpenMBV::Arrow * openMBVFrictionArrow;
00315
00319 double openMBVContactFrameSize;
00320
00324 bool openMBVContactFrameEnabled;
00325
00329 OpenMBV::Arrow *contactArrow, *frictionArrow;
00330 #endif
00331
00335 int rootID;
00336
00337 private:
00338 std::string saved_ref1, saved_ref2;
00339 };
00340
00341 }
00342
00343 #endif
00344