From e81483a808ce7b0217c11d3dc0fce90685f44353 Mon Sep 17 00:00:00 2001 From: kritz Date: Wed, 18 Dec 2019 14:12:47 +0100 Subject: [PATCH] Catch quaternion canonical corner cases (#116) --- matrix/Quaternion.hpp | 10 ++++--- matrix/helper_functions.hpp | 6 ++++ test/attitude.cpp | 58 +++++++++++++++++++++++++++++++------ test/copyto.cpp | 2 +- 4 files changed, 62 insertions(+), 14 deletions(-) diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 9df08a9b3c..b05f063369 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -367,11 +367,13 @@ public: Quaternion canonical() const { const Quaternion &q = *this; - if (q(0) < Type(0)) { - return Quaternion(-q(0),-q(1),-q(2),-q(3)); - } else { - return Quaternion(q(0),q(1),q(2),q(3)); + + for (size_t i = 0; i < 4; i++) { + if (fabs(q(i)) > FLT_EPSILON) { + return q * Type(matrix::sign(q(i))); + } } + return q; } /** diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp index d3ecf841b1..89f2dbffb9 100644 --- a/matrix/helper_functions.hpp +++ b/matrix/helper_functions.hpp @@ -78,4 +78,10 @@ Type wrap_2pi(Type x) return wrap(x, Type(0), Type(M_TWOPI)); } +template +int sign(T val) +{ + return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON)); } + +} // namespace matrix diff --git a/test/attitude.cpp b/test/attitude.cpp index 86ba63d894..3acab686f5 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -255,15 +255,55 @@ int main() TEST(fabs(q_check(3) + q(3)) < eps); // quaternion canonical - Quatf q_non_canonical(-0.7f,0.4f, 0.3f, -0.3f); - Quatf q_canonical(0.7f,-0.4f, -0.3f, 0.3f); - Quatf q_canonical_ref(0.7f,-0.4f, -0.3f, 0.3f); - TEST(isEqual(q_non_canonical.canonical(),q_canonical_ref)); - TEST(isEqual(q_canonical.canonical(),q_canonical_ref)); - q_non_canonical.canonicalize(); - q_canonical.canonicalize(); - TEST(isEqual(q_non_canonical,q_canonical_ref)); - TEST(isEqual(q_canonical,q_canonical_ref)); + Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f); + Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f); + Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f); + TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1)); + TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1)); + q_non_canonical_1.canonicalize(); + q_canonical_1.canonicalize(); + TEST(isEqual(q_non_canonical_1,q_canonical_ref_1)); + TEST(isEqual(q_canonical_1,q_canonical_ref_1)); + + Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f); + Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f); + Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f); + TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2)); + TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2)); + q_non_canonical_2.canonicalize(); + q_canonical_2.canonicalize(); + TEST(isEqual(q_non_canonical_2,q_canonical_ref_2)); + TEST(isEqual(q_canonical_2,q_canonical_ref_2)); + + Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f); + Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f); + Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f); + TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3)); + TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3)); + q_non_canonical_3.canonicalize(); + q_canonical_3.canonicalize(); + TEST(isEqual(q_non_canonical_3,q_canonical_ref_3)); + TEST(isEqual(q_canonical_3,q_canonical_ref_3)); + + Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f); + Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f); + Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f); + TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4)); + TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4)); + q_non_canonical_4.canonicalize(); + q_canonical_4.canonicalize(); + TEST(isEqual(q_non_canonical_4,q_canonical_ref_4)); + TEST(isEqual(q_canonical_4,q_canonical_ref_4)); + + Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); + Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); + Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f); + TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5)); + TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5)); + q_non_canonical_5.canonicalize(); + q_canonical_5.canonicalize(); + TEST(isEqual(q_non_canonical_5,q_canonical_ref_5)); + TEST(isEqual(q_canonical_5,q_canonical_ref_5)); // quaternion setIdentity Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f); diff --git a/test/copyto.cpp b/test/copyto.cpp index 4989adb0f1..4e72a1a91d 100644 --- a/test/copyto.cpp +++ b/test/copyto.cpp @@ -15,7 +15,7 @@ int main() float eps = 1e-6f; // Vector3 copyTo - Vector3f v(1, 2, 3); + const Vector3f v(1, 2, 3); float dst3[3] = {}; v.copyTo(dst3); for (size_t i = 0; i < 3; i++) {