CFD/eigen
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
master
eigen/test/geo_eulerangles.cpp

200 lines
6.4 KiB
C++
Raw Permalink Blame History

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2023 Juraj Oršulić, University of Zagreb <juraj.orsulic@fer.hr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// Silence warnings about using the deprecated non-canonical .eulerAngles(), which are still being tested.
#define EIGEN_NO_DEPRECATED_WARNING
#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
template <typename Scalar>
void verify_euler(const Matrix<Scalar, 3, 1>& ea, int i, int j, int k) {
typedef Matrix<Scalar, 3, 3> Matrix3;
typedef Matrix<Scalar, 3, 1> Vector3;
typedef AngleAxis<Scalar> AngleAxisx;
const Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) *
AngleAxisx(ea[2], Vector3::Unit(k)));
// Test non-canonical eulerAngles
{
Vector3 eabis = m.eulerAngles(i, j, k);
Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) *
AngleAxisx(eabis[2], Vector3::Unit(k)));
VERIFY_IS_APPROX(m, mbis);
// approx_or_less_than does not work for 0
VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI));
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI));
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI));
}
// Test canonicalEulerAngles
{
Vector3 eabis = m.canonicalEulerAngles(i, j, k);
Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) *
AngleAxisx(eabis[2], Vector3::Unit(k)));
VERIFY_IS_APPROX(m, mbis);
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[0]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI));
if (i != k) {
// Tait-Bryan sequence
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI / 2), eabis[1]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI / 2));
} else {
// Proper Euler sequence
// approx_or_less_than does not work for 0
VERIFY(0 < eabis[1] || test_isMuchSmallerThan(eabis[1], Scalar(1)));
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI));
}
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI));
}
}
template <typename Scalar>
void check_all_var(const Matrix<Scalar, 3, 1>& ea) {
auto verify_permutation = [](const Matrix<Scalar, 3, 1>& eap) {
verify_euler(eap, 0, 1, 2);
verify_euler(eap, 0, 1, 0);
verify_euler(eap, 0, 2, 1);
verify_euler(eap, 0, 2, 0);
verify_euler(eap, 1, 2, 0);
verify_euler(eap, 1, 2, 1);
verify_euler(eap, 1, 0, 2);
verify_euler(eap, 1, 0, 1);
verify_euler(eap, 2, 0, 1);
verify_euler(eap, 2, 0, 2);
verify_euler(eap, 2, 1, 0);
verify_euler(eap, 2, 1, 2);
};
int i, j, k;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
for (k = 0; k < 3; k++) {
Matrix<Scalar, 3, 1> eap(ea(i), ea(j), ea(k));
verify_permutation(eap);
}
}
template <typename Scalar>
void eulerangles() {
typedef Matrix<Scalar, 3, 3> Matrix3;
typedef Matrix<Scalar, 3, 1> Vector3;
typedef Array<Scalar, 3, 1> Array3;
typedef Quaternion<Scalar> Quaternionx;
typedef AngleAxis<Scalar> AngleAxisx;
Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
Quaternionx q1;
q1 = AngleAxisx(a, Vector3::Random().normalized());
Matrix3 m;
m = q1;
Vector3 ea = m.eulerAngles(0, 1, 2);
check_all_var(ea);
ea = m.eulerAngles(0, 1, 0);
check_all_var(ea);
// Check with purely random Quaternion:
q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
m = q1;
ea = m.eulerAngles(0, 1, 2);
check_all_var(ea);
ea = m.eulerAngles(0, 1, 0);
check_all_var(ea);
// Check with random angles in range [-pi:pi]x[-pi:pi]x[-pi:pi].
ea = Array3::Random() * Scalar(EIGEN_PI);
check_all_var(ea);
auto test_with_some_zeros = [](const Vector3& eaz) {
check_all_var(eaz);
Vector3 ea_glz = eaz;
ea_glz[0] = Scalar(0);
check_all_var(ea_glz);
ea_glz[0] = internal::random<Scalar>(-0.001, 0.001);
check_all_var(ea_glz);
ea_glz[2] = Scalar(0);
check_all_var(ea_glz);
ea_glz[2] = internal::random<Scalar>(-0.001, 0.001);
check_all_var(ea_glz);
};
// Check gimbal lock configurations and a bit noisy gimbal locks
Vector3 ea_gl = ea;
ea_gl[1] = EIGEN_PI / 2;
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_gl[1] = -EIGEN_PI / 2;
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_gl[1] = EIGEN_PI / 2;
ea_gl[2] = ea_gl[0];
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_gl[1] = -EIGEN_PI / 2;
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
// Similar to above, but with pi instead of pi/2
Vector3 ea_pi = ea;
ea_pi[1] = EIGEN_PI;
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_pi[1] = -EIGEN_PI;
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_pi[1] = EIGEN_PI;
ea_pi[2] = ea_pi[0];
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_pi[1] = -EIGEN_PI;
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea[2] = ea[0] = internal::random<Scalar>(0, Scalar(EIGEN_PI));
check_all_var(ea);
ea[0] = ea[1] = internal::random<Scalar>(0, Scalar(EIGEN_PI));
check_all_var(ea);
ea[1] = 0;
check_all_var(ea);
ea.head(2).setZero();
check_all_var(ea);
ea.setZero();
check_all_var(ea);
}
EIGEN_DECLARE_TEST(geo_eulerangles) {
for (int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1(eulerangles<float>());
CALL_SUBTEST_2(eulerangles<double>());
}
}
Reference in New Issue View Git Blame Copy Permalink