MBSim

Project Page
kinematics_observer.h
00001 /* Copyright (C) 2004-2013 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@gmail.com
00018  */
00019 
00020 #ifndef _KINEMATICS_OBSERVER_H__
00021 #define _KINEMATICS_OBSERVER_H__
00022 #include "mbsim/observer.h"
00023 
00024 #ifdef HAVE_OPENMBVCPPINTERFACE
00025 #include <mbsim/utils/boost_parameters.h>
00026 #include <mbsim/utils/openmbv_utils.h>
00027 #endif
00028 
00029 namespace MBSim {
00030   class Frame;
00031 
00032   class KinematicsObserver : public Observer {
00033     protected:
00034       Frame* frame;
00035       std::string saved_frame;
00036 #ifdef HAVE_OPENMBVCPPINTERFACE
00037       boost::shared_ptr<OpenMBV::Group> openMBVPosGrp, openMBVVelGrp, openMBVAngVelGrp, openMBVAccGrp, openMBVAngAccGrp;
00038       boost::shared_ptr<OpenMBV::Arrow> openMBVPosition, openMBVVelocity, openMBVAngularVelocity, openMBVAcceleration, openMBVAngularAcceleration;
00039 #endif
00040 
00041     public:
00042       KinematicsObserver(const std::string &name="");
00043       void setFrame(Frame *frame_) { frame = frame_; } 
00044       void init(InitStage stage);
00045       virtual void plot(double t, double dt);
00046       virtual void initializeUsingXML(xercesc::DOMElement *element);
00047 
00048 #ifdef HAVE_OPENMBVCPPINTERFACE
00049       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVPosition, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::fromPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00050         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00051         openMBVPosition=ombv.createOpenMBV(); 
00052       }
00053       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVVelocity, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::fromPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00054         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00055         openMBVVelocity=ombv.createOpenMBV(); 
00056       }
00057       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVAngularVelocity, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::fromPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00058         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00059         openMBVAngularVelocity=ombv.createOpenMBV(); 
00060       }
00061       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVAcceleration, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::fromPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00062         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00063         openMBVAcceleration=ombv.createOpenMBV(); 
00064       }
00065       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVAngularAcceleration, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::fromPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00066         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00067         openMBVAngularAcceleration=ombv.createOpenMBV(); 
00068       }
00069 #endif
00070   };
00071 
00072   class AbsoluteKinematicsObserver : public KinematicsObserver {
00073     public:
00074       AbsoluteKinematicsObserver(const std::string &name="") : KinematicsObserver(name) {}
00075   };
00076 
00077   class RelativeKinematicsObserver : public KinematicsObserver {
00078     private:
00079       Frame* refFrame;
00080       std::string saved_frameOfReference;
00081 #ifdef HAVE_OPENMBVCPPINTERFACE
00082       boost::shared_ptr<OpenMBV::Arrow> openMBVrTrans, openMBVrRel;
00083       boost::shared_ptr<OpenMBV::Arrow> openMBVvTrans, openMBVvRot, openMBVvRel, openMBVvF;
00084       boost::shared_ptr<OpenMBV::Arrow> openMBVaTrans, openMBVaRot, openMBVaZp, openMBVaCor, openMBVaRel, openMBVaF;
00085       boost::shared_ptr<OpenMBV::Arrow> openMBVomTrans, openMBVomRel;
00086       boost::shared_ptr<OpenMBV::Arrow> openMBVpsiTrans, openMBVpsiRot, openMBVpsiRel;
00087 #endif
00088 
00089     public:
00090       RelativeKinematicsObserver(const std::string &name="");
00091       void setFrameOfReference(Frame *frame_) { refFrame = frame_; }
00092 
00093       void init(InitStage stage);
00094       virtual void plot(double t, double dt);
00095       virtual void initializeUsingXML(xercesc::DOMElement *element);
00096   };
00097 
00098 }  
00099 
00100 #endif
00101