From 7618f4c5a4b28eb3747d787803e5462ada0122c2 Mon Sep 17 00:00:00 2001
From: Jakob Bornecrantz <jakob@collabora.com>
Date: Wed, 25 Dec 2019 14:13:34 +0000
Subject: [PATCH] math: Make it possible to get a rotation from a rotation
 matrix

---
 src/xrt/auxiliary/math/m_api.h    | 11 +++++++++++
 src/xrt/auxiliary/math/m_base.cpp | 13 +++++++++++++
 2 files changed, 24 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index f0ec53d80..d91f5a473 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -102,6 +102,17 @@ math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut);
  *
  */
 
+/*!
+ * Create a rotation from a 3x3 rotation matrix.
+ *
+ * @relates xrt_quat
+ * @relates xrt_matrix_3x3
+ * @ingroup aux_math
+ */
+void
+math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat,
+                          struct xrt_quat *result);
+
 /*!
  * Check if this quat can be used in transformation operations.
  *
diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp
index be39140b7..752533c86 100644
--- a/src/xrt/auxiliary/math/m_base.cpp
+++ b/src/xrt/auxiliary/math/m_base.cpp
@@ -81,6 +81,19 @@ math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut)
  *
  */
 
+
+extern "C" void
+math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat,
+                          struct xrt_quat *result)
+{
+	Eigen::Matrix3f m;
+	m << mat->v[0], mat->v[1], mat->v[2], mat->v[3], mat->v[4], mat->v[5],
+	    mat->v[6], mat->v[7], mat->v[8];
+
+	Eigen::Quaternionf q(m);
+	map_quat(*result) = q;
+}
+
 extern "C" bool
 math_quat_validate(const struct xrt_quat *quat)
 {