mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 02:26:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Fix math_pose_invert breaking on rotated poses, and add a test

Browse source 
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.
This commit is contained in:
Campbell Suter 2022-06-02 03:13:38 +12:00 committed by Jakob Bornecrantz
parent bb414f7d39
commit 0f8da1900b
3 changed files with 45 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/xrt/auxiliary/math/m_base.cpp
View file

@ -685,7 +685,7 @@ math_pose_validate(const struct xrt_pose *pose)
extern "C" void
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
{
Eigen::Isometry3f transform{orientation(*pose) * Eigen::Translation3f{position(*pose)}};
Eigen::Isometry3f transform{Eigen::Translation3f{position(*pose)} * orientation(*pose)};
Eigen::Isometry3f inverse = transform.inverse();
position(*outPose) = inverse.translation();
orientation(*outPose) = inverse.rotation();

2
tests/CMakeLists.txt
View file

@ -22,6 +22,7 @@ set(tests
tests_quatexpmap
tests_rational
tests_worker
tests_maths
)
if(XRT_HAVE_D3D11)
list(APPEND tests tests_aux_d3d tests_comp_client_d3d11)
@ -46,6 +47,7 @@ target_link_libraries(tests_lowpass_float PRIVATE aux_math)
target_link_libraries(tests_lowpass_integer PRIVATE aux_math)
target_link_libraries(tests_quatexpmap PRIVATE aux_math)
target_link_libraries(tests_rational PRIVATE aux_math)
target_link_libraries(tests_maths PRIVATE aux_math)
if(XRT_HAVE_D3D11)
target_link_libraries(tests_aux_d3d PRIVATE aux_d3d)

42
tests/tests_maths.cpp Normal file
View file

@ -0,0 +1,42 @@
// 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);
}