All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
rotation_about_axes_xyz.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_XYZ_H_
21 #define _ROTATION_ABOUT_AXES_XYZ_H_
22 
23 #include "mbsim/functions/function.h"
24 
25 namespace MBSim {
26 
30  template<class Arg>
31  class RotationAboutAxesXYZ : public Function<fmatvec::RotMat3(Arg)> {
33  private:
34  fmatvec::RotMat3 A;
35  fmatvec::Mat3xV J, Jd;
36  public:
37  RotationAboutAxesXYZ() : J(3), Jd(3) { J.e(0,0) = 1; }
38  int getArgSize() const { return 3; }
39  fmatvec::RotMat3 operator()(const Arg &q) {
40  double a=q.e(0);
41  double b=q.e(1);
42  double g=q.e(2);
43  double cosa = cos(a);
44  double sina = sin(a);
45  double cosb = cos(b);
46  double sinb = sin(b);
47  double cosg = cos(g);
48  double sing = sin(g);
49  A.e(0,0) = cosb*cosg;
50  A.e(1,0) = sina*sinb*cosg+cosa*sing;
51  A.e(2,0) = -cosa*sinb*cosg+sina*sing;
52  A.e(0,1) = -cosb*sing;
53  A.e(1,1) = -sing*sinb*sina+cosa*cosg;
54  A.e(2,1) = cosa*sinb*sing+sina*cosg;
55  A.e(0,2) = sinb;
56  A.e(1,2) = -sina*cosb;
57  A.e(2,2) = cosa*cosb;
58  return A;
59  }
60  typename B::DRetDArg parDer(const Arg &q) {
61  double a = q.e(0);
62  double b = q.e(1);
63  double cosa = cos(a);
64  double sina = sin(a);
65  double cosb = cos(b);
66  //J.e(0,0) = 1;
67  //J.e(0,1) = 0;
68  J.e(0,2) = sin(b);
69  //J.e(1,0) = 0;
70  J.e(1,1) = cosa;
71  J.e(1,2) = -sina*cosb;
72  //J.e(2,0) = 0;
73  J.e(2,1) = sina;
74  J.e(2,2) = cosa*cosb;
75  return J;
76  }
77  typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) {
78  double a = q.e(0);
79  double b = q.e(1);
80  double ad = qd.e(0);
81  double bd = qd.e(1);
82  double cosa = cos(a);
83  double sina = sin(a);
84  double cosb = cos(b);
85  double sinb = sin(b);
86  //Jd.e(0,0) = 0;
87  //Jd.e(0,1) = 0;
88  Jd.e(0,2) = cosb*bd;
89  //Jd.e(1,0) = 0;
90  Jd.e(1,1) = -sina*ad;
91  Jd.e(1,2) = -cosa*cosb*ad + sina*sinb*bd;
92  //Jd.e(2,0) = 0;
93  Jd.e(2,1) = cosa*ad;
94  Jd.e(2,2) = -sina*cosb*ad - cosa*sinb*bd;
95  return Jd;
96  }
97  };
98 
99 }
100 
101 #endif
Definition: planar_contour.h:31
rotation class for rotation about all three axis using the cardan description
Definition: rotation_about_axes_xyz.h:31

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML