From 7e0d93e3fb08362d9d53fb2fc3c2715596deaf21 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Wed, 22 Dec 2021 12:16:27 -0300 Subject: [PATCH] tests: Add tests for quatexpmap interface functions and minor changes in their docs --- src/xrt/auxiliary/math/m_api.h | 8 ++--- tests/CMakeLists.txt | 7 +++- tests/tests_quatexpmap.cpp | 58 ++++++++++++++++++++++++++++++++++ 3 files changed, 68 insertions(+), 5 deletions(-) create mode 100644 tests/tests_quatexpmap.cpp diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index 0506e02e9..44c3d6e41 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -145,7 +145,7 @@ math_vec3_normalize(struct xrt_vec3 *in); */ /*! - * Create a rotation from a angle in radians and a vector. + * Create a rotation from an angle in radians and a unit vector. * * @relates xrt_quat * @see xrt_vec3 @@ -246,7 +246,7 @@ math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, stru /*! - * Integrate an angular velocity vector (exponential map) and apply to a + * Integrate a local angular velocity vector (exponential map) and apply to a * quaternion. * * ang_vel and dt should share the same units of time, and the ang_vel @@ -263,8 +263,8 @@ math_quat_integrate_velocity(const struct xrt_quat *quat, struct xrt_quat *result); /*! - * Compute an angular velocity vector (exponential map format) by taking the - * finite difference of two quaternions. + * Compute a global angular velocity vector (exponential map format) by taking + * the finite difference of two quaternions. * * quat1 is the orientation dt time after the orientation was quat0 * diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0fd37e3c7..5059f0321 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -29,7 +29,6 @@ target_link_libraries(tests_json PRIVATE tests_main) target_link_libraries(tests_json PRIVATE aux_util) add_test(NAME tests_json COMMAND tests_json --success) - # history add_executable(tests_history_buf tests_history_buf.cpp) target_link_libraries(tests_history_buf PRIVATE tests_main) @@ -41,3 +40,9 @@ add_executable(tests_cxx_wrappers tests_cxx_wrappers.cpp) target_link_libraries(tests_cxx_wrappers PRIVATE tests_main) target_link_libraries(tests_cxx_wrappers PRIVATE aux_util xrt-interfaces) add_test(NAME tests_cxx_wrappers COMMAND tests_cxx_wrappers --success) + +# quatexpmap +add_executable(tests_quatexpmap tests_quatexpmap.cpp) +target_link_libraries(tests_quatexpmap PRIVATE tests_main) +target_link_libraries(tests_quatexpmap PRIVATE aux_math) +add_test(NAME tests_quatexpmap COMMAND tests_quatexpmap --success) diff --git a/tests/tests_quatexpmap.cpp b/tests/tests_quatexpmap.cpp new file mode 100644 index 000000000..0b1770b9d --- /dev/null +++ b/tests/tests_quatexpmap.cpp @@ -0,0 +1,58 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Test C++ quatexpmap interface. + * @author Mateo de Mayo + */ + +#include "catch/catch.hpp" +#include "math/m_api.h" +#include "math/m_vec3.h" +#include + +using std::vector; + +TEST_CASE("m_quatexpmap") +{ + 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}}; + float vel_angle = GENERATE(-M_PI, M_PI / 5); + float dt = GENERATE(0.01, 0.1, 1); + + for (xrt_vec3 q1_axis : q1_axes) { + for (xrt_vec3 vel_axis : vel_axes) { + // First orientation q1 + xrt_quat q1{}; + math_quat_from_angle_vector(q1_angle, &q1_axis, &q1); + + // Second orientation q2: q1 rotated by vel_angle*dt radians around its local vel_axis + xrt_quat q2{}; + xrt_vec3 vel = vel_axis * vel_angle; + math_quat_integrate_velocity(&q1, &vel, dt, &q2); + + // Global velocity vector from q1 to q2 + xrt_vec3 new_global_vel{}; + math_quat_finite_difference(&q1, &q2, dt, &new_global_vel); + + // Adjust global velocity back to local (w.r.t. q1) + xrt_quat inv_q1{}; + xrt_vec3 new_vel{}; + math_quat_invert(&q1, &inv_q1); + math_quat_rotate_derivative(&inv_q1, &new_global_vel, &new_vel); + + INFO("vel=" << vel.x << ", " << vel.y << ", " << vel.z); + INFO("new_vel=" << new_vel.x << ", " << new_vel.y << ", " << new_vel.z); + CHECK(m_vec3_len(new_vel - vel) <= 0.001); + } + } + } +}