aux/math: Implement math_matrix_3x3_inverse.

Add a function to invert 3x3 matrices, to reverse 2D
affine transforms.

src/xrt/auxiliary/math/m_api.h

@@ -323,6 +323,15 @@ math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
                          const struct xrt_matrix_3x3 *right,
                          struct xrt_matrix_3x3 *result);
 
+/*!
+ * Invert Matrix3x3
+ *
+ * @relates xrt_matrix_3x3
+ * @ingroup aux_math
+ */
+void
+math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result);
+
 /*!
  * Initialize Matrix4x4 with identity.
  *

src/xrt/auxiliary/math/m_base.cpp

@@ -52,6 +52,18 @@ copy(const struct xrt_vec3 *v)
 	return copy(*v);
 }
 
+static inline Eigen::Matrix3f
+copy(const struct xrt_matrix_3x3 *m)
+{
+	Eigen::Matrix3f res;
+	// clang-format off
+	res << m->v[0], m->v[3], m->v[6],
+	       m->v[1], m->v[4], m->v[7],
+	       m->v[2], m->v[5], m->v[8];
+	// clang-format on
+	return res;
+}
+
 static inline Eigen::Matrix4f
 copy(const struct xrt_matrix_4x4 *m)
 {
@@ -334,6 +346,13 @@ math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
 	result->v[8] = left->v[6] * right->v[2] + left->v[7] * right->v[5] + left->v[8] * right->v[8];
 }
 
+extern "C" void
+math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
+{
+	Eigen::Matrix3f m = copy(in);
+	map_matrix_3x3(*result) = m.inverse();
+}
+
 void
 math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
 {

src/xrt/auxiliary/math/m_eigen_interop.hpp

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