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 "SingleContact"; }
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(const boost::shared_ptr<OpenMBV::Frame> &frame) {
00136 openMBVContactFrame[0]=frame;
00137 openMBVContactFrame[1]=OpenMBV::ObjectFactory::create(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(const boost::shared_ptr<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(const boost::shared_ptr<OpenMBV::Arrow> &arrow) { frictionArrow=arrow; }
00163 #endif
00164
00165
00166 void setNormalForceLaw(GeneralizedForceLaw *fcl_);
00167 GeneralizedForceLaw * getNormalForceLaw() const {return fcl; }
00168 void setNormalImpactLaw(GeneralizedImpactLaw *fnil_);
00169 void setTangentialForceLaw(FrictionForceLaw *fdf_);
00170 FrictionForceLaw * getTangentialForceLaw() const {return fdf;}
00171 void setTangentialImpactLaw(FrictionImpactLaw *ftil_);
00172 void setContactKinematics(ContactKinematics* ck) { contactKinematics = ck; }
00173 ContactKinematics* getContactKinematics() const { return contactKinematics; }
00174 ContourPointData* & getcpData() { return cpData; }
00175 ContourPointData* getcpData() const { return cpData; }
00176 fmatvec::Vec & getlaN() { return laN; }
00177 fmatvec::Vec getlaN() const { return laN; }
00178 fmatvec::Vec & getlaT() { return laT; }
00179 fmatvec::Vec getlaT() const { return laT; }
00180 fmatvec::Vec & getgdN() { return gdN; }
00181 fmatvec::Vec getgdN() const { return gdN; }
00182 fmatvec::Vec & getgdT() { return gdT; }
00183 fmatvec::Vec getgdT() const { return gdT; }
00184
00185
00189 virtual int getFrictionDirections();
00190 virtual int getFrictionDirections() const;
00191
00199 void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0);
00200
00206 void applyh(double t, int j);
00207
00208 void computeCurvatures(fmatvec::Vec & r) const;
00209
00210 virtual void initializeUsingXML(xercesc::DOMElement *element);
00211
00212 void calccorrSize(int j);
00213 void updatecorr(int j);
00214
00215 void checkRoot();
00216
00217 protected:
00221 ContactKinematics *contactKinematics;
00222
00226 GeneralizedForceLaw *fcl;
00227
00231 FrictionForceLaw *fdf;
00232
00236 GeneralizedImpactLaw *fnil;
00237
00241 FrictionImpactLaw *ftil;
00242
00246 ContourPointData* cpData;
00247
00253 fmatvec::Vec laN;
00254
00260 fmatvec::Vec laT;
00261
00267 fmatvec::Vec gdN;
00268
00274 fmatvec::Vec gdT;
00275
00279 unsigned int gActive, gActive0;
00280
00286 unsigned int* gdActive;
00287
00293 unsigned int* gddActive;
00294
00298 fmatvec::Index iT;
00299
00303 fmatvec::Vec gdnN, gdnT;
00304
00308 fmatvec::Vec gddN, gddT;
00309
00313 fmatvec::Vec gddNBuf, gddTBuf;
00314
00315 #ifdef HAVE_OPENMBVCPPINTERFACE
00316
00319 boost::shared_ptr<OpenMBV::Group> openMBVContactGrp;
00320
00324 std::vector<boost::shared_ptr<OpenMBV::Frame> > openMBVContactFrame;
00325
00329 boost::shared_ptr<OpenMBV::Arrow> contactArrow, frictionArrow;
00330 #endif
00331
00339 int rootID;
00340
00341 private:
00342 std::string saved_ref1, saved_ref2;
00343 };
00344
00345 }
00346
00347 #endif
00348