mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 02:26:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Add math_quat_to_swing_twist

Browse source
This commit is contained in:
Moses Turner 2022-11-23 12:47:15 -06:00
parent b3c277196e
commit f5d4d49607
2 changed files with 37 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
src/xrt/auxiliary/math/m_api.h
View file

@ -413,6 +413,15 @@ math_quat_from_swing(const struct xrt_vec2 *swing, struct xrt_quat *result);
void
math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, struct xrt_quat *result);
/*!
* Converts a quaternion to XY-swing and Z-twist
*
* @relates xrt_quat
* @ingroup aux_math
*/
void
math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist);
/*
*
* Matrix functions

28
src/xrt/auxiliary/math/m_base.cpp
View file

@ -484,6 +484,34 @@ math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, stru
}
}
/*!
* Converts a quaternion to XY-swing and Z-twist
*
* @relates xrt_quat
* @ingroup aux_math
*/
extern "C" void
math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist)
{
Eigen::Quaternionf rot = map_quat(*in);
Eigen::Vector3f our_z = rot * (Eigen::Vector3f::UnitZ());
Eigen::Quaternionf swing = Eigen::Quaternionf().setFromTwoVectors(Eigen::Vector3f::UnitZ(), our_z);
Eigen::Quaternionf twist = swing.inverse() * rot;
Eigen::AngleAxisf twist_aax = Eigen::AngleAxisf(twist);
Eigen::AngleAxisf swing_aax = Eigen::AngleAxisf(swing);
out_swing->x = swing_aax.axis().x() * swing_aax.angle();
out_swing->y = swing_aax.axis().y() * swing_aax.angle();
assert(swing_aax.axis().z() < 0.001);
*out_twist = twist_aax.axis().z() * twist_aax.angle();
}
/*
*
* Exported matrix functions.