/**
|
* @file Euler.hpp
|
*
|
* All rotations and axis systems follow the right-hand rule
|
*
|
* An instance of this class defines a rotation from coordinate frame 1 to coordinate frame 2.
|
* It follows the convention of a 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
* In order to go from frame 1 to frame 2 we apply the following rotations consecutively.
|
* 1) We rotate about our initial Z axis by an angle of _psi.
|
* 2) We rotate about the newly created Y' axis by an angle of _theta.
|
* 3) We rotate about the newly created X'' axis by an angle of _phi.
|
*
|
* @author James Goppert <james.goppert@gmail.com>
|
*/
|
|
#pragma once
|
|
#include "math.hpp"
|
|
#ifndef M_PI
|
#define M_PI (3.14159265358979323846f)
|
#endif
|
|
namespace matrix
|
{
|
|
template <typename Type>
|
class Dcm;
|
|
template <typename Type>
|
class Quaternion;
|
|
/**
|
* Euler angles class
|
*
|
* This class describes the rotation from frame 1
|
* to frame 2 via 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
*/
|
template<typename Type>
|
class Euler : public Vector<Type, 3>
|
{
|
public:
|
virtual ~Euler() {};
|
|
/**
|
* Standard constructor
|
*/
|
Euler() : Vector<Type, 3>()
|
{
|
}
|
|
/**
|
* Copy constructor
|
*
|
* @param other vector to copy
|
*/
|
Euler(const Vector<Type, 3> &other) :
|
Vector<Type, 3>(other)
|
{
|
}
|
|
/**
|
* Constructor from Matrix31
|
*
|
* @param other Matrix31 to copy
|
*/
|
Euler(const Matrix<Type, 3, 1> &other) :
|
Vector<Type, 3>(other)
|
{
|
}
|
|
/**
|
* Constructor from euler angles
|
*
|
* Instance is initialized from an 3-2-1 intrinsic Tait-Bryan
|
* rotation sequence representing transformation from frame 1
|
* to frame 2.
|
*
|
* @param phi_ rotation angle about X axis
|
* @param theta_ rotation angle about Y axis
|
* @param psi_ rotation angle about Z axis
|
*/
|
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
|
{
|
phi() = phi_;
|
theta() = theta_;
|
psi() = psi_;
|
}
|
|
/**
|
* Constructor from DCM matrix
|
*
|
* Instance is set from Dcm representing transformation from
|
* frame 2 to frame 1.
|
* This instance will hold the angles defining the 3-2-1 intrinsic
|
* Tait-Bryan rotation sequence from frame 1 to frame 2.
|
*
|
* @param dcm Direction cosine matrix
|
*/
|
Euler(const Dcm<Type> &dcm) : Vector<Type, 3>()
|
{
|
Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2)));
|
Type theta_val = Type(asin(-dcm(2, 0)));
|
Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0)));
|
Type pi = Type(M_PI);
|
|
if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
|
phi_val = Type(0.0);
|
psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
|
|
} else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
|
phi_val = Type(0.0);
|
psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
|
}
|
|
phi() = phi_val;
|
theta() = theta_val;
|
psi() = psi_val;
|
}
|
|
/**
|
* Constructor from quaternion instance.
|
*
|
* Instance is set from a quaternion representing transformation
|
* from frame 2 to frame 1.
|
* This instance will hold the angles defining the 3-2-1 intrinsic
|
* Tait-Bryan rotation sequence from frame 1 to frame 2.
|
*
|
* @param q quaternion
|
*/
|
Euler(const Quaternion<Type> &q) :
|
Vector<Type, 3>()
|
{
|
*this = Euler(Dcm<Type>(q));
|
}
|
|
inline Type phi() const
|
{
|
return (*this)(0);
|
}
|
inline Type theta() const
|
{
|
return (*this)(1);
|
}
|
inline Type psi() const
|
{
|
return (*this)(2);
|
}
|
|
inline Type &phi()
|
{
|
return (*this)(0);
|
}
|
inline Type &theta()
|
{
|
return (*this)(1);
|
}
|
inline Type &psi()
|
{
|
return (*this)(2);
|
}
|
|
};
|
|
typedef Euler<float> Eulerf;
|
|
} // namespace matrix
|
|
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
