tests: Add tests for quatexpmap interface functions and minor changes in their docs

src/xrt/auxiliary/math/m_api.h

@@ -145,7 +145,7 @@ math_vec3_normalize(struct xrt_vec3 *in);
  */
 
 /*!
- * Create a rotation from a angle in radians and a vector.
+ * Create a rotation from an angle in radians and a unit vector.
  *
  * @relates xrt_quat
  * @see xrt_vec3
@@ -246,7 +246,7 @@ math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, stru
 
 
 /*!
- * Integrate an angular velocity vector (exponential map) and apply to a
+ * Integrate a local angular velocity vector (exponential map) and apply to a
  * quaternion.
  *
  * ang_vel and dt should share the same units of time, and the ang_vel
@@ -263,8 +263,8 @@ math_quat_integrate_velocity(const struct xrt_quat *quat,
                              struct xrt_quat *result);
 
 /*!
- * Compute an angular velocity vector (exponential map format) by taking the
+ * Compute a global angular velocity vector (exponential map format) by taking
- * finite difference of two quaternions.
+ * the finite difference of two quaternions.
  *
  * quat1 is the orientation dt time after the orientation was quat0
  *

tests/CMakeLists.txt

@@ -29,7 +29,6 @@ target_link_libraries(tests_json PRIVATE tests_main)
 target_link_libraries(tests_json PRIVATE aux_util)
 add_test(NAME tests_json COMMAND tests_json --success)
 
-
 # history
 add_executable(tests_history_buf tests_history_buf.cpp)
 target_link_libraries(tests_history_buf PRIVATE tests_main)
@@ -41,3 +40,9 @@ add_executable(tests_cxx_wrappers tests_cxx_wrappers.cpp)
 target_link_libraries(tests_cxx_wrappers PRIVATE tests_main)
 target_link_libraries(tests_cxx_wrappers PRIVATE aux_util xrt-interfaces)
 add_test(NAME tests_cxx_wrappers COMMAND tests_cxx_wrappers --success)
+
+# quatexpmap
+add_executable(tests_quatexpmap tests_quatexpmap.cpp)
+target_link_libraries(tests_quatexpmap PRIVATE tests_main)
+target_link_libraries(tests_quatexpmap PRIVATE aux_math)
+add_test(NAME tests_quatexpmap COMMAND tests_quatexpmap --success)

tests/tests_quatexpmap.cpp

@@ -0,0 +1,58 @@
+// Copyright 2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief Test C++ quatexpmap interface.
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
+ */
+
+#include "catch/catch.hpp"
+#include "math/m_api.h"
+#include "math/m_vec3.h"
+#include <vector>
+
+using std::vector;
+
+TEST_CASE("m_quatexpmap")
+{
+	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}};
+		float vel_angle = GENERATE(-M_PI, M_PI / 5);
+		float dt = GENERATE(0.01, 0.1, 1);
+
+		for (xrt_vec3 q1_axis : q1_axes) {
+			for (xrt_vec3 vel_axis : vel_axes) {
+				// First orientation q1
+				xrt_quat q1{};
+				math_quat_from_angle_vector(q1_angle, &q1_axis, &q1);
+
+				// Second orientation q2: q1 rotated by vel_angle*dt radians around its local vel_axis
+				xrt_quat q2{};
+				xrt_vec3 vel = vel_axis * vel_angle;
+				math_quat_integrate_velocity(&q1, &vel, dt, &q2);
+
+				// Global velocity vector from q1 to q2
+				xrt_vec3 new_global_vel{};
+				math_quat_finite_difference(&q1, &q2, dt, &new_global_vel);
+
+				// Adjust global velocity back to local (w.r.t. q1)
+				xrt_quat inv_q1{};
+				xrt_vec3 new_vel{};
+				math_quat_invert(&q1, &inv_q1);
+				math_quat_rotate_derivative(&inv_q1, &new_global_vel, &new_vel);
+
+				INFO("vel=" << vel.x << ", " << vel.y << ", " << vel.z);
+				INFO("new_vel=" << new_vel.x << ", " << new_vel.y << ", " << new_vel.z);
+				CHECK(m_vec3_len(new_vel - vel) <= 0.001);
+			}
+		}
+	}
+}