From b04c26e5227952f3b372e38f80f7096fe49418fd Mon Sep 17 00:00:00 2001
From: Mateo de Mayo <mateo.demayo@collabora.com>
Date: Mon, 14 Feb 2022 16:30:59 -0300
Subject: [PATCH] a/math: Expose quat_{exp, ln} and test them

---
 src/xrt/auxiliary/math/m_api.h          | 21 +++++++++++++++++++++
 src/xrt/auxiliary/math/m_quatexpmap.cpp | 13 +++++++++++++
 tests/tests_quatexpmap.cpp              | 25 ++++++++++++++++++++-----
 3 files changed, 54 insertions(+), 5 deletions(-)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index 1267144bd..ecc8eb8d7 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -298,6 +298,27 @@ math_quat_finite_difference(const struct xrt_quat *quat0,
                             float dt,
                             struct xrt_vec3 *out_ang_vel);
 
+/*!
+ * Converts a rotation vector in axis-angle form to its corresponding unit quaternion.
+ *
+ * @relates xrt_quat
+ * @see xrt_vec3
+ * @ingroup aux_math
+ */
+void
+math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat);
+
+
+/*!
+ * Converts a unit quaternion into its corresponding axis-angle vector representation.
+ *
+ * @relates xrt_quat
+ * @see xrt_vec3
+ * @ingroup aux_math
+ */
+void
+math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle);
+
 /*!
  * Used to rotate a derivative like a angular velocity.
  *
diff --git a/src/xrt/auxiliary/math/m_quatexpmap.cpp b/src/xrt/auxiliary/math/m_quatexpmap.cpp
index a5363cc14..02e69fb38 100644
--- a/src/xrt/auxiliary/math/m_quatexpmap.cpp
+++ b/src/xrt/auxiliary/math/m_quatexpmap.cpp
@@ -164,3 +164,16 @@ math_quat_finite_difference(const struct xrt_quat *quat0,
 	Eigen::Quaternionf inc_quat = map_quat(*quat1) * map_quat(*quat0).conjugate();
 	map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat) / dt;
 }
+
+extern "C" void
+math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat)
+{
+	map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle));
+}
+
+extern "C" void
+math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle)
+{
+	Eigen::Quaternionf eigen_quat = map_quat(*quat);
+	map_vec3(*out_axis_angle) = quat_ln(eigen_quat);
+}
diff --git a/tests/tests_quatexpmap.cpp b/tests/tests_quatexpmap.cpp
index 0b1770b9d..881b8bfe7 100644
--- a/tests/tests_quatexpmap.cpp
+++ b/tests/tests_quatexpmap.cpp
@@ -15,13 +15,12 @@ using std::vector;
 
 TEST_CASE("m_quatexpmap")
 {
+	xrt_vec3 axis1 = m_vec3_normalize({4, -7, 3});
+	xrt_vec3 axis2 = m_vec3_normalize({-1, -2, -3});
+	xrt_vec3 axis3 = m_vec3_normalize({1, -1, 1});
+	xrt_vec3 axis4 = m_vec3_normalize({-11, 23, 91});
 	SECTION("Test integrate velocity and finite difference mappings")
 	{
-		xrt_vec3 axis1 = m_vec3_normalize({4, -7, 3});
-		xrt_vec3 axis2 = m_vec3_normalize({-1, -2, -3});
-		xrt_vec3 axis3 = m_vec3_normalize({1, -1, 1});
-		xrt_vec3 axis4 = m_vec3_normalize({-11, 23, 91});
-
 		vector<xrt_vec3> q1_axes{{axis1, axis2}};
 		float q1_angle = GENERATE(M_PI, -M_PI / 6);
 		vector<xrt_vec3> vel_axes{{axis3, axis4}};
@@ -55,4 +54,20 @@ TEST_CASE("m_quatexpmap")
 			}
 		}
 	}
+
+	SECTION("Test quat_exp and quat_ln are inverses")
+	{
+		// We use rotations with less than PI radians as quat_ln will return the negative rotation otherwise
+		vector<xrt_vec3> aas = {{0, 0, 0}, axis1 * M_PI * 0.01, axis2 * M_PI * 0.5, axis3 * 0.99 * M_PI};
+
+		for (xrt_vec3 aa : aas) {
+			xrt_quat quat{};
+			math_quat_exp(&aa, &quat);
+
+			xrt_vec3 expected_aa{};
+			math_quat_ln(&quat, &expected_aa);
+
+			CHECK(m_vec3_len(expected_aa - aa) <= 0.001);
+		}
+	}
 }