Loading...
Searching...
No Matches
EulerAngles.h
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#ifndef EIGEN_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition?
11#define EIGEN_EULERANGLESCLASS_H
12
13namespace Eigen
14{
15 /*template<typename Other,
16 int OtherRows=Other::RowsAtCompileTime,
17 int OtherCols=Other::ColsAtCompileTime>
18 struct ei_eulerangles_assign_impl;*/
19
110 template <typename _Scalar, class _System>
111 class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
112 {
113 public:
115 typedef _Scalar Scalar;
116
118 typedef _System System;
119
120 typedef Matrix<Scalar,3,3> Matrix3;
121 typedef Matrix<Scalar,3,1> Vector3;
122 typedef Quaternion<Scalar> QuaternionType;
123 typedef AngleAxis<Scalar> AngleAxisType;
127 const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1);
128 return System::IsAlphaOpposite ? -u : u;
129 }
130
133 const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1);
134 return System::IsBetaOpposite ? -u : u;
135 }
136
139 const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1);
140 return System::IsGammaOpposite ? -u : u;
141 }
142
143 private:
144 Vector3 m_angles;
145
146 public:
150 EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
151 m_angles(alpha, beta, gamma) {}
152
157 template<typename Derived>
158 EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
159
171 template<typename Derived>
173 const MatrixBase<Derived>& m,
174 bool positiveRangeAlpha,
175 bool positiveRangeBeta,
176 bool positiveRangeGamma) {
177
178 System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
179 }
180
187 template<typename Derived>
188 EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; }
189
201 template<typename Derived>
203 const RotationBase<Derived, 3>& rot,
204 bool positiveRangeAlpha,
205 bool positiveRangeBeta,
206 bool positiveRangeGamma) {
207
208 System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
209 }
210
212 const Vector3& angles() const { return m_angles; }
214 Vector3& angles() { return m_angles; }
215
217 Scalar alpha() const { return m_angles[0]; }
219 Scalar& alpha() { return m_angles[0]; }
220
222 Scalar beta() const { return m_angles[1]; }
224 Scalar& beta() { return m_angles[1]; }
225
227 Scalar gamma() const { return m_angles[2]; }
229 Scalar& gamma() { return m_angles[2]; }
230
235 {
236 EulerAngles res;
237 res.m_angles = -m_angles;
238 return res;
239 }
240
245 {
246 return inverse();
247 }
248
260 template<
261 bool PositiveRangeAlpha,
262 bool PositiveRangeBeta,
263 bool PositiveRangeGamma,
264 typename Derived>
265 static EulerAngles FromRotation(const MatrixBase<Derived>& m)
266 {
267 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
268
269 EulerAngles e;
270 System::template CalcEulerAngles<
271 PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m);
272 return e;
273 }
274
286 template<
287 bool PositiveRangeAlpha,
288 bool PositiveRangeBeta,
289 bool PositiveRangeGamma,
290 typename Derived>
291 static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot)
292 {
293 return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
294 }
295
296 /*EulerAngles& fromQuaternion(const QuaternionType& q)
297 {
298 // TODO: Implement it in a faster way for quaternions
299 // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
300 // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
301 // Currently we compute all matrix cells from quaternion.
302
303 // Special case only for ZYX
304 //Scalar y2 = q.y() * q.y();
305 //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
306 //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
307 //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
308 }*/
309
311 template<typename Derived>
312 EulerAngles& operator=(const MatrixBase<Derived>& m) {
313 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
314
315 System::CalcEulerAngles(*this, m);
316 return *this;
317 }
318
319 // TODO: Assign and construct from another EulerAngles (with different system)
320
322 template<typename Derived>
323 EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
324 System::CalcEulerAngles(*this, rot.toRotationMatrix());
325 return *this;
326 }
327
328 // TODO: Support isApprox function
329
332 {
333 return static_cast<QuaternionType>(*this).toRotationMatrix();
334 }
335
337 operator QuaternionType() const
338 {
339 return
343 }
344
345 friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles)
346 {
347 s << eulerAngles.angles().transpose();
348 return s;
349 }
350 };
351
352#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
353 \
354 typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX;
355
356#define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \
357 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \
358 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \
359 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \
360 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \
361 \
362 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \
363 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \
364 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
365 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \
366 \
367 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \
368 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \
369 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \
370 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX)
371
372EIGEN_EULER_ANGLES_TYPEDEFS(float, f)
373EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
374
375 namespace internal
376 {
377 template<typename _Scalar, class _System>
378 struct traits<EulerAngles<_Scalar, _System> >
379 {
380 typedef _Scalar Scalar;
381 };
382 }
383
384}
385
386#endif // EIGEN_EULERANGLESCLASS_H
Represents a rotation in a 3 dimensional space as three Euler angles.
Definition: EulerAngles.h:112
Vector3 & angles()
Definition: EulerAngles.h:214
AngleAxis< Scalar > AngleAxisType
Definition: EulerAngles.h:123
EulerAngles & operator=(const MatrixBase< Derived > &m)
Definition: EulerAngles.h:312
Matrix3 toRotationMatrix() const
Definition: EulerAngles.h:331
_System System
Definition: EulerAngles.h:118
Scalar beta() const
Definition: EulerAngles.h:222
EulerAngles inverse() const
Definition: EulerAngles.h:234
EulerAngles(const RotationBase< Derived, 3 > &rot)
Definition: EulerAngles.h:188
EulerAngles operator-() const
Definition: EulerAngles.h:244
_Scalar Scalar
Definition: EulerAngles.h:115
static Vector3 AlphaAxisVector()
Definition: EulerAngles.h:126
EulerAngles()
Definition: EulerAngles.h:148
EulerAngles & operator=(const RotationBase< Derived, 3 > &rot)
Definition: EulerAngles.h:323
Scalar alpha() const
Definition: EulerAngles.h:217
EulerAngles(const Scalar &alpha, const Scalar &beta, const Scalar &gamma)
Definition: EulerAngles.h:150
const Vector3 & angles() const
Definition: EulerAngles.h:212
static Vector3 GammaAxisVector()
Definition: EulerAngles.h:138
Scalar gamma() const
Definition: EulerAngles.h:227
EulerAngles(const MatrixBase< Derived > &m, bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
Definition: EulerAngles.h:172
Scalar & gamma()
Definition: EulerAngles.h:229
static EulerAngles FromRotation(const MatrixBase< Derived > &m)
Definition: EulerAngles.h:265
Matrix< Scalar, 3, 3 > Matrix3
Definition: EulerAngles.h:120
Quaternion< Scalar > QuaternionType
Definition: EulerAngles.h:122
EulerAngles(const MatrixBase< Derived > &m)
Definition: EulerAngles.h:158
Scalar & alpha()
Definition: EulerAngles.h:219
Scalar & beta()
Definition: EulerAngles.h:224
EulerAngles(const RotationBase< Derived, 3 > &rot, bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
Definition: EulerAngles.h:202
static EulerAngles FromRotation(const RotationBase< Derived, 3 > &rot)
Definition: EulerAngles.h:291
static Vector3 BetaAxisVector()
Definition: EulerAngles.h:132
Matrix< Scalar, 3, 1 > Vector3
Definition: EulerAngles.h:121
Namespace containing all symbols from the Eigen library.
Definition: AdolcForward:45