diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt index 3e3789c4c3..d51f3dc75d 100644 --- a/src/lib/matrix/test/CMakeLists.txt +++ b/src/lib/matrix/test/CMakeLists.txt @@ -16,7 +16,6 @@ set(tests vector vector2 vector3 - attitude filter integration squareMatrix @@ -43,5 +42,6 @@ foreach(test_name ${tests}) endforeach() 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 MatrixUnwrapTest.cpp) diff --git a/src/lib/matrix/test/attitude.cpp b/src/lib/matrix/test/MatrixAttitudeTest.cpp similarity index 59% rename from src/lib/matrix/test/attitude.cpp rename to src/lib/matrix/test/MatrixAttitudeTest.cpp index 84961e827a..5e51f1ca77 100644 --- a/src/lib/matrix/test/attitude.cpp +++ b/src/lib/matrix/test/MatrixAttitudeTest.cpp @@ -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 #include -#include using namespace matrix; -// manually instantiated all files we intend to test -// so that coverage works correctly -// doesn't matter what test this is in -namespace matrix -{ -template class Matrix; -template class Vector3; -template class Vector2; -template class Vector; -template class Quaternion; -template class AxisAngle; -template class Scalar; -template class SquareMatrix; -} - -int main() +TEST(MatrixAttitudeTest, Attitude) { // check data Eulerf euler_check(0.1f, 0.2f, 0.3f); @@ -32,70 +49,68 @@ int main() Dcmf dcm_check(dcm_data); // 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 Eulerf e; Eulerf e_zero = zeros(); - TEST(isEqual(e, e_zero)); - TEST(isEqual(e, e)); + EXPECT_EQ(e, e_zero); + EXPECT_EQ(e, e); // euler vector ctor Vector3f v(0.1f, 0.2f, 0.3f); Eulerf euler_copy(v); - TEST(isEqual(euler_copy, euler_check)); + EXPECT_EQ(euler_copy, euler_check); // quaternion ctor Quatf q0(1, 2, 3, 4); Quatf q(q0); - double eps = 1e-6; - TEST(fabs(q(0) - 1) < eps); - TEST(fabs(q(1) - 2) < eps); - TEST(fabs(q(2) - 3) < eps); - TEST(fabs(q(3) - 4) < eps); + EXPECT_FLOAT_EQ(q(0), 1); + EXPECT_FLOAT_EQ(q(1), 2); + EXPECT_FLOAT_EQ(q(2), 3); + EXPECT_FLOAT_EQ(q(3), 4); // quaternion ctor: vector to vector // identity test 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) Vector3f v1(-80.1f, 1.5f, -6.89f); 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 v1 = Vector3f(0.f, 1.f, 1.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.rotateVector(v1), -v1)); + EXPECT_EQ(quat_v.rotateVector(v1), -v1); // special 180 degree case 2 v1 = Vector3f(1.f, 2.f, 0.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.rotateVector(v1), -v1)); + EXPECT_EQ(quat_v.rotateVector(v1), -v1); // special 180 degree case 3 v1 = Vector3f(0.f, 0.f, 1.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.rotateVector(v1), -v1)); + EXPECT_EQ(quat_v.rotateVector(v1), -v1); // special 180 degree case 4 v1 = Vector3f(1.f, 1.f, 1.f); quat_v = Quatf(v1, -v1); - TEST(isEqual(quat_v.rotateVector(v1), -v1)); + EXPECT_EQ(quat_v.rotateVector(v1), -v1); // quat normalization q.normalize(); - TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f, - 0.54772256f, 0.73029674f))); - TEST(isEqual(q0.unit(), q)); - TEST(isEqual(q0.unit(), q0.normalized())); + EXPECT_EQ(q, Quatf(0.18257419f, 0.36514837f, 0.54772256f, 0.73029674f)); + EXPECT_EQ(q0.unit(), q); + EXPECT_EQ(q0.unit(), q0.normalized()); // quat default ctor q = Quatf(); - TEST(isEqual(q, Quatf(1, 0, 0, 0))); + EXPECT_EQ(q, Quatf(1, 0, 0, 0)); // quaternion exponential with v=0 v = Vector3f(); q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); Dcmf M = Dcmf() * 0.5f; - TEST(isEqual(q, Quatf::expq(v))); - TEST(isEqual(M, Quatf::inv_r_jacobian(v))); + EXPECT_EQ(q, Quatf::expq(v)); + EXPECT_EQ(M, Quatf::inv_r_jacobian(v)); // quaternion exponential with small v v = Vector3f(0.001f, 0.002f, -0.003f); @@ -109,8 +124,8 @@ int main() }; M = Dcmf(M_data); } - TEST(isEqual(q, Quatf::expq(v))); - TEST(isEqual(M, Quatf::inv_r_jacobian(v))); + EXPECT_EQ(q, Quatf::expq(v)); + EXPECT_EQ(M, Quatf::inv_r_jacobian(v)); // quaternion exponential with v v = Vector3f(1.0f, -2.0f, 3.0f); @@ -124,8 +139,8 @@ int main() }; M = Dcmf(M_data); } - TEST(isEqual(q, Quatf::expq(v))); - TEST(isEqual(M, Quatf::inv_r_jacobian(v))); + EXPECT_EQ(q, Quatf::expq(v)); + EXPECT_EQ(M, Quatf::inv_r_jacobian(v)); // quaternion kinematic update q = Quatf(); @@ -134,40 +149,40 @@ int main() Quatf qa = q + 0.5f * h * q.derivative1(w_B); qa.normalize(); Quatf qb = q * Quatf::expq(0.5f * h * w_B); - TEST(isEqual(qa, qb)); + EXPECT_EQ(qa, qb); // euler to quaternion q = Quatf(euler_check); - TEST(isEqual(q, q_check)); + EXPECT_EQ(q, q_check); // euler to dcm Dcmf dcm(euler_check); - TEST(isEqual(dcm, dcm_check)); + EXPECT_EQ(dcm, dcm_check); // quaternion to euler Eulerf e1(q_check); - TEST(isEqual(e1, euler_check)); + EXPECT_EQ(e1, euler_check); // quaternion to dcm Dcmf dcm1(q_check); - TEST(isEqual(dcm1, dcm_check)); + EXPECT_EQ(dcm1, dcm_check); // quaternion z-axis unit base vector Vector3f q_z = q_check.dcm_z(); 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 Dcmf dcm2; SquareMatrix I = eye(); - TEST(isEqual(dcm2, I)); + EXPECT_EQ(dcm2, I); // dcm to euler Eulerf e2(dcm_check); - TEST(isEqual(e2, euler_check)); + EXPECT_EQ(e2, euler_check); // dcm to quaterion Quatf q2(dcm_check); - TEST(isEqual(q2, q_check)); + EXPECT_EQ(q2, q_check); // dcm renormalize Dcmf A = eye(); @@ -178,15 +193,12 @@ int main() } A.renormalize(); - float err = 0.0f; for (size_t r = 0; r < 3; r++) { Vector3f rvec(matrix::Matrix(A.row(r)).transpose()); - err += fabs(1.0f - rvec.length()); + EXPECT_FLOAT_EQ(1.0f, rvec.length()); } - TEST(err < eps); - // constants double deg2rad = M_PI / 180.0; double rad2deg = 180.0 / M_PI; @@ -199,11 +211,11 @@ int main() double roll_expected = roll; double yaw_expected = yaw; - if (fabs(pitch - 90) < eps) { + if (isEqualF(pitch, 90.0)) { roll_expected = 0; yaw_expected = yaw - roll; - } else if (fabs(pitch + 90) < eps) { + } else if (isEqualF(pitch, -90.0)) { roll_expected = 0; yaw_expected = yaw + roll; } @@ -228,7 +240,7 @@ int main() Dcm dcm_from_euler(euler); //dcm_from_euler.print(); Euler euler_out(dcm_from_euler); - TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out)); + EXPECT_EQ(rad2deg * euler_expected, rad2deg * euler_out); Eulerf eulerf_expected( float(deg2rad)*float(roll_expected), @@ -240,8 +252,8 @@ int main() Dcm dcm_from_eulerf; dcm_from_eulerf = eulerf; Euler euler_outf(dcm_from_eulerf); - TEST(isEqual(float(rad2deg)*eulerf_expected, - float(rad2deg)*euler_outf)); + EXPECT_EQ(float(rad2deg)*eulerf_expected, + float(rad2deg)*euler_outf); } } } @@ -250,31 +262,31 @@ int main() float data_v4[] = {1, 2, 3, 4}; Vector v4(data_v4); Quatf q_from_v(v4); - TEST(isEqual(q_from_v, v4)); + EXPECT_EQ(q_from_v, v4); Matrix m4(data_v4); Quatf q_from_m(m4); - TEST(isEqual(q_from_m, m4)); + EXPECT_EQ(q_from_m, m4); // quaternion derivative in frame 1 Quatf q1(0, 1, 0, 0); Vector q1_dot1 = q1.derivative1(Vector3f(1, 2, 3)); float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f}; Vector 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 Vector q1_dot2 = q1.derivative2(Vector3f(1, 2, 3)); float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f}; Vector q1_dot2_check(data_q_dot2_check); - TEST(isEqual(q1_dot2, q1_dot2_check)); + EXPECT_EQ(q1_dot2, q1_dot2_check); // quaternion product Quatf q_prod_check( 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; - TEST(isEqual(q_prod_check, q_check)); + EXPECT_EQ(q_prod_check, q_check); // Quaternion scalar multiplication float scalar = 0.5; @@ -282,210 +294,205 @@ int main() Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar, 3.0f * scalar, 4.0f * scalar); 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; - 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); 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 q = q_check.inversed(); - TEST(fabs(q_check(0) - q(0)) < eps); - TEST(fabs(q_check(1) + q(1)) < eps); - TEST(fabs(q_check(2) + q(2)) < eps); - TEST(fabs(q_check(3) + q(3)) < eps); + EXPECT_FLOAT_EQ(q_check(0), q(0)); + EXPECT_FLOAT_EQ(q_check(1), -q(1)); + EXPECT_FLOAT_EQ(q_check(2), -q(2)); + EXPECT_FLOAT_EQ(q_check(3), -q(3)); q = q_check; q.invert(); - TEST(fabs(q_check(0) - q(0)) < eps); - TEST(fabs(q_check(1) + q(1)) < eps); - TEST(fabs(q_check(2) + q(2)) < eps); - TEST(fabs(q_check(3) + q(3)) < eps); + EXPECT_FLOAT_EQ(q_check(0), q(0)); + EXPECT_FLOAT_EQ(q_check(1), -q(1)); + EXPECT_FLOAT_EQ(q_check(2), -q(2)); + EXPECT_FLOAT_EQ(q_check(3), -q(3)); // quaternion canonical 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)); + EXPECT_EQ(q_non_canonical_1.canonical(), q_canonical_ref_1); + EXPECT_EQ(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)); + EXPECT_EQ(q_non_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_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)); + EXPECT_EQ(q_non_canonical_2.canonical(), q_canonical_ref_2); + EXPECT_EQ(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)); + EXPECT_EQ(q_non_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_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)); + EXPECT_EQ(q_non_canonical_3.canonical(), q_canonical_ref_3); + EXPECT_EQ(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)); + EXPECT_EQ(q_non_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_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)); + EXPECT_EQ(q_non_canonical_4.canonical(), q_canonical_ref_4); + EXPECT_EQ(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)); + EXPECT_EQ(q_non_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_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)); + EXPECT_EQ(q_non_canonical_5.canonical(), q_canonical_ref_5); + EXPECT_EQ(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)); + EXPECT_EQ(q_non_canonical_5, q_canonical_ref_5); + EXPECT_EQ(q_canonical_5, q_canonical_ref_5); // quaternion setIdentity Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f); q_nonIdentity.setIdentity(); - TEST(isEqual(q_nonIdentity, Quatf())); + EXPECT_EQ(q_nonIdentity, Quatf()); // non-unit quaternion invese 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) Vector3f rot(1.f, 0.f, 0.f); Quatf q_test; q_test.rotate(rot); 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) rot(0) = rot(1) = rot(2) = 0.0f; q_test = Quatf(); q_test.rotate(rot); 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) q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f)); rot = Vector3f(1.1f, 2.5f, 3.8f); q.rotate(rot); 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) q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f); rot = AxisAnglef(q); - TEST(fabs(rot(0)) < eps); - TEST(fabs(rot(1) - 1.0f) < eps); - TEST(fabs(rot(2)) < eps); + EXPECT_FLOAT_EQ(rot(0), 0.0f); + EXPECT_FLOAT_EQ(rot(1), 1.0f); + EXPECT_FLOAT_EQ(rot(2), 0.0f); // get rotation axis from quaternion (zero rotation) q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); rot = AxisAnglef(q); - TEST(fabs(rot(0)) < eps); - TEST(fabs(rot(1)) < eps); - TEST(fabs(rot(2)) < eps); + EXPECT_FLOAT_EQ(rot(0), 0.0f); + EXPECT_FLOAT_EQ(rot(1), 0.0f); + EXPECT_FLOAT_EQ(rot(2), 0.0f); // from axis angle (zero rotation) rot(0) = rot(1) = rot(2) = 0.0f; q = Quatf(AxisAnglef(rot)); 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 float n = float(sqrt(4 * M_PI * M_PI / 3)); 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); - TEST(isEqual(q, Quatf(1, 0, 0, 0))); + EXPECT_EQ(q, Quatf(1, 0, 0, 0)); // Quaternion initialisation per array float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f}; Quaternionq_from_array(q_array); 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 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; - 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}; 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)); - TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0))); - TEST(isEqualF(aa_norm_check.angle(), 0.0f)); + EXPECT_EQ(aa_norm_check.axis(), Vector3f(1, 0, 0)); + EXPECT_FLOAT_EQ(aa_norm_check.angle(), 0.0f); 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}; 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 - TEST(isEqual(Quatf(R), q)); - TEST(isEqual(Quatf(Dcmf(q)), q)); + EXPECT_EQ(Quatf(R), q); + EXPECT_EQ(Quatf(Dcmf(q)), q); // to dcm - TEST(isEqual(Dcmf(q), R)); - TEST(isEqual(Dcmf(Quatf(R)), R)); + EXPECT_EQ(Dcmf(q), R); + EXPECT_EQ(Dcmf(Quatf(R)), R); // conjugate v = Vector3f(1.5f, 2.2f, 3.2f); - TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1)); - TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1)); + EXPECT_EQ(q.rotateVectorInverse(v1), Dcmf(q).T()*v1); + EXPECT_EQ(q.rotateVector(v1), Dcmf(q)*v1); 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)); - 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); - 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); - TEST(isEqual(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))); - TEST(isEqualF(aa_axis_angle_init.angle(), 3.0f)); - TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))), - Quatf(1.0f, 0.0f, 0.0f, 0.0f))); + EXPECT_EQ(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f)); + EXPECT_EQ(aa_axis_angle_init.axis(), Vector3f(0.26726124f, 0.53452248f, 0.80178373f)); + EXPECT_EQ(aa_axis_angle_init.angle(), 3.0f); + EXPECT_EQ(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))), + Quatf(1.0f, 0.0f, 0.0f, 0.0f)); // check consistentcy of quaternion and dcm product Dcmf dcm3(Eulerf(1, 2, 3)); Dcmf dcm4(Eulerf(4, 5, 6)); 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 q = Quatf(0, 1, 0, 0); // 180 degree rotation around the x axis 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 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 R = Dcmf(q); - TEST(isEqual(q, Quatf(R))); - -#if defined(SUPPORT_STDIOSTREAM) - std::cout << "q:" << q; -#endif - return 0; + EXPECT_EQ(q, Quatf(R)); }