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 #include <map>
00025
00026 #ifdef HAVE_OPENMBVCPPINTERFACE
00027 #include "mbsim/utils/boost_parameters.h"
00028 #include "mbsim/utils/openmbv_utils.h"
00029 #endif
00030
00031 namespace MBSim {
00032
00033 class ContactKinematics;
00034 class GeneralizedForceLaw;
00035 class GeneralizedImpactLaw;
00036 class FrictionForceLaw;
00037 class FrictionImpactLaw;
00038 class ContourPointData;
00039
00054 class SingleContact: public LinkMechanics {
00055 public:
00060 SingleContact(const std::string &name="");
00061
00065 virtual ~SingleContact();
00066
00067
00068 virtual void updatewb(double t, int i=0);
00069 virtual void updateW(double t, int i=0);
00070 virtual void updateV(double t, int i=0);
00071 virtual void updateh(double t, int i=0);
00072 virtual void updateg(double t);
00073 virtual void updategd(double t);
00074 virtual void updateStopVector(double t);
00075 virtual void updateJacobians(double t, int j=0);
00076
00077
00078
00079 virtual void updateWRef(const fmatvec::Mat &ref, int j=0);
00080 virtual void updateVRef(const fmatvec::Mat &ref, int j=0);
00081 virtual void updatehRef(const fmatvec::Vec &hRef, int j=0);
00082 virtual void updatelaRef(const fmatvec::Vec& ref);
00083 virtual void updategdRef(const fmatvec::Vec& ref);
00084 virtual void calcxSize();
00085 virtual void calclaSize(int j);
00086 virtual void calcgSize(int j);
00087 virtual void calcgdSize(int j);
00088 virtual void calcrFactorSize(int j);
00089 virtual void calcsvSize();
00090 virtual void calcLinkStatusSize();
00091 virtual void calcLinkStatusRegSize();
00092 virtual void init(InitStage stage);
00093 virtual bool isSetValued() const;
00094 virtual bool isSingleValued() const;
00095 virtual void updateLinkStatus(double dt);
00096 virtual void updateLinkStatusReg(double dt);
00097 virtual bool isActive() const;
00098 virtual bool gActiveChanged();
00099 virtual bool detectImpact();
00100 virtual void solveImpactsFixpointSingle(double dt);
00101 virtual void solveConstraintsFixpointSingle();
00102 virtual void solveImpactsGaussSeidel(double dt);
00103 virtual void solveConstraintsGaussSeidel();
00104 virtual void solveImpactsRootFinding(double dt);
00105 virtual void solveConstraintsRootFinding();
00106 virtual void jacobianConstraints();
00107 virtual void jacobianImpacts();
00108 virtual void updaterFactors();
00109 virtual void checkConstraintsForTermination();
00110 virtual void checkImpactsForTermination(double dt);
00111 using LinkMechanics::connect;
00112 virtual void checkActive(int j);
00113 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_,fmatvec::Vec &gdInActive_,int *IndInActive_,fmatvec::Vec &gAct_,int *IndActive_);
00114 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00115
00116
00117
00118
00119 virtual std::string getType() const { return "Contact"; }
00120 virtual void plot(double t, double dt = 1);
00121 virtual void closePlot();
00122
00123
00124 #ifdef HAVE_OPENMBVCPPINTERFACE
00125
00131 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVContactPoints, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00132 OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00133 setOpenMBVContactPoints(ombv.createOpenMBV());
00134 }
00135 void setOpenMBVContactPoints(OpenMBV::Frame *frame) {
00136 openMBVContactFrame[0]=frame;
00137 openMBVContactFrame[1]=new OpenMBV::Frame(*openMBVContactFrame[0]);
00138 }
00139
00145 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))) {
00146 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00147 contactArrow=ombv.createOpenMBV();
00148 }
00149 void setOpenMBVNormalForce(OpenMBV::Arrow *arrow) { contactArrow=arrow; }
00150
00158 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))) {
00159 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00160 frictionArrow=ombv.createOpenMBV();
00161 }
00162 void setOpenMBVTangentialForce(OpenMBV::Arrow *arrow) { frictionArrow=arrow; }
00163 #endif
00164
00165
00166 void setNormalForceLaw(GeneralizedForceLaw *fcl_) { fcl = fcl_; }
00167 GeneralizedForceLaw * getNormalForceLaw() const {return fcl; }
00168 void setNormalImpactLaw(GeneralizedImpactLaw *fnil_) { fnil = fnil_; }
00169 void setTangentialForceLaw(FrictionForceLaw *fdf_) { fdf = fdf_; }
00170 void setTangentialImpactLaw(FrictionImpactLaw *ftil_) { ftil = ftil_; }
00171 void setContactKinematics(ContactKinematics* ck) { contactKinematics = ck; }
00172 ContactKinematics* getContactKinematics() const { return contactKinematics; }
00173 ContourPointData* & getcpData() { return cpData; }
00174 ContourPointData* getcpData() const { return cpData; }
00175 fmatvec::Vec & getlaN() { return laN; }
00176 fmatvec::Vec getlaN() const { return laN; }
00177 fmatvec::Vec & getlaT() { return laT; }
00178 fmatvec::Vec getlaT() const { return laT; }
00179 fmatvec::Vec & getgdN() { return gdN; }
00180 fmatvec::Vec getgdN() const { return gdN; }
00181 fmatvec::Vec & getgdT() { return gdT; }
00182 fmatvec::Vec getgdT() const { return gdT; }
00183
00184
00188 virtual int getFrictionDirections();
00189 virtual int getFrictionDirections() const;
00190
00198 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0);
00199
00205 void applyh(double t, int j);
00206
00207 void computeCurvatures(fmatvec::Vec & r) const;
00208
00209 virtual void initializeUsingXML(xercesc::DOMElement *element);
00210
00211 void calccorrSize(int j);
00212 void updatecorr(int j);
00213
00214 void checkRoot();
00215
00216 protected:
00220 ContactKinematics *contactKinematics;
00221
00225 GeneralizedForceLaw *fcl;
00226
00230 FrictionForceLaw *fdf;
00231
00235 GeneralizedImpactLaw *fnil;
00236
00240 FrictionImpactLaw *ftil;
00241
00245 ContourPointData* cpData;
00246
00252 fmatvec::Vec laN;
00253
00259 fmatvec::Vec laT;
00260
00266 fmatvec::Vec gdN;
00267
00273 fmatvec::Vec gdT;
00274
00278 unsigned int gActive, gActive0;
00279
00285 unsigned int* gdActive;
00286
00292 unsigned int* gddActive;
00293
00297 fmatvec::Index iT;
00298
00302 fmatvec::Vec gdnN, gdnT;
00303
00307 fmatvec::Vec gddN, gddT;
00308
00312 fmatvec::Vec gddNBuf, gddTBuf;
00313
00314 #ifdef HAVE_OPENMBVCPPINTERFACE
00315
00318 OpenMBV::Group* openMBVContactGrp;
00319
00323 std::vector<OpenMBV::Frame*> openMBVContactFrame;
00324
00328 OpenMBV::Arrow *contactArrow, *frictionArrow;
00329 #endif
00330
00338 int rootID;
00339
00340 private:
00341 std::string saved_ref1, saved_ref2;
00342 };
00343
00344 }
00345
00346 #endif
00347