Lines 1-38
Link Here
|
1 |
// This file is part of Eigen, a lightweight C++ template library |
1 |
// This file is part of Eigen, a lightweight C++ template library |
2 |
// for linear algebra. |
2 |
// for linear algebra. |
3 |
// |
3 |
// |
4 |
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> |
4 |
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr> |
5 |
// |
5 |
// |
6 |
// This Source Code Form is subject to the terms of the Mozilla |
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 |
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/. |
8 |
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
9 |
|
9 |
|
10 |
#include "main.h" |
10 |
#include "main.h" |
11 |
#include <Eigen/Geometry> |
11 |
#include <Eigen/Geometry> |
12 |
#include <Eigen/LU> |
12 |
#include <Eigen/LU> |
13 |
#include <Eigen/SVD> |
13 |
#include <Eigen/SVD> |
14 |
|
14 |
|
15 |
template<typename Scalar> void eulerangles(void) |
15 |
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) |
16 |
{ |
16 |
{ |
17 |
typedef Matrix<Scalar,3,3> Matrix3; |
17 |
typedef Matrix<Scalar,3,3> Matrix3; |
18 |
typedef Matrix<Scalar,3,1> Vector3; |
18 |
typedef Matrix<Scalar,3,1> Vector3; |
19 |
typedef Quaternion<Scalar> Quaternionx; |
|
|
20 |
typedef AngleAxis<Scalar> AngleAxisx; |
19 |
typedef AngleAxis<Scalar> AngleAxisx; |
21 |
|
20 |
|
22 |
Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); |
|
|
23 |
Quaternionx q1; |
24 |
q1 = AngleAxisx(a, Vector3::Random().normalized()); |
25 |
Matrix3 m; |
26 |
m = q1; |
27 |
|
28 |
#define VERIFY_EULER(I,J,K, X,Y,Z) { \ |
21 |
#define VERIFY_EULER(I,J,K, X,Y,Z) { \ |
29 |
Vector3 ea = m.eulerAngles(I,J,K); \ |
22 |
Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ |
30 |
VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ |
23 |
Vector3 eabis = m.eulerAngles(I,J,K); \ |
|
|
24 |
Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \ |
25 |
VERIFY_IS_APPROX(m, mbis); \ |
26 |
if(I!=K || ea[1]!=0) VERIFY_IS_APPROX(ea, eabis); \ |
31 |
} |
27 |
} |
32 |
VERIFY_EULER(0,1,2, X,Y,Z); |
28 |
VERIFY_EULER(0,1,2, X,Y,Z); |
33 |
VERIFY_EULER(0,1,0, X,Y,X); |
29 |
VERIFY_EULER(0,1,0, X,Y,X); |
34 |
VERIFY_EULER(0,2,1, X,Z,Y); |
30 |
VERIFY_EULER(0,2,1, X,Z,Y); |
35 |
VERIFY_EULER(0,2,0, X,Z,X); |
31 |
VERIFY_EULER(0,2,0, X,Z,X); |
36 |
|
32 |
|
37 |
VERIFY_EULER(1,2,0, Y,Z,X); |
33 |
VERIFY_EULER(1,2,0, Y,Z,X); |
38 |
VERIFY_EULER(1,2,1, Y,Z,Y); |
34 |
VERIFY_EULER(1,2,1, Y,Z,Y); |
Lines 40-54
template<typename Scalar> void eulerangl
Link Here
|
40 |
VERIFY_EULER(1,0,1, Y,X,Y); |
36 |
VERIFY_EULER(1,0,1, Y,X,Y); |
41 |
|
37 |
|
42 |
VERIFY_EULER(2,0,1, Z,X,Y); |
38 |
VERIFY_EULER(2,0,1, Z,X,Y); |
43 |
VERIFY_EULER(2,0,2, Z,X,Z); |
39 |
VERIFY_EULER(2,0,2, Z,X,Z); |
44 |
VERIFY_EULER(2,1,0, Z,Y,X); |
40 |
VERIFY_EULER(2,1,0, Z,Y,X); |
45 |
VERIFY_EULER(2,1,2, Z,Y,Z); |
41 |
VERIFY_EULER(2,1,2, Z,Y,Z); |
46 |
} |
42 |
} |
47 |
|
43 |
|
|
|
44 |
template<typename Scalar> void eulerangles(void) |
45 |
{ |
46 |
typedef Matrix<Scalar,3,3> Matrix3; |
47 |
typedef Matrix<Scalar,3,1> Vector3; |
48 |
typedef Array<Scalar,3,1> Array3; |
49 |
typedef Quaternion<Scalar> Quaternionx; |
50 |
typedef AngleAxis<Scalar> AngleAxisx; |
51 |
|
52 |
Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); |
53 |
Quaternionx q1; |
54 |
q1 = AngleAxisx(a, Vector3::Random().normalized()); |
55 |
Matrix3 m; |
56 |
m = q1; |
57 |
|
58 |
Vector3 ea = m.eulerAngles(0,1,2); |
59 |
check_all_var(ea); |
60 |
ea = m.eulerAngles(0,1,0); |
61 |
check_all_var(ea); |
62 |
|
63 |
ea = (Array3::Random() + Array3(1,1,0))*M_PI*Array3(0.5,0.5,1); |
64 |
check_all_var(ea); |
65 |
|
66 |
ea[2] = ea[0] = internal::random<Scalar>(0,M_PI); |
67 |
check_all_var(ea); |
68 |
|
69 |
ea[0] = ea[1] = internal::random<Scalar>(0,M_PI); |
70 |
check_all_var(ea); |
71 |
|
72 |
ea[1] = 0; |
73 |
check_all_var(ea); |
74 |
|
75 |
ea.head(2).setZero(); |
76 |
check_all_var(ea); |
77 |
|
78 |
ea.setZero(); |
79 |
check_all_var(ea); |
80 |
} |
81 |
|
48 |
void test_geo_eulerangles() |
82 |
void test_geo_eulerangles() |
49 |
{ |
83 |
{ |
50 |
for(int i = 0; i < g_repeat; i++) { |
84 |
for(int i = 0; i < g_repeat; i++) { |
51 |
CALL_SUBTEST_1( eulerangles<float>() ); |
85 |
CALL_SUBTEST_1( eulerangles<float>() ); |
52 |
CALL_SUBTEST_2( eulerangles<double>() ); |
86 |
CALL_SUBTEST_2( eulerangles<double>() ); |
53 |
} |
87 |
} |
54 |
} |
88 |
} |