All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
rotation_about_axes_zxz.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@gmail.com
18  */
19 
20 #ifndef _ROTATION_ABOUT_AXES_ZXZ_H_
21 #define _ROTATION_ABOUT_AXES_ZXZ_H_
22 
23 #include "mbsim/functions/kinematics/rotation_about_three_axes.h"
24 #include "mbsim/functions/kinematics/rotation_about_axes_zxz_mapping.h"
25 #include "mbsim/functions/kinematics/rotation_about_axes_zxz_transformed_mapping.h"
26 
27 namespace MBSim {
28 
29  template<class Arg>
35  public:
36  RotationAboutAxesZXZ() { J.e(2,0) = 1; }
37  fmatvec::RotMat3 operator()(const Arg &q) override {
38  double psi=q.e(0);
39  double theta=q.e(1);
40  double phi=q.e(2);
41  double spsi = sin(psi);
42  double stheta = sin(theta);
43  double sphi = sin(phi);
44  double cpsi = cos(psi);
45  double ctheta = cos(theta);
46  double cphi = cos(phi);
47  A.e(0,0) = cpsi*cphi-spsi*ctheta*sphi;
48  A.e(1,0) = spsi*cphi+cpsi*ctheta*sphi;
49  A.e(2,0) = stheta*sphi;
50  A.e(0,1) = -cpsi*sphi-spsi*ctheta*cphi;
51  A.e(1,1) = -spsi*sphi+cpsi*ctheta*cphi;
52  A.e(2,1) = stheta*cphi;
53  A.e(0,2) = spsi*stheta;
54  A.e(1,2) = -cpsi*stheta;
55  A.e(2,2) = ctheta;
56  return A;
57  }
58  typename B::DRetDArg parDer(const Arg &q) override {
59  double psi=q.e(0);
60  double theta=q.e(1);
61  double sint = sin(theta);
62  if(fabs(sint)<=1e-13)
63  Element::throwError("Singularity in rotation.");
64  double cosp = cos(psi);
65  double sinp = sin(psi);
66  //J.e(0,0) = 0;
67  J.e(0,1) = cosp;
68  J.e(0,2) = sinp*sint;
69  //J.e(1,0) = 0;
70  J.e(1,1) = sinp;
71  J.e(1,2) = -cosp*sint;
72  //J.e(2,0) = 1;
73  //J.e(2,1) = 0;
74  J.e(2,2) = cos(theta);
75  return J;
76  }
77  typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) override {
78  double psi=q.e(0);
79  double theta=q.e(1);
80  double psid=qd.e(0);
81  double thetad=qd.e(1);
82  double sint = sin(theta);
83  double cost = cos(theta);
84  double cosp = cos(psi);
85  double sinp = sin(psi);
86  Jd.e(0,1) = -sinp*psid;
87  Jd.e(0,2) = cosp*sint*psid + sinp*cost*thetad;
88  Jd.e(1,1) = cosp*psid;
89  Jd.e(1,2) = sinp*sint*psid - cosp*cost*thetad;
90  Jd.e(2,2) = -sint*thetad;
91  return Jd;
92  }
93  Function<fmatvec::MatV(Arg)>* getMappingFunction() const override { return new RotationAboutAxesZXZMapping<Arg>; }
94  Function<fmatvec::MatV(Arg)>* getTransformedMappingFunction() const override { return new RotationAboutAxesZXZTransformedMapping<Arg>; }
95  };
96 
97 }
98 
99 #endif
Definition: rotation_about_axes_zxz_transformed_mapping.h:28
Definition: rotation_about_axes_zxz.h:30
rotation class for rotation about three axes
Definition: rotation_about_three_axes.h:31
Definition: rotation_about_axes_zxz_mapping.h:28

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Home