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

tests: Add tests for swing-twist conversions

Browse source
This commit is contained in:
Moses Turner 2022-11-23 13:02:24 -06:00
parent f5d4d49607
commit 352fda971c
2 changed files with 106 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
tests/CMakeLists.txt
View file

@ -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(

103
tests/tests_quat_swing_twist.cpp Normal file
View file

@ -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 <moses@collabora.com>
*/
#include "math/m_api.h"
#include "xrt/xrt_defines.h"
#include <util/u_worker.hpp>
#include <util/u_logging.h>
#include <math/m_space.h>
#include <math/m_vec3.h>
#include "catch/catch.hpp"
#include <random>
#include "math/m_eigen_interop.hpp"
#include <Eigen/Core>
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<float>(-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<float>(-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);
}
}