diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index 1267144bd..ecc8eb8d7 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -298,6 +298,27 @@ math_quat_finite_difference(const struct xrt_quat *quat0, float dt, struct xrt_vec3 *out_ang_vel); +/*! + * Converts a rotation vector in axis-angle form to its corresponding unit quaternion. + * + * @relates xrt_quat + * @see xrt_vec3 + * @ingroup aux_math + */ +void +math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat); + + +/*! + * Converts a unit quaternion into its corresponding axis-angle vector representation. + * + * @relates xrt_quat + * @see xrt_vec3 + * @ingroup aux_math + */ +void +math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle); + /*! * Used to rotate a derivative like a angular velocity. * diff --git a/src/xrt/auxiliary/math/m_quatexpmap.cpp b/src/xrt/auxiliary/math/m_quatexpmap.cpp index a5363cc14..02e69fb38 100644 --- a/src/xrt/auxiliary/math/m_quatexpmap.cpp +++ b/src/xrt/auxiliary/math/m_quatexpmap.cpp @@ -164,3 +164,16 @@ math_quat_finite_difference(const struct xrt_quat *quat0, Eigen::Quaternionf inc_quat = map_quat(*quat1) * map_quat(*quat0).conjugate(); map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat) / dt; } + +extern "C" void +math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat) +{ + map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle)); +} + +extern "C" void +math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) +{ + Eigen::Quaternionf eigen_quat = map_quat(*quat); + map_vec3(*out_axis_angle) = quat_ln(eigen_quat); +} diff --git a/tests/tests_quatexpmap.cpp b/tests/tests_quatexpmap.cpp index 0b1770b9d..881b8bfe7 100644 --- a/tests/tests_quatexpmap.cpp +++ b/tests/tests_quatexpmap.cpp @@ -15,13 +15,12 @@ using std::vector; TEST_CASE("m_quatexpmap") { + xrt_vec3 axis1 = m_vec3_normalize({4, -7, 3}); + xrt_vec3 axis2 = m_vec3_normalize({-1, -2, -3}); + xrt_vec3 axis3 = m_vec3_normalize({1, -1, 1}); + xrt_vec3 axis4 = m_vec3_normalize({-11, 23, 91}); SECTION("Test integrate velocity and finite difference mappings") { - xrt_vec3 axis1 = m_vec3_normalize({4, -7, 3}); - xrt_vec3 axis2 = m_vec3_normalize({-1, -2, -3}); - xrt_vec3 axis3 = m_vec3_normalize({1, -1, 1}); - xrt_vec3 axis4 = m_vec3_normalize({-11, 23, 91}); - vector q1_axes{{axis1, axis2}}; float q1_angle = GENERATE(M_PI, -M_PI / 6); vector vel_axes{{axis3, axis4}}; @@ -55,4 +54,20 @@ TEST_CASE("m_quatexpmap") } } } + + SECTION("Test quat_exp and quat_ln are inverses") + { + // We use rotations with less than PI radians as quat_ln will return the negative rotation otherwise + vector aas = {{0, 0, 0}, axis1 * M_PI * 0.01, axis2 * M_PI * 0.5, axis3 * 0.99 * M_PI}; + + for (xrt_vec3 aa : aas) { + xrt_quat quat{}; + math_quat_exp(&aa, &quat); + + xrt_vec3 expected_aa{}; + math_quat_ln(&quat, &expected_aa); + + CHECK(m_vec3_len(expected_aa - aa) <= 0.001); + } + } }