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 "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       /* GETTER / SETTER */
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 /* _SINGLE_CONTACT_H_ */
00347