Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 1 | // This file is part of Eigen, a lightweight C++ template library |
| 2 | // for linear algebra. |
| 3 | // |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 4 | // Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr> |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 5 | // |
| 6 | // This Source Code Form is subject to the terms of the Mozilla |
| 7 | // Public License v. 2.0. If a copy of the MPL was not distributed |
| 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
| 9 | |
| 10 | #include "main.h" |
| 11 | #include <Eigen/Geometry> |
| 12 | #include <Eigen/LU> |
| 13 | #include <Eigen/SVD> |
| 14 | |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 15 | |
| 16 | template<typename Scalar> |
| 17 | void verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k) |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 18 | { |
| 19 | typedef Matrix<Scalar,3,3> Matrix3; |
| 20 | typedef Matrix<Scalar,3,1> Vector3; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 21 | typedef AngleAxis<Scalar> AngleAxisx; |
| 22 | using std::abs; |
| 23 | Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); |
| 24 | Vector3 eabis = m.eulerAngles(i, j, k); |
| 25 | Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); |
| 26 | VERIFY_IS_APPROX(m, mbis); |
| 27 | /* If I==K, and ea[1]==0, then there no unique solution. */ |
| 28 | /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ |
| 29 | if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision<Scalar>())) ) |
| 30 | VERIFY((ea-eabis).norm() <= test_precision<Scalar>()); |
| 31 | |
| 32 | // approx_or_less_than does not work for 0 |
| 33 | VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); |
| 34 | VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI)); |
| 35 | VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]); |
| 36 | VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI)); |
| 37 | VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]); |
| 38 | VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI)); |
| 39 | } |
| 40 | |
| 41 | template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) |
| 42 | { |
| 43 | verify_euler(ea, 0,1,2); |
| 44 | verify_euler(ea, 0,1,0); |
| 45 | verify_euler(ea, 0,2,1); |
| 46 | verify_euler(ea, 0,2,0); |
| 47 | |
| 48 | verify_euler(ea, 1,2,0); |
| 49 | verify_euler(ea, 1,2,1); |
| 50 | verify_euler(ea, 1,0,2); |
| 51 | verify_euler(ea, 1,0,1); |
| 52 | |
| 53 | verify_euler(ea, 2,0,1); |
| 54 | verify_euler(ea, 2,0,2); |
| 55 | verify_euler(ea, 2,1,0); |
| 56 | verify_euler(ea, 2,1,2); |
| 57 | } |
| 58 | |
| 59 | template<typename Scalar> void eulerangles() |
| 60 | { |
| 61 | typedef Matrix<Scalar,3,3> Matrix3; |
| 62 | typedef Matrix<Scalar,3,1> Vector3; |
| 63 | typedef Array<Scalar,3,1> Array3; |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 64 | typedef Quaternion<Scalar> Quaternionx; |
| 65 | typedef AngleAxis<Scalar> AngleAxisx; |
| 66 | |
| 67 | Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); |
| 68 | Quaternionx q1; |
| 69 | q1 = AngleAxisx(a, Vector3::Random().normalized()); |
| 70 | Matrix3 m; |
| 71 | m = q1; |
Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 72 | |
| 73 | Vector3 ea = m.eulerAngles(0,1,2); |
| 74 | check_all_var(ea); |
| 75 | ea = m.eulerAngles(0,1,0); |
| 76 | check_all_var(ea); |
| 77 | |
| 78 | // Check with purely random Quaternion: |
| 79 | q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); |
| 80 | m = q1; |
| 81 | ea = m.eulerAngles(0,1,2); |
| 82 | check_all_var(ea); |
| 83 | ea = m.eulerAngles(0,1,0); |
| 84 | check_all_var(ea); |
| 85 | |
| 86 | // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. |
| 87 | ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1); |
| 88 | check_all_var(ea); |
| 89 | |
| 90 | ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(M_PI)); |
| 91 | check_all_var(ea); |
| 92 | |
| 93 | ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(M_PI)); |
| 94 | check_all_var(ea); |
| 95 | |
| 96 | ea[1] = 0; |
| 97 | check_all_var(ea); |
| 98 | |
| 99 | ea.head(2).setZero(); |
| 100 | check_all_var(ea); |
| 101 | |
| 102 | ea.setZero(); |
| 103 | check_all_var(ea); |
Narayan Kamath | c981c48 | 2012-11-02 10:59:05 +0000 | [diff] [blame] | 104 | } |
| 105 | |
| 106 | void test_geo_eulerangles() |
| 107 | { |
| 108 | for(int i = 0; i < g_repeat; i++) { |
| 109 | CALL_SUBTEST_1( eulerangles<float>() ); |
| 110 | CALL_SUBTEST_2( eulerangles<double>() ); |
| 111 | } |
| 112 | } |