MBSim

Project Page
kinematic_functions.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: markus.ms.schneider@gmail.com
00018  */
00019 
00020 #ifndef _KINEMATIC_FUNCTIONS_H_
00021 #define _KINEMATIC_FUNCTIONS_H_
00022 
00023 #include "mbsim/functions/function.h"
00024 #include "mbsim/objectfactory.h"
00025 #include "mbsim/element.h"
00026 #include "mbsim/utils/utils.h"
00027 
00028 namespace MBSim {
00029 
00030   template<typename Arg>
00031     class TranslationAlongXAxis : public Function<fmatvec::Vec3(Arg)> {
00032       private:
00033         fmatvec::Vec3 r, a;
00034       public:
00035         TranslationAlongXAxis() { a.e(0) = 1; }
00036         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00037         fmatvec::Vec3 operator()(const Arg &q) { 
00038           r.e(0) = ToDouble<Arg>::cast(q);
00039           return r; 
00040         }
00041         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00042         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00043         bool constParDer() const { return true; }
00044     };
00045 
00046   template<typename Arg>
00047     class TranslationAlongYAxis : public Function<fmatvec::Vec3(Arg)> {
00048       private:
00049         fmatvec::Vec3 r, a;
00050       public:
00051         TranslationAlongYAxis() { a.e(1) = 1; }
00052         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00053         fmatvec::Vec3 operator()(const Arg &q) { 
00054           r.e(1) = ToDouble<Arg>::cast(q);
00055           return r; 
00056         }
00057         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00058         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00059         bool constParDer() const { return true; }
00060     };
00061 
00062   template<typename Arg>
00063     class TranslationAlongZAxis : public Function<fmatvec::Vec3(Arg)> {
00064       private:
00065         fmatvec::Vec3 r, a;
00066       public:
00067         TranslationAlongZAxis() { a.e(2) = 1; }
00068         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00069         fmatvec::Vec3 operator()(const Arg &q) { 
00070           r.e(2) = ToDouble<Arg>::cast(q);
00071           return r; 
00072         }
00073         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00074         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00075         bool constParDer() const { return true; }
00076     };
00077 
00078   template<typename Arg>
00079     class TranslationAlongAxesXY : public Function<fmatvec::Vec3(Arg)> {
00080       private:
00081         fmatvec::Vec3 r;
00082         fmatvec::Mat3xV A;
00083       public:
00084         TranslationAlongAxesXY() : A(2) { A.e(0,0) = 1; A.e(1,1) = 1; }
00085         typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00086         fmatvec::Vec3 operator()(const Arg &q) { 
00087           r.e(0) = q.e(0);
00088           r.e(1) = q.e(1);
00089           return r; 
00090         }
00091         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00092         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(2); }
00093         bool constParDer() const { return true; }
00094     };
00095 
00096   template<typename Arg>
00097     class TranslationAlongAxesYZ : public Function<fmatvec::Vec3(Arg)> {
00098       private:
00099         fmatvec::Vec3 r;
00100         fmatvec::Mat3xV A;
00101       public:
00102         TranslationAlongAxesYZ() : A(2) { A.e(1,0) = 1; A.e(2,1) = 1; }
00103         typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00104         fmatvec::Vec3 operator()(const Arg &q) { 
00105           r.e(1) = q.e(0);
00106           r.e(2) = q.e(1);
00107           return r; 
00108         }
00109         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00110         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(2); }
00111         bool constParDer() const { return true; }
00112     };
00113 
00114   template<typename Arg>
00115     class TranslationAlongAxesXZ : public Function<fmatvec::Vec3(Arg)> {
00116       private:
00117         fmatvec::Vec3 r;
00118         fmatvec::Mat3xV A;
00119       public:
00120         TranslationAlongAxesXZ() : A(2) { A.e(0,0) = 1; A.e(2,1) = 1; }
00121         typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00122         fmatvec::Vec3 operator()(const Arg &q) { 
00123           r.e(0) = q.e(0);
00124           r.e(2) = q.e(1);
00125           return r; 
00126         }
00127         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00128         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(2); }
00129         bool constParDer() const { return true; }
00130     };
00131 
00132   template<typename Arg>
00133     class TranslationAlongAxesXYZ : public Function<fmatvec::Vec3(Arg)> {
00134       private:
00135         fmatvec::Vec3 r;
00136         fmatvec::Mat3xV A;
00137       public:
00138         TranslationAlongAxesXYZ() : A(3) { A.e(0,0) = 1; A.e(1,1) = 1; A.e(2,2) = 1; }
00139         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00140         fmatvec::Vec3 operator()(const Arg &q) { 
00141           r.e(0) = q.e(0);
00142           r.e(1) = q.e(1);
00143           r.e(2) = q.e(2);
00144           return r; 
00145         }
00146         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00147         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(3); }
00148         bool constParDer() const { return true; }
00149     };
00150 
00151   template<class Arg>
00152     class TranslationAlongFixedAxis : public Function<fmatvec::Vec3(Arg)> {
00153       private:
00154         fmatvec::Vec3 a;
00155         fmatvec::Vec3 zeros(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &x) { return fmatvec::Vec3(x.rows()); }
00156       public:
00157         TranslationAlongFixedAxis() { }
00158         TranslationAlongFixedAxis(const fmatvec::Vec3 &a_) : a(a_) { }
00159         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00160         fmatvec::Vec3 operator()(const Arg &arg) { return a*arg; }
00161         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00162         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00163         typename fmatvec::Der<typename fmatvec::Der<fmatvec::Vec3, double>::type, double>::type parDerParDer(const double &arg) { THROW_MBSIMERROR("parDerParDer is not available for given template parameters."); }
00164         bool constParDer() const { return true; }
00165         void initializeUsingXML(xercesc::DOMElement *element) {
00166           xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"axisOfTranslation");
00167           a=FromMatStr<fmatvec::Vec3>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str());
00168         }
00169         xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *parent) { return 0; } 
00170     };
00171 
00172   template<>
00173   inline fmatvec::Vec3 TranslationAlongFixedAxis<double>::parDerParDer(const double &arg) { return fmatvec::Vec3(); }
00174 
00175   template<class Arg>
00176     class LinearTranslation : public Function<fmatvec::Vec3(Arg)> {
00177       private:
00178         typename fmatvec::Der<fmatvec::Vec3, Arg>::type A;
00179         fmatvec::Vec3 b;
00180         fmatvec::Vec3 zeros(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &x) { return fmatvec::Vec3(x.rows()); }
00181       public:
00182         LinearTranslation() { }
00183         LinearTranslation(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &A_) : A(A_) { }
00184         LinearTranslation(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &A_, const fmatvec::Vec3 &b_) : A(A_), b(b_) { }
00185         typename fmatvec::Size<Arg>::type getArgSize() const { return A.cols(); }
00186         fmatvec::Vec3 operator()(const Arg &arg) { return A*arg+b; }
00187         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00188         typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(A.rows(),A.cols()); }
00189         typename fmatvec::Der<typename fmatvec::Der<fmatvec::Vec3, double>::type, double>::type parDerParDer(const double &arg) { THROW_MBSIMERROR("parDerParDer is not available for given template parameters."); }
00190         bool constParDer() const { return true; }
00191         void initializeUsingXML(xercesc::DOMElement *element) {
00192           xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"translationVectors");
00193           A=FromMatStr<typename fmatvec::Der<fmatvec::Vec3, Arg>::type>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str());
00194           e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"offset");
00195           b=e?FromMatStr<fmatvec::Vec3>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str()):zeros(A);
00196         }
00197         xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *parent) { return 0; } 
00198         void setSlope(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &A_) { A = A_; }
00199         void setIntercept(const fmatvec::Vec3 &b_) { b = b_; }
00200     };
00201 
00202   template<>
00203     inline fmatvec::Vec3 LinearTranslation<double>::parDerDirDer(const double &arg1Dir, const double &arg1) { return fmatvec::Vec3(); }
00204   template<>
00205     inline fmatvec::Vec3 LinearTranslation<double>::parDerParDer(const double &arg) { return fmatvec::Vec3(); }
00206 
00207   template<class Arg> 
00208     class RotationAboutXAxis : public Function<fmatvec::RotMat3(Arg)> {
00209       private:
00210         fmatvec::RotMat3 A;
00211         fmatvec::Vec3 a;
00212       public:
00213         RotationAboutXAxis() { a.e(0) = 1; A.e(0,0) = 1; }
00214         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00215         fmatvec::RotMat3 operator()(const Arg &q) {
00216           double alpha = ToDouble<Arg>::cast(q);
00217           const double cosq=cos(alpha);
00218           const double sinq=sin(alpha);
00219           A.e(1,1) = cosq;
00220           A.e(2,1) = sinq;
00221           A.e(1,2) = -sinq;
00222           A.e(2,2) = cosq;
00223           return A;
00224         }
00225         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00226         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00227         bool constParDer() const { return true; }
00228     };
00229 
00230   template<class Arg> 
00231     class RotationAboutYAxis : public Function<fmatvec::RotMat3(Arg)> {
00232       private:
00233         fmatvec::RotMat3 A;
00234         fmatvec::Vec3 a;
00235       public:
00236         RotationAboutYAxis() { a.e(1) = 1; A.e(1,1) = 1; }
00237         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00238         fmatvec::RotMat3 operator()(const Arg &q) {
00239           double alpha = ToDouble<Arg>::cast(q);
00240           const double cosq=cos(alpha);
00241           const double sinq=sin(alpha);
00242           A.e(0,0) = cosq;
00243           A.e(2,0) = -sinq;
00244           A.e(0,2) = sinq;
00245           A.e(2,2) = cosq;
00246           return A;
00247         }
00248         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00249         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00250         bool constParDer() const { return true; }
00251     };
00252 
00253   template<class Arg> 
00254     class RotationAboutZAxis : public Function<fmatvec::RotMat3(Arg)> {
00255       private:
00256         fmatvec::RotMat3 A;
00257         fmatvec::Vec3 a;
00258       public:
00259         RotationAboutZAxis() { a.e(2) = 1; A.e(2,2) = 1; }
00260         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00261         fmatvec::RotMat3 operator()(const Arg &q) {
00262           double alpha = ToDouble<Arg>::cast(q);
00263           const double cosq=cos(alpha);
00264           const double sinq=sin(alpha);
00265           A.e(0,0) = cosq;
00266           A.e(1,0) = sinq;
00267           A.e(0,1) = -sinq;
00268           A.e(1,1) = cosq;
00269           return A;
00270         }
00271         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00272         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00273         bool constParDer() const { return true; }
00274     };
00275 
00276   template<class Arg> 
00277     class RotationAboutFixedAxis : public Function<fmatvec::RotMat3(Arg)> {
00278       private:
00279         fmatvec::RotMat3 A;
00280         fmatvec::Vec3 a;
00281       public:
00282         RotationAboutFixedAxis() { }
00283         RotationAboutFixedAxis(const fmatvec::Vec3 &a_) : a(a_) { }
00284         typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00285         fmatvec::RotMat3 operator()(const Arg &q) {
00286           double alpha = ToDouble<Arg>::cast(q);
00287           const double cosq=cos(alpha);
00288           const double sinq=sin(alpha);
00289           const double onemcosq=1-cosq;
00290           const double a0a1=a.e(0)*a.e(1);
00291           const double a0a2=a.e(0)*a.e(2);
00292           const double a1a2=a.e(1)*a.e(2);
00293           A.e(0,0) = cosq+onemcosq*a.e(0)*a.e(0);
00294           A.e(1,0) = onemcosq*a0a1+a.e(2)*sinq;
00295           A.e(2,0) = onemcosq*a0a2-a.e(1)*sinq;
00296           A.e(0,1) = onemcosq*a0a1-a.e(2)*sinq;
00297           A.e(1,1) = cosq+onemcosq*a.e(1)*a.e(1);
00298           A.e(2,1) = onemcosq*a1a2+a.e(0)*sinq;
00299           A.e(0,2) = onemcosq*a0a2+a.e(1)*sinq;
00300           A.e(1,2) = onemcosq*a1a2-a.e(0)*sinq;
00301           A.e(2,2) = cosq+onemcosq*a.e(2)*a.e(2);
00302           return A;
00303         }
00304         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00305         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00306         typename fmatvec::Der<typename fmatvec::Der<fmatvec::RotMat3, Arg>::type, Arg>::type parDerParDer(const Arg &arg) { THROW_MBSIMERROR("parDerParDer is not available for given template parameters."); }
00307         bool constParDer() const { return true; }
00308         const fmatvec::Vec3& getAxisOfRotation() const { return a; }
00309         void setAxisOfRotation(const fmatvec::Vec3 &a_) { a = a_; }
00310         void initializeUsingXML(xercesc::DOMElement *element) {
00311           xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"axisOfRotation");
00312           a=FromMatStr<fmatvec::Vec3>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str());
00313         }
00314         xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *parent) { return 0; } 
00315     };
00316 
00317   template<>
00318   inline fmatvec::Vec3 RotationAboutFixedAxis<double>::parDerParDer(const double &arg) { return fmatvec::Vec3(); }
00319 
00320   template<class Arg> 
00321     class RotationAboutAxesXY : public Function<fmatvec::RotMat3(Arg)> {
00322       private:
00323         fmatvec::RotMat3 A;
00324         fmatvec::Mat3xV J, Jd;
00325       public:
00326         RotationAboutAxesXY() : J(2), Jd(2) { J.e(0,0) = 1; }
00327         typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00328         fmatvec::RotMat3 operator()(const Arg &q) {
00329           double a=q.e(0);
00330           double b=q.e(1);
00331           double cosa = cos(a);
00332           double sina = sin(a);
00333           double cosb = cos(b);
00334           double sinb = sin(b);
00335 
00336           A.e(0,0) = cosb;
00337           A.e(1,0) = sina*sinb;
00338           A.e(2,0) = -cosa*sinb;
00339           A.e(1,1) = cosa;
00340           A.e(2,1) = sina;
00341           A.e(0,2) = sinb;
00342           A.e(1,2) = -sina*cosb;
00343           A.e(2,2) = cosa*cosb;
00344           return A;
00345         }
00346         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00347           double a = q.e(0);
00348           J.e(1,1) = cos(a);
00349           J.e(2,1) = sin(a);
00350           return J;
00351         }
00352         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00353           double a = q.e(0);
00354           double ad = qd.e(0);
00355           Jd.e(1,1) = -sin(a)*ad;
00356           Jd.e(2,1) = cos(a)*ad;
00357           return Jd;
00358         }
00359     };
00360 
00361   template<class Arg> 
00362     class RotationAboutAxesYZ : public Function<fmatvec::RotMat3(Arg)> {
00363       private:
00364         fmatvec::RotMat3 A;
00365         fmatvec::Mat3xV J, Jd;
00366       public:
00367         RotationAboutAxesYZ() : J(2), Jd(2) { J.e(1,0) = 1; }
00368         typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00369         fmatvec::RotMat3 operator()(const Arg &q) {
00370           double b=q.e(0);
00371           double g=q.e(1);
00372           double cosb = cos(b);
00373           double sinb = sin(b);
00374           double cosg = cos(g);
00375           double sing = sin(g);
00376 
00377           A.e(0,0) = cosb*cosg;
00378           A.e(1,0) = sing;
00379           A.e(2,0) = -sinb*cosg;
00380           A.e(0,1) = -cosb*sing;
00381           A.e(1,1) = cosg;
00382           A.e(2,1) = sinb*sing;
00383           A.e(0,2) = sinb;
00384           A.e(2,2) = cosb;
00385           return A;
00386         }
00387         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00388           double beta = q.e(0);
00389           J.e(0,1) = sin(beta);
00390           J.e(2,1) = cos(beta);
00391           return J;
00392         }
00393         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00394           double beta = q.e(0);
00395           double betad = qd.e(0);
00396           Jd.e(0,1) = cos(beta)*betad;
00397           Jd.e(2,1) = -sin(beta)*betad;
00398           return Jd;
00399         }
00400     };
00401 
00402   template<class Arg> 
00403     class RotationAboutAxesXZ : public Function<fmatvec::RotMat3(Arg)> {
00404       private:
00405         fmatvec::RotMat3 A;
00406         fmatvec::Mat3xV J, Jd;
00407       public:
00408         RotationAboutAxesXZ() : J(2), Jd(2) { J.e(0,0) = 1; }
00409         typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00410         fmatvec::RotMat3 operator()(const Arg &q) {
00411           double a=q.e(0);
00412           double b=q.e(1);
00413           double cosa = cos(a);
00414           double sina = sin(a);
00415           double cosb = cos(b);
00416           double sinb = sin(b);
00417 
00418           A.e(0,0) = cosb;
00419           A.e(1,0) = cosa*sinb;
00420           A.e(2,0) = sina*sinb;
00421           A.e(0,1) = -sinb;
00422           A.e(1,1) = cosa*cosb;
00423           A.e(2,1) = sina*cosb;
00424           A.e(1,2) = -sina;
00425           A.e(2,2) = cosa;
00426 
00427           return A;
00428         }
00429         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00430           double a = q.e(0);
00431           J.e(1,1) = -sin(a);
00432           J.e(2,1) = cos(a);
00433           return J;
00434         }
00435         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00436           double a = q.e(0);
00437           double ad = qd.e(0);
00438           Jd.e(1,1) = -cos(a)*ad;
00439           Jd.e(2,1) = -sin(a)*ad;
00440           return Jd;
00441         }
00442     };
00443 
00447   template<class Arg> 
00448     class RotationAboutAxesXYZ : public Function<fmatvec::RotMat3(Arg)> {
00449       private:
00450         fmatvec::RotMat3 A;
00451         fmatvec::Mat3xV J, Jd;
00452       public:
00453         RotationAboutAxesXYZ() : J(3), Jd(3) { J.e(0,0) = 1; }
00454         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00455         fmatvec::RotMat3 operator()(const Arg &q) {
00456           double a=q.e(0);
00457           double b=q.e(1);
00458           double g=q.e(2);
00459           double cosa = cos(a);
00460           double sina = sin(a);
00461           double cosb = cos(b);
00462           double sinb = sin(b);
00463           double cosg = cos(g);
00464           double sing = sin(g);
00465           A.e(0,0) = cosb*cosg;
00466           A.e(1,0) = sina*sinb*cosg+cosa*sing;
00467           A.e(2,0) = -cosa*sinb*cosg+sina*sing;
00468           A.e(0,1) = -cosb*sing;
00469           A.e(1,1) = -sing*sinb*sina+cosa*cosg;
00470           A.e(2,1) = cosa*sinb*sing+sina*cosg;
00471           A.e(0,2) = sinb;
00472           A.e(1,2) = -sina*cosb;
00473           A.e(2,2) = cosa*cosb;
00474           return A;
00475         }
00476         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00477           double a = q.e(0);
00478           double b = q.e(1);
00479           double cosa = cos(a);
00480           double sina = sin(a);
00481           double cosb = cos(b);
00482           //J.e(0,0) = 1;
00483           //J.e(0,1) = 0;
00484           J.e(0,2) = sin(b);
00485           //J.e(1,0) = 0;
00486           J.e(1,1) = cosa;
00487           J.e(1,2) = -sina*cosb;
00488           //J.e(2,0) = 0;
00489           J.e(2,1) = sina;
00490           J.e(2,2) = cosa*cosb;
00491           return J;
00492         }
00493         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00494           double a = q.e(0);
00495           double b = q.e(1);
00496           double ad = qd.e(0);
00497           double bd = qd.e(1);
00498           double cosa = cos(a);
00499           double sina = sin(a);
00500           double cosb = cos(b);
00501           double sinb = sin(b);
00502           //Jd.e(0,0) = 0;
00503           //Jd.e(0,1) = 0;
00504           Jd.e(0,2) = cosb*bd;
00505           //Jd.e(1,0) = 0;
00506           Jd.e(1,1) = -sina*ad;
00507           Jd.e(1,2) = -cosa*cosb*ad + sina*sinb*bd;
00508           //Jd.e(2,0) = 0;
00509           Jd.e(2,1) = cosa*ad;
00510           Jd.e(2,2) = -sina*cosb*ad - cosa*sinb*bd;
00511           return Jd;
00512         }
00513     };
00514 
00515     template<class Arg> 
00516     class RotationAboutAxesXYZ2 : public Function<fmatvec::RotMat3(Arg)> {
00517       private:
00518         fmatvec::RotMat3 A;
00519         fmatvec::Mat3xV J, Jd;
00520       public:
00521         RotationAboutAxesXYZ2() : J(3), Jd(3) { J.e(2,2) = 1; }
00522         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00523         fmatvec::RotMat3 operator()(const Arg &q) {
00524           double a=q.e(0);
00525           double b=q.e(1);
00526           double g=q.e(2);
00527           double cosa = cos(a);
00528           double sina = sin(a);
00529           double cosb = cos(b);
00530           double sinb = sin(b);
00531           double cosg = cos(g);
00532           double sing = sin(g);
00533           A.e(0,0) = cosb*cosg;
00534           A.e(1,0) = sina*sinb*cosg+cosa*sing;
00535           A.e(2,0) = -cosa*sinb*cosg+sina*sing;
00536           A.e(0,1) = -cosb*sing;
00537           A.e(1,1) = -sing*sinb*sina+cosa*cosg;
00538           A.e(2,1) = cosa*sinb*sing+sina*cosg;
00539           A.e(0,2) = sinb;
00540           A.e(1,2) = -sina*cosb;
00541           A.e(2,2) = cosa*cosb;
00542           return A;
00543         }
00544         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00545           double b = q.e(1);
00546           double g = q.e(2);
00547           J.e(0,0) = cos(b)*cos(g);
00548           J.e(0,1) = sin(g);
00549           //J.e(0,2) = 0;
00550           J.e(1,0) = -cos(b)*sin(g);
00551           J.e(1,1) = cos(g);
00552           //J.e(1,2) = 0;
00553           J.e(2,0) = sin(b);
00554           //J.e(2,1) = 0;
00555           //J.e(2,2) = 1;
00556           return J;
00557         }
00558         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00559           double b = q.e(1);
00560           double g = q.e(2);
00561           double bd = qd.e(1);
00562           double gd = qd.e(2);
00563           Jd.e(0,0) = -sin(b)*cos(g)*bd - cos(b)*sin(g)*gd;
00564           Jd.e(0,1) = cos(g)*gd;
00565           //Jd.e(0,2) = 0;
00566           Jd.e(1,0) = sin(b)*sin(g)*bd - cos(b)*cos(g)*gd;
00567           Jd.e(1,1) = -sin(g)*gd;
00568           //Jd.e(1,2) = 0; 
00569           Jd.e(2,0) = cos(b)*bd;
00570           //Jd.e(2,1) = 0;
00571           //Jd.e(2,2) = 0;
00572           return Jd;
00573         }
00574     };
00575   
00576   template<class Arg> 
00577     class RotationAboutAxesZXZ : public Function<fmatvec::RotMat3(Arg)> {
00578       private:
00579         fmatvec::RotMat3 A;
00580         fmatvec::Mat3xV J, Jd;
00581       public:
00582         RotationAboutAxesZXZ() : J(3), Jd(3) { J.e(2,0) = 1; }
00583         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00584         fmatvec::RotMat3 operator()(const Arg &q) {
00585           double psi=q.e(0);
00586           double theta=q.e(1);
00587           double phi=q.e(2);
00588           double spsi = sin(psi);
00589           double stheta = sin(theta);
00590           double sphi = sin(phi);
00591           double cpsi = cos(psi);
00592           double ctheta = cos(theta);
00593           double cphi = cos(phi);
00594           A.e(0,0) = cpsi*cphi-spsi*ctheta*sphi;
00595           A.e(1,0) = spsi*cphi+cpsi*ctheta*sphi;
00596           A.e(2,0) = stheta*sphi;
00597           A.e(0,1) = -cpsi*sphi-spsi*ctheta*cphi;
00598           A.e(1,1) = -spsi*sphi+cpsi*ctheta*cphi;
00599           A.e(2,1) = stheta*cphi;
00600           A.e(0,2) = spsi*stheta;
00601           A.e(1,2) = -cpsi*stheta;
00602           A.e(2,2) = ctheta;
00603           return A;
00604         }
00605         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00606           double psi=q.e(0);
00607           double theta=q.e(1);
00608           //J.e(0,0) = 0;
00609           J.e(0,1) = cos(psi);
00610           J.e(0,2) = sin(psi)*sin(theta);
00611           //J.e(1,0) = 0;
00612           J.e(1,1) = sin(psi);
00613           J.e(1,2) = -cos(psi)*sin(theta);
00614           //J.e(2,0) = 1;
00615           //J.e(2,1) = 1;
00616           J.e(2,2) = cos(theta);
00617           return J;
00618         }
00619         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00620           double psi=q.e(0);
00621           double theta=q.e(1);
00622           double psid=qd.e(0);
00623           double thetad=qd.e(1);
00624           Jd.e(0,1) = -sin(psi)*psid;
00625           Jd.e(0,2) = cos(psi)*sin(theta)*psid + sin(psi)*cos(theta)*thetad;
00626           Jd.e(1,1) = cos(psi)*psid;
00627           Jd.e(1,2) = sin(psi)*sin(theta)*psid - cos(psi)*cos(theta)*thetad;
00628           Jd.e(2,2) = -sin(theta)*thetad;
00629           return Jd;
00630         }
00631     };
00632 
00633   template<class Arg> 
00634     class RotationAboutAxesZYX : public Function<fmatvec::RotMat3(Arg)> {
00635       private:
00636         fmatvec::RotMat3 A;
00637         fmatvec::Mat3xV J, Jd;
00638       public:
00639         RotationAboutAxesZYX() : J(3), Jd(3) { J.e(2,0) = 1; }
00640         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00641         fmatvec::RotMat3 operator()(const Arg &q) {
00642           double a=q.e(0);
00643           double b=q.e(1);
00644           double g=q.e(2);
00645           double cosa = cos(a);
00646           double sina = sin(a);
00647           double cosb = cos(b);
00648           double sinb = sin(b);
00649           double cosg = cos(g);
00650           double sing = sin(g);
00651           A.e(0,0) = cosa*cosb;
00652           A.e(1,0) = sina*cosb;
00653           A.e(2,0) = -sinb;
00654           A.e(0,1) = -sina*cosg+cosa*sinb*sing;
00655           A.e(1,1) = cosa*cosg+sina*sinb*sing;
00656           A.e(2,1) = cosb*sing;
00657           A.e(0,2) = sina*sing+cosa*sinb*cosg;
00658           A.e(1,2) = -cosa*sing+sina*sinb*cosg;
00659           A.e(2,2) = cosb*cosg;
00660           return A;
00661         }
00662         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00663           double a = q.e(0);
00664           double b = q.e(1);
00665           double cosa = cos(a);
00666           double sina = sin(a);
00667           double cosb = cos(b);
00668           //J.e(0,0) = 0;
00669           J.e(0,1) = -sina;
00670           J.e(0,2) = cosa*cosb;
00671           //J.e(1,0) = 0;
00672           J.e(1,1) = cosa;
00673           J.e(1,2) = sina*cosb;
00674           //J.e(2,0) = 1;
00675           //J.e(2,1) = 0;
00676           J.e(2,2) = -sin(b);
00677           return J;
00678         }
00679         typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00680           double a = q.e(0);
00681           double b = q.e(1);
00682           double ad = qd.e(0);
00683           double bd = qd.e(1);
00684           double cosa = cos(a);
00685           double sina = sin(a);
00686           double cosb = cos(b);
00687           double sinb = sin(b);
00688           Jd.e(0,1) = -cosa*ad;
00689           Jd.e(0,2) = -sina*cosb*ad - cosa*sinb*bd;
00690           Jd.e(1,1) = -sina*ad;
00691           Jd.e(1,2) = cosa*cosb*ad - sina*sinb*bd;
00692           Jd.e(2,2) = -cosb*bd;
00693           return Jd;
00694         }
00695     };
00696 
00697   template<class Arg> 
00698     class RotationAboutAxesXYZMapping : public Function<fmatvec::MatV(Arg)> {
00699       private:
00700         fmatvec::MatV T;
00701       public:
00702         RotationAboutAxesXYZMapping() : T(3,3,fmatvec::Eye()) { }
00703         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00704         fmatvec::MatV operator()(const Arg &q) {
00705           double alpha = q.e(0);
00706           double beta = q.e(1);
00707           double cos_beta = cos(beta);
00708           double sin_beta = sin(beta);
00709           double cos_alpha = cos(alpha);
00710           double sin_alpha = sin(alpha);
00711           double tan_beta = sin_beta/cos_beta;
00712           T.e(0,1) = tan_beta*sin_alpha;
00713           T.e(0,2) = -tan_beta*cos_alpha;
00714           T.e(1,1) = cos_alpha;
00715           T.e(1,2) = sin_alpha;
00716           T.e(2,1) = -sin_alpha/cos_beta;
00717           T.e(2,2) = cos_alpha/cos_beta;
00718           return T;
00719         }
00720     };
00721 
00722   template<class Arg> 
00723     class RotationAboutAxesXYZMapping2 : public Function<fmatvec::MatV(Arg)> {
00724       private:
00725         fmatvec::MatV T;
00726       public:
00727         RotationAboutAxesXYZMapping2() : T(3,3,fmatvec::Eye()) { }
00728         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00729         fmatvec::MatV operator()(const Arg &q) {
00730           double beta = q.e(1);
00731           double gamma = q.e(2);
00732           double cos_beta = cos(beta);
00733           double sin_beta = sin(beta);
00734           double cos_gamma = cos(gamma);
00735           double sin_gamma = sin(gamma);
00736           double tan_beta = sin_beta/cos_beta;
00737           T.e(0,0) = cos_gamma/cos_beta;
00738           T.e(0,1) = -sin_gamma/cos_beta;
00739           T.e(1,0) = sin_gamma;
00740           T.e(1,1) = cos_gamma;
00741           T.e(2,0) = -cos_gamma*tan_beta;
00742           T.e(2,1) = sin_gamma*tan_beta;
00743           return T;
00744         }
00745     };
00746 
00747   template<class Arg> 
00748     class RotationAboutAxesZXZMapping : public Function<fmatvec::MatV(Arg)> {
00749       private:
00750         fmatvec::MatV T;
00751       public:
00752         RotationAboutAxesZXZMapping() : T(3,3) { T.e(0,2) = 1; }
00753         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00754         fmatvec::MatV operator()(const Arg &q) {
00755           double psi = q.e(0);
00756           double theta = q.e(1);
00757           double cos_theta = cos(theta);
00758           double sin_theta = sin(theta);
00759           double cos_psi = cos(psi);
00760           double sin_psi = sin(psi);
00761           double tan_theta = sin_theta/cos_theta;
00762 
00763           T.e(0,0) = -sin_psi/tan_theta;
00764           T.e(0,1) = cos_psi/tan_theta;
00765           T.e(1,0) = cos_psi;
00766           T.e(1,1) = sin_psi;
00767           T.e(2,0) = sin_psi/sin_theta;
00768           T.e(2,1) = -cos_psi/sin_theta;
00769 
00770           return T;
00771         }
00772     };
00773 
00774   template<class Arg> 
00775     class RotationAboutAxesZXZMapping2 : public Function<fmatvec::MatV(Arg)> {
00776       private:
00777         fmatvec::MatV T;
00778       public:
00779         RotationAboutAxesZXZMapping2() : T(3,3,fmatvec::Eye()) { }
00780         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00781         fmatvec::MatV operator()(const Arg &q) {
00782           double theta = q.e(1);
00783           double phi = q.e(2);
00784           double cos_theta = cos(theta);
00785           double sin_theta = sin(theta);
00786           double cos_phi = cos(phi);
00787           double sin_phi = sin(phi);
00788           double tan_theta = sin_theta/cos_theta;
00789 
00790           T.e(0,0) = sin_phi/sin_theta;
00791           T.e(0,1) = cos_phi/sin_theta;
00792           T.e(1,0) = cos_phi;
00793           T.e(1,1) = -sin_phi;
00794           T.e(2,0) = -sin_phi/tan_theta;
00795           T.e(2,1) = -cos_phi/tan_theta;
00796 
00797           return T;
00798         }
00799     };
00800 
00801   template<class Arg> 
00802     class RotationAboutAxesZYXMapping : public Function<fmatvec::MatV(Arg)> {
00803       private:
00804         fmatvec::MatV T;
00805       public:
00806         RotationAboutAxesZYXMapping() : T(3,3) { T.e(0,2) = 1; }
00807         typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00808         fmatvec::MatV operator()(const Arg &q) {
00809           double alpha = q.e(0);
00810           double beta = q.e(1);
00811           double cos_beta = cos(beta);
00812           double sin_beta = sin(beta);
00813           double cos_alpha = cos(alpha);
00814           double sin_alpha = sin(alpha);
00815           double tan_beta = sin_beta/cos_beta;
00816           T.e(0,0) = cos_alpha*tan_beta;
00817           T.e(0,1) = sin_alpha*tan_beta;
00818           T.e(1,0) = -sin_alpha;
00819           T.e(1,1) = cos_alpha;
00820           T.e(2,0) = cos_alpha/cos_beta;
00821           T.e(2,1) = sin_alpha/cos_beta;
00822           return T;
00823         }
00824     };
00825 
00826 }
00827 
00828 #endif