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