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       OpenMBV::Group *openMBVPosGrp, *openMBVVelGrp, *openMBVAngVelGrp, *openMBVAccGrp, *openMBVAngAccGrp;
00038       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       OpenMBV::Arrow *openMBVrTrans, *openMBVrRel;
00083       OpenMBV::Arrow *openMBVvTrans, *openMBVvRot, *openMBVvRel, *openMBVvF;
00084       OpenMBV::Arrow *openMBVaTrans, *openMBVaRot, *openMBVaZp, *openMBVaCor, *openMBVaRel, *openMBVaF;
00085       OpenMBV::Arrow *openMBVomTrans, *openMBVomRel;
00086       OpenMBV::Arrow *openMBVpsiTrans, *openMBVpsiRot, *openMBVpsiRel;
00087 #endif
00088 
00089     public:
00090       RelativeKinematicsObserver(const std::string &name="");
00091       void setFrames(Frame *frame0, Frame *frame1) { frame = frame0; refFrame = frame1; } 
00092       void setFrameOfReference(Frame *frame_) { refFrame = frame_; }
00093 
00094       void init(InitStage stage);
00095       virtual void plot(double t, double dt);
00096       virtual void initializeUsingXML(xercesc::DOMElement *element);
00097   };
00098 
00099 }  
00100 
00101 #endif
00102