MBSim

Project Page
kinematics.h
00001 /* Copyright (C) 2004-2009 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 _KINEMATICS_H_
00021 #define _KINEMATICS_H_
00022 
00023 #include "fmatvec/fmatvec.h"
00024 #include "mbsim/utils/function.h"
00025 
00026 namespace MBSim {
00027 
00034   class Translation : public Function2<fmatvec::Vec3,fmatvec::Vec,double> {
00035     public:
00039       Translation() {}
00040 
00044       virtual ~Translation() {}
00045 
00046       /* INTERFACE FOR DERIVED CLASSES */
00050       virtual int getqSize() const = 0;
00051 
00057       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
00058 
00059       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00060       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) { return 0; }
00061       /***************************************************/
00062   };
00063 
00064   class RotationIndependentTranslation : public Translation {
00065     public:
00069       RotationIndependentTranslation() {}
00070 
00071       virtual int getqSize() const { throw; return 0; }
00072       virtual int getqTSize() const = 0;
00073       virtual int getuTSize() const = 0;
00074 
00075     protected:
00079       fmatvec::Vec3 r;
00080   };
00081 
00082   class TranslationInXDirection : public RotationIndependentTranslation {
00083     public:
00087       TranslationInXDirection() {} 
00088 
00089       /* INTERFACE OF TRANSLATION */
00090       virtual int getqTSize() const {return 1;}
00091       virtual int getuTSize() const {return 1;}
00092       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(0) = q(0); return r;}
00093       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00094       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00095   };
00096 
00097   class TranslationInYDirection : public RotationIndependentTranslation {
00098     public:
00102       TranslationInYDirection() {} 
00103 
00104       /* INTERFACE OF TRANSLATION */
00105       virtual int getqTSize() const {return 1;}
00106       virtual int getuTSize() const {return 1;}
00107       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(1) = q(0); return r;}
00108       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00109       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00110   };
00111 
00112   class TranslationInZDirection : public RotationIndependentTranslation {
00113     public:
00117       TranslationInZDirection() {} 
00118 
00119       /* INTERFACE OF TRANSLATION */
00120       virtual int getqTSize() const {return 1;}
00121       virtual int getuTSize() const {return 1;}
00122       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(2) = q(0); return r;}
00123       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00124       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00125   };
00126 
00127   class TranslationInXYDirection : public RotationIndependentTranslation {
00128     public:
00132       TranslationInXYDirection() {} 
00133 
00134       /* INTERFACE OF TRANSLATION */
00135       virtual int getqTSize() const {return 2;}
00136       virtual int getuTSize() const {return 2;}
00137       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(0) = q(0); r(1) = q(1); return r;}
00138       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00139       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00140   };
00141 
00142   class TranslationInXZDirection : public RotationIndependentTranslation {
00143     public:
00147       TranslationInXZDirection() {} 
00148 
00149       /* INTERFACE OF TRANSLATION */
00150       virtual int getqTSize() const {return 2;}
00151       virtual int getuTSize() const {return 2;}
00152       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(0) = q(0); r(2) = q(1); return r;}
00153       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00154       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00155   };
00156 
00157   class TranslationInYZDirection : public RotationIndependentTranslation {
00158     public:
00162       TranslationInYZDirection() {} 
00163 
00164       /* INTERFACE OF TRANSLATION */
00165       virtual int getqTSize() const {return 2;}
00166       virtual int getuTSize() const {return 2;}
00167       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(1) = q(0); r(2) = q(1); return r;}
00168       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00169       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00170   };
00171 
00172   class TranslationInXYZDirection : public RotationIndependentTranslation {
00173     public:
00177       TranslationInXYZDirection() {} 
00178 
00179       /* INTERFACE OF TRANSLATION */
00180       virtual int getqTSize() const {return 3;}
00181       virtual int getuTSize() const {return 3;}
00182       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {r(0) = q(0); r(1) = q(1); r(2) = q(2); return r;}
00183       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00184       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00185   };
00186 
00193   class LinearTranslation : public RotationIndependentTranslation {
00194     public:
00198       LinearTranslation() {}
00199 
00204       LinearTranslation(const fmatvec::Mat3xV &PJT_) { PJT = PJT_; }
00205 
00206       /* INTERFACE OF TRANSLATION */
00207       virtual int getqTSize() const {return PJT.cols();}
00208       virtual int getuTSize() const {return PJT.cols();}
00209       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return PJT*q(0,PJT.cols()-1); }
00210       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00211       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00212       /***************************************************/
00213 
00214       /* GETTER / SETTER */
00215       const fmatvec::Mat3xV& getTranslationVectors() const { return PJT; }
00216 
00221       void setTranslationVectors(const fmatvec::Mat3xV &PJT_) { PJT = PJT_; }
00222       /***************************************************/
00223 
00224     private:
00228       fmatvec::Mat3xV PJT;
00229   };
00230 
00231   class TimeDependentLinearTranslation: public RotationIndependentTranslation {
00232     public:
00236       TimeDependentLinearTranslation() : pos(NULL) {}
00237 
00243       TimeDependentLinearTranslation(Function1<fmatvec::VecV, double> *pos_, const fmatvec::Mat3xV &PJT_) : PJT(PJT_), pos(pos_) {}
00244 
00248       virtual ~TimeDependentLinearTranslation() { delete pos; pos = 0; }
00249 
00250       /* INTERFACE OF ROTATION */
00251       virtual int getqTSize() const {return 0;}
00252       virtual int getuTSize() const {return 0;}
00253       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return PJT*(*pos)(t); }
00254       //virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00255       /***************************************************/
00256 
00257       /* GETTER / SETTER */
00258       Function1<fmatvec::VecV, double>* getTranslationalFunction() { return pos; }
00259       void setTranslationalFunction(Function1<fmatvec::VecV, double> *pos_) { pos = pos_; }
00260       const fmatvec::Mat3xV& getTranslationVectors() const { return PJT; }
00261       void setTranslationVectors(const fmatvec::Mat3xV& PJT_) { PJT = PJT_; }
00262       /***************************************************/
00263 
00264     private:
00268       fmatvec::Mat3xV PJT;
00269 
00273       Function1<fmatvec::VecV, double> *pos;
00274   };
00275 
00282   class TimeDependentTranslation : public RotationIndependentTranslation {
00283     public:
00287       TimeDependentTranslation() : pos(NULL) {}
00288 
00293       TimeDependentTranslation(Function1<fmatvec::Vec3, double> *pos_) : pos(pos_) {}
00294 
00298       virtual ~TimeDependentTranslation() { delete pos; pos = 0; }
00299 
00300       /* INTERFACE OF TRANSLATION */
00301       virtual int getqTSize() const {return 0;}
00302       virtual int getuTSize() const {return 0;}
00303       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return (*pos)(t); }
00304       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00305       /***************************************************/
00306 
00307       /* GETTER / SETTER */
00311       Function1<fmatvec::Vec3, double>* getTranslationFunction() { return pos; }
00312       void setTranslationFunction(Function1<fmatvec::Vec3, double> *pos_) { pos = pos_; }
00313       /***************************************************/
00314 
00315     private:
00319       Function1<fmatvec::Vec3, double> *pos;
00320   };
00321 
00322   class StateDependentTranslation : public Translation {
00323     public:
00324       StateDependentTranslation() : qSize(0), pos(NULL) {}
00325 
00329       StateDependentTranslation(int qSize_, Function1<fmatvec::Vec3,fmatvec::Vec> *pos_) : qSize(qSize_), pos(pos_) {}
00330 
00334       virtual ~StateDependentTranslation() { delete pos; pos = 0; }
00335 
00336       /* INTERFACE FOR DERIVED CLASSES */
00340       virtual int getqSize() const {return qSize;}
00341 
00347       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return (*pos)(q); }
00348 
00349       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00350       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) { return 0; }
00351 
00352       /* GETTER / SETTER */
00356       Function1<fmatvec::Vec3,fmatvec::Vec>* getTranslationFunction() { return pos; }
00357       void setTranslationFunction(Function1<fmatvec::Vec3,fmatvec::Vec> *pos_) { pos = pos_; }
00358       /***************************************************/
00359 
00360     private:
00361       int qSize;
00362       Function1<fmatvec::Vec3,fmatvec::Vec> *pos;
00363   };
00364 
00365   class GeneralTranslation : public Translation {
00366     public:
00367       GeneralTranslation() : qSize(0), pos(NULL) {}
00371       GeneralTranslation(int qSize_, Function2<fmatvec::Vec3,fmatvec::Vec,double> *pos_) : qSize(qSize_), pos(pos_) {}
00372 
00376       virtual ~GeneralTranslation() { delete pos; pos = 0; }
00377 
00378       /* INTERFACE FOR DERIVED CLASSES */
00382       virtual int getqSize() const {return qSize;}
00383 
00389       virtual fmatvec::Vec3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return (*pos)(q,t); }
00390 
00391       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00392       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) { return 0; }
00393 
00394       /* GETTER / SETTER */
00398       Function2<fmatvec::Vec3,fmatvec::Vec,double>* getTranslationFunction() { return pos; }
00399       void setTranslationFunction(Function2<fmatvec::Vec3,fmatvec::Vec,double> *pos_) { pos = pos_; }
00400       /***************************************************/
00401 
00402     private:
00403       int qSize;
00404       Function2<fmatvec::Vec3,fmatvec::Vec,double> *pos;
00405   };
00406 
00413   class Rotation : public Function2<fmatvec::SqrMat3,fmatvec::Vec,double> {
00414     public:
00418       Rotation() {}
00419 
00423       virtual ~Rotation() {}
00424 
00425       /* INTERFACE FOR DERIVED CLASSES */
00429       virtual int getqSize() const = 0;
00430 
00436       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
00437 
00438      virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00439      virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent) { return 0;}
00440       /***************************************************/
00441   };
00442 
00443   class TranslationIndependentRotation: public Rotation {
00444     public:
00448       TranslationIndependentRotation() {}
00449 
00450       virtual int getqSize() const { throw; return 0; }
00451       virtual int getqRSize() const = 0;
00452       virtual int getuRSize() const = 0;
00453 
00454     protected:
00458       fmatvec::SqrMat3 APK;
00459   };
00460 
00465   class RotationAboutXAxis: public TranslationIndependentRotation {
00466     public:
00470       RotationAboutXAxis();
00471 
00472       /* INTERFACE OF ROTATION */
00473       virtual int getqRSize() const {return 1;}
00474       virtual int getuRSize() const {return 1;}
00475       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00476       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00477       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00478   };
00479 
00484   class RotationAboutYAxis: public TranslationIndependentRotation {
00485     public:
00489       RotationAboutYAxis();
00490 
00491       /* INTERFACE OF ROTATION */
00492       virtual int getqRSize() const {return 1;}
00493       virtual int getuRSize() const {return 1;}
00494       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00495       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00496       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00497   };
00498 
00504   class RotationAboutZAxis: public TranslationIndependentRotation {
00505     public:
00509       RotationAboutZAxis();
00510 
00511       /* INTERFACE OF ROTATION */
00512       virtual int getqRSize() const {return 1;}
00513       virtual int getuRSize() const {return 1;}
00514       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00515       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {}
00516       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00517   };
00518 
00525   class RotationAboutFixedAxis: public TranslationIndependentRotation {
00526     public:
00530       RotationAboutFixedAxis() {}
00531 
00536       RotationAboutFixedAxis(const fmatvec::Vec3 &a_) { a = a_; }
00537 
00538       /* INTERFACE OF ROTATION */
00539       virtual int getqRSize() const {return 1;}
00540       virtual int getuRSize() const {return 1;}
00541       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00542       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00543       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00544       /***************************************************/
00545 
00546       /* GETTER / SETTER */
00547       const fmatvec::Vec3& getAxisOfRotation() const { return a; }
00548       void setAxisOfRotation(const fmatvec::Vec3& a_) { a = a_; }
00549       /***************************************************/
00550 
00551     protected:
00555       fmatvec::Vec3 a;
00556   };
00557 
00561   class StateDependentRotationAboutFixedAxis: public Rotation {
00562     public:
00566       StateDependentRotationAboutFixedAxis() : qSize(0), rot(new RotationAboutFixedAxis()), angle(NULL) {}
00567 
00571       StateDependentRotationAboutFixedAxis(int qSize_, Function1<double, fmatvec::Vec> *angle_, const fmatvec::Vec3 &a_) : qSize(qSize_), rot(new RotationAboutFixedAxis(a_)), angle(angle_) {}
00572 
00576       virtual ~StateDependentRotationAboutFixedAxis() { delete rot; rot = 0; delete angle; angle = 0; }
00577 
00578       /* INTERFACE OF ROTATION */
00579       virtual int getqSize() const {return qSize;}
00580       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {return (*rot)(fmatvec::Vec(1,fmatvec::INIT,(*angle)(q)),t);} 
00581       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00582       /***************************************************/
00583 
00584       /* GETTER / SETTER */
00585       Function1<double, fmatvec::Vec>* getRotationalFunction() { return angle; }
00586       void setRotationalFunction(Function1<double, fmatvec::Vec> *angle_) { angle = angle_; }
00587       const fmatvec::Vec3& getAxisOfRotation() const { return rot->getAxisOfRotation(); }
00588       void setAxisOfRotation(const fmatvec::Vec3& a_) { rot->setAxisOfRotation(a_); }
00589       /***************************************************/
00590 
00591     private:
00592       int qSize;
00593       RotationAboutFixedAxis *rot;
00594       Function1<double, fmatvec::Vec> *angle;
00595   };
00596 
00604   class TimeDependentRotationAboutFixedAxis: public TranslationIndependentRotation {
00605     public:
00609       TimeDependentRotationAboutFixedAxis() : rot(new RotationAboutFixedAxis()), angle(NULL) {}
00610 
00616       TimeDependentRotationAboutFixedAxis(Function1<double, double> *angle_, const fmatvec::Vec3 &a_) : rot(new RotationAboutFixedAxis(a_)), angle(angle_) {}
00617 
00621       virtual ~TimeDependentRotationAboutFixedAxis() { delete rot; rot = 0; delete angle; angle = 0; }
00622 
00623       /* INTERFACE OF ROTATION */
00624       virtual int getqRSize() const { return 0; }
00625       virtual int getuRSize() const {return 0;}
00626       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) {return (*rot)(fmatvec::Vec(1,fmatvec::INIT,(*angle)(t)),t);}
00627       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00628       /***************************************************/
00629 
00630       /* GETTER / SETTER */
00631       Function1<double, double>* getRotationalFunction() { return angle; }
00632       void setRotationalFunction(Function1<double, double> *angle_) { angle = angle_; }
00633       const fmatvec::Vec3& getAxisOfRotation() const { return rot->getAxisOfRotation(); }
00634       void setAxisOfRotation(const fmatvec::Vec3& a_) { rot->setAxisOfRotation(a_); }
00635       /***************************************************/
00636 
00637     private:
00641       RotationAboutFixedAxis *rot;
00642 
00646       Function1<double, double> *angle;
00647   };
00648 
00655   class RotationAboutAxesXY: public TranslationIndependentRotation {
00656     public:
00660       RotationAboutAxesXY() {}
00661 
00662       /* INTERFACE OF ROTATION */
00663       virtual int getqRSize() const { return 2; }
00664       virtual int getuRSize() const { return 2; }
00665       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00666       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00667       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00668       /***************************************************/
00669   };
00674   class RotationAboutAxesXZ: public TranslationIndependentRotation {
00675     public:
00679       RotationAboutAxesXZ() {}
00680 
00681       /* INTERFACE OF ROTATION */
00682       virtual int getqRSize() const { return 2; }
00683       virtual int getuRSize() const { return 2; }
00684       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00685       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00686       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00687       /***************************************************/
00688   };
00689 
00696   class RotationAboutAxesYZ: public TranslationIndependentRotation {
00697     public:
00701       RotationAboutAxesYZ() {}
00702 
00703       /* INTERFACE OF ROTATION */
00704       virtual int getqRSize() const { return 2; }
00705       virtual int getuRSize() const { return 2; }
00706       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00707       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00708       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00709       /***************************************************/
00710   };
00711 
00718   class CardanAngles: public TranslationIndependentRotation {
00719     public:
00723       CardanAngles() {}
00724 
00725       /* INTERFACE OF ROTATION */
00726       virtual int getqRSize() const { return 3; }
00727       virtual int getuRSize() const { return 3; }
00728       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00729       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00730       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00731       /***************************************************/
00732   };
00733 
00739   class EulerAngles: public TranslationIndependentRotation {
00740     public:
00744       EulerAngles() {}
00745 
00746       /* INTERFACE OF ROTATION */
00747       virtual int getqRSize() const { return 3; }
00748       virtual int getuRSize() const { return 3; }
00749       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00750       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00751       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00752       /***************************************************/
00753   };
00754 
00761   class RotationAboutAxesXYZ: public TranslationIndependentRotation {
00762     public:
00766       RotationAboutAxesXYZ() {}
00767 
00768       /* INTERFACE OF ROTATION */
00769       virtual int getqRSize() const { return 3; }
00770       virtual int getuRSize() const { return 3; }
00771       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00772       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00773       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *parent);
00774       /***************************************************/
00775   };
00776 
00784   class TimeDependentCardanAngles: public TranslationIndependentRotation {
00785     public:
00789       TimeDependentCardanAngles() : rot(new CardanAngles()), angle(NULL) {}
00790 
00795       TimeDependentCardanAngles(Function1<fmatvec::Vec3, double> *angle_) : rot(new CardanAngles()), angle(angle_) {}
00796 
00800       virtual ~TimeDependentCardanAngles() { delete rot; rot = 0; delete angle; angle = 0; }
00801 
00802       /* INTERFACE OF ROTATION */
00803       virtual int getqRSize() const { return 0; }
00804       virtual int getuRSize() const { return 0; }
00805       virtual fmatvec::SqrMat3 operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
00806       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00807       /***************************************************/
00808 
00809       /* GETTER / SETTER */
00810       Function1<fmatvec::Vec3, double>* getRotationalFunction() { return angle; }
00811       void setRotationalFunction(Function1<fmatvec::Vec3, double> *angle_) { angle = angle_; }
00812     private:
00816       CardanAngles *rot;
00817 
00821       Function1<fmatvec::Vec3, double> *angle;
00822   };
00823 
00824   class TMatrix : public Function2<fmatvec::MatV,fmatvec::Vec,double> {
00825     public:
00829       TMatrix() {}
00830 
00834       virtual ~TMatrix() {}
00835 
00836       /* INTERFACE FOR DERIVED CLASSES */
00840       virtual int getqSize() const = 0;
00841       virtual int getuSize() const = 0;
00842 
00843       virtual fmatvec::MatV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
00844 
00845       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00846       /***************************************************/
00847   };
00848 
00856   class Jacobian : public Function2<fmatvec::Mat3xV,fmatvec::Vec,double> {
00857     public:
00861       Jacobian() {}
00862 
00866       virtual ~Jacobian() {}
00867 
00868       /* INTERFACE FOR DERIVED CLASSES */
00872       virtual int getuSize() const = 0;
00873 
00880       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
00881 
00882       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00883       /***************************************************/
00884   };
00885 
00892   class ConstantJacobian : public Jacobian {
00893     public:
00897       ConstantJacobian(const fmatvec::Mat &J_) { J = J_; }
00898 
00899       ConstantJacobian() {}
00900 
00904       virtual ~ConstantJacobian() {}
00905 
00906       /* INTERFACE OF JACOBIAN */
00907       virtual int getuSize() const { return J.cols(); }
00908       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return J; }
00909       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00910       /***************************************************/
00911 
00912     private:
00916       fmatvec::Mat3xV J;
00917   };
00918 
00919   class StateDependentJacobian : public Jacobian {
00920     public:
00924       StateDependentJacobian(int uSize_, Function1<fmatvec::Mat3xV,fmatvec::Vec> *J_) : uSize(uSize_), J(J_) {}
00925 
00929       virtual ~StateDependentJacobian() { delete J; J = 0; }
00930 
00931       /* INTERFACE FOR DERIVED CLASSES */
00935       virtual int getuSize() const {return uSize;}
00936 
00943       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return (*J)(q); }
00944 
00945       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00946 
00947       /* GETTER / SETTER */
00951       Function1<fmatvec::Mat3xV,fmatvec::Vec>* getJacobianFunction() { return J; }
00952       void setJacobianFunction(Function1<fmatvec::Mat3xV,fmatvec::Vec> *J_) { J = J_; }
00953       /***************************************************/
00954 
00955     private:
00956       int uSize;
00957       Function1<fmatvec::Mat3xV,fmatvec::Vec> *J;
00958   };
00959 
00960   class GeneralJacobian : public Jacobian {
00961     public:
00965       GeneralJacobian(int uSize_, Function2<fmatvec::Mat3xV,fmatvec::Vec,double> *J_) : uSize(uSize_), J(J_) {}
00966 
00970       virtual ~GeneralJacobian() { delete J; J = 0; }
00971 
00972       /* INTERFACE FOR DERIVED CLASSES */
00976       virtual int getuSize() const {return uSize;}
00977 
00984       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return (*J)(q,t); }
00985 
00986       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
00987 
00988       /* GETTER / SETTER */
00992       Function2<fmatvec::Mat3xV,fmatvec::Vec,double>* getJacobianFunction() { return J; }
00993       void setJacobianFunction(Function2<fmatvec::Mat3xV,fmatvec::Vec,double> *J_) { J = J_; }
00994       /***************************************************/
00995 
00996     private:
00997       int uSize;
00998       Function2<fmatvec::Mat3xV,fmatvec::Vec,double> *J;
00999   };
01000 
01006   class JRotationAboutAxesXY : public Jacobian {
01007     public:
01012       JRotationAboutAxesXY(int uSize_) : uSize(uSize_), J(uSize) {}
01013 
01014       /* INTERFACE OF JACOBIAN */
01015       int getuSize() const { return uSize; }
01016       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01017       /***************************************************/
01018 
01019     private:
01023       int uSize;
01024 
01028       fmatvec::Mat3xV J;
01029   };
01030 
01035   class JRotationAboutAxesXZ : public Jacobian {
01036     public:
01041       JRotationAboutAxesXZ(int uSize_) : uSize(uSize_), J(uSize) {}
01042 
01043       /* INTERFACE OF JACOBIAN */
01044       int getuSize() const { return uSize; }
01045       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01046       /***************************************************/
01047 
01048     private:
01052       int uSize;
01053 
01057       fmatvec::Mat3xV J;
01058   };
01059 
01065   class JRotationAboutAxesYZ : public Jacobian {
01066     public:
01071       JRotationAboutAxesYZ(int uSize_) : uSize(uSize_), J(uSize) {}
01072 
01073       /* INTERFACE OF JACOBIAN */
01074       int getuSize() const { return uSize; }
01075       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01076       /***************************************************/
01077 
01078     private:
01082       int uSize;
01083 
01087       fmatvec::Mat3xV J;
01088   };
01089 
01095   class JRotationAboutAxesXYZ : public Jacobian {
01096     public:
01101       JRotationAboutAxesXYZ(int uSize_) : uSize(uSize_), J(uSize) {}
01102 
01103       /* INTERFACE OF JACOBIAN */
01104       int getuSize() const { return uSize; }
01105       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01106       /***************************************************/
01107 
01108     private:
01112       int uSize;
01113 
01117       fmatvec::Mat3xV J;
01118   };
01119 
01126   class TCardanAngles : public TMatrix {
01127     public:
01133       TCardanAngles(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {}
01134 
01135       /* INTERFACE OF JACOBIAN */
01136       int getqSize() const { return qSize; }
01137       int getuSize() const { return uSize; }
01138       virtual fmatvec::MatV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01139       /***************************************************/
01140 
01141     private:
01145       int qSize, uSize;
01146 
01150       fmatvec::MatV T;
01151   };
01152 
01158   class TEulerAngles : public TMatrix {
01159     public:
01165       TEulerAngles(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {
01166         int iq = qSize-1;
01167         int iu = uSize-1;
01168         T(iq-2,iu) = 1;
01169         T(iq,iu) = 0;
01170       }
01171 
01172       /* INTERFACE OF JACOBIAN */
01173       int getqSize() const { return qSize; }
01174       int getuSize() const { return uSize; }
01175       virtual fmatvec::MatV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01176       /***************************************************/
01177 
01178     private:
01182       int qSize, uSize;
01183 
01187       fmatvec::MatV T;
01188   };
01189 
01196   class TCardanAngles2 : public TMatrix {
01197     public:
01203       TCardanAngles2(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {}
01204 
01205       /* INTERFACE OF JACOBIAN */
01206       int getqSize() const { return qSize; }
01207       int getuSize() const { return uSize; }
01208       virtual fmatvec::MatV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01209       /***************************************************/
01210 
01211     private:
01215       int qSize, uSize;
01216 
01220       fmatvec::MatV T;
01221   };
01222 
01228   class TEulerAngles2 : public TMatrix {
01229     public:
01235       TEulerAngles2(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {
01236       }
01237 
01238       /* INTERFACE OF JACOBIAN */
01239       int getqSize() const { return qSize; }
01240       int getuSize() const { return uSize; }
01241       virtual fmatvec::MatV operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
01242       /***************************************************/
01243 
01244     private:
01248       int qSize, uSize;
01249 
01253       fmatvec::MatV T;
01254   };
01255 
01260   class DerivativeOfJacobian : public Function3<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec,double> {
01261     public:
01265       DerivativeOfJacobian() {}
01266 
01270       virtual ~DerivativeOfJacobian() {}
01271 
01279       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL) = 0;
01280 
01281       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01282       /***************************************************/
01283   };
01284 
01285   class StateDependentDerivativeOfJacobian : public DerivativeOfJacobian {
01286     public:
01290       StateDependentDerivativeOfJacobian(Function2<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec> *Jd_) : Jd(Jd_) {}
01291 
01295       virtual ~StateDependentDerivativeOfJacobian() { delete Jd; Jd = 0; }
01296 
01304       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL) { return (*Jd)(qd,q); }
01305 
01306       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01307 
01308       /* GETTER / SETTER */
01312       Function2<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec>* getDerivativeOfJacobianFunction() { return Jd; }
01313       void setDerivativeOfJacobianFunction(Function2<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec> *Jd_) { Jd = Jd_; }
01314       /***************************************************/
01315 
01316     private:
01317       Function2<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec> *Jd;
01318   };
01319 
01320   class GeneralDerivativeOfJacobian : public DerivativeOfJacobian {
01321     public:
01325       GeneralDerivativeOfJacobian(Function3<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec,double> *Jd_) : Jd(Jd_) {}
01326 
01330       virtual ~GeneralDerivativeOfJacobian() { delete Jd; Jd = 0; }
01331 
01339       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL) { return (*Jd)(qd,q,t); }
01340 
01341       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01342 
01343       /* GETTER / SETTER */
01347       Function3<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec,double>* getDerivativeOfJacobianFunction() { return Jd; }
01348       void setDerivativeOfJacobianFunction(Function3<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec,double> *Jd_) { Jd = Jd_; }
01349       /***************************************************/
01350 
01351     private:
01352       Function3<fmatvec::Mat3xV,fmatvec::Vec,fmatvec::Vec,double> *Jd;
01353   };
01354 
01359   class JdRotationAboutAxesXY : public DerivativeOfJacobian {
01360     public:
01365       JdRotationAboutAxesXY(int uSize_) : uSize(uSize_), Jd(uSize) {}
01366 
01367       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL);
01368 
01369     private:
01373       int uSize;
01374 
01378       fmatvec::Mat3xV Jd;
01379   };
01380 
01385   class JdRotationAboutAxesXZ : public DerivativeOfJacobian {
01386     public:
01391       JdRotationAboutAxesXZ(int uSize_) : uSize(uSize_), Jd(uSize) {}
01392 
01393       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL);
01394 
01395     private:
01399       int uSize;
01400 
01404       fmatvec::Mat3xV Jd;
01405   };
01406 
01411   class JdRotationAboutAxesYZ : public DerivativeOfJacobian {
01412     public:
01417       JdRotationAboutAxesYZ(int uSize_) : uSize(uSize_), Jd(uSize) {}
01418 
01419       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL);
01420 
01421     private:
01425       int uSize;
01426 
01430       fmatvec::Mat3xV Jd;
01431   };
01432 
01437   class JdRotationAboutAxesXYZ : public DerivativeOfJacobian {
01438     public:
01443       JdRotationAboutAxesXYZ(int uSize_) : uSize(uSize_), Jd(uSize) {}
01444 
01445       virtual fmatvec::Mat3xV operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void* =NULL);
01446 
01447     private:
01451       int uSize;
01452 
01456       fmatvec::Mat3xV Jd;
01457   };
01458 
01463   class GuidingVelocity : public Function2<fmatvec::Vec3,fmatvec::Vec,double> {
01464     public:
01468       GuidingVelocity() {}
01469 
01473       virtual ~GuidingVelocity() {}
01474 
01482       virtual fmatvec::Vec3 operator()(const fmatvec::Vec& q, const double& t, const void* =NULL) = 0;
01483 
01484       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01485       /***************************************************/
01486   };
01487 
01488   class TimeDependentGuidingVelocity : public GuidingVelocity {
01489     public:
01493       TimeDependentGuidingVelocity() : j(NULL) {}
01494 
01498       TimeDependentGuidingVelocity(Function1<fmatvec::Vec3,double> *j_) : j(j_) {}
01499 
01503       virtual ~TimeDependentGuidingVelocity() { delete j; j = 0; }
01504 
01511       virtual fmatvec::Vec3 operator()(const fmatvec::Vec& q, const double& t, const void* =NULL) { return (*j)(t); }
01512 
01513       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
01514 
01515       /* GETTER / SETTER */
01519       Function1<fmatvec::Vec3,double>* getGuidingVelocityFunction() { return j; }
01520       void setGuidingVelocityFunction(Function1<fmatvec::Vec3,double> *j_) { j = j_; }
01521       /***************************************************/
01522 
01523     private:
01524       Function1<fmatvec::Vec3,double> *j;
01525   };
01526 
01527   class GeneralGuidingVelocity : public GuidingVelocity {
01528     public:
01532       GeneralGuidingVelocity(Function2<fmatvec::Vec3,fmatvec::Vec,double> *j_) : j(j_) {}
01533 
01537       virtual ~GeneralGuidingVelocity() { delete j; j = 0; }
01538 
01546       virtual fmatvec::Vec3 operator()(const fmatvec::Vec& q, const double& t, const void* =NULL) { return (*j)(q,t); }
01547 
01548       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01549 
01550       /* GETTER / SETTER */
01554       Function2<fmatvec::Vec3,fmatvec::Vec,double>* getGuidingVelocityFunction() { return j; }
01555       void setGuidingVelocityFunction(Function2<fmatvec::Vec3,fmatvec::Vec,double> *j_) { j = j_; }
01556       /***************************************************/
01557 
01558     private:
01559       Function2<fmatvec::Vec3,fmatvec::Vec,double> *j;
01560   };
01561 
01566   class DerivativeOfGuidingVelocity : public Function3<fmatvec::Vec3,fmatvec::Vec,fmatvec::Vec,double> {
01567     public:
01571       DerivativeOfGuidingVelocity() {}
01572 
01576       virtual ~DerivativeOfGuidingVelocity() {}
01577 
01585       virtual fmatvec::Vec3 operator()(const fmatvec::Vec& qd, const fmatvec::Vec& q, const double& t, const void* =NULL) = 0;
01586 
01587       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01588       /***************************************************/
01589   };
01590 
01591   class TimeDependentDerivativeOfGuidingVelocity : public DerivativeOfGuidingVelocity {
01592     public:
01596       TimeDependentDerivativeOfGuidingVelocity() : jd(NULL) {}
01597 
01601       TimeDependentDerivativeOfGuidingVelocity(Function1<fmatvec::Vec3,double> *jd_) : jd(jd_) {}
01602 
01606       virtual ~TimeDependentDerivativeOfGuidingVelocity() { delete jd; jd = 0; }
01607 
01614       virtual fmatvec::Vec3 operator()(const fmatvec::Vec& qd, const fmatvec::Vec& q, const double& t, const void* =NULL) { return (*jd)(t); }
01615 
01616       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
01617 
01618       /* GETTER / SETTER */
01622       Function1<fmatvec::Vec3,double>* getDerivativeOfGuidingVelocityFunction() { return jd; }
01623       void setDerivativeOfGuidingVelocityFunction(Function1<fmatvec::Vec3,double> *jd_) { jd = jd_; }
01624       /***************************************************/
01625 
01626     private:
01627       Function1<fmatvec::Vec3,double> *jd;
01628   };
01629 
01630   class GeneralDerivativeOfGuidingVelocity : public DerivativeOfGuidingVelocity {
01631     public:
01635       GeneralDerivativeOfGuidingVelocity(Function3<fmatvec::Vec3,fmatvec::Vec,fmatvec::Vec,double> *jd_) : jd(jd_) {}
01636 
01640       virtual ~GeneralDerivativeOfGuidingVelocity() { delete jd; jd = 0; }
01641 
01649       virtual fmatvec::Vec3 operator()(const fmatvec::Vec& qd, const fmatvec::Vec& q, const double& t, const void* =NULL) { return (*jd)(qd,q,t); }
01650 
01651       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {};
01652 
01653       /* GETTER / SETTER */
01657       Function3<fmatvec::Vec3,fmatvec::Vec,fmatvec::Vec,double>* getDerivativeOfGuidingVelocityFunction() { return jd; }
01658       void setDerivativeOfGuidingVelocityFunction(Function3<fmatvec::Vec3,fmatvec::Vec,fmatvec::Vec,double> *jd_) { jd = jd_; }
01659       /***************************************************/
01660 
01661     private:
01662       Function3<fmatvec::Vec3,fmatvec::Vec,fmatvec::Vec,double> *jd;
01663   };
01664 
01665 }
01666 
01667 #endif /* _KINEMATICS_H_ */
01668