The open source OpenXR runtime
matrixfurry.com
1// Copyright 2021, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Test C++ quatexpmap interface.
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 */
8
9#include "catch_amalgamated.hpp"
10#include "math/m_api.h"
11#include "math/m_vec3.h"
12#include <vector>
13
14using std::vector;
15
16TEST_CASE("m_quatexpmap")
17{
18 xrt_vec3 axis1 = m_vec3_normalize({4, -7, 3});
19 xrt_vec3 axis2 = m_vec3_normalize({-1, -2, -3});
20 xrt_vec3 axis3 = m_vec3_normalize({1, -1, 1});
21 xrt_vec3 axis4 = m_vec3_normalize({-11, 23, 91});
22 SECTION("Test integrate velocity and finite difference mappings")
23 {
24 vector<xrt_vec3> q1_axes{{axis1, axis2}};
25 float q1_angle = (float)GENERATE(M_PI, -M_PI / 6);
26 vector<xrt_vec3> vel_axes{{axis3, axis4}};
27 float vel_angle = (float)GENERATE(-M_PI, M_PI / 5);
28 float dt = (float)GENERATE(0.01, 0.1, 1);
29
30 for (xrt_vec3 q1_axis : q1_axes) {
31 for (xrt_vec3 vel_axis : vel_axes) {
32 // First orientation q1
33 xrt_quat q1{};
34 math_quat_from_angle_vector(q1_angle, &q1_axis, &q1);
35
36 // Second orientation q2: q1 rotated by vel_angle*dt radians around its local vel_axis
37 xrt_quat q2{};
38 xrt_vec3 vel = vel_axis * vel_angle;
39 math_quat_integrate_velocity(&q1, &vel, dt, &q2);
40
41 // Global velocity vector from q1 to q2
42 xrt_vec3 new_global_vel{};
43 math_quat_finite_difference(&q1, &q2, dt, &new_global_vel);
44
45 // Adjust global velocity back to local (w.r.t. q1)
46 xrt_quat inv_q1{};
47 xrt_vec3 new_vel{};
48 math_quat_invert(&q1, &inv_q1);
49 math_quat_rotate_derivative(&inv_q1, &new_global_vel, &new_vel);
50
51 INFO("vel=" << vel.x << ", " << vel.y << ", " << vel.z);
52 INFO("new_vel=" << new_vel.x << ", " << new_vel.y << ", " << new_vel.z);
53 CHECK(m_vec3_len(new_vel - vel) <= 0.001);
54 }
55 }
56 }
57
58 SECTION("Test quat_exp and quat_ln are inverses")
59 {
60 // We use rotations with less than PI radians as quat_ln will return the negative rotation otherwise
61 vector<xrt_vec3> aas = {
62 {0, 0, 0},
63 axis1 * (float)M_PI * 0.01f,
64 axis2 * (float)M_PI * 0.5f,
65 axis3 * (float)M_PI * 0.99f,
66 };
67
68 for (xrt_vec3 aa : aas) {
69 xrt_quat quat{};
70 math_quat_exp(&aa, &quat);
71
72 xrt_vec3 expected_aa{};
73 math_quat_ln(&quat, &expected_aa);
74
75 CHECK(m_vec3_len(expected_aa - aa) <= 0.001);
76 }
77 }
78
79//! @todo Fix quat_exp
80#if 0
81 SECTION("Test quat_exp(angle_axis) returns the appropriate quaternion")
82 {
83 float angle = M_PI_2;
84 xrt_vec3 axis = axis4;
85 xrt_vec3 aa = axis * angle;
86 xrt_quat q{};
87 math_quat_exp(&aa, &q);
88
89 CHECK(q.x == Approx(axis.x * sin(angle / 2)));
90 CHECK(q.y == Approx(axis.y * sin(angle / 2)));
91 CHECK(q.z == Approx(axis.z * sin(angle / 2)));
92 CHECK(q.w == Approx(cos(angle / 2)));
93 }
94#endif
95}