28#include "Eigen/Geometry"
99 : roll_(
std::atan2(static_cast<T>(2.0) * (qw * qy - qx * qz),
101 static_cast<T>(1.0))),
102 pitch_(
std::asin(static_cast<T>(2.0) * (qw * qx + qy * qz))),
103 yaw_(
std::atan2(static_cast<T>(2.0) * (qw * qz - qx * qy),
105 static_cast<T>(1.0))) {}
118 T
roll()
const {
return roll_; }
130 T
yaw()
const {
return yaw_; }
147 return pitch_ < M_PI_2 && pitch_ > -M_PI_2;
155 T coeff =
static_cast<T
>(0.5);
157 T p = pitch_ * coeff;
168 T qw = cr * cp * cy - sr * sp * sy;
169 T qx = cr * sp * cy - sr * cp * sy;
170 T qy = cr * sp * sy + sr * cp * cy;
171 T qz = cr * cp * sy + sr * sp * cy;
173 return {-qw, -qx, -qy, -qz};
175 return {qw, qx, qy, qz};
Any orientation of a rigid body on a 3-D space can be achieved by composing three rotations about the...
T roll() const
Getter for roll_
T pitch() const
Getter for pitch_
EulerAnglesZXY(T yaw)
Constructs a rotation using only yaw (i.e., around the z-axis).
T yaw() const
Getter for yaw_
EulerAnglesZXY(T qw, T qx, T qy, T qz)
Constructs a rotation using components of a quaternion.
bool IsValid()
Verifies the validity of the specified rotation.
Eigen::Quaternion< T > ToQuaternion() const
Converts to a quaternion with a non-negative scalar part
EulerAnglesZXY(T roll, T pitch, T yaw)
Constructs a rotation using arbitrary roll, pitch, and yaw.
EulerAnglesZXY(const Eigen::Quaternion< T > &q)
Constructs a rotation from quaternion.
void Normalize()
Normalizes roll_, pitch_, and yaw_ to [-PI, PI).
EulerAnglesZXY()
Constructs an identity rotation.
Math-related util functions.
EulerAnglesZXY< double > EulerAnglesZXYd
EulerAnglesZXY< float > EulerAnglesZXYf
T Square(const T value)
Compute squared value.
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).