mbsim 4.0.0 MBSim Kernel

rotation_about_fixed_axis.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_FIXED_AXIS_H_
21 #define _ROTATION_ABOUT_FIXED_AXIS_H_
22
23 #include "mbsim/functions/function.h"
24 #include "mbsim/utils/utils.h"
25
26 namespace MBSim {
27
28 template<class Arg>
29 class RotationAboutFixedAxis : public Function<fmatvec::RotMat3(Arg)> {
30 using B = fmatvec::Function<fmatvec::RotMat3(Arg)>;
31 private:
32 fmatvec::RotMat3 A;
33 fmatvec::Vec3 a;
34 public:
35 RotationAboutFixedAxis() = default;
36 RotationAboutFixedAxis(const fmatvec::Vec3 &a_) : a(a_) { }
37 void setAxisOfRotation(const fmatvec::Vec3 &a_) { a = a_; }
38 int getArgSize() const override { return 1; }
39 fmatvec::RotMat3 operator()(const Arg &q) override {
40 double alpha = ToDouble<Arg>::cast(q);
41 const double cosq=cos(alpha);
42 const double sinq=sin(alpha);
43 const double onemcosq=1-cosq;
44 const double a0a1=a.e(0)*a.e(1);
45 const double a0a2=a.e(0)*a.e(2);
46 const double a1a2=a.e(1)*a.e(2);
47 A.e(0,0) = cosq+onemcosq*a.e(0)*a.e(0);
48 A.e(1,0) = onemcosq*a0a1+a.e(2)*sinq;
49 A.e(2,0) = onemcosq*a0a2-a.e(1)*sinq;
50 A.e(0,1) = onemcosq*a0a1-a.e(2)*sinq;
51 A.e(1,1) = cosq+onemcosq*a.e(1)*a.e(1);
52 A.e(2,1) = onemcosq*a1a2+a.e(0)*sinq;
53 A.e(0,2) = onemcosq*a0a2+a.e(1)*sinq;
54 A.e(1,2) = onemcosq*a1a2-a.e(0)*sinq;
55 A.e(2,2) = cosq+onemcosq*a.e(2)*a.e(2);
56 return A;
57 }
58 typename B::DRetDArg parDer(const Arg &q) override { return a; }
59 typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) override { return typename B::DRetDArg(3); }
60 bool constParDer() const override { return true; }
61 const fmatvec::Vec3& getAxisOfRotation() const { return a; }
62 void initializeUsingXML(xercesc::DOMElement *element) override {
63 xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"axisOfRotation");
64 setAxisOfRotation(MBXMLUtils::E(e)->getText<fmatvec::Vec3>());
65 }
66 };
67
68 }
69
70 #endif
MBSim::RotationAboutFixedAxis
Definition: rotation_about_fixed_axis.h:29
MBSim::ToDouble
Definition: utils.h:61
MBSim::Function
Definition: contact_kinematics.h:32
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30