mbsim 4.0.0 MBSim Kernel

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>
30 class RotationAboutAxesZXZ : public RotationAboutThreeAxes<Arg> {
31 using B = fmatvec::Function<fmatvec::RotMat3(Arg)>;
32 using RotationAboutThreeAxes<Arg>::A;
33 using RotationAboutThreeAxes<Arg>::J;
34 using RotationAboutThreeAxes<Arg>::Jd;
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
MBSim::RotationAboutAxesZXZTransformedMapping
Definition: rotation_about_axes_zxz_transformed_mapping.h:28
MBSim::RotationAboutAxesZXZ
Definition: rotation_about_axes_zxz.h:30
MBSim::RotationAboutThreeAxes
rotation class for rotation about three axes
Definition: rotation_about_three_axes.h:31
MBSim::RotationAboutAxesZXZMapping
Definition: rotation_about_axes_zxz_mapping.h:28
MBSim::Function
Definition: contact_kinematics.h:32
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30