mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-15 11:25:26 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

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

Browse source
This commit is contained in:
Mateo de Mayo 2021-12-22 12:16:27 -03:00
parent 70576c2f02
commit 7e0d93e3fb
3 changed files with 68 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/xrt/auxiliary/math/m_api.h
View file

@ -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
* finite difference of two quaternions.
* Compute a global angular velocity vector (exponential map format) by taking
* the finite difference of two quaternions.
*
* quat1 is the orientation dt time after the orientation was quat0
*

7
tests/CMakeLists.txt
View file

@ -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)

58
tests/tests_quatexpmap.cpp Normal file
View file

@ -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);
}
}
}
}