MBSim

Project Page
single_contact.h
00001 /* Copyright (C) 2004-2014 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
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       /* INHERITED INTERFACE OF LINKINTERFACE */
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       /* INHERITED INTERFACE OF LINK */
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       /* INHERITED INTERFACE OF ELEMENT */
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       /* GETTER / SETTER */
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 /* _SINGLE_CONTACT_H_ */
00348