mbsim 4.0.0 MBSim Kernel

rotation_about_axes_zxz_mapping.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_MAPPING_H_
21 #define _ROTATION_ABOUT_AXES_ZXZ_MAPPING_H_
22
23 #include "mbsim/functions/function.h"
24
25 namespace MBSim {
26
27 template<class Arg>
28 class RotationAboutAxesZXZMapping : public Function<fmatvec::MatV(Arg)> {
29 private:
30 fmatvec::MatV T;
31 public:
32 RotationAboutAxesZXZMapping() : T(3,3) { T.e(0,2) = 1; }
33 int getArgSize() const override { return 3; }
34 fmatvec::MatV operator()(const Arg &q) override {
35 double psi = q.e(0);
36 double theta = q.e(1);
37 double cos_theta = cos(theta);
38 double sin_theta = sin(theta);
39 if(fabs(sin_theta)<=1e-13)
40 Element::throwError("Singularity in rotation.");
41 double cos_psi = cos(psi);
42 double sin_psi = sin(psi);
43 double tan_theta = sin_theta/cos_theta;
44
45 T.e(0,0) = -sin_psi/tan_theta;
46 T.e(0,1) = cos_psi/tan_theta;
47 T.e(1,0) = cos_psi;
48 T.e(1,1) = sin_psi;
49 T.e(2,0) = sin_psi/sin_theta;
50 T.e(2,1) = -cos_psi/sin_theta;
51
52 return T;
53 }
54 };
55
56 }
57
58 #endif
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