diff --git a/libraries/AP_Math/tests/test_quaternion.cpp b/libraries/AP_Math/tests/test_quaternion.cpp new file mode 100644 index 0000000000..e7be857fa6 --- /dev/null +++ b/libraries/AP_Math/tests/test_quaternion.cpp @@ -0,0 +1,166 @@ +#include + +#include + +// Tests that quaternion multiplication obeys Hamilton's quaternion multiplication convention +// i*i == j*j == k*k == i*j*k == -1 +TEST(QuaternionTest, QuaternionMultiplicationOfBases) { + const Quaternion unit(1.0f, 0.0f, 0.0f, 0.0f); + const Quaternion i(0.0f, 1.0f, 0.0f, 0.0f); + const Quaternion j(0.0f, 0.0f, 1.0f, 0.0f); + const Quaternion k(0.0f, 0.0f, 0.0f, 1.0f); + + Quaternion ii, ij, ik, ji, jj, jk, ki, kj, kk, ijk; + ii = i * i; + ij = i * j; + ik = i * k; + ji = j * i; + jj = j * j; + jk = j * k; + ki = k * i; + kj = k * j; + kk = k * k; + ijk = i * j * k; + + for (int a = 0; a < 4; ++a) { + EXPECT_FLOAT_EQ(ii[a], jj[a]); + EXPECT_FLOAT_EQ(jj[a], kk[a]); + EXPECT_FLOAT_EQ(kk[a], ijk[a]); + EXPECT_FLOAT_EQ(ijk[a], -unit[a]); + EXPECT_FLOAT_EQ(ij[a], k[a]); + EXPECT_FLOAT_EQ(ii[a], -unit[a]); + EXPECT_FLOAT_EQ(ik[a], -j[a]); + EXPECT_FLOAT_EQ(ji[a], -k[a]); + EXPECT_FLOAT_EQ(jj[a], -unit[a]); + EXPECT_FLOAT_EQ(jk[a], i[a]); + EXPECT_FLOAT_EQ(ki[a], j[a]); + EXPECT_FLOAT_EQ(kj[a], -i[a]); + EXPECT_FLOAT_EQ(kk[a], -unit[a]); + EXPECT_FLOAT_EQ(ijk[a], -unit[a]); + } +} + +// Tests that the quaternion to rotation matrix conversion formula is correctly derived from the Hamilton's quaternion +// multiplication convention. This specific example is taken from "Why and How to Avoid the Flipped Quaternion +// Multiplication" (https://arxiv.org/pdf/1801.07478.pdf) +TEST(QuaternionTest, QuaternionToRotationMatrix) { + Matrix3f res; + Quaternion(0.5f * sqrtf(2.0f), 0.0f, 0.0f, 0.5f * sqrtf(2.0f)).rotation_matrix(res); + + EXPECT_NEAR(res.a.x, 0.0f, 1e-6f); + EXPECT_NEAR(res.a.y, -1.0f, 1e-6f); + EXPECT_NEAR(res.a.z, 0.0f, 1e-6f); + EXPECT_NEAR(res.b.x, 1.0f, 1e-6f); + EXPECT_NEAR(res.b.y, 0.0f, 1e-6f); + EXPECT_NEAR(res.b.z, 0.0f, 1e-6f); + EXPECT_NEAR(res.c.x, 0.0f, 1e-6f); + EXPECT_NEAR(res.c.y, 0.0f, 1e-6f); + EXPECT_NEAR(res.c.z, 1.0f, 1e-6f); +} + +// Tests that quaternion multiplication is homomorphic with rotation matrix +// multiplication, or C(q0 * q1) = C(q0) * C(q1) +TEST(QuaternionTest, QuaternionMultiplicationIsHomomorphism) { + Quaternion l_quat(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f); + Quaternion r_quat(0.9576622f, 0.03378266f, 0.12607862f, 0.25660481f); + + Matrix3f res_mat_0; + (l_quat * r_quat).rotation_matrix(res_mat_0); + + Matrix3f res_mat_1, l_mat, r_mat; + l_quat.rotation_matrix(l_mat); + r_quat.rotation_matrix(r_mat); + res_mat_1 = l_mat * r_mat; + + EXPECT_NEAR(res_mat_0.a.x, res_mat_1.a.x, 1e-6f); + EXPECT_NEAR(res_mat_0.a.y, res_mat_1.a.y, 1e-6f); + EXPECT_NEAR(res_mat_0.a.z, res_mat_1.a.z, 1e-6f); + EXPECT_NEAR(res_mat_0.b.x, res_mat_1.b.x, 1e-6f); + EXPECT_NEAR(res_mat_0.b.y, res_mat_1.b.y, 1e-6f); + EXPECT_NEAR(res_mat_0.b.z, res_mat_1.b.z, 1e-6f); + EXPECT_NEAR(res_mat_0.c.x, res_mat_1.c.x, 1e-6f); + EXPECT_NEAR(res_mat_0.c.y, res_mat_1.c.y, 1e-6f); + EXPECT_NEAR(res_mat_0.c.z, res_mat_1.c.z, 1e-6f); +} + +// Tests that applying a rotation by a unit quaternion does nothing +TEST(QuaternionTest, QuatenionRotationByUnitQuaternion) { + Vector3f v(1.0f, 2.0f, 3.0f); + Quaternion q(1.0f, 0.0f, 0.0f, 0.0f); + + Vector3f res = q * v; + + for (int i = 0; i < 3; ++i) { + EXPECT_FLOAT_EQ(res[i], v[i]); + } +} + +// Tests that applying a rotation by a quaternion whose axis is parallel to the vector does nothing +TEST(QuaternionTest, QuatenionRotationByParallelQuaternion) { + Vector3f v(1.0f, 2.0f, 3.0f); + Quaternion q(0.730296743340221, 0.182574185835055, 0.365148371670111, 0.547722557505166f); + + Vector3f res = q * v; + + for (int i = 0; i < 3; ++i) { + EXPECT_FLOAT_EQ(res[i], v[i]); + } +} + +// Tests that applying a rotation by a unit quaternion does not change the vector's length +TEST(QuaternionTest, QuatenionRotationLengthPreserving) { + Vector3f v(1.0f, 2.0f, 3.0f); + Quaternion q(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f); + + Vector3f res = q * v; + + EXPECT_FLOAT_EQ(res.length(), v.length()); +} + +// Tests that calling the quaternion rotation operator is equivalent to the formula q * v * q.inverse(), and to +// converting to rotation matrix followed by matrix multiplication +TEST(QuaternionTest, QuatenionRotationFormulaEquivalence) { + Vector3f res_1, res_0, res_2; + Vector3f v(1.0f, 2.0f, 3.0f); + Quaternion q(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f); + + res_0 = q * v; + + Quaternion qv(0.0f, v.x, v.y, v.z); + Quaternion res_qv = q * qv * q.inverse(); + res_1 = Vector3f(res_qv.q2, res_qv.q3, res_qv.q4); + + Matrix3f q_equiv_mat; + q.rotation_matrix(q_equiv_mat); + res_2 = q_equiv_mat * v; + + for (int i = 0; i < 3; ++i) { + EXPECT_FLOAT_EQ(res_0[i], res_1[i]); + EXPECT_FLOAT_EQ(res_0[i], res_2[i]); + } +} + +// Tests that the calling the rotation operator on a inverted quaternion is equivalent to q.inverse() * v * q, and to +// converting to rotation matrix, taking transpose, followed by matrix multiplication +TEST(QuaternionTest, QuatenionInverseRotationFormulaEquivalence) { + Vector3f res_0, res_1, res_2; + Vector3f v(1.0f, 2.0f, 3.0f); + Quaternion q(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f); + + res_0 = q.inverse() * v; + + Quaternion qv(0.0f, v.x, v.y, v.z); + Quaternion res_qv = q.inverse() * qv * q; + res_1 = Vector3f(res_qv.q2, res_qv.q3, res_qv.q4); + + Matrix3f q_equiv_mat; + q.rotation_matrix(q_equiv_mat); + res_2 = q_equiv_mat.transposed() * v; + + for (int i = 0; i < 3; ++i) { + EXPECT_FLOAT_EQ(res_0[i], res_1[i]); + EXPECT_FLOAT_EQ(res_0[i], res_2[i]); + } +} + +AP_GTEST_MAIN() \ No newline at end of file