From 06597a0655bc6198da6be71139e43bed37ed8efc Mon Sep 17 00:00:00 2001
From: Mateo de Mayo <mateo.demayo@collabora.com>
Date: Thu, 30 Dec 2021 17:31:46 -0300
Subject: [PATCH] a/math: Add math_quat_{len, unrotate}

---
 src/xrt/auxiliary/math/m_api.h    | 17 +++++++++++++++++
 src/xrt/auxiliary/math/m_base.cpp | 21 +++++++++++++++++++++
 2 files changed, 38 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index 44c3d6e41..1267144bd 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -204,6 +204,15 @@ math_quat_validate_within_1_percent(const struct xrt_quat *quat);
 void
 math_quat_invert(const struct xrt_quat *quat, struct xrt_quat *out_quat);
 
+/*!
+ * The euclidean norm or length of a quaternion. Same as if it were a vec4.
+ *
+ * @relates xrt_quat
+ * @ingroup aux_math
+ */
+float
+math_quat_len(const struct xrt_quat *quat);
+
 /*!
  * Normalize a quaternion.
  *
@@ -244,6 +253,14 @@ math_quat_rotate_vec3(const struct xrt_quat *left, const struct xrt_vec3 *right,
 void
 math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result);
 
+/*!
+ * Inverse of @ref math_quat_rotate. Removes @p left rotation from @p right.
+ *
+ * @relates xrt_quat
+ * @ingroup aux_math
+ */
+void
+math_quat_unrotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result);
 
 /*!
  * Integrate a local angular velocity vector (exponential map) and apply to a
diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp
index 25cab19d8..9b01fa049 100644
--- a/src/xrt/auxiliary/math/m_base.cpp
+++ b/src/xrt/auxiliary/math/m_base.cpp
@@ -251,6 +251,12 @@ math_quat_invert(const struct xrt_quat *quat, struct xrt_quat *out_quat)
 	map_quat(*out_quat) = map_quat(*quat).conjugate();
 }
 
+extern "C" float
+math_quat_len(const struct xrt_quat *quat)
+{
+	return map_quat(*quat).norm();
+}
+
 extern "C" void
 math_quat_normalize(struct xrt_quat *inout)
 {
@@ -295,6 +301,21 @@ math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, stru
 	map_quat(*result) = q;
 }
 
+extern "C" void
+math_quat_unrotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result)
+{
+	assert(left != NULL);
+	assert(right != NULL);
+	assert(result != NULL);
+
+	auto l = copy(left);
+	auto r = copy(right);
+
+	auto q = l.inverse() * r;
+
+	map_quat(*result) = q;
+}
+
 extern "C" void
 math_quat_rotate_vec3(const struct xrt_quat *left, const struct xrt_vec3 *right, struct xrt_vec3 *result)
 {