The open source OpenXR runtime
matrixfurry.com
1// Copyright 2022, Campbell Suter
2// Copyright 2022-2024, Collabora, Ltd.
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief Test xrt_pose functions.
7 * @author Campbell Suter <znix@znix.xyz>
8 * @author Mateo de Mayo <mateo.demayo@collabora.com>
9 */
10
11#include "catch_amalgamated.hpp"
12#include "math/m_api.h"
13#include "math/m_vec3.h"
14
15TEST_CASE("Pose invert works")
16{
17 // Test that inverting a pose works correctly
18 // Pick an arbitrary and non-trivial original pose
19 struct xrt_pose orig = {};
20 orig.position = {123.f, 456.f, 789.f};
21 orig.orientation = {-0.439f, -0.561f, 0.072f, -0.698f};
22 math_quat_normalize(&orig.orientation);
23
24 // Invert it
25 struct xrt_pose invert;
26 math_pose_invert(&orig, &invert);
27
28 // Multiply the poses together in both orders
29 struct xrt_pose out_a;
30 math_pose_transform(&orig, &invert, &out_a);
31 struct xrt_pose out_b;
32 math_pose_transform(&invert, &orig, &out_b);
33
34 // A pose multiplied by it's inverse or vice-verse should have both a negligible rotation and position
35 CHECK(m_vec3_len(out_a.position) < 0.001f);
36 CHECK(1 - abs(out_a.orientation.w) < 0.001f);
37
38 CHECK(m_vec3_len(out_b.position) < 0.001f);
39 CHECK(1 - abs(out_b.orientation.w) < 0.001f);
40}
41
42TEST_CASE("Pose interpolation works")
43{
44 // A random pose
45 struct xrt_vec3 pos_a = {1, 2, 3};
46 struct xrt_quat ori_a = {1, 2, 3, 4};
47 math_quat_normalize(&ori_a);
48 struct xrt_pose a = {ori_a, pos_a};
49
50 // The inverse of that pose
51 struct xrt_vec3 pos_b = pos_a * -1;
52 struct xrt_quat ori_b = {};
53 math_quat_invert(&ori_a, &ori_b);
54 struct xrt_pose b = {ori_b, pos_b};
55
56 // The interpolation at 0.5 should be the identity
57 struct xrt_pose res = {};
58 math_pose_interpolate(&a, &b, 0.5, &res);
59
60 constexpr float e = std::numeric_limits<float>::epsilon();
61 CHECK(res.position.x == Catch::Approx(0).margin(e));
62 CHECK(res.position.y == Catch::Approx(0).margin(e));
63 CHECK(res.position.z == Catch::Approx(0).margin(e));
64 CHECK(res.orientation.x == Catch::Approx(0).margin(e));
65 CHECK(res.orientation.x == Catch::Approx(0).margin(e));
66 CHECK(res.orientation.y == Catch::Approx(0).margin(e));
67 CHECK(res.orientation.w == Catch::Approx(1).margin(e));
68}