fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 04:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Matrix: convert attitude test to gtest

Browse Source
This commit is contained in:
Matthias Grob
2022-03-17 13:02:22 +01:00
parent c33bf5b705
commit 35b035e880
2 changed files with 151 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/test/CMakeLists.txt
+1 -1
View File
@@ -16,7 +16,6 @@ set(tests
vector vector
vector2 vector2
vector3 vector3
attitude
filter filter
integration integration
squareMatrix squareMatrix
@@ -43,5 +42,6 @@ foreach(test_name ${tests})
endforeach() endforeach()
px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp) px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp)
px4_add_unit_gtest(SRC MatrixAttitudeTest.cpp)
px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp) px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp)
px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp) px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp)
src/lib/matrix/test/attitude.cpp → src/lib/matrix/test/MatrixAttitudeTest.cpp
+150 -143
View File
@@ -1,25 +1,42 @@
#include "test_macros.hpp" /****************************************************************************
*
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <matrix/math.hpp> #include <matrix/math.hpp>
#include <iostream>
using namespace matrix; using namespace matrix;
// manually instantiated all files we intend to test TEST(MatrixAttitudeTest, Attitude)
// so that coverage works correctly
// doesn't matter what test this is in
namespace matrix
{
template class Matrix<float, 3, 3>;
template class Vector3<float>;
template class Vector2<float>;
template class Vector<float, 4>;
template class Quaternion<float>;
template class AxisAngle<float>;
template class Scalar<float>;
template class SquareMatrix<float, 4>;
}
int main()
{ {
// check data // check data
Eulerf euler_check(0.1f, 0.2f, 0.3f); Eulerf euler_check(0.1f, 0.2f, 0.3f);
@@ -32,70 +49,68 @@ int main()
Dcmf dcm_check(dcm_data); Dcmf dcm_check(dcm_data);
// euler ctor // euler ctor
TEST(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f))); EXPECT_EQ(euler_check, Vector3f(0.1f, 0.2f, 0.3f));
// euler default ctor // euler default ctor
Eulerf e; Eulerf e;
Eulerf e_zero = zeros<float, 3, 1>(); Eulerf e_zero = zeros<float, 3, 1>();
TEST(isEqual(e, e_zero)); EXPECT_EQ(e, e_zero);
TEST(isEqual(e, e)); EXPECT_EQ(e, e);
// euler vector ctor // euler vector ctor
Vector3f v(0.1f, 0.2f, 0.3f); Vector3f v(0.1f, 0.2f, 0.3f);
Eulerf euler_copy(v); Eulerf euler_copy(v);
TEST(isEqual(euler_copy, euler_check)); EXPECT_EQ(euler_copy, euler_check);
// quaternion ctor // quaternion ctor
Quatf q0(1, 2, 3, 4); Quatf q0(1, 2, 3, 4);
Quatf q(q0); Quatf q(q0);
double eps = 1e-6; EXPECT_FLOAT_EQ(q(0), 1);
TEST(fabs(q(0) - 1) < eps); EXPECT_FLOAT_EQ(q(1), 2);
TEST(fabs(q(1) - 2) < eps); EXPECT_FLOAT_EQ(q(2), 3);
TEST(fabs(q(2) - 3) < eps); EXPECT_FLOAT_EQ(q(3), 4);
TEST(fabs(q(3) - 4) < eps);
// quaternion ctor: vector to vector // quaternion ctor: vector to vector
// identity test // identity test
Quatf quat_v(v, v); Quatf quat_v(v, v);
TEST(isEqual(quat_v.rotateVector(v), v)); EXPECT_EQ(quat_v.rotateVector(v), v);
// random test (vector norm can not be preserved with a pure rotation) // random test (vector norm can not be preserved with a pure rotation)
Vector3f v1(-80.1f, 1.5f, -6.89f); Vector3f v1(-80.1f, 1.5f, -6.89f);
quat_v = Quatf(v1, v); quat_v = Quatf(v1, v);
TEST(isEqual(quat_v.rotateVector(v1).normalized() * v.norm(), v)); EXPECT_EQ(quat_v.rotateVector(v1).normalized() * v.norm(), v);
// special 180 degree case 1 // special 180 degree case 1
v1 = Vector3f(0.f, 1.f, 1.f); v1 = Vector3f(0.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1); quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.rotateVector(v1), -v1)); EXPECT_EQ(quat_v.rotateVector(v1), -v1);
// special 180 degree case 2 // special 180 degree case 2
v1 = Vector3f(1.f, 2.f, 0.f); v1 = Vector3f(1.f, 2.f, 0.f);
quat_v = Quatf(v1, -v1); quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.rotateVector(v1), -v1)); EXPECT_EQ(quat_v.rotateVector(v1), -v1);
// special 180 degree case 3 // special 180 degree case 3
v1 = Vector3f(0.f, 0.f, 1.f); v1 = Vector3f(0.f, 0.f, 1.f);
quat_v = Quatf(v1, -v1); quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.rotateVector(v1), -v1)); EXPECT_EQ(quat_v.rotateVector(v1), -v1);
// special 180 degree case 4 // special 180 degree case 4
v1 = Vector3f(1.f, 1.f, 1.f); v1 = Vector3f(1.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1); quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.rotateVector(v1), -v1)); EXPECT_EQ(quat_v.rotateVector(v1), -v1);
// quat normalization // quat normalization
q.normalize(); q.normalize();
TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f, EXPECT_EQ(q, Quatf(0.18257419f, 0.36514837f, 0.54772256f, 0.73029674f));
0.54772256f, 0.73029674f))); EXPECT_EQ(q0.unit(), q);
TEST(isEqual(q0.unit(), q)); EXPECT_EQ(q0.unit(), q0.normalized());
TEST(isEqual(q0.unit(), q0.normalized()));
// quat default ctor // quat default ctor
q = Quatf(); q = Quatf();
TEST(isEqual(q, Quatf(1, 0, 0, 0))); EXPECT_EQ(q, Quatf(1, 0, 0, 0));
// quaternion exponential with v=0 // quaternion exponential with v=0
v = Vector3f(); v = Vector3f();
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
Dcmf M = Dcmf() * 0.5f; Dcmf M = Dcmf() * 0.5f;
TEST(isEqual(q, Quatf::expq(v))); EXPECT_EQ(q, Quatf::expq(v));
TEST(isEqual(M, Quatf::inv_r_jacobian(v))); EXPECT_EQ(M, Quatf::inv_r_jacobian(v));
// quaternion exponential with small v // quaternion exponential with small v
v = Vector3f(0.001f, 0.002f, -0.003f); v = Vector3f(0.001f, 0.002f, -0.003f);
@@ -109,8 +124,8 @@ int main()
}; };
M = Dcmf(M_data); M = Dcmf(M_data);
} }
TEST(isEqual(q, Quatf::expq(v))); EXPECT_EQ(q, Quatf::expq(v));
TEST(isEqual(M, Quatf::inv_r_jacobian(v))); EXPECT_EQ(M, Quatf::inv_r_jacobian(v));
// quaternion exponential with v // quaternion exponential with v
v = Vector3f(1.0f, -2.0f, 3.0f); v = Vector3f(1.0f, -2.0f, 3.0f);
@@ -124,8 +139,8 @@ int main()
}; };
M = Dcmf(M_data); M = Dcmf(M_data);
} }
TEST(isEqual(q, Quatf::expq(v))); EXPECT_EQ(q, Quatf::expq(v));
TEST(isEqual(M, Quatf::inv_r_jacobian(v))); EXPECT_EQ(M, Quatf::inv_r_jacobian(v));
// quaternion kinematic update // quaternion kinematic update
q = Quatf(); q = Quatf();
@@ -134,40 +149,40 @@ int main()
Quatf qa = q + 0.5f * h * q.derivative1(w_B); Quatf qa = q + 0.5f * h * q.derivative1(w_B);
qa.normalize(); qa.normalize();
Quatf qb = q * Quatf::expq(0.5f * h * w_B); Quatf qb = q * Quatf::expq(0.5f * h * w_B);
TEST(isEqual(qa, qb)); EXPECT_EQ(qa, qb);
// euler to quaternion // euler to quaternion
q = Quatf(euler_check); q = Quatf(euler_check);
TEST(isEqual(q, q_check)); EXPECT_EQ(q, q_check);
// euler to dcm // euler to dcm
Dcmf dcm(euler_check); Dcmf dcm(euler_check);
TEST(isEqual(dcm, dcm_check)); EXPECT_EQ(dcm, dcm_check);
// quaternion to euler // quaternion to euler
Eulerf e1(q_check); Eulerf e1(q_check);
TEST(isEqual(e1, euler_check)); EXPECT_EQ(e1, euler_check);
// quaternion to dcm // quaternion to dcm
Dcmf dcm1(q_check); Dcmf dcm1(q_check);
TEST(isEqual(dcm1, dcm_check)); EXPECT_EQ(dcm1, dcm_check);
// quaternion z-axis unit base vector // quaternion z-axis unit base vector
Vector3f q_z = q_check.dcm_z(); Vector3f q_z = q_check.dcm_z();
Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2)); Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2));
TEST(isEqual(q_z, R_z)); EXPECT_EQ(q_z, R_z);
// dcm default ctor // dcm default ctor
Dcmf dcm2; Dcmf dcm2;
SquareMatrix<float, 3> I = eye<float, 3>(); SquareMatrix<float, 3> I = eye<float, 3>();
TEST(isEqual(dcm2, I)); EXPECT_EQ(dcm2, I);
// dcm to euler // dcm to euler
Eulerf e2(dcm_check); Eulerf e2(dcm_check);
TEST(isEqual(e2, euler_check)); EXPECT_EQ(e2, euler_check);
// dcm to quaterion // dcm to quaterion
Quatf q2(dcm_check); Quatf q2(dcm_check);
TEST(isEqual(q2, q_check)); EXPECT_EQ(q2, q_check);
// dcm renormalize // dcm renormalize
Dcmf A = eye<float, 3>(); Dcmf A = eye<float, 3>();
@@ -178,15 +193,12 @@ int main()
} }
A.renormalize(); A.renormalize();
float err = 0.0f;
for (size_t r = 0; r < 3; r++) { for (size_t r = 0; r < 3; r++) {
Vector3f rvec(matrix::Matrix<float, 1, 3>(A.row(r)).transpose()); Vector3f rvec(matrix::Matrix<float, 1, 3>(A.row(r)).transpose());
err += fabs(1.0f - rvec.length()); EXPECT_FLOAT_EQ(1.0f, rvec.length());
} }
TEST(err < eps);
// constants // constants
double deg2rad = M_PI / 180.0; double deg2rad = M_PI / 180.0;
double rad2deg = 180.0 / M_PI; double rad2deg = 180.0 / M_PI;
@@ -199,11 +211,11 @@ int main()
double roll_expected = roll; double roll_expected = roll;
double yaw_expected = yaw; double yaw_expected = yaw;
if (fabs(pitch - 90) < eps) { if (isEqualF(pitch, 90.0)) {
roll_expected = 0; roll_expected = 0;
yaw_expected = yaw - roll; yaw_expected = yaw - roll;
} else if (fabs(pitch + 90) < eps) { } else if (isEqualF(pitch, -90.0)) {
roll_expected = 0; roll_expected = 0;
yaw_expected = yaw + roll; yaw_expected = yaw + roll;
} }
@@ -228,7 +240,7 @@ int main()
Dcm<double> dcm_from_euler(euler); Dcm<double> dcm_from_euler(euler);
//dcm_from_euler.print(); //dcm_from_euler.print();
Euler<double> euler_out(dcm_from_euler); Euler<double> euler_out(dcm_from_euler);
TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out)); EXPECT_EQ(rad2deg * euler_expected, rad2deg * euler_out);
Eulerf eulerf_expected( Eulerf eulerf_expected(
float(deg2rad)*float(roll_expected), float(deg2rad)*float(roll_expected),
@@ -240,8 +252,8 @@ int main()
Dcm<float> dcm_from_eulerf; Dcm<float> dcm_from_eulerf;
dcm_from_eulerf = eulerf; dcm_from_eulerf = eulerf;
Euler<float> euler_outf(dcm_from_eulerf); Euler<float> euler_outf(dcm_from_eulerf);
TEST(isEqual(float(rad2deg)*eulerf_expected, EXPECT_EQ(float(rad2deg)*eulerf_expected,
float(rad2deg)*euler_outf)); float(rad2deg)*euler_outf);
} }
} }
} }
@@ -250,31 +262,31 @@ int main()
float data_v4[] = {1, 2, 3, 4}; float data_v4[] = {1, 2, 3, 4};
Vector<float, 4> v4(data_v4); Vector<float, 4> v4(data_v4);
Quatf q_from_v(v4); Quatf q_from_v(v4);
TEST(isEqual(q_from_v, v4)); EXPECT_EQ(q_from_v, v4);
Matrix<float, 4, 1> m4(data_v4); Matrix<float, 4, 1> m4(data_v4);
Quatf q_from_m(m4); Quatf q_from_m(m4);
TEST(isEqual(q_from_m, m4)); EXPECT_EQ(q_from_m, m4);
// quaternion derivative in frame 1 // quaternion derivative in frame 1
Quatf q1(0, 1, 0, 0); Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3)); Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f}; float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot1_check(data_q_dot1_check); Vector<float, 4> q1_dot1_check(data_q_dot1_check);
TEST(isEqual(q1_dot1, q1_dot1_check)); EXPECT_EQ(q1_dot1, q1_dot1_check);
// quaternion derivative in frame 2 // quaternion derivative in frame 2
Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3)); Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f}; float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
Vector<float, 4> q1_dot2_check(data_q_dot2_check); Vector<float, 4> q1_dot2_check(data_q_dot2_check);
TEST(isEqual(q1_dot2, q1_dot2_check)); EXPECT_EQ(q1_dot2, q1_dot2_check);
// quaternion product // quaternion product
Quatf q_prod_check( Quatf q_prod_check(
0.93394439f, 0.0674002f, 0.20851f, 0.28236266f); 0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
TEST(isEqual(q_prod_check, q_check * q_check)); EXPECT_EQ(q_prod_check, q_check * q_check);
q_check *= q_check; q_check *= q_check;
TEST(isEqual(q_prod_check, q_check)); EXPECT_EQ(q_prod_check, q_check);
// Quaternion scalar multiplication // Quaternion scalar multiplication
float scalar = 0.5; float scalar = 0.5;
@@ -282,210 +294,205 @@ int main()
Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar, Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
3.0f * scalar, 4.0f * scalar); 3.0f * scalar, 4.0f * scalar);
Quatf q_scalar_mul_res = scalar * q_scalar_mul; Quatf q_scalar_mul_res = scalar * q_scalar_mul;
TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res)); EXPECT_EQ(q_scalar_mul_check, q_scalar_mul_res);
Quatf q_scalar_mul_res2 = q_scalar_mul * scalar; Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res2)); EXPECT_EQ(q_scalar_mul_check, q_scalar_mul_res2);
Quatf q_scalar_mul_res3(q_scalar_mul); Quatf q_scalar_mul_res3(q_scalar_mul);
q_scalar_mul_res3 *= scalar; q_scalar_mul_res3 *= scalar;
TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res3)); EXPECT_EQ(q_scalar_mul_check, q_scalar_mul_res3);
// quaternion inverse // quaternion inverse
q = q_check.inversed(); q = q_check.inversed();
TEST(fabs(q_check(0) - q(0)) < eps); EXPECT_FLOAT_EQ(q_check(0), q(0));
TEST(fabs(q_check(1) + q(1)) < eps); EXPECT_FLOAT_EQ(q_check(1), -q(1));
TEST(fabs(q_check(2) + q(2)) < eps); EXPECT_FLOAT_EQ(q_check(2), -q(2));
TEST(fabs(q_check(3) + q(3)) < eps); EXPECT_FLOAT_EQ(q_check(3), -q(3));
q = q_check; q = q_check;
q.invert(); q.invert();
TEST(fabs(q_check(0) - q(0)) < eps); EXPECT_FLOAT_EQ(q_check(0), q(0));
TEST(fabs(q_check(1) + q(1)) < eps); EXPECT_FLOAT_EQ(q_check(1), -q(1));
TEST(fabs(q_check(2) + q(2)) < eps); EXPECT_FLOAT_EQ(q_check(2), -q(2));
TEST(fabs(q_check(3) + q(3)) < eps); EXPECT_FLOAT_EQ(q_check(3), -q(3));
// quaternion canonical // quaternion canonical
Quatf q_non_canonical_1(-0.7f, 0.4f, 0.3f, -0.3f); 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_1(0.7f, -0.4f, -0.3f, 0.3f);
Quatf q_canonical_ref_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)); EXPECT_EQ(q_non_canonical_1.canonical(), q_canonical_ref_1);
TEST(isEqual(q_canonical_1.canonical(), q_canonical_ref_1)); EXPECT_EQ(q_canonical_1.canonical(), q_canonical_ref_1);
q_non_canonical_1.canonicalize(); q_non_canonical_1.canonicalize();
q_canonical_1.canonicalize(); q_canonical_1.canonicalize();
TEST(isEqual(q_non_canonical_1, q_canonical_ref_1)); EXPECT_EQ(q_non_canonical_1, q_canonical_ref_1);
TEST(isEqual(q_canonical_1, q_canonical_ref_1)); EXPECT_EQ(q_canonical_1, q_canonical_ref_1);
Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f); 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_2(0.0f, 1.0f, 0.0f, 0.0f);
Quatf q_canonical_ref_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)); EXPECT_EQ(q_non_canonical_2.canonical(), q_canonical_ref_2);
TEST(isEqual(q_canonical_2.canonical(), q_canonical_ref_2)); EXPECT_EQ(q_canonical_2.canonical(), q_canonical_ref_2);
q_non_canonical_2.canonicalize(); q_non_canonical_2.canonicalize();
q_canonical_2.canonicalize(); q_canonical_2.canonicalize();
TEST(isEqual(q_non_canonical_2, q_canonical_ref_2)); EXPECT_EQ(q_non_canonical_2, q_canonical_ref_2);
TEST(isEqual(q_canonical_2, q_canonical_ref_2)); EXPECT_EQ(q_canonical_2, q_canonical_ref_2);
Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f); 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_3(0.0f, 0.0f, 1.0f, 0.0f);
Quatf q_canonical_ref_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)); EXPECT_EQ(q_non_canonical_3.canonical(), q_canonical_ref_3);
TEST(isEqual(q_canonical_3.canonical(), q_canonical_ref_3)); EXPECT_EQ(q_canonical_3.canonical(), q_canonical_ref_3);
q_non_canonical_3.canonicalize(); q_non_canonical_3.canonicalize();
q_canonical_3.canonicalize(); q_canonical_3.canonicalize();
TEST(isEqual(q_non_canonical_3, q_canonical_ref_3)); EXPECT_EQ(q_non_canonical_3, q_canonical_ref_3);
TEST(isEqual(q_canonical_3, q_canonical_ref_3)); EXPECT_EQ(q_canonical_3, q_canonical_ref_3);
Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f); 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_4(0.0f, 0.0f, 0.0f, 1.0f);
Quatf q_canonical_ref_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)); EXPECT_EQ(q_non_canonical_4.canonical(), q_canonical_ref_4);
TEST(isEqual(q_canonical_4.canonical(), q_canonical_ref_4)); EXPECT_EQ(q_canonical_4.canonical(), q_canonical_ref_4);
q_non_canonical_4.canonicalize(); q_non_canonical_4.canonicalize();
q_canonical_4.canonicalize(); q_canonical_4.canonicalize();
TEST(isEqual(q_non_canonical_4, q_canonical_ref_4)); EXPECT_EQ(q_non_canonical_4, q_canonical_ref_4);
TEST(isEqual(q_canonical_4, q_canonical_ref_4)); EXPECT_EQ(q_canonical_4, q_canonical_ref_4);
Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f); 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_5(0.0f, 0.0f, 0.0f, 0.0f);
Quatf q_canonical_ref_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)); EXPECT_EQ(q_non_canonical_5.canonical(), q_canonical_ref_5);
TEST(isEqual(q_canonical_5.canonical(), q_canonical_ref_5)); EXPECT_EQ(q_canonical_5.canonical(), q_canonical_ref_5);
q_non_canonical_5.canonicalize(); q_non_canonical_5.canonicalize();
q_canonical_5.canonicalize(); q_canonical_5.canonicalize();
TEST(isEqual(q_non_canonical_5, q_canonical_ref_5)); EXPECT_EQ(q_non_canonical_5, q_canonical_ref_5);
TEST(isEqual(q_canonical_5, q_canonical_ref_5)); EXPECT_EQ(q_canonical_5, q_canonical_ref_5);
// quaternion setIdentity // quaternion setIdentity
Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f); Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);
q_nonIdentity.setIdentity(); q_nonIdentity.setIdentity();
TEST(isEqual(q_nonIdentity, Quatf())); EXPECT_EQ(q_nonIdentity, Quatf());
// non-unit quaternion invese // non-unit quaternion invese
Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f); Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
TEST(isEqual(q_nonunit * q_nonunit.inversed(), Quatf())); EXPECT_EQ(q_nonunit * q_nonunit.inversed(), Quatf());
// rotate quaternion (nonzero rotation) // rotate quaternion (nonzero rotation)
Vector3f rot(1.f, 0.f, 0.f); Vector3f rot(1.f, 0.f, 0.f);
Quatf q_test; Quatf q_test;
q_test.rotate(rot); q_test.rotate(rot);
Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f); Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
TEST(isEqual(q_test, q_true)); EXPECT_EQ(q_test, q_true);
// rotate quaternion (zero rotation) // rotate quaternion (zero rotation)
rot(0) = rot(1) = rot(2) = 0.0f; rot(0) = rot(1) = rot(2) = 0.0f;
q_test = Quatf(); q_test = Quatf();
q_test.rotate(rot); q_test.rotate(rot);
q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f); q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f);
TEST(isEqual(q_test, q_true)); EXPECT_EQ(q_test, q_true);
// rotate quaternion (random non-commutating rotation) // rotate quaternion (random non-commutating rotation)
q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f)); q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f));
rot = Vector3f(1.1f, 2.5f, 3.8f); rot = Vector3f(1.1f, 2.5f, 3.8f);
q.rotate(rot); q.rotate(rot);
q_true = Quatf(0.3019f, 0.2645f, 0.2268f, 0.8874f); q_true = Quatf(0.3019f, 0.2645f, 0.2268f, 0.8874f);
TEST(isEqual(q, q_true)); EXPECT_EQ(q, q_true);
// get rotation axis from quaternion (nonzero rotation) // get rotation axis from quaternion (nonzero rotation)
q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f); q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
rot = AxisAnglef(q); rot = AxisAnglef(q);
TEST(fabs(rot(0)) < eps); EXPECT_FLOAT_EQ(rot(0), 0.0f);
TEST(fabs(rot(1) - 1.0f) < eps); EXPECT_FLOAT_EQ(rot(1), 1.0f);
TEST(fabs(rot(2)) < eps); EXPECT_FLOAT_EQ(rot(2), 0.0f);
// get rotation axis from quaternion (zero rotation) // get rotation axis from quaternion (zero rotation)
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot = AxisAnglef(q); rot = AxisAnglef(q);
TEST(fabs(rot(0)) < eps); EXPECT_FLOAT_EQ(rot(0), 0.0f);
TEST(fabs(rot(1)) < eps); EXPECT_FLOAT_EQ(rot(1), 0.0f);
TEST(fabs(rot(2)) < eps); EXPECT_FLOAT_EQ(rot(2), 0.0f);
// from axis angle (zero rotation) // from axis angle (zero rotation)
rot(0) = rot(1) = rot(2) = 0.0f; rot(0) = rot(1) = rot(2) = 0.0f;
q = Quatf(AxisAnglef(rot)); q = Quatf(AxisAnglef(rot));
q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f); q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
TEST(isEqual(q, q_true)); EXPECT_EQ(q, q_true);
// from axis angle, with length of vector the rotation // from axis angle, with length of vector the rotation
float n = float(sqrt(4 * M_PI * M_PI / 3)); float n = float(sqrt(4 * M_PI * M_PI / 3));
q = AxisAnglef(n, n, n); q = AxisAnglef(n, n, n);
TEST(isEqual(q, Quatf(-1, 0, 0, 0))); EXPECT_EQ(q, Quatf(-1, 0, 0, 0));
q = AxisAnglef(0, 0, 0); q = AxisAnglef(0, 0, 0);
TEST(isEqual(q, Quatf(1, 0, 0, 0))); EXPECT_EQ(q, Quatf(1, 0, 0, 0));
// Quaternion initialisation per array // Quaternion initialisation per array
float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f}; float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
Quaternion<float>q_from_array(q_array); Quaternion<float>q_from_array(q_array);
for (size_t i = 0; i < 4; i++) { for (size_t i = 0; i < 4; i++) {
TEST(fabs(q_from_array(i) - q_array[i]) < eps); EXPECT_FLOAT_EQ(q_from_array(i), q_array[i]);
} }
// axis angle // axis angle
AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f)); AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f));
TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f))); EXPECT_EQ(aa_true, Vector3f(1.0f, 2.0f, 3.0f));
AxisAnglef aa_empty; AxisAnglef aa_empty;
TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f))); EXPECT_EQ(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f));
float aa_data[] = {4.0f, 5.0f, 6.0f}; float aa_data[] = {4.0f, 5.0f, 6.0f};
AxisAnglef aa_data_init(aa_data); AxisAnglef aa_data_init(aa_data);
TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f))); EXPECT_EQ(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f));
AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f)); AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0))); EXPECT_EQ(aa_norm_check.axis(), Vector3f(1, 0, 0));
TEST(isEqualF(aa_norm_check.angle(), 0.0f)); EXPECT_FLOAT_EQ(aa_norm_check.angle(), 0.0f);
q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f); q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f);
float r_array[9] = {-0.6949206f, 0.713521f, 0.089292854f, -0.19200698f, -0.30378509f, 0.93319237f, 0.69297814f, 0.63134968f, 0.34810752f}; float r_array[9] = {-0.6949206f, 0.713521f, 0.089292854f, -0.19200698f, -0.30378509f, 0.93319237f, 0.69297814f, 0.63134968f, 0.34810752f};
R = Dcmf(r_array); R = Dcmf(r_array);
TEST(isEqual(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f))); EXPECT_EQ(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f));
// from dcm // from dcm
TEST(isEqual(Quatf(R), q)); EXPECT_EQ(Quatf(R), q);
TEST(isEqual(Quatf(Dcmf(q)), q)); EXPECT_EQ(Quatf(Dcmf(q)), q);
// to dcm // to dcm
TEST(isEqual(Dcmf(q), R)); EXPECT_EQ(Dcmf(q), R);
TEST(isEqual(Dcmf(Quatf(R)), R)); EXPECT_EQ(Dcmf(Quatf(R)), R);
// conjugate // conjugate
v = Vector3f(1.5f, 2.2f, 3.2f); v = Vector3f(1.5f, 2.2f, 3.2f);
TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1)); EXPECT_EQ(q.rotateVectorInverse(v1), Dcmf(q).T()*v1);
TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1)); EXPECT_EQ(q.rotateVector(v1), Dcmf(q)*v1);
AxisAnglef aa_q_init(q); AxisAnglef aa_q_init(q);
TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f))); EXPECT_EQ(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f));
AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f)); AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f))); EXPECT_EQ(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f));
Dcmf dcm_aa_check = AxisAnglef(dcm_check); Dcmf dcm_aa_check = AxisAnglef(dcm_check);
TEST(isEqual(dcm_aa_check, dcm_check)); EXPECT_EQ(dcm_aa_check, dcm_check);
AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f); AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f);
TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f))); EXPECT_EQ(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f));
TEST(isEqual(aa_axis_angle_init.axis(), Vector3f(0.26726124f, 0.53452248f, 0.80178373f))); EXPECT_EQ(aa_axis_angle_init.axis(), Vector3f(0.26726124f, 0.53452248f, 0.80178373f));
TEST(isEqualF(aa_axis_angle_init.angle(), 3.0f)); EXPECT_EQ(aa_axis_angle_init.angle(), 3.0f);
TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))), EXPECT_EQ(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))),
Quatf(1.0f, 0.0f, 0.0f, 0.0f))); Quatf(1.0f, 0.0f, 0.0f, 0.0f));
// check consistentcy of quaternion and dcm product // check consistentcy of quaternion and dcm product
Dcmf dcm3(Eulerf(1, 2, 3)); Dcmf dcm3(Eulerf(1, 2, 3));
Dcmf dcm4(Eulerf(4, 5, 6)); Dcmf dcm4(Eulerf(4, 5, 6));
Dcmf dcm34 = dcm3 * dcm4; Dcmf dcm34 = dcm3 * dcm4;
TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34))); EXPECT_EQ(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34));
// check corner cases of matrix to quaternion conversion // check corner cases of matrix to quaternion conversion
q = Quatf(0, 1, 0, 0); // 180 degree rotation around the x axis q = Quatf(0, 1, 0, 0); // 180 degree rotation around the x axis
R = Dcmf(q); R = Dcmf(q);
TEST(isEqual(q, Quatf(R))); EXPECT_EQ(q, Quatf(R));
q = Quatf(0, 0, 1, 0); // 180 degree rotation around the y axis q = Quatf(0, 0, 1, 0); // 180 degree rotation around the y axis
R = Dcmf(q); R = Dcmf(q);
TEST(isEqual(q, Quatf(R))); EXPECT_EQ(q, Quatf(R));
q = Quatf(0, 0, 0, 1); // 180 degree rotation around the z axis q = Quatf(0, 0, 0, 1); // 180 degree rotation around the z axis
R = Dcmf(q); R = Dcmf(q);
TEST(isEqual(q, Quatf(R))); EXPECT_EQ(q, Quatf(R));
#if defined(SUPPORT_STDIOSTREAM)
std::cout << "q:" << q;
#endif
return 0;
} }