The open source OpenXR runtime
tests: Add tests for swing-twist conversions
+106
+3
tests/CMakeLists.txt
+3
tests/CMakeLists.txt
···
22
22
tests_pacing
23
23
tests_quatexpmap
24
24
tests_quat_change_of_basis
25
+
tests_quat_swing_twist
25
26
tests_rational
26
27
tests_vector
27
28
tests_worker
···
66
67
target_link_libraries(tests_rational PRIVATE aux_math)
67
68
target_link_libraries(tests_pose PRIVATE aux_math)
68
69
target_link_libraries(tests_quat_change_of_basis PRIVATE aux_math)
70
+
target_link_libraries(tests_quat_swing_twist PRIVATE aux_math)
69
71
70
72
target_include_directories(tests_quat_change_of_basis SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
73
+
target_include_directories(tests_quat_swing_twist SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
71
74
72
75
if(XRT_BUILD_DRIVER_HANDTRACKING)
73
76
target_link_libraries(
+103
tests/tests_quat_swing_twist.cpp
+103
tests/tests_quat_swing_twist.cpp
···
1
+
// Copyright 2022, Collabora, Inc.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Test for the quaternion swing-twist composition.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
*/
8
+
#include "math/m_api.h"
9
+
#include "xrt/xrt_defines.h"
10
+
#include <util/u_worker.hpp>
11
+
#include <util/u_logging.h>
12
+
#include <math/m_space.h>
13
+
#include <math/m_vec3.h>
14
+
15
+
16
+
#include "catch/catch.hpp"
17
+
18
+
#include <random>
19
+
20
+
21
+
#include "math/m_eigen_interop.hpp"
22
+
#include <Eigen/Core>
23
+
24
+
float
25
+
quat_difference(xrt_quat q1, xrt_quat q2)
26
+
{
27
+
// https://math.stackexchange.com/a/90098
28
+
// d(q1,q2)=1−⟨q1,q2⟩2
29
+
30
+
float inner_product = (q1.w * q2.w) + (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z);
31
+
return 1.0 - (inner_product * inner_product);
32
+
}
33
+
34
+
TEST_CASE("SwingTwistTriviallyInvertibleIn180DegreeHemisphere")
35
+
36
+
{
37
+
std::random_device dev;
38
+
39
+
auto mt = std::mt19937(dev());
40
+
41
+
auto rd = std::uniform_real_distribution<float>(-M_PI / 2, M_PI / 2);
42
+
43
+
for (int i = 0; i < 20; i++) {
44
+
xrt_vec2 swing = {rd(mt), rd(mt)};
45
+
float twist = rd(mt);
46
+
47
+
xrt_quat combquat;
48
+
49
+
math_quat_from_swing_twist(&swing, twist, &combquat);
50
+
51
+
xrt_vec2 recovered_swing;
52
+
float recovered_twist;
53
+
54
+
math_quat_to_swing_twist(&combquat, &recovered_swing, &recovered_twist);
55
+
56
+
bool success = (fabsf(swing.x - recovered_swing.x) <= 0.001) &&
57
+
(fabsf(swing.y - recovered_swing.y) <= 0.001) &&
58
+
(fabsf(twist - recovered_twist) <= 0.001);
59
+
60
+
if (!success) {
61
+
U_LOG_E("Fail! Used swing %f %f, twist %f", swing.x, swing.y, twist);
62
+
}
63
+
CHECK(success);
64
+
}
65
+
}
66
+
67
+
68
+
TEST_CASE("SwingTwistAlwaysInvertibleIfYouUseSoundRotationEqualities")
69
+
70
+
{
71
+
std::random_device dev;
72
+
73
+
auto mt = std::mt19937(dev());
74
+
75
+
auto rd = std::uniform_real_distribution<float>(-1000, 1000);
76
+
77
+
for (int i = 0; i < 20; i++) {
78
+
79
+
xrt_vec2 swing = {rd(mt), rd(mt)};
80
+
float twist = rd(mt);
81
+
82
+
xrt_quat combquat;
83
+
84
+
math_quat_from_swing_twist(&swing, twist, &combquat);
85
+
86
+
xrt_vec2 recovered_swing;
87
+
float recovered_twist;
88
+
89
+
math_quat_to_swing_twist(&combquat, &recovered_swing, &recovered_twist);
90
+
91
+
xrt_quat recovered_quat;
92
+
93
+
math_quat_from_swing_twist(&recovered_swing, recovered_twist, &recovered_quat);
94
+
95
+
bool success = quat_difference(combquat, recovered_quat) <= 0.001;
96
+
97
+
if (!success) {
98
+
// Test failed
99
+
U_LOG_E("Fail! Used swing %f %f, twist %f", swing.x, swing.y, twist);
100
+
}
101
+
CHECK(success);
102
+
}
103
+
}