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.

src/xrt/auxiliary/math/m_base.cpp

@@ -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();

tests/CMakeLists.txt

@@ -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)

tests/tests_maths.cpp

@@ -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);
+}