From 4e3a66a4d36c305e0678b7b3cee1964e015ff82a Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Tue, 11 Jan 2022 23:36:09 -0500 Subject: [PATCH] AP_Math: quaternion add is_zero() & zero() & length_squared() & add unit tests --- libraries/AP_Math/quaternion.cpp | 52 +++++++++++-- libraries/AP_Math/quaternion.h | 13 ++++ libraries/AP_Math/tests/test_quaternion.cpp | 86 +++++++++++++++++++++ 3 files changed, 143 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 623b48a8b7..ee24d3a1bb 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -447,7 +447,7 @@ template void QuaternionT::from_axis_angle(Vector3 v) { const T theta = v.length(); - if (is_zero(theta)) { + if (::is_zero(theta)) { q1 = 1.0f; q2=q3=q4=0.0f; return; @@ -462,7 +462,7 @@ template void QuaternionT::from_axis_angle(const Vector3 &axis, T theta) { // axis must be a unit vector as there is no check for length - if (is_zero(theta)) { + if (::is_zero(theta)) { q1 = 1.0f; q2=q3=q4=0.0f; return; @@ -491,7 +491,7 @@ void QuaternionT::to_axis_angle(Vector3 &v) const { const T l = sqrtF(sq(q2)+sq(q3)+sq(q4)); v = Vector3(q2,q3,q4); - if (!is_zero(l)) { + if (!::is_zero(l)) { v /= l; v *= wrap_PI(2.0f * atan2F(l,q1)); } @@ -503,7 +503,7 @@ template void QuaternionT::from_axis_angle_fast(Vector3 v) { const T theta = v.length(); - if (is_zero(theta)) { + if (::is_zero(theta)) { q1 = 1.0f; q2=q3=q4=0.0f; return; @@ -533,7 +533,7 @@ template void QuaternionT::rotate_fast(const Vector3 &v) { const T theta = v.length(); - if (is_zero(theta)) { + if (::is_zero(theta)) { return; } const T t2 = 0.5*theta; @@ -613,6 +613,13 @@ T QuaternionT::length(void) const return sqrtF(sq(q1) + sq(q2) + sq(q3) + sq(q4)); } +// gets the length squared of the quaternion +template +T QuaternionT::length_squared() const +{ + return (T)(q1*q1 + q2*q2 + q3*q3 + q4*q4); +} + // return the reverse rotation of this quaternion template QuaternionT QuaternionT::inverse(void) const @@ -633,7 +640,7 @@ template void QuaternionT::normalize(void) { const T quatMag = length(); - if (!is_zero(quatMag)) { + if (!::is_zero(quatMag)) { const T quatMagInv = 1.0f/quatMag; q1 *= quatMagInv; q2 *= quatMagInv; @@ -645,6 +652,36 @@ void QuaternionT::normalize(void) } } +// Checks if each element of the quaternion is zero +template +bool QuaternionT::is_zero(void) const { + return (fabsf(q1) < FLT_EPSILON) && + (fabsf(q2) < FLT_EPSILON) && + (fabsf(q3) < FLT_EPSILON) && + (fabsf(q4) < FLT_EPSILON); +} + +// zeros the quaternion to [0, 0, 0, 0], an invalid quaternion +// See initialize() if you want the zero rotation quaternion +template +void QuaternionT::zero(void) +{ + q1 = q2 = q3 = q4 = 0.0; +} + +// Checks if the quaternion is unit_length within a tolerance +// Returns True: if its magnitude is close to unit length +/- 1E-3 +// This limit is somewhat greater than sqrt(FLT_EPSL) +template +bool QuaternionT::is_unit_length(void) const +{ + if (fabsf(length_squared() - 1) < 1E-3) { + return true; + } + + return false; +} + template QuaternionT QuaternionT::operator*(const QuaternionT &v) const { @@ -725,8 +762,7 @@ QuaternionT QuaternionT::operator/(const QuaternionT &v) const const T &quat2 = q3; const T &quat3 = q4; - const T quatMag = length(); - if (is_zero(quatMag)) { + if (is_zero()) { // The code goes here if the quaternion is [0,0,0,0]. This shouldn't happen. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index ce14159382..e73eeaa297 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -130,9 +130,22 @@ public: // create eulers from a quaternion Vector3 to_vector312(void) const; + T length_squared(void) const; T length(void) const; void normalize(); + // Checks if each element of the quaternion is zero + bool is_zero(void) const; + + // zeros the quaternion to [0, 0, 0, 0], an invalid quaternion + // See initialize() if you want the zero rotation quaternion + void zero(void); + + // Checks if the quaternion is unit_length within a tolerance + // Returns True: if its magnitude is close to unit length +/- 1E-3 + // This limit is somewhat greater than sqrt(FLT_EPSL) + bool is_unit_length(void) const; + // initialise the quaternion to no rotation void initialise() { diff --git a/libraries/AP_Math/tests/test_quaternion.cpp b/libraries/AP_Math/tests/test_quaternion.cpp index e7be857fa6..664307d9c7 100644 --- a/libraries/AP_Math/tests/test_quaternion.cpp +++ b/libraries/AP_Math/tests/test_quaternion.cpp @@ -163,4 +163,90 @@ TEST(QuaternionTest, QuatenionInverseRotationFormulaEquivalence) { } } +// Test zero() +TEST(QuaternionTest, Quaternion_zero) +{ + Quaternion q {0.0, 0.0, 0.0, 0.0}; + q.zero(); + EXPECT_TRUE(q.is_zero()); + + q = Quaternion{0.8365163, 0.48296291, 0.22414387, -0.12940952}; + q.zero(); + EXPECT_TRUE(q.is_zero()); + + // unit length + q = Quaternion{1.0, 0.0, 0.0, 0.0}; + q.zero(); + EXPECT_TRUE(q.is_zero()); +} + +// Tests is_zero() +TEST(QuaternionTest, Quaternion_is_zero) +{ + Quaternion q {0.0, 0.0, 0.0, 0.0}; + EXPECT_TRUE(q.is_zero()); + + q = Quaternion{0.8365163, 0.48296291, 0.22414387, -0.12940952}; + EXPECT_FALSE(q.is_zero()); + + q = Quaternion{0.9, 0.0, 0.0, 0.0}; + EXPECT_FALSE(q.is_zero()); +} + +// Tests is_unit_length() +TEST(QuaternionTest, Quaternion_is_unit_length) +{ + // zero length + Quaternion q {0.0, 0.0, 0.0, 0.0}; + EXPECT_FALSE(q.is_unit_length()); + + // Length == 1.0 - 0.0009, slightly within the tolerance + q = Quaternion{0.8361398, 0.4827455, 0.2240430, -0.1293512}; + EXPECT_TRUE(q.is_unit_length()); + + // unit length + q = Quaternion{0.8365163, 0.48296291, 0.22414387, -0.12940952}; + EXPECT_TRUE(q.is_unit_length()); + + // unit length + q = Quaternion{1.0, 0.0, 0.0, 0.0}; + EXPECT_TRUE(q.is_unit_length()); + + // Length == 1.0 + 0.0009, slightly within the tolerance + q = Quaternion{0.8368926, 0.4831802, 0.2242447, -0.1294677}; + EXPECT_TRUE(q.is_unit_length()); + + // Length == 1.2 + q = Quaternion{1.00382, 0.579555, 0.268973, -0.155291}; + EXPECT_FALSE(q.is_unit_length()); +} + +// Tests length_squared() +TEST(QuaternionTest, Quaternion_length_squared) +{ + // zero length + Quaternion q {0.0, 0.0, 0.0, 0.0}; + EXPECT_FLOAT_EQ(q.length_squared(), 0.0); + + // Length == 1.0 - 0.0009, slightly within the tolerance + q = Quaternion{0.8361398, 0.4827455, 0.2240430, -0.1293512}; + EXPECT_FLOAT_EQ(q.length_squared(), 1.0 - 0.0009); + + // unit length + q = Quaternion{0.8365163, 0.48296291, 0.22414387, -0.12940952}; + EXPECT_FLOAT_EQ(q.length_squared(), 1.0); + + // unit length + q = Quaternion{1.0, 0.0, 0.0, 0.0}; + EXPECT_FLOAT_EQ(q.length_squared(), 1.0); + + // Length == 1.0 + 0.0009, slightly within the tolerance + q = Quaternion{0.8368926, 0.4831802, 0.2242447, -0.1294677}; + EXPECT_FLOAT_EQ(q.length_squared(), 1.0 + 0.0009); + + // Length == 1.2 + q = Quaternion{1.00382, 0.579555, 0.268973, -0.155291}; + EXPECT_FLOAT_EQ(q.length_squared(), 1.44); +} + AP_GTEST_MAIN() \ No newline at end of file