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
00053 class SingleContact: public LinkMechanics {
00054 public:
00059 SingleContact(const std::string &name="");
00060
00064 virtual ~SingleContact();
00065
00066
00067 virtual void updatewb(double t, int i=0);
00068 virtual void updateW(double t, int i=0);
00069 virtual void updateV(double t, int i=0);
00070 virtual void updateh(double t, int i=0);
00071 virtual void updateg(double t);
00072 virtual void updategd(double t);
00073 virtual void updateStopVector(double t);
00074 virtual void updateJacobians(double t, int j=0);
00075
00076
00077
00078 virtual void updateWRef(const fmatvec::Mat &ref, int j=0);
00079 virtual void updateVRef(const fmatvec::Mat &ref, int j=0);
00080 virtual void updatehRef(const fmatvec::Vec &hRef, int j=0);
00081 virtual void updatelaRef(const fmatvec::Vec& ref);
00082 virtual void updategdRef(const fmatvec::Vec& ref);
00083 virtual void calcxSize();
00084 virtual void calclaSize(int j);
00085 virtual void calcgSize(int j);
00086 virtual void calcgdSize(int j);
00087 virtual void calcrFactorSize(int j);
00088 virtual void calcsvSize();
00089 virtual void calcLinkStatusSize();
00090 virtual void calcLinkStatusRegSize();
00091 virtual void init(InitStage stage);
00092 virtual bool isSetValued() const;
00093 virtual bool isSingleValued() const;
00094 virtual void updateLinkStatus(double dt);
00095 virtual void updateLinkStatusReg(double dt);
00096 virtual bool isActive() const;
00097 virtual bool gActiveChanged();
00098 virtual void solveImpactsFixpointSingle(double dt);
00099 virtual void solveConstraintsFixpointSingle();
00100 virtual void solveImpactsGaussSeidel(double dt);
00101 virtual void solveConstraintsGaussSeidel();
00102 virtual void solveImpactsRootFinding(double dt);
00103 virtual void solveConstraintsRootFinding();
00104 virtual void jacobianConstraints();
00105 virtual void jacobianImpacts();
00106 virtual void updaterFactors();
00107 virtual void checkConstraintsForTermination();
00108 virtual void checkImpactsForTermination(double dt);
00109 using LinkMechanics::connect;
00110 virtual void checkActive(int j);
00111 virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_,fmatvec::Vec &gdInActive_,int *IndInActive_,fmatvec::Vec &gAct_,int *IndActive_);
00112 virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00113
00114
00115
00116
00117 virtual std::string getType() const { return "Contact"; }
00118 virtual void plot(double t, double dt = 1);
00119 virtual void closePlot();
00120
00121
00122 #ifdef HAVE_OPENMBVCPPINTERFACE
00123
00129 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVContactPoints, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00130 OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00131 setOpenMBVContactPoints(ombv.createOpenMBV());
00132 }
00133 void setOpenMBVContactPoints(OpenMBV::Frame *frame) {
00134 openMBVContactFrame[0]=frame;
00135 openMBVContactFrame[1]=new OpenMBV::Frame(*openMBVContactFrame[0]);
00136 }
00137
00143 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))) {
00144 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00145 contactArrow=ombv.createOpenMBV();
00146 }
00147 void setOpenMBVNormalForce(OpenMBV::Arrow *arrow) { contactArrow=arrow; }
00148
00156 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))) {
00157 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00158 frictionArrow=ombv.createOpenMBV();
00159 }
00160 void setOpenMBVTangentialForce(OpenMBV::Arrow *arrow) { frictionArrow=arrow; }
00161 #endif
00162
00163
00164 void setNormalForceLaw(GeneralizedForceLaw *fcl_) { fcl = fcl_; }
00165 GeneralizedForceLaw * getNormalForceLaw() const {return fcl; }
00166 void setNormalImpactLaw(GeneralizedImpactLaw *fnil_) { fnil = fnil_; }
00167 void setTangentialForceLaw(FrictionForceLaw *fdf_) { fdf = fdf_; }
00168 void setTangentialImpactLaw(FrictionImpactLaw *ftil_) { ftil = ftil_; }
00169 void setContactKinematics(ContactKinematics* ck) { contactKinematics = ck; }
00170 ContactKinematics* getContactKinematics() const { return contactKinematics; }
00171 ContourPointData* & getcpData() { return cpData; }
00172 ContourPointData* getcpData() const { return cpData; }
00173 fmatvec::Vec & getlaN() { return laN; }
00174 fmatvec::Vec getlaN() const { return laN; }
00175 fmatvec::Vec & getlaT() { return laT; }
00176 fmatvec::Vec getlaT() const { return laT; }
00177 fmatvec::Vec & getgdN() { return gdN; }
00178 fmatvec::Vec getgdN() const { return gdN; }
00179 fmatvec::Vec & getgdT() { return gdT; }
00180 fmatvec::Vec getgdT() const { return gdT; }
00181
00182
00186 virtual int getFrictionDirections();
00187 virtual int getFrictionDirections() const;
00188
00196 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0);
00197
00203 void applyh(int t, int j);
00204
00205 void computeCurvatures(fmatvec::Vec & r) const;
00206
00207 virtual void initializeUsingXML(xercesc::DOMElement *element);
00208
00209 void calccorrSize(int j);
00210 void updatecorr(int j);
00211
00212 void checkRoot();
00213
00214 protected:
00218 ContactKinematics *contactKinematics;
00219
00223 GeneralizedForceLaw *fcl;
00224
00228 FrictionForceLaw *fdf;
00229
00233 GeneralizedImpactLaw *fnil;
00234
00238 FrictionImpactLaw *ftil;
00239
00243 ContourPointData* cpData;
00244
00250 fmatvec::Vec laN;
00251
00257 fmatvec::Vec laT;
00258
00264 fmatvec::Vec gdN;
00265
00271 fmatvec::Vec gdT;
00272
00276 unsigned int gActive, gActive0;
00277
00283 unsigned int* gdActive;
00284
00290 unsigned int* gddActive;
00291
00295 fmatvec::Index iT;
00296
00300 fmatvec::Vec gdnN, gdnT, gddN, gddT;
00301
00305 fmatvec::Vec gddNBuf, gddTBuf;
00306
00307 #ifdef HAVE_OPENMBVCPPINTERFACE
00308
00311 OpenMBV::Group* openMBVContactGrp;
00312
00316 std::vector<OpenMBV::Frame*> openMBVContactFrame;
00317
00321 OpenMBV::Arrow *contactArrow, *frictionArrow;
00322 #endif
00323
00327 int rootID;
00328
00329 private:
00330 std::string saved_ref1, saved_ref2;
00331 };
00332
00333 }
00334
00335 #endif
00336