dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Matrix: convert attitude test to gtest

This commit is contained in:
Matthias Grob 2022-02-23 14:44:59 +01:00
parent c33bf5b705
commit 35b035e880
2 changed files with 151 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/matrix/test/CMakeLists.txt
View File

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

293
src/lib/matrix/test/attitude.cpp → src/lib/matrix/test/MatrixAttitudeTest.cpp
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 <iostream>
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<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()
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<float, 3, 1>();
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<float, 3> I = eye<float, 3>();
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<float, 3>();
@ -178,15 +193,12 @@ int main()
}
A.renormalize();
float err = 0.0f;
for (size_t r = 0; r < 3; r++) {
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
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<double> dcm_from_euler(euler);
//dcm_from_euler.print();
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(
float(deg2rad)*float(roll_expected),
@ -240,8 +252,8 @@ int main()
Dcm<float> dcm_from_eulerf;
dcm_from_eulerf = eulerf;
Euler<float> 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<float, 4> v4(data_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);
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<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
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
Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
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
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};
Quaternion<float>q_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));
}