Commit 2b99ce91 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[geometry] Added tait-bryan angles to quaternion conversion

parent 61e4edb6
...@@ -23,6 +23,32 @@ constexpr T to_radians(T degrees) ...@@ -23,6 +23,32 @@ constexpr T to_radians(T degrees)
return degrees * M_PI / 180.0; return degrees * M_PI / 180.0;
} }
/**
* Conversion from Tait-Bryan angles to Quaternion in NED convention.
*
* Input as radians.
*/
template <typename T>
Quaternion<T> quaternion_from_nautical_rad(T yaw, T pitch, T roll)
{
return Eigen::AngleAxis<T>(yaw, Eigen::Vector3<T>::UnitZ())
* Eigen::AngleAxis<T>(pitch, Eigen::Vector3<T>::UnitY())
* Eigen::AngleAxis<T>(roll, Eigen::Vector3<T>::UnitX());
}
/**
* Conversion from Tait-Bryan angles to Quaternion in NED convention.
*
* Input as degrees.
*/
template <typename T>
Quaternion<T> quaternion_from_nautical_degrees(T yaw, T pitch, T roll)
{
return quaternion_from_nautical_rad(to_radians(yaw),
to_radians(pitch),
to_radians(roll));
}
/** /**
* Find a vector non-colinear to v. * Find a vector non-colinear to v.
* *
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment