PX4 Firmware
PX4 Autopilot Software http://px4.io
Dcm.hpp
Go to the documentation of this file.
1 /**
2  * @file Dcm.hpp
3  *
4  * A direction cosine matrix class.
5  * All rotations and axis systems follow the right-hand rule.
6  *
7  * This library uses the convention that premultiplying a three dimensional
8  * vector represented in coordinate system 1 will apply a rotation from coordinate system
9  * 1 to coordinate system 2 to the vector.
10  * Likewise, a matrix instance of this class also represents a coordinate transformation
11  * from frame 2 to frame 1.
12  *
13  * @author James Goppert <james.goppert@gmail.com>
14  */
15 
16 #pragma once
17 
18 #include "math.hpp"
19 
20 namespace matrix
21 {
22 
23 template<typename Type>
24 class Quaternion;
25 
26 template<typename Type>
27 class Euler;
28 
29 template<typename Type>
30 class AxisAngle;
31 
32 /**
33  * Direction cosine matrix class
34  *
35  * The rotation between two coordinate frames is
36  * described by this class.
37  */
38 template<typename Type>
39 class Dcm : public SquareMatrix<Type, 3>
40 {
41 public:
43 
44  /**
45  * Standard constructor
46  *
47  * Initializes to identity
48  */
49  Dcm()
50  {
51  (*this) = eye<Type, 3>();
52  }
53 
54  /**
55  * Constructor from array
56  *
57  * @param _data pointer to array
58  */
59  explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
60  {
61  }
62 
63  /**
64  * Constructor from array
65  *
66  * @param _data pointer to array
67  */
68  explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
69  {
70  }
71 
72  /**
73  * Copy constructor
74  *
75  * @param other Matrix33 to set dcm to
76  */
77  Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(other)
78  {
79  }
80 
81  /**
82  * Constructor from quaternion
83  *
84  * Instance is initialized from quaternion representing
85  * coordinate transformation from frame 2 to frame 1.
86  *
87  * @param q quaternion to set dcm to
88  */
90  {
91  Dcm &dcm = *this;
92  const Type a = q(0);
93  const Type b = q(1);
94  const Type c = q(2);
95  const Type d = q(3);
96  const Type aa = a * a;
97  const Type ab = a * b;
98  const Type ac = a * c;
99  const Type ad = a * d;
100  const Type bb = b * b;
101  const Type bc = b * c;
102  const Type bd = b * d;
103  const Type cc = c * c;
104  const Type cd = c * d;
105  const Type dd = d * d;
106  dcm(0, 0) = aa + bb - cc - dd;
107  dcm(0, 1) = 2 * (bc - ad);
108  dcm(0, 2) = 2 * (ac + bd);
109  dcm(1, 0) = 2 * (bc + ad);
110  dcm(1, 1) = aa - bb + cc - dd;
111  dcm(1, 2) = 2 * (cd - ab);
112  dcm(2, 0) = 2 * (bd - ac);
113  dcm(2, 1) = 2 * (ab + cd);
114  dcm(2, 2) = aa - bb - cc + dd;
115  }
116 
117  /**
118  * Constructor from euler angles
119  *
120  * This sets the transformation matrix from frame 2 to frame 1 where the rotation
121  * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
122  *
123  *
124  * @param euler euler angle instance
125  */
126  Dcm(const Euler<Type> &euler)
127  {
128  Dcm &dcm = *this;
129  Type cosPhi = Type(cos(euler.phi()));
130  Type sinPhi = Type(sin(euler.phi()));
131  Type cosThe = Type(cos(euler.theta()));
132  Type sinThe = Type(sin(euler.theta()));
133  Type cosPsi = Type(cos(euler.psi()));
134  Type sinPsi = Type(sin(euler.psi()));
135 
136  dcm(0, 0) = cosThe * cosPsi;
137  dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
138  dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
139 
140  dcm(1, 0) = cosThe * sinPsi;
141  dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
142  dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
143 
144  dcm(2, 0) = -sinThe;
145  dcm(2, 1) = sinPhi * cosThe;
146  dcm(2, 2) = cosPhi * cosThe;
147  }
148 
149 
150  /**
151  * Constructor from axis angle
152  *
153  * This sets the transformation matrix from frame 2 to frame 1 where the rotation
154  * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
155  *
156  *
157  * @param euler euler angle instance
158  */
159  Dcm(const AxisAngle<Type> &aa)
160  {
161  Dcm &dcm = *this;
162  dcm = Quaternion<Type>(aa);
163  }
164 
165  Vector<Type, 3> vee() const // inverse to Vector.hat() operation
166  {
167  const Dcm &A(*this);
168  Vector<Type, 3> v;
169  v(0) = -A(1, 2);
170  v(1) = A(0, 2);
171  v(2) = -A(0, 1);
172  return v;
173  }
174 
175  void renormalize()
176  {
177  /* renormalize rows */
178  for (size_t r = 0; r < 3; r++) {
180  this->Matrix<Type,3,3>::row(r) = rvec.normalized();
181  }
182  }
183 };
184 
185 typedef Dcm<float> Dcmf;
186 
187 } // namespace matrix
188 
189 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Type theta() const
Definition: Euler.hpp:132
Dcm< float > Dcmf
Definition: Dcm.hpp:185
Direction cosine matrix class.
Definition: AxisAngle.hpp:15
Dcm(const Type data_[9])
Constructor from array.
Definition: Dcm.hpp:68
Dcm(const Matrix< Type, 3, 3 > &other)
Copy constructor.
Definition: Dcm.hpp:77
Type phi() const
Definition: Euler.hpp:128
Quaternion class.
Definition: Dcm.hpp:24
Matrix< Type, N, M > transpose() const
Definition: Matrix.hpp:353
Type psi() const
Definition: Euler.hpp:136
Vector< Type, 3 > vee() const
Definition: Dcm.hpp:165
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:286
void renormalize()
Definition: Dcm.hpp:175
Dcm(const AxisAngle< Type > &aa)
Constructor from axis angle.
Definition: Dcm.hpp:159
Dcm(const Quaternion< Type > &q)
Constructor from quaternion.
Definition: Dcm.hpp:89
Euler angles class.
Definition: AxisAngle.hpp:18
Matrix< Type, 3, 1 > Vector3
Definition: Dcm.hpp:42
Dcm(const Euler< Type > &euler)
Constructor from euler angles.
Definition: Dcm.hpp:126
Vector3 normalized() const
Definition: Vector3.hpp:104
const Slice< Type, 1, N, M, N > row(size_t i) const
Definition: Matrix.hpp:385
AxisAngle class.
Definition: AxisAngle.hpp:21
Dcm()
Standard constructor.
Definition: Dcm.hpp:49
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:279
Dcm(const Type data_[3][3])
Constructor from array.
Definition: Dcm.hpp:59