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 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 setlaInd(int laInd_);
00137 virtual void setgInd(int gInd_);
00138 virtual void setgdInd(int gdInd_);
00139 virtual void setrFactorInd(int rFactorInd_);
00140 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_, fmatvec::Vec &gdInActive_, int *IndInActive_, fmatvec::Vec &gAct_, int *IndActive_);
00141 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00142 virtual void updatecorrRef(const fmatvec::Vec& ref);
00143 virtual void updatecorr(int j);
00144 virtual void calccorrSize(int j);
00145 virtual void setcorrInd(int corrInd_);
00146 virtual void checkRoot();
00147
00148 #ifdef HAVE_OPENMBVCPPINTERFACE
00149 OpenMBV::Frame* getOpenMBVFrame() { return openMBVFrame; }
00150 OpenMBV::Arrow* getOpenMBVNormalForceArrow() { return contactArrow; }
00151 OpenMBV::Arrow* getOpenMBVTangentialForceArrow() { return frictionArrow; }
00152 #endif
00153
00154
00155
00156 virtual std::string getType() const {
00157 return "Contact";
00158 }
00159 virtual void plot(double t, double dt = 1);
00160 virtual void closePlot();
00161
00162
00163 #ifdef HAVE_OPENMBVCPPINTERFACE
00164
00170 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVContactPoints, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00171 OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00172 openMBVFrame=ombv.createOpenMBV();
00173 }
00174
00180 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))) {
00181 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00182 contactArrow=ombv.createOpenMBV();
00183 }
00184
00192 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))) {
00193 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00194 frictionArrow=ombv.createOpenMBV();
00195 }
00196 #endif
00197
00198
00199
00200 void setNormalForceLaw(GeneralizedForceLaw *fcl_) {
00201 fcl = fcl_;
00202 }
00203 GeneralizedForceLaw * getNormalForceLaw() const {
00204 return fcl;
00205 }
00206 void setNormalImpactLaw(GeneralizedImpactLaw *fnil_) {
00207 fnil = fnil_;
00208 }
00209 void setTangentialForceLaw(FrictionForceLaw *fdf_) {
00210 fdf = fdf_;
00211 }
00212 void setTangentialImpactLaw(FrictionImpactLaw *ftil_) {
00213 ftil = ftil_;
00214 }
00215 void setContactKinematics(ContactKinematics* ck, size_t index = 0) {
00216 assert(index < contactKinematics.size());
00217 contactKinematics[index] = ck;
00218 }
00219 ContactKinematics* getContactKinematics(size_t index = 0) const {
00220 assert(index < contactKinematics.size());
00221 return contactKinematics[index];
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 OpenMBV::Group * openMBVGrp;
00300
00304 OpenMBV::Frame * openMBVFrame;
00305
00309 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