mbsim  4.0.0
MBSim Kernel
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
rotation_about_z_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_Z_AXIS_H_
21 #define _ROTATION_ABOUT_Z_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 RotationAboutZAxis : public Function<fmatvec::RotMat3(Arg)> {
30  using B = fmatvec::Function<fmatvec::RotMat3(Arg)>;
31  private:
32  fmatvec::RotMat3 A;
33  fmatvec::Vec3 a, zero;
34  public:
35  RotationAboutZAxis() { a.e(2) = 1; A.e(2,2) = 1; }
36  int getArgSize() const override { return 1; }
37  fmatvec::RotMat3 operator()(const Arg &q) override {
38  double alpha = ToDouble<Arg>::cast(q);
39  const double cosq=cos(alpha);
40  const double sinq=sin(alpha);
41  A.e(0,0) = cosq;
42  A.e(1,0) = sinq;
43  A.e(0,1) = -sinq;
44  A.e(1,1) = cosq;
45  return A;
46  }
47  typename B::DRetDArg parDer(const Arg &q) override { return a; }
48  typename B::DRetDArg parDerDirDer(const Arg &qd, const Arg &q) override { return zero; }
49  bool constParDer() const override { return true; }
50  };
51 
52 }
53 
54 #endif
Definition: utils.h:61
Definition: contact_kinematics.h:32
Definition: rotation_about_z_axis.h:29