The open source OpenXR runtime
matrixfurry.com
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 Moshi Turner <moshiturner@protonmail.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_amalgamated.hpp"
17
18#include <random>
19
20
21#include "math/m_eigen_interop.hpp"
22#include <Eigen/Core>
23
24float
25quat_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
34TEST_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
68TEST_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}