PX4 Firmware
PX4 Autopilot Software http://px4.io
AxisAngle.hpp
Go to the documentation of this file.
1 /**
2  * @file AxisAngle.hpp
3  *
4  * @author James Goppert <james.goppert@gmail.com>
5  */
6 
7 #pragma once
8 
9 #include "math.hpp"
10 
11 namespace matrix
12 {
13 
14 template <typename Type>
15 class Dcm;
16 
17 template <typename Type>
18 class Euler;
19 
20 template <typename Type>
21 class AxisAngle;
22 
23 /**
24  * AxisAngle class
25  *
26  * The rotation between two coordinate frames is
27  * described by this class.
28  */
29 template<typename Type>
30 class AxisAngle : public Vector<Type, 3>
31 {
32 public:
34 
35  /**
36  * Constructor from array
37  *
38  * @param data_ array
39  */
40  explicit AxisAngle(const Type data_[3]) :
41  Vector<Type, 3>(data_)
42  {
43  }
44 
45  /**
46  * Standard constructor
47  */
48  AxisAngle() = default;
49 
50  /**
51  * Constructor from Matrix31
52  *
53  * @param other Matrix31 to copy
54  */
55  AxisAngle(const Matrix31 &other) :
56  Vector<Type, 3>(other)
57  {
58  }
59 
60  /**
61  * Constructor from quaternion
62  *
63  * This sets the instance from a quaternion representing coordinate transformation from
64  * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
65  * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
66  *
67  * @param q quaternion
68  */
70  {
71  AxisAngle &v = *this;
72  Type ang = Type(2) * acos(q(0));
73  Type mag = sin(ang/2.0f);
74  if (fabs(mag) > 0) {
75  v(0) = ang*q(1)/mag;
76  v(1) = ang*q(2)/mag;
77  v(2) = ang*q(3)/mag;
78  } else {
79  v(0) = 0;
80  v(1) = 0;
81  v(2) = 0;
82  }
83  }
84 
85  /**
86  * Constructor from dcm
87  *
88  * Instance is initialized from a dcm representing coordinate transformation
89  * from frame 2 to frame 1.
90  *
91  * @param dcm dcm to set quaternion to
92  */
93  AxisAngle(const Dcm<Type> &dcm)
94  {
95  AxisAngle &v = *this;
96  v = Quaternion<Type>(dcm);
97  }
98 
99  /**
100  * Constructor from euler angles
101  *
102  * This sets the instance to a quaternion representing coordinate transformation from
103  * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
104  * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
105  *
106  * @param euler euler angle instance
107  */
108  AxisAngle(const Euler<Type> &euler)
109  {
110  AxisAngle &v = *this;
111  v = Quaternion<Type>(euler);
112  }
113 
114  /**
115  * Constructor from 3 axis angle values (unit vector * angle)
116  *
117  * @param x r_x*angle
118  * @param y r_y*angle
119  * @param z r_z*angle
120  */
121  AxisAngle(Type x, Type y, Type z)
122  {
123  AxisAngle &v = *this;
124  v(0) = x;
125  v(1) = y;
126  v(2) = z;
127  }
128 
129  /**
130  * Constructor from axis and angle
131  *
132  * @param axis An axis of rotation, normalized if not unit length
133  * @param angle The amount to rotate
134  */
135  AxisAngle(const Matrix31 & axis_, Type angle_)
136  {
137  AxisAngle &v = *this;
138  // make sure axis is a unit vector
139  Vector<Type, 3> a = axis_;
140  a = a.unit();
141  v(0) = a(0)*angle_;
142  v(1) = a(1)*angle_;
143  v(2) = a(2)*angle_;
144  }
145 
146 
148  if (Vector<Type, 3>::norm() > 0) {
149  return Vector<Type, 3>::unit();
150  } else {
151  return Vector3<Type>(1, 0, 0);
152  }
153  }
154 
155  Type angle() {
156  return Vector<Type, 3>::norm();
157  }
158 };
159 
161 
162 } // namespace matrix
163 
164 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Dual< Scalar, N > acos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:309
Vector< Type, 3 > axis()
Definition: AxisAngle.hpp:147
AxisAngle(Type x, Type y, Type z)
Constructor from 3 axis angle values (unit vector * angle)
Definition: AxisAngle.hpp:121
AxisAngle(const Matrix31 &axis_, Type angle_)
Constructor from axis and angle.
Definition: AxisAngle.hpp:135
AxisAngle()=default
Standard constructor.
Direction cosine matrix class.
Definition: AxisAngle.hpp:15
AxisAngle(const Type data_[3])
Constructor from array.
Definition: AxisAngle.hpp:40
Quaternion class.
Definition: Dcm.hpp:24
AxisAngle(const Dcm< Type > &dcm)
Constructor from dcm.
Definition: AxisAngle.hpp:93
AxisAngle(const Euler< Type > &euler)
Constructor from euler angles.
Definition: AxisAngle.hpp:108
Matrix< Type, 3, 1 > Matrix31
Definition: AxisAngle.hpp:33
Vector unit() const
Definition: Vector.hpp:91
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Euler angles class.
Definition: AxisAngle.hpp:18
AxisAngle(const Matrix31 &other)
Constructor from Matrix31.
Definition: AxisAngle.hpp:55
AxisAngle< float > AxisAnglef
Definition: AxisAngle.hpp:160
AxisAngle class.
Definition: AxisAngle.hpp:21
AxisAngle(const Quaternion< Type > &q)
Constructor from quaternion.
Definition: AxisAngle.hpp:69
Type norm() const
Definition: Vector.hpp:73
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:279