mbsim 4.0.0 MBSim Kernel

rotation_about_axes_yz.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_YZ_H_
21 #define _ROTATION_ABOUT_AXES_YZ_H_
22
23 #include "mbsim/functions/function.h"
24
25 namespace MBSim {
26
27 template<class Arg>
28 class RotationAboutAxesYZ : public Function<fmatvec::RotMat3(Arg)> {
29 using B = fmatvec::Function<fmatvec::RotMat3(Arg)>;
30 private:
31 fmatvec::RotMat3 A;
32 fmatvec::Mat3xV J, Jd;
33 public:
34 RotationAboutAxesYZ() : J(2), Jd(2) { J.e(1,0) = 1; }
35 int getArgSize() const override { return 2; }
36 fmatvec::RotMat3 operator()(const Arg &q) override {
37 double b=q.e(0);
38 double g=q.e(1);
39 double cosb = cos(b);
40 double sinb = sin(b);
41 double cosg = cos(g);
42 double sing = sin(g);
43
44 A.e(0,0) = cosb*cosg;
45 A.e(1,0) = sing;
46 A.e(2,0) = -sinb*cosg;
47 A.e(0,1) = -cosb*sing;
48 A.e(1,1) = cosg;
49 A.e(2,1) = sinb*sing;
50 A.e(0,2) = sinb;
51 A.e(2,2) = cosb;
52 return A;
53 }
54 typename B::DRetDArg parDer(const Arg &q) override {
55 double beta = q.e(0);
56 J.e(0,1) = sin(beta);
57 J.e(2,1) = cos(beta);
58 return J;
59 }
60 typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) override {
61 double beta = q.e(0);
62 double betad = qd.e(0);
63 Jd.e(0,1) = cos(beta)*betad;
64 Jd.e(2,1) = -sin(beta)*betad;
65 return Jd;
66 }
67 };
68
69 }
70
71 #endif
MBSim::RotationAboutAxesYZ
Definition: rotation_about_axes_yz.h:28
MBSim::Function
Definition: contact_kinematics.h:32
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30