00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _CONTACT_H_
00021 #define _CONTACT_H_
00022
00023 #include <mbsim/link_mechanics.h>
00024
00025 #include <mbsim/single_contact.h>
00026
00027 #include <map>
00028
00029 #ifdef HAVE_OPENMBVCPPINTERFACE
00030 namespace OpenMBV {
00031 class Frame;
00032 class Arrow;
00033 }
00034 #endif
00035
00036 namespace MBSim {
00037
00038 class ContactKinematics;
00039 class GeneralizedForceLaw;
00040 class GeneralizedImpactLaw;
00041 class FrictionForceLaw;
00042 class FrictionImpactLaw;
00043
00057 class Contact : public LinkMechanics {
00058 public:
00063 Contact(const std::string &name="");
00064
00068 virtual ~Contact();
00069
00070
00071 virtual void setDynamicSystemSolver(DynamicSystemSolver *sys);
00072 #ifdef HAVE_OPENMBVCPPINTERFACE
00073 OpenMBV::Group* getOpenMBVGrp();
00074 #endif
00075
00076
00077
00078 virtual void updatewb(double t, int i = 0);
00079 virtual void updateW(double t, int i = 0);
00080 virtual void updateV(double t, int i = 0);
00081 virtual void updateh(double t, int i = 0);
00082 virtual void updateg(double t);
00083 virtual void updategd(double t);
00084 virtual void updateStopVector(double t);
00085 virtual void updateJacobians(double t, int j = 0);
00086
00087
00088
00089 virtual void updateWRef(const fmatvec::Mat &ref, int j = 0);
00090 virtual void updateVRef(const fmatvec::Mat &ref, int j = 0);
00091 virtual void updatehRef(const fmatvec::Vec &hRef, int j = 0);
00092 virtual void updatewbRef(const fmatvec::Vec &ref);
00093 virtual void updatelaRef(const fmatvec::Vec& ref);
00094 virtual void updategRef(const fmatvec::Vec& ref);
00095 virtual void updategdRef(const fmatvec::Vec& ref);
00096 virtual void updateresRef(const fmatvec::Vec& ref);
00097 virtual void updaterFactorRef(const fmatvec::Vec &ref);
00098 virtual void updatesvRef(const fmatvec::Vec &ref);
00099 virtual void updatejsvRef(const fmatvec::VecInt &ref);
00100 virtual void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00101 virtual void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00102 virtual void calcxSize();
00103 virtual void calclaSize(int j);
00104 virtual void calcgSize(int j);
00105 virtual void calcgdSize(int j);
00106 virtual void calcrFactorSize(int j);
00107 virtual void calcsvSize();
00108 virtual void calcLinkStatusSize();
00109 virtual void calcLinkStatusRegSize();
00110 virtual void init(InitStage stage);
00111 virtual bool isSetValued() const;
00112 virtual bool isSingleValued() const;
00113 virtual void updateLinkStatus(double dt);
00114 virtual void updateLinkStatusReg(double dt);
00115 virtual bool isActive() const;
00116 virtual bool gActiveChanged();
00117 virtual void solveImpactsFixpointSingle(double dt);
00118 virtual void solveConstraintsFixpointSingle();
00119 virtual void solveImpactsGaussSeidel(double dt);
00120 virtual void solveConstraintsGaussSeidel();
00121 virtual void solveImpactsRootFinding(double dt);
00122 virtual void solveConstraintsRootFinding();
00123 virtual void jacobianConstraints();
00124 virtual void jacobianImpacts();
00125 virtual void updaterFactors();
00126 virtual void checkConstraintsForTermination();
00127 virtual void checkImpactsForTermination(double dt);
00128 using LinkMechanics::connect;
00129 virtual void checkActive(int j);
00130 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_, fmatvec::Vec &gdInActive_, int *IndInActive_, fmatvec::Vec &gAct_, int *IndActive_);
00131 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00132
00133
00134 virtual void setlaTol(double tol);
00135 virtual void setLaTol(double tol);
00136 virtual void setgTol(double tol);
00137 virtual void setgdTol(double tol);
00138 virtual void setgddTol(double tol);
00139 virtual void setrMax(double rMax_);
00140
00141
00142
00143 virtual std::string getType() const {
00144 return "Contact";
00145 }
00146 virtual void plot(double t, double dt = 1);
00147 virtual void closePlot();
00148
00149
00150 #ifdef HAVE_OPENMBVCPPINTERFACE
00151
00157 void enableOpenMBVContactPoints(double size = 1., bool enable = true) {
00158 openMBVContactFrameSize = size;
00159 openMBVContactFrameEnabled = enable;
00160 }
00161
00167 void setOpenMBVNormalForceArrow(OpenMBV::Arrow *arrow) {
00168 contactArrow = arrow;
00169 }
00170
00178 void setOpenMBVFrictionArrow(OpenMBV::Arrow *arrow) {
00179 frictionArrow = arrow;
00180 }
00181 #endif
00182
00183
00184 virtual void setgInd(int gInd_);
00185 virtual void setgdInd(int gdInd_);
00186 virtual void setlaInd(int laInd_);
00187 virtual void setrFactorInd(int rFactorInd_);
00188 virtual void setcorrInd(int corrInd_);
00189 virtual void setLinkStatusInd(int LinkStatusInd_);
00190 virtual void setLinkStatusRegInd(int LinkStatusRegInd_);
00191
00192 void setContactForceLaw(GeneralizedForceLaw *fcl_) {
00193 fcl = fcl_;
00194 }
00195 GeneralizedForceLaw * getContactForceLaw() const {
00196 return fcl;
00197 }
00198 void setContactImpactLaw(GeneralizedImpactLaw *fnil_) {
00199 fnil = fnil_;
00200 }
00201 void setFrictionForceLaw(FrictionForceLaw *fdf_) {
00202 fdf = fdf_;
00203 }
00204 void setFrictionImpactLaw(FrictionImpactLaw *ftil_) {
00205 ftil = ftil_;
00206 }
00207 void setContactKinematics(ContactKinematics* ck, size_t index = 0) {
00208 assert(index < contactKinematics.size());
00209 contactKinematics[index] = ck;
00210 }
00211 ContactKinematics* getContactKinematics(size_t index = 0) const {
00212 assert(index < contactKinematics.size());
00213 return contactKinematics[index];
00214 }
00215
00216 const std::vector<std::vector<SingleContact> > & getSubcontacts() const {
00217 return contacts;
00218 }
00219
00220 virtual void setPlotFeatureContactKinematics(std::string cKName, PlotFeature pf, PlotFeatureStatus value);
00221
00222
00226 virtual int getFrictionDirections();
00227
00235 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0, const std::string & name = "");
00236
00237 void computeCurvatures(fmatvec::Vec & r, int contactKinematicsIndex) const;
00238
00239 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00240 virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00241
00242 void calccorrSize(int j);
00243 void updatecorr(int j);
00244 void updatecorrRef(const fmatvec::Vec& ref);
00245
00246 void checkRoot();
00247
00248 protected:
00252 std::vector<std::vector<SingleContact> > contacts;
00253
00257 std::vector<ContactKinematics*> contactKinematics;
00258
00264 std::vector<std::string> ckNames;
00265
00271 std::map<std::pair<std::string, MBSim::PlotFeature>, MBSim::PlotFeatureStatus> plotFeatureMap;
00272
00276 GeneralizedForceLaw *fcl;
00277
00281 FrictionForceLaw *fdf;
00282
00286 GeneralizedImpactLaw *fnil;
00287
00291 FrictionImpactLaw *ftil;
00292
00293 #ifdef HAVE_OPENMBVCPPINTERFACE
00294
00297 OpenMBV::Group * openMBVGrp;
00298
00302 double openMBVContactFrameSize;
00303
00307 bool openMBVContactFrameEnabled;
00308
00312 OpenMBV::Arrow *contactArrow, *frictionArrow;
00313 #endif
00314
00315 private:
00316 struct saved_references {
00317 std::string name1;
00318 std::string name2;
00319 std::string contourPairingName;
00320 };
00321 std::vector<saved_references> saved_ref;
00322 };
00323
00324 }
00325
00326 #endif
00327