a/math: Add pose interpolate function and others

- math_matrix_4x4_isometry_from_pose
- math_pose_from_isometry
- math_pose_interpolate
- m_vec3_equal

src/xrt/auxiliary/math/m_api.h

@@ -207,7 +207,7 @@ void
 math_quat_from_angle_vector(float angle_rads, const struct xrt_vec3 *vector, struct xrt_quat *result);
 
 /*!
- * Create a rotation from a 3x3 rotation matrix.
+ * Create a rotation from a 3x3 rotation (row major) matrix.
  *
  * @relates xrt_quat
  * @see xrt_matrix_3x3
@@ -569,6 +569,15 @@ math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
                                  const struct xrt_vec3 *translation,
                                  struct xrt_matrix_4x4 *result);
 
+/*!
+ * Get a col-major isometry matrix —in SE(3)— from a pose.
+ *
+ * @relates xrt_matrix_4x4
+ * @ingroup aux_math
+ */
+void
+math_matrix_4x4_isometry_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result);
+
 /*!
  * Compute quad layer model matrix from xrt_pose and xrt_vec2 size.
  *
@@ -626,6 +635,25 @@ math_pose_validate(const struct xrt_pose *pose);
 void
 math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose);
 
+/*!
+ * Converts a (col-major) isometry into a pose.
+ *
+ * @relates xrt_pose
+ * @ingroup aux_math
+ */
+void
+math_pose_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_pose *result);
+
+/*!
+ * Interpolated pose between poses `a` and `b` by lerping position and slerping
+ * orientation by t.
+ *
+ * @relates xrt_pose
+ * @ingroup aux_math
+ */
+void
+math_pose_interpolate(const struct xrt_pose *a, const struct xrt_pose *b, float t, struct xrt_pose *outPose);
+
 /*!
  * Apply a rigid-body transformation to a pose.
  *

src/xrt/auxiliary/math/m_base.cpp

@@ -12,6 +12,7 @@
 
 #include "math/m_api.h"
 #include "math/m_eigen_interop.hpp"
+#include "math/m_vec3.h"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -565,6 +566,13 @@ math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
 	map_matrix_4x4(*result) = transformation.matrix();
 }
 
+extern "C" void
+math_matrix_4x4_isometry_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
+{
+	Eigen::Isometry3f transform{Eigen::Translation3f{position(*pose)} * orientation(*pose)};
+	map_matrix_4x4(*result) = transform.matrix();
+}
+
 extern "C" void
 math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
 {
@@ -691,6 +699,21 @@ math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
 	orientation(*outPose) = inverse.rotation();
 }
 
+extern "C" void
+math_pose_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_pose *result)
+{
+	Eigen::Isometry3f isometry{map_matrix_4x4(*transform)};
+	position(*result) = isometry.translation();
+	orientation(*result) = isometry.rotation();
+}
+
+extern "C" void
+math_pose_interpolate(const struct xrt_pose *a, const struct xrt_pose *b, float t, struct xrt_pose *outPose)
+{
+	math_quat_slerp(&a->orientation, &b->orientation, t, &outPose->orientation);
+	outPose->position = m_vec3_lerp(a->position, b->position, t);
+}
+
 extern "C" void
 math_pose_identity(struct xrt_pose *pose)
 {

src/xrt/auxiliary/math/m_vec3.h

@@ -136,6 +136,12 @@ m_vec3_lerp(struct xrt_vec3 from, struct xrt_vec3 to, float amount)
 	return m_vec3_add(m_vec3_mul_scalar(from, 1.0f - amount), m_vec3_mul_scalar(to, amount));
 }
 
+static inline bool
+m_vec3_equal_exact(struct xrt_vec3 l, struct xrt_vec3 r)
+{
+	return l.x == r.x && l.y == r.y && l.z == r.z;
+}
+
 #ifdef __cplusplus
 }
 

tests/tests_pose.cpp

@@ -1,21 +1,18 @@
 // Copyright 2022, Campbell Suter
+// Copyright 2022, Collabora, Ltd.
 // SPDX-License-Identifier: BSL-1.0
 /*!
  * @file
- * @brief Maths function tests.
+ * @brief Test xrt_pose functions.
  * @author Campbell Suter <znix@znix.xyz>
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
  */
 
-#include <util/u_worker.hpp>
-#include <math/m_space.h>
-#include <math/m_vec3.h>
-
 #include "catch/catch.hpp"
+#include "math/m_api.h"
+#include "math/m_vec3.h"
 
-#include <thread>
-#include <chrono>
-
-TEST_CASE("CorrectPoseInverse")
+TEST_CASE("Pose invert works")
 {
 	// Test that inverting a pose works correctly
 	// Pick an arbitrary and non-trivial original pose
@@ -40,3 +37,30 @@ TEST_CASE("CorrectPoseInverse")
 	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));
+}