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/function.h"
24 
25 namespace MBSim {
26 
27  template<class Arg>
28  class RotationAboutAxesZXZ : public Function<fmatvec::RotMat3(Arg)> {
30  private:
31  fmatvec::RotMat3 A;
32  fmatvec::Mat3xV J, Jd;
33  public:
34  RotationAboutAxesZXZ() : J(3), Jd(3) { J.e(2,0) = 1; }
35  int getArgSize() const { return 3; }
36  fmatvec::RotMat3 operator()(const Arg &q) {
37  double psi=q.e(0);
38  double theta=q.e(1);
39  double phi=q.e(2);
40  double spsi = sin(psi);
41  double stheta = sin(theta);
42  double sphi = sin(phi);
43  double cpsi = cos(psi);
44  double ctheta = cos(theta);
45  double cphi = cos(phi);
46  A.e(0,0) = cpsi*cphi-spsi*ctheta*sphi;
47  A.e(1,0) = spsi*cphi+cpsi*ctheta*sphi;
48  A.e(2,0) = stheta*sphi;
49  A.e(0,1) = -cpsi*sphi-spsi*ctheta*cphi;
50  A.e(1,1) = -spsi*sphi+cpsi*ctheta*cphi;
51  A.e(2,1) = stheta*cphi;
52  A.e(0,2) = spsi*stheta;
53  A.e(1,2) = -cpsi*stheta;
54  A.e(2,2) = ctheta;
55  return A;
56  }
57  typename B::DRetDArg parDer(const Arg &q) {
58  double psi=q.e(0);
59  double theta=q.e(1);
60  //J.e(0,0) = 0;
61  J.e(0,1) = cos(psi);
62  J.e(0,2) = sin(psi)*sin(theta);
63  //J.e(1,0) = 0;
64  J.e(1,1) = sin(psi);
65  J.e(1,2) = -cos(psi)*sin(theta);
66  //J.e(2,0) = 1;
67  //J.e(2,1) = 1;
68  J.e(2,2) = cos(theta);
69  return J;
70  }
71  typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) {
72  double psi=q.e(0);
73  double theta=q.e(1);
74  double psid=qd.e(0);
75  double thetad=qd.e(1);
76  Jd.e(0,1) = -sin(psi)*psid;
77  Jd.e(0,2) = cos(psi)*sin(theta)*psid + sin(psi)*cos(theta)*thetad;
78  Jd.e(1,1) = cos(psi)*psid;
79  Jd.e(1,2) = sin(psi)*sin(theta)*psid - cos(psi)*cos(theta)*thetad;
80  Jd.e(2,2) = -sin(theta)*thetad;
81  return Jd;
82  }
83  };
84 
85 }
86 
87 #endif
Definition: rotation_about_axes_zxz.h:28
Definition: planar_contour.h:31

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML