aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: quaternion add is_zero() & zero()

Browse Source 
& length_squared() &  add unit tests
This commit is contained in:
Joshua Henderson 2022-01-11 23:36:09 -05:00 committed by Andrew Tridgell
parent f9a4c86ad6
commit 4e3a66a4d3
3 changed files with 143 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/AP_Math/quaternion.cpp
View File

@ -447,7 +447,7 @@ template <typename T>
void QuaternionT<T>::from_axis_angle(Vector3<T> 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 <typename T>
void QuaternionT<T>::from_axis_angle(const Vector3<T> &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<T>::to_axis_angle(Vector3<T> &v) const
{
const T l = sqrtF(sq(q2)+sq(q3)+sq(q4));
v = Vector3<T>(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 <typename T>
void QuaternionT<T>::from_axis_angle_fast(Vector3<T> 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 <typename T>
void QuaternionT<T>::rotate_fast(const Vector3<T> &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<T>::length(void) const
return sqrtF(sq(q1) + sq(q2) + sq(q3) + sq(q4));
}
// gets the length squared of the quaternion
template <typename T>
T QuaternionT<T>::length_squared() const
{
return (T)(q1*q1 + q2*q2 + q3*q3 + q4*q4);
}
// return the reverse rotation of this quaternion
template <typename T>
QuaternionT<T> QuaternionT<T>::inverse(void) const
@ -633,7 +640,7 @@ template <typename T>
void QuaternionT<T>::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<T>::normalize(void)
}
}
// Checks if each element of the quaternion is zero
template <typename T>
bool QuaternionT<T>::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 <typename T>
void QuaternionT<T>::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 <typename T>
bool QuaternionT<T>::is_unit_length(void) const
{
if (fabsf(length_squared() - 1) < 1E-3) {
return true;
}
return false;
}
template <typename T>
QuaternionT<T> QuaternionT<T>::operator*(const QuaternionT<T> &v) const
{
@ -725,8 +762,7 @@ QuaternionT<T> QuaternionT<T>::operator/(const QuaternionT<T> &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);
}

13
libraries/AP_Math/quaternion.h
View File

@ -130,9 +130,22 @@ public:
// create eulers from a quaternion
Vector3<T> 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()
{

86
libraries/AP_Math/tests/test_quaternion.cpp
View File

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