MBSim

Project Page
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 _CONTACT_H_
00021 #define _CONTACT_H_
00022 
00023 #include <mbsim/link_mechanics.h>
00024 #include <mbsim/single_contact.h>
00025 #include <map>
00026 
00027 #ifdef HAVE_OPENMBVCPPINTERFACE
00028 #include "mbsim/utils/boost_parameters.h"
00029 #include "mbsim/utils/openmbv_utils.h"
00030 #endif
00031 
00032 namespace MBSim {
00033 
00034   class ContactKinematics;
00035   class GeneralizedForceLaw;
00036   class GeneralizedImpactLaw;
00037   class FrictionForceLaw;
00038   class FrictionImpactLaw;
00039 
00054   class Contact : public LinkMechanics {
00055     public:
00060       Contact(const std::string &name = "");
00061 
00065       virtual ~Contact();
00066 
00067       /*INHERITED INTERFACE OF ELEMENT*/
00068       virtual void setDynamicSystemSolver(DynamicSystemSolver *sys);
00069 #ifdef HAVE_OPENMBVCPPINTERFACE
00070       boost::shared_ptr<OpenMBV::Group> getOpenMBVGrp();
00071 #endif
00072       /********************************/
00073 
00074       /* INHERITED INTERFACE OF LINKINTERFACE */
00075       virtual void updatewb(double t, int i = 0);
00076       virtual void updateW(double t, int i = 0);
00077       virtual void updateV(double t, int i = 0);
00078       virtual void updateh(double t, int i = 0);
00079       virtual void updateg(double t);
00080       virtual void updategd(double t);
00081       virtual void updateStopVector(double t);
00082       virtual void updateJacobians(double t, int j = 0);
00083       /***************************************************/
00084 
00085       /* INHERITED INTERFACE OF LINK */
00086       virtual void updateWRef(const fmatvec::Mat &ref, int j = 0);
00087       virtual void updateVRef(const fmatvec::Mat &ref, int j = 0);
00088       virtual void updatehRef(const fmatvec::Vec &hRef, int j = 0);
00089       virtual void updatewbRef(const fmatvec::Vec &ref);
00090       virtual void updatelaRef(const fmatvec::Vec& ref);
00091       virtual void updategRef(const fmatvec::Vec& ref);
00092       virtual void updategdRef(const fmatvec::Vec& ref);
00093       virtual void updateresRef(const fmatvec::Vec& ref);
00094       virtual void updaterFactorRef(const fmatvec::Vec &ref);
00095       virtual void updatesvRef(const fmatvec::Vec &ref);
00096       virtual void updatejsvRef(const fmatvec::VecInt &ref);
00097       virtual void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00098       virtual void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00099       virtual void calcxSize();
00100       virtual void calclaSize(int j);
00101       virtual void calcgSize(int j);
00102       virtual void calcgdSize(int j);
00103       virtual void calcrFactorSize(int j);
00104       virtual void calcsvSize();
00105       virtual void calcLinkStatusSize();
00106       virtual void calcLinkStatusRegSize();
00107       virtual void init(InitStage stage);
00108       virtual bool isSetValued() const;
00109       virtual bool isSingleValued() const;
00110       virtual void updateLinkStatus(double dt);
00111       virtual void updateLinkStatusReg(double dt);
00112       virtual bool isActive() const;
00113       virtual bool gActiveChanged();
00114       virtual bool detectImpact();
00115       virtual void solveImpactsFixpointSingle(double dt);
00116       virtual void solveConstraintsFixpointSingle();
00117       virtual void solveImpactsGaussSeidel(double dt);
00118       virtual void solveConstraintsGaussSeidel();
00119       virtual void solveImpactsRootFinding(double dt);
00120       virtual void solveConstraintsRootFinding();
00121       virtual void jacobianConstraints();
00122       virtual void jacobianImpacts();
00123       virtual void updaterFactors();
00124       virtual void checkConstraintsForTermination();
00125       virtual void checkImpactsForTermination(double dt);
00126       using LinkMechanics::connect;
00127       virtual void checkActive(int j);
00128       virtual void setlaTol(double tol);
00129       virtual void setLaTol(double tol);
00130       virtual void setgTol(double tol);
00131       virtual void setgdTol(double tol);
00132       virtual void setgddTol(double tol);
00133       virtual void setrMax(double rMax_);
00134       virtual void setLinkStatusInd(int LinkStatusInd_);
00135       virtual void setLinkStatusRegInd(int LinkStatusRegInd_);
00136       virtual void setsvInd(int svInd_);
00137       virtual void setlaInd(int laInd_);
00138       virtual void setgInd(int gInd_);
00139       virtual void setgdInd(int gdInd_);
00140       virtual void setrFactorInd(int rFactorInd_);
00141       virtual void LinearImpactEstimation(fmatvec::Vec &gInActive_, fmatvec::Vec &gdInActive_, int *IndInActive_, fmatvec::Vec &gAct_, int *IndActive_);
00142       virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_);
00143       virtual void updatecorrRef(const fmatvec::Vec& ref);
00144       virtual void updatecorr(int j);
00145       virtual void calccorrSize(int j);
00146       virtual void setcorrInd(int corrInd_);
00147       virtual void checkRoot();
00148 
00149 #ifdef HAVE_OPENMBVCPPINTERFACE
00150       boost::shared_ptr<OpenMBV::Frame>& getOpenMBVFrame() {return openMBVFrame;}
00151       boost::shared_ptr<OpenMBV::Arrow>& getOpenMBVNormalForceArrow() {return contactArrow;}
00152       boost::shared_ptr<OpenMBV::Arrow>& getOpenMBVTangentialForceArrow() {return frictionArrow;}
00153 #endif
00154       /***************************************************/
00155 
00156       /* INHERITED INTERFACE OF ELEMENT */
00157       virtual std::string getType() const {
00158         return "Contact";
00159       }
00160       virtual void plot(double t, double dt = 1);
00161       virtual void closePlot();
00162       /***************************************************/
00163 
00164 #ifdef HAVE_OPENMBVCPPINTERFACE
00165 
00171       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVContactPoints, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00172         OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00173         openMBVFrame=ombv.createOpenMBV();
00174       }
00175 
00181       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))) {
00182         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00183         contactArrow=ombv.createOpenMBV();
00184       }
00185 
00193       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))) {
00194         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00195         frictionArrow=ombv.createOpenMBV();
00196       }
00197 #endif
00198 
00199       /* GETTER / SETTER */
00200 
00201       void setNormalForceLaw(GeneralizedForceLaw *fcl_);
00202       GeneralizedForceLaw * getNormalForceLaw() const { return fcl; }
00203       void setNormalImpactLaw(GeneralizedImpactLaw *fnil_);
00204       void setTangentialForceLaw(FrictionForceLaw *fdf_); 
00205       void setTangentialImpactLaw(FrictionImpactLaw *ftil_);
00206       void setContactKinematics(ContactKinematics* ck, size_t index = 0) {
00207         assert(index < contactKinematics.size());
00208         contactKinematics[index] = ck;
00209       }
00210       ContactKinematics* getContactKinematics(size_t index = 0) const {
00211         assert(index < contactKinematics.size());
00212         return contactKinematics[index];
00213       }
00214 
00215       ContactKinematics* findContactKinematics(std::string cKName) const {
00216         int pos = find(ckNames.begin(), ckNames.end(), cKName) - ckNames.begin();
00217         if (pos < static_cast<int>(ckNames.size())) {
00218           return contactKinematics[pos];
00219         }
00220         throw MBSimError("Name of contact Kinematics is not valid");
00221         return NULL;
00222       }
00223 
00224       const std::vector<std::vector<SingleContact> > & getSubcontacts() const {
00225         return contacts;
00226       }
00227 
00228       virtual void setPlotFeatureContactKinematics(std::string cKName, PlotFeature pf, PlotFeatureStatus value);
00229       /***************************************************/
00230 
00234       virtual int getFrictionDirections();
00235 
00243       void connect(Contour *contour1, Contour* contour2, ContactKinematics* contactKinematics = 0, const std::string & name = "");
00244 
00245       void computeCurvatures(fmatvec::Vec & r, int contactKinematicsIndex) const;
00246 
00247       virtual void initializeUsingXML(xercesc::DOMElement *element);
00248       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00249 
00250     protected:
00254       std::vector<std::vector<SingleContact> > contacts;
00255 
00259       std::vector<ContactKinematics*> contactKinematics;
00260 
00266       std::vector<std::string> ckNames;
00267 
00273       std::map<std::pair<std::string, PlotFeature>, PlotFeatureStatus> plotFeatureMap;
00274 
00278       GeneralizedForceLaw *fcl;
00279 
00283       FrictionForceLaw *fdf;
00284 
00288       GeneralizedImpactLaw *fnil;
00289 
00293       FrictionImpactLaw *ftil;
00294 
00295 #ifdef HAVE_OPENMBVCPPINTERFACE
00296 
00299       boost::shared_ptr<OpenMBV::Group> openMBVGrp;
00300 
00304       boost::shared_ptr<OpenMBV::Frame> openMBVFrame;
00305 
00309       boost::shared_ptr<OpenMBV::Arrow> contactArrow, frictionArrow;
00310 #endif
00311 
00312     private:
00313       struct saved_references {
00314           std::string name1;
00315           std::string name2;
00316           std::string contourPairingName;
00317       };
00318       std::vector<saved_references> saved_ref;
00319   };
00320 
00321 }
00322 
00323 #endif /* _CONTACT_H_ */
00324