mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 02:26:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Expose quat_{exp, ln} and test them

Browse source
This commit is contained in:
Mateo de Mayo 2022-02-14 16:30:59 -03:00 committed by Jakob Bornecrantz
parent be3546eb92
commit b04c26e522
3 changed files with 54 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -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.
*

13
src/xrt/auxiliary/math/m_quatexpmap.cpp
View file

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

25
tests/tests_quatexpmap.cpp
View file

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