diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 572b57448..3812a7f6a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -22,6 +22,7 @@ set(tests tests_pacing tests_quatexpmap tests_quat_change_of_basis + tests_quat_swing_twist tests_rational tests_vector tests_worker @@ -66,8 +67,10 @@ target_link_libraries(tests_quatexpmap PRIVATE aux_math) target_link_libraries(tests_rational PRIVATE aux_math) target_link_libraries(tests_pose PRIVATE aux_math) target_link_libraries(tests_quat_change_of_basis PRIVATE aux_math) +target_link_libraries(tests_quat_swing_twist PRIVATE aux_math) target_include_directories(tests_quat_change_of_basis SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) +target_include_directories(tests_quat_swing_twist SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) if(XRT_BUILD_DRIVER_HANDTRACKING) target_link_libraries( diff --git a/tests/tests_quat_swing_twist.cpp b/tests/tests_quat_swing_twist.cpp new file mode 100644 index 000000000..227971686 --- /dev/null +++ b/tests/tests_quat_swing_twist.cpp @@ -0,0 +1,103 @@ +// Copyright 2022, Collabora, Inc. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Test for the quaternion swing-twist composition. + * @author Moses Turner + */ +#include "math/m_api.h" +#include "xrt/xrt_defines.h" +#include +#include +#include +#include + + +#include "catch/catch.hpp" + +#include + + +#include "math/m_eigen_interop.hpp" +#include + +float +quat_difference(xrt_quat q1, xrt_quat q2) +{ + // https://math.stackexchange.com/a/90098 + // d(q1,q2)=1−⟨q1,q2⟩2 + + float inner_product = (q1.w * q2.w) + (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z); + return 1.0 - (inner_product * inner_product); +} + +TEST_CASE("SwingTwistTriviallyInvertibleIn180DegreeHemisphere") + +{ + std::random_device dev; + + auto mt = std::mt19937(dev()); + + auto rd = std::uniform_real_distribution(-M_PI / 2, M_PI / 2); + + for (int i = 0; i < 20; i++) { + xrt_vec2 swing = {rd(mt), rd(mt)}; + float twist = rd(mt); + + xrt_quat combquat; + + math_quat_from_swing_twist(&swing, twist, &combquat); + + xrt_vec2 recovered_swing; + float recovered_twist; + + math_quat_to_swing_twist(&combquat, &recovered_swing, &recovered_twist); + + bool success = (fabsf(swing.x - recovered_swing.x) <= 0.001) && + (fabsf(swing.y - recovered_swing.y) <= 0.001) && + (fabsf(twist - recovered_twist) <= 0.001); + + if (!success) { + U_LOG_E("Fail! Used swing %f %f, twist %f", swing.x, swing.y, twist); + } + CHECK(success); + } +} + + +TEST_CASE("SwingTwistAlwaysInvertibleIfYouUseSoundRotationEqualities") + +{ + std::random_device dev; + + auto mt = std::mt19937(dev()); + + auto rd = std::uniform_real_distribution(-1000, 1000); + + for (int i = 0; i < 20; i++) { + + xrt_vec2 swing = {rd(mt), rd(mt)}; + float twist = rd(mt); + + xrt_quat combquat; + + math_quat_from_swing_twist(&swing, twist, &combquat); + + xrt_vec2 recovered_swing; + float recovered_twist; + + math_quat_to_swing_twist(&combquat, &recovered_swing, &recovered_twist); + + xrt_quat recovered_quat; + + math_quat_from_swing_twist(&recovered_swing, recovered_twist, &recovered_quat); + + bool success = quat_difference(combquat, recovered_quat) <= 0.001; + + if (!success) { + // Test failed + U_LOG_E("Fail! Used swing %f %f, twist %f", swing.x, swing.y, twist); + } + CHECK(success); + } +}