mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-01 12:46:12 +00:00
0f8da1900b
Previously, math_pose_invert would apply a multiplication in the wrong order. This lead to the position of the 'original' pose being rotated. This patch fixes that, and adds a unit test to check this case.
43 lines
1.1 KiB
C++
43 lines
1.1 KiB
C++
// Copyright 2022, Campbell Suter
|
|
// SPDX-License-Identifier: BSL-1.0
|
|
/*!
|
|
* @file
|
|
* @brief Maths function tests.
|
|
* @author Campbell Suter <znix@znix.xyz>
|
|
*/
|
|
|
|
#include <util/u_worker.hpp>
|
|
#include <math/m_space.h>
|
|
#include <math/m_vec3.h>
|
|
|
|
#include "catch/catch.hpp"
|
|
|
|
#include <thread>
|
|
#include <chrono>
|
|
|
|
TEST_CASE("CorrectPoseInverse")
|
|
{
|
|
// 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);
|
|
}
|