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
00054 class Contact : public LinkMechanics {
00055 public:
00060 Contact(const std::string &name = "");
00061
00065 virtual ~Contact();
00066
00067
00068 virtual void setDynamicSystemSolver(DynamicSystemSolver *sys);
00069 #ifdef HAVE_OPENMBVCPPINTERFACE
00070 boost::shared_ptr<OpenMBV::Group> getOpenMBVGrp();
00071 #endif
00072
00073
00074
00075 virtual void updatewb(double t, int i = 0);
00076 virtual void updateW(double t, int i = 0);
00077 virtual void updateV(double t, int i = 0);
00078 virtual void updateh(double t, int i = 0);
00079 virtual void updateg(double t);
00080 virtual void updategd(double t);
00081 virtual void updateStopVector(double t);
00082 virtual void updateJacobians(double t, int j = 0);
00083
00084
00085
00086 virtual void updateWRef(const fmatvec::Mat &ref, int j = 0);
00087 virtual void updateVRef(const fmatvec::Mat &ref, int j = 0);
00088 virtual void updatehRef(const fmatvec::Vec &hRef, int j = 0);
00089 virtual void updatewbRef(const fmatvec::Vec &ref);
00090 virtual void updatelaRef(const fmatvec::Vec& ref);
00091 virtual void updategRef(const fmatvec::Vec& ref);
00092 virtual void updategdRef(const fmatvec::Vec& ref);
00093 virtual void updateresRef(const fmatvec::Vec& ref);
00094 virtual void updaterFactorRef(const fmatvec::Vec &ref);
00095 virtual void updatesvRef(const fmatvec::Vec &ref);
00096 virtual void updatejsvRef(const fmatvec::VecInt &ref);
00097 virtual void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00098 virtual void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00099 virtual void calcxSize();
00100 virtual void calclaSize(int j);
00101 virtual void calcgSize(int j);
00102 virtual void calcgdSize(int j);
00103 virtual void calcrFactorSize(int j);
00104 virtual void calcsvSize();
00105 virtual void calcLinkStatusSize();
00106 virtual void calcLinkStatusRegSize();
00107 virtual void init(InitStage stage);
00108 virtual bool isSetValued() const;
00109 virtual bool isSingleValued() const;
00110 virtual void updateLinkStatus(double dt);
00111 virtual void updateLinkStatusReg(double dt);
00112 virtual bool isActive() const;
00113 virtual bool gActiveChanged();
00114 virtual bool detectImpact();
00115 virtual void solveImpactsFixpointSingle(double dt);
00116 virtual void solveConstraintsFixpointSingle();
00117 virtual void solveImpactsGaussSeidel(double dt);
00118 virtual void solveConstraintsGaussSeidel();
00119 virtual void solveImpactsRootFinding(double dt);
00120 virtual void solveConstraintsRootFinding();
00121 virtual void jacobianConstraints();
00122 virtual void jacobianImpacts();
00123 virtual void updaterFactors();
00124 virtual void checkConstraintsForTermination();
00125 virtual void checkImpactsForTermination(double dt);
00126 using LinkMechanics::connect;
00127 virtual void checkActive(int j);
00128 virtual void setlaTol(double tol);
00129 virtual void setLaTol(double tol);
00130 virtual void setgTol(double tol);
00131 virtual void setgdTol(double tol);
00132 virtual void setgddTol(double tol);
00133 virtual void setrMax(double rMax_);
00134 virtual void setLinkStatusInd(int LinkStatusInd_);
00135 virtual void setLinkStatusRegInd(int LinkStatusRegInd_);
00136 virtual void setsvInd(int svInd_);
00137 virtual void setlaInd(int laInd_);
00138 virtual void setgInd(int gInd_);
00139 virtual void setgdInd(int gdInd_);
00140 virtual void setrFactorInd(int rFactorInd_);
00141 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_, fmatvec::Vec &gdInActive_, int *IndInActive_, fmatvec::Vec &gAct_, int *IndActive_);
00142 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00143 virtual void updatecorrRef(const fmatvec::Vec& ref);
00144 virtual void updatecorr(int j);
00145 virtual void calccorrSize(int j);
00146 virtual void setcorrInd(int corrInd_);
00147 virtual void checkRoot();
00148
00149 #ifdef HAVE_OPENMBVCPPINTERFACE
00150 boost::shared_ptr<OpenMBV::Frame>& getOpenMBVFrame() {return openMBVFrame;}
00151 boost::shared_ptr<OpenMBV::Arrow>& getOpenMBVNormalForceArrow() {return contactArrow;}
00152 boost::shared_ptr<OpenMBV::Arrow>& getOpenMBVTangentialForceArrow() {return frictionArrow;}
00153 #endif
00154
00155
00156
00157 virtual std::string getType() const {
00158 return "Contact";
00159 }
00160 virtual void plot(double t, double dt = 1);
00161 virtual void closePlot();
00162
00163
00164 #ifdef HAVE_OPENMBVCPPINTERFACE
00165
00171 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVContactPoints, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00172 OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00173 openMBVFrame=ombv.createOpenMBV();
00174 }
00175
00181 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))) {
00182 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00183 contactArrow=ombv.createOpenMBV();
00184 }
00185
00193 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))) {
00194 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00195 frictionArrow=ombv.createOpenMBV();
00196 }
00197 #endif
00198
00199
00200
00201 void setNormalForceLaw(GeneralizedForceLaw *fcl_);
00202 GeneralizedForceLaw * getNormalForceLaw() const { return fcl; }
00203 void setNormalImpactLaw(GeneralizedImpactLaw *fnil_);
00204 void setTangentialForceLaw(FrictionForceLaw *fdf_);
00205 void setTangentialImpactLaw(FrictionImpactLaw *ftil_);
00206 void setContactKinematics(ContactKinematics* ck, size_t index = 0) {
00207 assert(index < contactKinematics.size());
00208 contactKinematics[index] = ck;
00209 }
00210 ContactKinematics* getContactKinematics(size_t index = 0) const {
00211 assert(index < contactKinematics.size());
00212 return contactKinematics[index];
00213 }
00214
00215 ContactKinematics* findContactKinematics(std::string cKName) const {
00216 int pos = find(ckNames.begin(), ckNames.end(), cKName) - ckNames.begin();
00217 if (pos < static_cast<int>(ckNames.size())) {
00218 return contactKinematics[pos];
00219 }
00220 throw MBSimError("Name of contact Kinematics is not valid");
00221 return NULL;
00222 }
00223
00224 const std::vector<std::vector<SingleContact> > & getSubcontacts() const {
00225 return contacts;
00226 }
00227
00228 virtual void setPlotFeatureContactKinematics(std::string cKName, PlotFeature pf, PlotFeatureStatus value);
00229
00230
00234 virtual int getFrictionDirections();
00235
00243 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0, const std::string & name = "");
00244
00245 void computeCurvatures(fmatvec::Vec & r, int contactKinematicsIndex) const;
00246
00247 virtual void initializeUsingXML(xercesc::DOMElement *element);
00248 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00249
00250 protected:
00254 std::vector<std::vector<SingleContact> > contacts;
00255
00259 std::vector<ContactKinematics*> contactKinematics;
00260
00266 std::vector<std::string> ckNames;
00267
00273 std::map<std::pair<std::string, PlotFeature>, PlotFeatureStatus> plotFeatureMap;
00274
00278 GeneralizedForceLaw *fcl;
00279
00283 FrictionForceLaw *fdf;
00284
00288 GeneralizedImpactLaw *fnil;
00289
00293 FrictionImpactLaw *ftil;
00294
00295 #ifdef HAVE_OPENMBVCPPINTERFACE
00296
00299 boost::shared_ptr<OpenMBV::Group> openMBVGrp;
00300
00304 boost::shared_ptr<OpenMBV::Frame> openMBVFrame;
00305
00309 boost::shared_ptr<OpenMBV::Arrow> contactArrow, frictionArrow;
00310 #endif
00311
00312 private:
00313 struct saved_references {
00314 std::string name1;
00315 std::string name2;
00316 std::string contourPairingName;
00317 };
00318 std::vector<saved_references> saved_ref;
00319 };
00320
00321 }
00322
00323 #endif
00324