MBSim

Project Page
spring_damper.h
00001 /* Copyright (C) 2004-2009 MBSim Development Team
00002  * This library is free software; you can redistribute it and/or 
00003  * modify it under the terms of the GNU Lesser General Public 
00004  * License as published by the Free Software Foundation; either 
00005  * version 2.1 of the License, or (at your option) any later version. 
00006  *  
00007  * This library is distributed in the hope that it will be useful, 
00008  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00009  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00010  * Lesser General Public License for more details. 
00011  *  
00012  * You should have received a copy of the GNU Lesser General Public 
00013  * License along with this library; if not, write to the Free Software 
00014  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00015  *
00016  * Contact: martin.o.foerg@googlemail.com
00017  */
00018 
00019 #ifndef _SPRINGDAMPER_H_
00020 #define _SPRINGDAMPER_H_
00021 
00022 #include "mbsim/link_mechanics.h"
00023 #include <mbsim/frame.h>
00024 #include "fmatvec/function.h"
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 RigidBody;
00034 
00039   class SpringDamper : public LinkMechanics {
00040     protected:
00041       double dist;
00042       fmatvec::Vec3 n;
00043       fmatvec::Function<double(double,double)> *func;
00044 #ifdef HAVE_OPENMBVCPPINTERFACE
00045       OpenMBV::CoilSpring *coilspringOpenMBV;
00046 #endif
00047     public:
00048       SpringDamper(const std::string &name="");
00049       ~SpringDamper();
00050       void updateh(double, int i=0);
00051       void updateg(double);
00052       void updategd(double);
00053 
00055       void connect(Frame *frame1, Frame* frame2);
00056 
00057       /*INHERITED INTERFACE OF LINK*/
00058       bool isActive() const { return true; }
00059       bool gActiveChanged() { return false; }
00060       bool isSingleValued() const { return true; }
00061       std::string getType() const { return "SpringDamper"; }
00062       void init(InitStage stage);
00063       /*****************************/
00064 
00070       void setForceFunction(fmatvec::Function<double(double,double)> *func_) { func=func_; }
00071 
00072       void plot(double t, double dt=1);
00073       void initializeUsingXML(xercesc::DOMElement *element);
00074 
00075 #ifdef HAVE_OPENMBVCPPINTERFACE
00076 
00077       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVCoilSpring, tag, (optional (numberOfCoils,(int),3)(springRadius,(double),1)(crossSectionRadius,(double),-1)(nominalLength,(double),-1)(type,(OpenMBV::CoilSpring::Type),OpenMBV::CoilSpring::tube)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00078         OpenMBVCoilSpring ombv(springRadius,crossSectionRadius,1,numberOfCoils,nominalLength,type,diffuseColor,transparency);
00079         coilspringOpenMBV=ombv.createOpenMBV();
00080       }
00081 
00083      BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVForce, 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))) { 
00084         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00085         std::vector<bool> which; which.resize(2, true);
00086         LinkMechanics::setOpenMBVForceArrow(ombv.createOpenMBV(), which);
00087       }
00088 #endif
00089     private:
00090       std::string saved_ref1, saved_ref2;
00091   };
00092 
00097   class DirectionalSpringDamper : public LinkMechanics {
00098     protected:
00099       double dist;
00100       fmatvec::Function<double(double,double)> *func;
00101       Frame *refFrame;
00102       fmatvec::Vec3 forceDir, WforceDir, WrP0P1;
00103       Frame C;
00104 #ifdef HAVE_OPENMBVCPPINTERFACE
00105       OpenMBV::CoilSpring *coilspringOpenMBV;
00106 #endif
00107     public:
00108       DirectionalSpringDamper(const std::string &name="");
00109       ~DirectionalSpringDamper();
00110       void updateh(double, int i=0);
00111       void updateg(double);
00112       void updategd(double);
00113 
00115       void connect(Frame *frame1, Frame* frame2);
00116 
00117       /*INHERITED INTERFACE OF LINK*/
00118       bool isActive() const { return true; }
00119       bool gActiveChanged() { return false; }
00120       bool isSingleValued() const { return true; }
00121       std::string getType() const { return "DirectionalSpringDamper"; }
00122       void init(InitStage stage);
00123       /*****************************/
00124 
00130       void setForceFunction(fmatvec::Function<double(double,double)> *func_) { func=func_; }
00131 
00135       void setForceDirection(const fmatvec::Vec3 &dir) { forceDir=dir/nrm2(dir); }
00136 
00137       void plot(double t, double dt=1);
00138       void initializeUsingXML(xercesc::DOMElement *element);
00139 
00140 #ifdef HAVE_OPENMBVCPPINTERFACE
00141       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVCoilSpring, tag, (optional (numberOfCoils,(int),3)(springRadius,(double),1)(crossSectionRadius,(double),-1)(nominalLength,(double),-1)(type,(OpenMBV::CoilSpring::Type),OpenMBV::CoilSpring::tube)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00142         OpenMBVCoilSpring ombv(springRadius,crossSectionRadius,1,numberOfCoils,nominalLength,type,diffuseColor,transparency);
00143         coilspringOpenMBV=ombv.createOpenMBV();
00144       }
00145 
00147      BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVForce, 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))) { 
00148         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00149         std::vector<bool> which; which.resize(2, true);
00150         LinkMechanics::setOpenMBVForceArrow(ombv.createOpenMBV(), which);
00151       }
00152 #endif
00153     private:
00154       std::string saved_ref1, saved_ref2;
00155   };
00156 
00157   class GeneralizedSpringDamper : public LinkMechanics {
00158     protected:
00159       fmatvec::Function<double(double,double)> *func;
00160       std::vector<RigidBody*> body;
00161 #ifdef HAVE_OPENMBVCPPINTERFACE
00162       OpenMBV::CoilSpring *coilspringOpenMBV;
00163 #endif
00164     public:
00165       GeneralizedSpringDamper(const std::string &name="");
00166       ~GeneralizedSpringDamper();
00167       void updateh(double, int i=0);
00168       void updateg(double);
00169       void updategd(double);
00170 
00171       bool isActive() const { return true; }
00172       bool gActiveChanged() { return false; }
00173       virtual bool isSingleValued() const { return true; }
00174       std::string getType() const { return "GeneralizedSpringDamper"; }
00175       void init(InitStage stage);
00176 
00178       void setGeneralizedForceFunction(fmatvec::Function<double(double,double)> *func_) { func=func_; }
00179 
00180       void setRigidBodyFirstSide(RigidBody* body_) { body[0] = body_; }
00181       void setRigidBodySecondSide(RigidBody* body_) { body[1] = body_; }
00182 
00183       void plot(double t, double dt=1);
00184       void initializeUsingXML(xercesc::DOMElement *element);
00185 
00186       void updatehRef(const fmatvec::Vec &hParent, int j=0);
00187 
00188 #ifdef HAVE_OPENMBVCPPINTERFACE
00189       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVCoilSpring, tag, (optional (numberOfCoils,(int),3)(springRadius,(double),1)(crossSectionRadius,(double),-1)(nominalLength,(double),-1)(type,(OpenMBV::CoilSpring::Type),OpenMBV::CoilSpring::tube)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00190         OpenMBVCoilSpring ombv(springRadius,crossSectionRadius,1,numberOfCoils,nominalLength,type,diffuseColor,transparency);
00191         coilspringOpenMBV=ombv.createOpenMBV();
00192       }
00193 
00195       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVForce, 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))) { 
00196         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00197         std::vector<bool> which; which.resize(2, true);
00198         LinkMechanics::setOpenMBVForceArrow(ombv.createOpenMBV(), which);
00199       }
00200 
00202       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVMoment, 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))) { 
00203         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00204         std::vector<bool> which; which.resize(2, true);
00205         LinkMechanics::setOpenMBVMomentArrow(ombv.createOpenMBV(), which);
00206       }
00207 #endif
00208     private:
00209       std::string saved_body1, saved_body2;
00210   };
00211 
00212 }
00213 
00214 #endif /* _SPRINGDAMPER_H_ */
00215