MBSim

Project Page
single_contact.h
00001 /* Copyright (C) 2004-2009 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 
00025 #include <map>
00026 
00027 #ifdef HAVE_OPENMBVCPPINTERFACE
00028 namespace OpenMBV {
00029   class Frame;
00030   class Arrow;
00031 }
00032 #endif
00033 
00034 namespace MBSim {
00035 
00036   class ContactKinematics;
00037   class GeneralizedForceLaw;
00038   class GeneralizedImpactLaw;
00039   class FrictionForceLaw;
00040   class FrictionImpactLaw;
00041   class ContourPointData;
00042 
00056   class SingleContact: public LinkMechanics {
00057     public:
00062       SingleContact(const std::string &name="");
00063 
00067       virtual ~SingleContact();
00068 
00069       /* INHERITED INTERFACE OF LINKINTERFACE */
00070       virtual void updatewb(double t, int i=0);
00071       virtual void updateW(double t, int i=0);
00072       virtual void updateV(double t, int i=0);
00073       virtual void updateh(double t, int i=0);
00074       virtual void updateg(double t);
00075       virtual void updategd(double t);
00076       virtual void updateStopVector(double t);
00077       virtual void updateJacobians(double t, int j=0);
00078       /***************************************************/
00079 
00080       /* INHERITED INTERFACE OF LINK */
00081       virtual void updateWRef(const fmatvec::Mat &ref, int j=0);
00082       virtual void updateVRef(const fmatvec::Mat &ref, int j=0);
00083       virtual void updatehRef(const fmatvec::Vec &hRef, int j=0);
00084       virtual void updatelaRef(const fmatvec::Vec& ref);
00085       virtual void updategdRef(const fmatvec::Vec& ref);
00086       virtual void calcxSize();
00087       virtual void calclaSize(int j);
00088       virtual void calcgSize(int j);
00089       virtual void calcgdSize(int j);
00090       virtual void calcrFactorSize(int j);
00091       virtual void calcsvSize();
00092       virtual void calcLinkStatusSize();
00093       virtual void calcLinkStatusRegSize();
00094       virtual void init(InitStage stage);
00095       virtual bool isSetValued() const;
00096       virtual bool isSingleValued() const;
00097       virtual void updateLinkStatus(double dt);
00098       virtual void updateLinkStatusReg(double dt);
00099       virtual bool isActive() const;
00100       virtual bool gActiveChanged();
00101       virtual void solveImpactsFixpointSingle(double dt);
00102       virtual void solveConstraintsFixpointSingle();
00103       virtual void solveImpactsGaussSeidel(double dt);
00104       virtual void solveConstraintsGaussSeidel();
00105       virtual void solveImpactsRootFinding(double dt);
00106       virtual void solveConstraintsRootFinding();
00107       virtual void jacobianConstraints();
00108       virtual void jacobianImpacts();
00109       virtual void updaterFactors();
00110       virtual void checkConstraintsForTermination();
00111       virtual void checkImpactsForTermination(double dt);
00112       using LinkMechanics::connect;
00113       virtual void checkActive(int j);
00114       virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_,fmatvec::Vec &gdInActive_,int *IndInActive_,fmatvec::Vec &gAct_,int *IndActive_);
00115       virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00116  
00117       /***************************************************/
00118 
00119       /* INHERITED INTERFACE OF ELEMENT */
00120       virtual std::string getType() const { return "Contact"; }
00121       virtual void plot(double t, double dt = 1);
00122       virtual void closePlot();
00123       /***************************************************/
00124 
00125 #ifdef HAVE_OPENMBVCPPINTERFACE
00126 
00132       void enableOpenMBVContactPoints(double size=1.,bool enable=true) { openMBVContactFrameSize=size; openMBVContactFrameEnabled=enable; }
00133 
00139       void setOpenMBVNormalForceArrow(OpenMBV::Arrow *arrow) { contactArrow=arrow; }
00140 
00148       void setOpenMBVFrictionArrow(OpenMBV::Arrow *arrow) { frictionArrow=arrow; }
00149 #endif
00150 
00151       /* GETTER / SETTER */
00152       void setContactForceLaw(GeneralizedForceLaw *fcl_) { fcl = fcl_; }
00153       GeneralizedForceLaw * getContactForceLaw() const {return fcl; }
00154       void setContactImpactLaw(GeneralizedImpactLaw *fnil_) { fnil = fnil_; }
00155       void setFrictionForceLaw(FrictionForceLaw *fdf_) { fdf = fdf_; }
00156       void setFrictionImpactLaw(FrictionImpactLaw *ftil_) { ftil = ftil_; }
00157       void setContactKinematics(ContactKinematics* ck) { contactKinematics = ck; }
00158       ContactKinematics* getContactKinematics() const { return contactKinematics; }
00159       ContourPointData* & getcpData() { return cpData; }
00160       ContourPointData* getcpData() const { return cpData; }
00161       fmatvec::Vec & getlaN() { return laN; }
00162       fmatvec::Vec getlaN() const { return laN; }
00163       fmatvec::Vec & getlaT() { return laT; }
00164       fmatvec::Vec getlaT() const { return laT; }
00165       fmatvec::Vec & getgdN() { return gdN; }
00166       fmatvec::Vec getgdN() const { return gdN; }
00167       fmatvec::Vec & getgdT() { return gdT; }
00168       fmatvec::Vec getgdT() const { return gdT; }
00169       /***************************************************/
00170 
00174       virtual int getFrictionDirections();
00175       virtual int getFrictionDirections() const;
00176 
00184       void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0);
00185 
00191       void applyh(int t, int j);
00192 
00193       void computeCurvatures(fmatvec::Vec & r) const;
00194 
00195       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00196 
00197       void calccorrSize(int j);
00198       void updatecorr(int j);
00199 
00200       void checkRoot();
00201 
00202     protected:
00206       ContactKinematics *contactKinematics;
00207 
00211       GeneralizedForceLaw *fcl;
00212 
00216       FrictionForceLaw *fdf;
00217       
00221       GeneralizedImpactLaw *fnil;
00222       
00226       FrictionImpactLaw *ftil;
00227 
00231       ContourPointData* cpData;
00232 
00238       fmatvec::Vec laN;
00239 
00245       fmatvec::Vec laT;
00246 
00252       fmatvec::Vec gdN;
00253 
00259       fmatvec::Vec gdT;
00260 
00264       unsigned int gActive, gActive0;
00265 
00271       unsigned int* gdActive;
00272 
00278       unsigned int* gddActive;
00279 
00283       fmatvec::Index iT;
00284 
00288       fmatvec::Vec gdnN, gdnT, gddN, gddT;
00289 
00293       fmatvec::Vec gddNBuf, gddTBuf;
00294 
00295 #ifdef HAVE_OPENMBVCPPINTERFACE
00296 
00299       OpenMBV::Group* openMBVContactGrp;
00300 
00304       std::vector<OpenMBV::Frame*> openMBVContactFrame;
00305 
00309       OpenMBV::Arrow * openMBVNormalForceArrow;
00310 
00314       OpenMBV::Arrow * openMBVFrictionArrow;
00315 
00319       double openMBVContactFrameSize;
00320 
00324       bool openMBVContactFrameEnabled;
00325 
00329       OpenMBV::Arrow *contactArrow, *frictionArrow;
00330 #endif
00331 
00335       int rootID;
00336 
00337     private:
00338       std::string saved_ref1, saved_ref2;
00339   };
00340 
00341 }
00342 
00343 #endif /* _SINGLE_CONTACT_H_ */
00344