mbsim 4.0.0 MBSim Kernel

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/kinematics/rotation_about_three_axes.h"
24 #include "mbsim/functions/kinematics/rotation_about_axes_xyz_mapping.h"
25 #include "mbsim/functions/kinematics/rotation_about_axes_xyz_transformed_mapping.h"
26
27 namespace MBSim {
28
32 template<class Arg>
33 class RotationAboutAxesXYZ : public RotationAboutThreeAxes<Arg> {
34 using B = fmatvec::Function<fmatvec::RotMat3(Arg)>;
35 using RotationAboutThreeAxes<Arg>::A;
36 using RotationAboutThreeAxes<Arg>::J;
37 using RotationAboutThreeAxes<Arg>::Jd;
38 public:
39 RotationAboutAxesXYZ() { J.e(0,0) = 1; }
40 fmatvec::RotMat3 operator()(const Arg &q) override {
41 double a=q.e(0);
42 double b=q.e(1);
43 double g=q.e(2);
44 double cosa = cos(a);
45 double sina = sin(a);
46 double cosb = cos(b);
47 double sinb = sin(b);
48 double cosg = cos(g);
49 double sing = sin(g);
50 A.e(0,0) = cosb*cosg;
51 A.e(1,0) = sina*sinb*cosg+cosa*sing;
52 A.e(2,0) = -cosa*sinb*cosg+sina*sing;
53 A.e(0,1) = -cosb*sing;
54 A.e(1,1) = -sing*sinb*sina+cosa*cosg;
55 A.e(2,1) = cosa*sinb*sing+sina*cosg;
56 A.e(0,2) = sinb;
57 A.e(1,2) = -sina*cosb;
58 A.e(2,2) = cosa*cosb;
59 return A;
60 }
61 typename B::DRetDArg parDer(const Arg &q) override {
62 double a = q.e(0);
63 double b = q.e(1);
64 double cosa = cos(a);
65 double sina = sin(a);
66 double cosb = cos(b);
67 if(fabs(cosb)<=1e-13)
68 Element::throwError("Singularity in rotation.");
69 //J.e(0,0) = 1;
70 //J.e(0,1) = 0;
71 J.e(0,2) = sin(b);
72 //J.e(1,0) = 0;
73 J.e(1,1) = cosa;
74 J.e(1,2) = -sina*cosb;
75 //J.e(2,0) = 0;
76 J.e(2,1) = sina;
77 J.e(2,2) = cosa*cosb;
78 return J;
79 }
80 typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) override {
81 double a = q.e(0);
82 double b = q.e(1);
83 double ad = qd.e(0);
84 double bd = qd.e(1);
85 double cosa = cos(a);
86 double sina = sin(a);
87 double cosb = cos(b);
88 double sinb = sin(b);
89 //Jd.e(0,0) = 0;
90 //Jd.e(0,1) = 0;
91 Jd.e(0,2) = cosb*bd;
92 //Jd.e(1,0) = 0;
93 Jd.e(1,1) = -sina*ad;
94 Jd.e(1,2) = -cosa*cosb*ad + sina*sinb*bd;
95 //Jd.e(2,0) = 0;
96 Jd.e(2,1) = cosa*ad;
97 Jd.e(2,2) = -sina*cosb*ad - cosa*sinb*bd;
98 return Jd;
99 }
100 Function<fmatvec::MatV(Arg)>* getMappingFunction() const override { return new RotationAboutAxesXYZMapping<Arg>; }
101 Function<fmatvec::MatV(Arg)>* getTransformedMappingFunction() const override { return new RotationAboutAxesXYZTransformedMapping<Arg>; }
102 };
103
104 }
105
106 #endif
MBSim::RotationAboutThreeAxes
rotation class for rotation about three axes
Definition: rotation_about_three_axes.h:31
MBSim::RotationAboutAxesXYZMapping
Definition: rotation_about_axes_xyz_mapping.h:28
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::RotationAboutAxesXYZTransformedMapping
Definition: rotation_about_axes_xyz_transformed_mapping.h:28
MBSim::RotationAboutAxesXYZ
rotation class for rotation about all three axes using the cardan description
Definition: rotation_about_axes_xyz.h:33
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30