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 #include <mbsim/single_contact.h>
00025 #include <map>
00026
00027 #ifdef HAVE_OPENMBVCPPINTERFACE
00028 #include "mbsim/utils/boost_parameters.h"
00029 #include "mbsim/utils/openmbv_utils.h"
00030 #endif
00031
00032 namespace MBSim {
00033
00034 class ContactKinematics;
00035 class GeneralizedForceLaw;
00036 class GeneralizedImpactLaw;
00037 class FrictionForceLaw;
00038 class FrictionImpactLaw;
00039
00053 class Contact : public LinkMechanics {
00054 public:
00059 Contact(const std::string &name="");
00060
00064 virtual ~Contact();
00065
00066
00067 virtual void setDynamicSystemSolver(DynamicSystemSolver *sys);
00068 #ifdef HAVE_OPENMBVCPPINTERFACE
00069 OpenMBV::Group* getOpenMBVGrp();
00070 #endif
00071
00072
00073
00074 virtual void updatewb(double t, int i = 0);
00075 virtual void updateW(double t, int i = 0);
00076 virtual void updateV(double t, int i = 0);
00077 virtual void updateh(double t, int i = 0);
00078 virtual void updateg(double t);
00079 virtual void updategd(double t);
00080 virtual void updateStopVector(double t);
00081 virtual void updateJacobians(double t, int j = 0);
00082
00083
00084
00085 virtual void updateWRef(const fmatvec::Mat &ref, int j = 0);
00086 virtual void updateVRef(const fmatvec::Mat &ref, int j = 0);
00087 virtual void updatehRef(const fmatvec::Vec &hRef, int j = 0);
00088 virtual void updatewbRef(const fmatvec::Vec &ref);
00089 virtual void updatelaRef(const fmatvec::Vec& ref);
00090 virtual void updategRef(const fmatvec::Vec& ref);
00091 virtual void updategdRef(const fmatvec::Vec& ref);
00092 virtual void updateresRef(const fmatvec::Vec& ref);
00093 virtual void updaterFactorRef(const fmatvec::Vec &ref);
00094 virtual void updatesvRef(const fmatvec::Vec &ref);
00095 virtual void updatejsvRef(const fmatvec::VecInt &ref);
00096 virtual void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00097 virtual void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00098 virtual void calcxSize();
00099 virtual void calclaSize(int j);
00100 virtual void calcgSize(int j);
00101 virtual void calcgdSize(int j);
00102 virtual void calcrFactorSize(int j);
00103 virtual void calcsvSize();
00104 virtual void calcLinkStatusSize();
00105 virtual void calcLinkStatusRegSize();
00106 virtual void init(InitStage stage);
00107 virtual bool isSetValued() const;
00108 virtual bool isSingleValued() const;
00109 virtual void updateLinkStatus(double dt);
00110 virtual void updateLinkStatusReg(double dt);
00111 virtual bool isActive() const;
00112 virtual bool gActiveChanged();
00113 virtual void solveImpactsFixpointSingle(double dt);
00114 virtual void solveConstraintsFixpointSingle();
00115 virtual void solveImpactsGaussSeidel(double dt);
00116 virtual void solveConstraintsGaussSeidel();
00117 virtual void solveImpactsRootFinding(double dt);
00118 virtual void solveConstraintsRootFinding();
00119 virtual void jacobianConstraints();
00120 virtual void jacobianImpacts();
00121 virtual void updaterFactors();
00122 virtual void checkConstraintsForTermination();
00123 virtual void checkImpactsForTermination(double dt);
00124 using LinkMechanics::connect;
00125 virtual void checkActive(int j);
00126 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_, fmatvec::Vec &gdInActive_, int *IndInActive_, fmatvec::Vec &gAct_, int *IndActive_);
00127 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00128
00129
00130 virtual void setlaTol(double tol);
00131 virtual void setLaTol(double tol);
00132 virtual void setgTol(double tol);
00133 virtual void setgdTol(double tol);
00134 virtual void setgddTol(double tol);
00135 virtual void setrMax(double rMax_);
00136 #ifdef HAVE_OPENMBVCPPINTERFACE
00137 OpenMBV::Frame* getOpenMBVFrame() { return openMBVFrame; }
00138 OpenMBV::Arrow* getOpenMBVNormalForceArrow() { return contactArrow; }
00139 OpenMBV::Arrow* getOpenMBVTangentialForceArrow() { return frictionArrow; }
00140 #endif
00141
00142
00143
00144 virtual std::string getType() const {
00145 return "Contact";
00146 }
00147 virtual void plot(double t, double dt = 1);
00148 virtual void closePlot();
00149
00150
00151 #ifdef HAVE_OPENMBVCPPINTERFACE
00152
00158 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVContactPoints, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00159 OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00160 openMBVFrame=ombv.createOpenMBV();
00161 }
00162
00168 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVNormalForce, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) {
00169 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00170 contactArrow=ombv.createOpenMBV();
00171 }
00172
00180 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVTangentialForce, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) {
00181 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00182 frictionArrow=ombv.createOpenMBV();
00183 }
00184 #endif
00185
00186
00187 virtual void setgInd(int gInd_);
00188 virtual void setgdInd(int gdInd_);
00189 virtual void setlaInd(int laInd_);
00190 virtual void setrFactorInd(int rFactorInd_);
00191 virtual void setcorrInd(int corrInd_);
00192 virtual void setLinkStatusInd(int LinkStatusInd_);
00193 virtual void setLinkStatusRegInd(int LinkStatusRegInd_);
00194
00195 void setNormalForceLaw(GeneralizedForceLaw *fcl_) {
00196 fcl = fcl_;
00197 }
00198 GeneralizedForceLaw * getNormalForceLaw() const {
00199 return fcl;
00200 }
00201 void setNormalImpactLaw(GeneralizedImpactLaw *fnil_) {
00202 fnil = fnil_;
00203 }
00204 void setTangentialForceLaw(FrictionForceLaw *fdf_) {
00205 fdf = fdf_;
00206 }
00207 void setTangentialImpactLaw(FrictionImpactLaw *ftil_) {
00208 ftil = ftil_;
00209 }
00210 void setContactKinematics(ContactKinematics* ck, size_t index = 0) {
00211 assert(index < contactKinematics.size());
00212 contactKinematics[index] = ck;
00213 }
00214 ContactKinematics* getContactKinematics(size_t index = 0) const {
00215 assert(index < contactKinematics.size());
00216 return contactKinematics[index];
00217 }
00218
00219 const std::vector<std::vector<SingleContact> > & getSubcontacts() const {
00220 return contacts;
00221 }
00222
00223 virtual void setPlotFeatureContactKinematics(std::string cKName, PlotFeature pf, PlotFeatureStatus value);
00224
00225
00229 virtual int getFrictionDirections();
00230
00238 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0, const std::string & name = "");
00239
00240 void computeCurvatures(fmatvec::Vec & r, int contactKinematicsIndex) const;
00241
00242 virtual void initializeUsingXML(xercesc::DOMElement *element);
00243 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00244
00245 void calccorrSize(int j);
00246 void updatecorr(int j);
00247 void updatecorrRef(const fmatvec::Vec& ref);
00248
00249 void checkRoot();
00250
00251 protected:
00255 std::vector<std::vector<SingleContact> > contacts;
00256
00260 std::vector<ContactKinematics*> contactKinematics;
00261
00267 std::vector<std::string> ckNames;
00268
00274 std::map<std::pair<std::string, PlotFeature>, PlotFeatureStatus> plotFeatureMap;
00275
00279 GeneralizedForceLaw *fcl;
00280
00284 FrictionForceLaw *fdf;
00285
00289 GeneralizedImpactLaw *fnil;
00290
00294 FrictionImpactLaw *ftil;
00295
00296 #ifdef HAVE_OPENMBVCPPINTERFACE
00297
00300 OpenMBV::Group * openMBVGrp;
00301
00305 OpenMBV::Frame * openMBVFrame;
00306
00310 OpenMBV::Arrow *contactArrow, *frictionArrow;
00311 #endif
00312
00313 private:
00314 struct saved_references {
00315 std::string name1;
00316 std::string name2;
00317 std::string contourPairingName;
00318 };
00319 std::vector<saved_references> saved_ref;
00320 };
00321
00322 }
00323
00324 #endif
00325