mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-28 02:26:16 +00:00
d773ab4cf2
- math_matrix_4x4_isometry_from_pose - math_pose_from_isometry - math_pose_interpolate - m_vec3_equal
67 lines
1.9 KiB
C++
67 lines
1.9 KiB
C++
// Copyright 2022, Campbell Suter
|
|
// Copyright 2022, 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/catch.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.439, -0.561, 0.072, -0.698};
|
|
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, out_b;
|
|
math_pose_transform(&orig, &invert, &out_a);
|
|
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.001);
|
|
CHECK(1 - abs(out_a.orientation.w) < 0.001);
|
|
|
|
CHECK(m_vec3_len(out_b.position) < 0.001);
|
|
CHECK(1 - abs(out_b.orientation.w) < 0.001);
|
|
}
|
|
|
|
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);
|
|
|
|
CHECK(res.position.x == Approx(0));
|
|
CHECK(res.position.y == Approx(0));
|
|
CHECK(res.position.z == Approx(0));
|
|
CHECK(res.orientation.x == Approx(0));
|
|
CHECK(res.orientation.x == Approx(0));
|
|
CHECK(res.orientation.y == Approx(0));
|
|
CHECK(res.orientation.w == Approx(1));
|
|
}
|