00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
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
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
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
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
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
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
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
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
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
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
00255
00256
00257
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
01668