The open source OpenXR runtime
a/math: Fix math_pose_invert breaking on rotated poses, and add a test
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.
authored by
Campbell Suter
and committed by
Jakob Bornecrantz
0f8da190
bb414f7d
+45
-1
+1
-1
src/xrt/auxiliary/math/m_base.cpp
+1
-1
src/xrt/auxiliary/math/m_base.cpp
···
685
685
extern "C" void
686
686
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
687
687
{
688
-
Eigen::Isometry3f transform{orientation(*pose) * Eigen::Translation3f{position(*pose)}};
688
+
Eigen::Isometry3f transform{Eigen::Translation3f{position(*pose)} * orientation(*pose)};
689
689
Eigen::Isometry3f inverse = transform.inverse();
690
690
position(*outPose) = inverse.translation();
691
691
orientation(*outPose) = inverse.rotation();
+2
tests/CMakeLists.txt
+2
tests/CMakeLists.txt
···
22
22
tests_quatexpmap
23
23
tests_rational
24
24
tests_worker
25
+
tests_maths
25
26
)
26
27
if(XRT_HAVE_D3D11)
27
28
list(APPEND tests tests_aux_d3d tests_comp_client_d3d11)
···
46
47
target_link_libraries(tests_lowpass_integer PRIVATE aux_math)
47
48
target_link_libraries(tests_quatexpmap PRIVATE aux_math)
48
49
target_link_libraries(tests_rational PRIVATE aux_math)
50
+
target_link_libraries(tests_maths PRIVATE aux_math)
49
51
50
52
if(XRT_HAVE_D3D11)
51
53
target_link_libraries(tests_aux_d3d PRIVATE aux_d3d)
+42
tests/tests_maths.cpp
+42
tests/tests_maths.cpp
···
1
+
// Copyright 2022, Campbell Suter
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Maths function tests.
6
+
* @author Campbell Suter <znix@znix.xyz>
7
+
*/
8
+
9
+
#include <util/u_worker.hpp>
10
+
#include <math/m_space.h>
11
+
#include <math/m_vec3.h>
12
+
13
+
#include "catch/catch.hpp"
14
+
15
+
#include <thread>
16
+
#include <chrono>
17
+
18
+
TEST_CASE("CorrectPoseInverse")
19
+
{
20
+
// Test that inverting a pose works correctly
21
+
// Pick an arbitrary and non-trivial original pose
22
+
struct xrt_pose orig = {};
23
+
orig.position = {123.f, 456.f, 789.f};
24
+
orig.orientation = {-0.439, -0.561, 0.072, -0.698};
25
+
math_quat_normalize(&orig.orientation);
26
+
27
+
// Invert it
28
+
struct xrt_pose invert;
29
+
math_pose_invert(&orig, &invert);
30
+
31
+
// Multiply the poses together in both orders
32
+
struct xrt_pose out_a, out_b;
33
+
math_pose_transform(&orig, &invert, &out_a);
34
+
math_pose_transform(&invert, &orig, &out_b);
35
+
36
+
// A pose multiplied by it's inverse or vice-verse should have both a negligible rotation and position
37
+
CHECK(m_vec3_len(out_a.position) < 0.001);
38
+
CHECK(1 - abs(out_a.orientation.w) < 0.001);
39
+
40
+
CHECK(m_vec3_len(out_b.position) < 0.001);
41
+
CHECK(1 - abs(out_b.orientation.w) < 0.001);
42
+
}