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 #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       /* INHERITED INTERFACE OF LINKINTERFACE */
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       /* INHERITED INTERFACE OF LINK */
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       /* INHERITED INTERFACE OF ELEMENT */
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       /* GETTER / SETTER */
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 /* _SINGLE_CONTACT_H_ */
00336