aux/math: Add matrix_4x4 identity and matrix_4x4_muliply.

src/xrt/auxiliary/math/m_api.h

@@ -106,7 +106,6 @@ math_vec3_cross(const struct xrt_vec3 *l,
                 const struct xrt_vec3 *r,
                 struct xrt_vec3 *result);
 
-
 /*
  *
  * Quat functions.
@@ -230,7 +229,7 @@ math_quat_finite_difference(const struct xrt_quat *quat0,
 
 /*
  *
- * Matrix function
+ * Matrix functions
  *
  */
 
@@ -239,6 +238,25 @@ math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left,
                                const struct xrt_vec3 *right,
                                struct xrt_vec3 *result);
 
+/*!
+ * Initialize Matrix4x4 with identity.
+ *
+ * @relates xrt_matrix_4x4
+ * @ingroup aux_math
+ */
+void
+math_matrix_4x4_identity(struct xrt_matrix_4x4 *result);
+
+/*!
+ * Multiply Matrix4x4.
+ *
+ * @relates xrt_matrix_4x4
+ * @ingroup aux_math
+ */
+void
+math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
+                         const struct xrt_matrix_4x4 *right,
+                         struct xrt_matrix_4x4 *result);
 
 /*
  *

src/xrt/auxiliary/math/m_base.cpp

@@ -50,6 +50,18 @@ copy(const struct xrt_vec3 *v)
 	return copy(*v);
 }
 
+static inline Eigen::Matrix4f
+copy(const struct xrt_matrix_4x4 *m)
+{
+	Eigen::Matrix4f res;
+	// clang-format off
+	res << m->v[0], m->v[4], m->v[8],  m->v[12],
+	       m->v[1], m->v[5], m->v[9],  m->v[13],
+	       m->v[2], m->v[6], m->v[10], m->v[14],
+	       m->v[3], m->v[7], m->v[11], m->v[15];
+	// clang-format on
+	return res;
+}
 
 /*
  *
@@ -198,7 +210,7 @@ math_quat_rotate_vec3(const struct xrt_quat *left,
 
 /*
  *
- * Exported pose functions.
+ * Exported matrix functions.
  *
  */
 
@@ -216,6 +228,20 @@ math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left,
 }
 
 
+void
+math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
+{
+	map_matrix_4x4(*result) = Eigen::Matrix4f::Identity();
+}
+
+void
+math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
+                         const struct xrt_matrix_4x4 *right,
+                         struct xrt_matrix_4x4 *result)
+{
+	map_matrix_4x4(*result) = copy(left) * copy(right);
+}
+
 /*
  *
  * Exported pose functions.

src/xrt/auxiliary/math/m_eigen_interop.hpp

@@ -70,6 +70,19 @@ map_vec3(struct xrt_vec3 &v)
 	return Eigen::Map<Eigen::Vector3f>{&v.x};
 }
 
+/*!
+ * @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const
+ * overload.
+ *
+ * Permits zero-overhead manipulation of `xrt_matrix_4x4&` by Eigen routines as
+ * if it were a `Eigen::Matrix4f&`.
+ */
+static inline Eigen::Map<Eigen::Matrix4f>
+map_matrix_4x4(struct xrt_matrix_4x4 &m)
+{
+	return Eigen::Map<Eigen::Matrix4f>(m.v);
+}
+
 /*
  *
  * Pose deconstruction helpers.