You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
102 lines
1.9 KiB
102 lines
1.9 KiB
/** |
|
* @file Euler.hpp |
|
* |
|
* Euler angle tait-bryan body 3-2-1 |
|
* |
|
* @author James Goppert <james.goppert@gmail.com> |
|
*/ |
|
|
|
#pragma once |
|
|
|
#include "math.hpp" |
|
|
|
namespace matrix |
|
{ |
|
|
|
template <typename Type> |
|
class Dcm; |
|
|
|
template <typename Type> |
|
class Quaternion; |
|
|
|
template<typename Type> |
|
class Euler : public Vector<Type, 3> |
|
{ |
|
public: |
|
virtual ~Euler() {}; |
|
|
|
Euler() : Vector<Type, 3>() |
|
{ |
|
} |
|
|
|
Euler(const Vector<Type, 3> & other) : |
|
Vector<Type, 3>(other) |
|
{ |
|
} |
|
|
|
Euler(const Matrix<Type, 3, 1> & other) : |
|
Vector<Type, 3>(other) |
|
{ |
|
} |
|
|
|
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>() |
|
{ |
|
phi() = phi_; |
|
theta() = theta_; |
|
psi() = psi_; |
|
} |
|
|
|
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>() |
|
{ |
|
Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0))); |
|
Type theta_val = Type(asin(-dcm(2,0))); |
|
Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2))); |
|
|
|
// protection against NaN if dcm(0,0) or dcm(2,2) == 0 |
|
psi() = 0; |
|
theta() = 0; |
|
phi() = 0; |
|
if (psi() >= -M_PI_2 && psi() <= M_PI_2) { |
|
psi() = psi_val; |
|
} |
|
if (theta() >= -M_PI_2 && theta() <= M_PI_2) { |
|
theta() = theta_val; |
|
} |
|
if (phi() >= -M_PI_2 && phi() <= M_PI_2) { |
|
phi() = phi_val; |
|
} |
|
} |
|
|
|
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 : */
|
|
|