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 "mbsim/functions/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       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(Function<double(double,double)> *func_) {
00071         func=func_;
00072         func->setParent(this);
00073         func->setName("Force");
00074       }
00075 
00076       void plot(double t, double dt=1);
00077       void initializeUsingXML(xercesc::DOMElement *element);
00078 
00079 #ifdef HAVE_OPENMBVCPPINTERFACE
00080 
00081       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))) { 
00082         OpenMBVCoilSpring ombv(springRadius,crossSectionRadius,1,numberOfCoils,nominalLength,type,diffuseColor,transparency);
00083         coilspringOpenMBV=ombv.createOpenMBV();
00084       }
00085 
00087      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))) { 
00088         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00089         std::vector<bool> which; which.resize(2, true);
00090         LinkMechanics::setOpenMBVForceArrow(ombv.createOpenMBV(), which);
00091       }
00092 #endif
00093     private:
00094       std::string saved_ref1, saved_ref2;
00095   };
00096 
00101   class DirectionalSpringDamper : public LinkMechanics {
00102     protected:
00103       double dist;
00104       Function<double(double,double)> *func;
00105       Frame *refFrame;
00106       fmatvec::Vec3 forceDir, WforceDir, WrP0P1;
00107       Frame C;
00108 #ifdef HAVE_OPENMBVCPPINTERFACE
00109       OpenMBV::CoilSpring *coilspringOpenMBV;
00110 #endif
00111     public:
00112       DirectionalSpringDamper(const std::string &name="");
00113       ~DirectionalSpringDamper();
00114       void updateh(double, int i=0);
00115       void updateg(double);
00116       void updategd(double);
00117 
00119       void connect(Frame *frame1, Frame* frame2);
00120 
00121       /*INHERITED INTERFACE OF LINK*/
00122       bool isActive() const { return true; }
00123       bool gActiveChanged() { return false; }
00124       bool isSingleValued() const { return true; }
00125       std::string getType() const { return "DirectionalSpringDamper"; }
00126       void init(InitStage stage);
00127       /*****************************/
00128 
00134       void setForceFunction(Function<double(double,double)> *func_) {
00135         func=func_;
00136         func->setParent(this);
00137         func->setName("Force");
00138       }
00139 
00143       void setForceDirection(const fmatvec::Vec3 &dir) { forceDir=dir/nrm2(dir); }
00144 
00145       void plot(double t, double dt=1);
00146       void initializeUsingXML(xercesc::DOMElement *element);
00147 
00148 #ifdef HAVE_OPENMBVCPPINTERFACE
00149       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))) { 
00150         OpenMBVCoilSpring ombv(springRadius,crossSectionRadius,1,numberOfCoils,nominalLength,type,diffuseColor,transparency);
00151         coilspringOpenMBV=ombv.createOpenMBV();
00152       }
00153 
00155      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))) { 
00156         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00157         std::vector<bool> which; which.resize(2, true);
00158         LinkMechanics::setOpenMBVForceArrow(ombv.createOpenMBV(), which);
00159       }
00160 #endif
00161     private:
00162       std::string saved_ref1, saved_ref2;
00163   };
00164 
00165   class GeneralizedSpringDamper : public LinkMechanics {
00166     protected:
00167       Function<double(double,double)> *func;
00168       std::vector<RigidBody*> body;
00169 #ifdef HAVE_OPENMBVCPPINTERFACE
00170       OpenMBV::CoilSpring *coilspringOpenMBV;
00171 #endif
00172     public:
00173       GeneralizedSpringDamper(const std::string &name="");
00174       ~GeneralizedSpringDamper();
00175       void updateh(double, int i=0);
00176       void updateg(double);
00177       void updategd(double);
00178 
00179       bool isActive() const { return true; }
00180       bool gActiveChanged() { return false; }
00181       virtual bool isSingleValued() const { return true; }
00182       std::string getType() const { return "GeneralizedSpringDamper"; }
00183       void init(InitStage stage);
00184 
00186       void setGeneralizedForceFunction(Function<double(double,double)> *func_) {
00187         func=func_;
00188         func->setParent(this);
00189         func->setName("GeneralizedFoce");
00190       }
00191 
00192       void setRigidBodyFirstSide(RigidBody* body_) { body[0] = body_; }
00193       void setRigidBodySecondSide(RigidBody* body_) { body[1] = body_; }
00194 
00195       void plot(double t, double dt=1);
00196       void initializeUsingXML(xercesc::DOMElement *element);
00197 
00198       void updatehRef(const fmatvec::Vec &hParent, int j=0);
00199 
00200 #ifdef HAVE_OPENMBVCPPINTERFACE
00201       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))) { 
00202         OpenMBVCoilSpring ombv(springRadius,crossSectionRadius,1,numberOfCoils,nominalLength,type,diffuseColor,transparency);
00203         coilspringOpenMBV=ombv.createOpenMBV();
00204       }
00205 
00207       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))) { 
00208         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00209         std::vector<bool> which; which.resize(2, true);
00210         LinkMechanics::setOpenMBVForceArrow(ombv.createOpenMBV(), which);
00211       }
00212 
00214       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))) { 
00215         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00216         std::vector<bool> which; which.resize(2, true);
00217         LinkMechanics::setOpenMBVMomentArrow(ombv.createOpenMBV(), which);
00218       }
00219 #endif
00220     private:
00221       std::string saved_body1, saved_body2;
00222   };
00223 
00224 }
00225 
00226 #endif /* _SPRINGDAMPER_H_ */
00227