zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Matrix: convert attitude test to gtest

v1.13.0-BW
Matthias Grob 3 years ago
parent
c33bf5b705
commit
35b035e880
2 changed files with 151 additions and 144 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/lib/matrix/test/CMakeLists.txt
  2. 293
      src/lib/matrix/test/MatrixAttitudeTest.cpp

2
src/lib/matrix/test/CMakeLists.txt
Unescape 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)

293
src/lib/matrix/test/attitude.cpp → src/lib/matrix/test/MatrixAttitudeTest.cpp
Unescape 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;
} }
Write Preview
Loading…
Cancel
Save