1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
|
#include "catch.hpp"
#include <heinz/Rotations3D.h>
#include <heinz/Vectors3D.h>
using Catch::Matchers::WithinAbs;
using Catch::Matchers::WithinRel;
const double epsilon = 1e-12;
using RotMatrix = Rotation3D<double>;
#undef M_PI
#define M_PI 3.14159265358979323846
const double w0 = M_PI / 5;
const double w1 = M_PI / 7;
const double w2 = M_PI / 11;
const RotMatrix mEul = RotMatrix::EulerZXZ(w0, w1, w2);
void InversionTest(const RotMatrix& mRot, const R3& a0)
{
const RotMatrix mInv = mRot.Inverse();
const R3 a1 = mRot.transformed(a0);
const R3 a2 = mInv.transformed(a1);
CHECK_THAT(a2.x(), WithinRel(a0.x(), epsilon));
CHECK_THAT(a2.y(), WithinRel(a0.y(), epsilon));
CHECK_THAT(a2.z(), WithinRel(a0.z(), epsilon));
}
TEST_CASE("RotMatrixTest:RotateY")
{
const R3 a(std::sqrt(3.) / 2., 2., 0.5);
const RotMatrix m2 = RotMatrix::AroundY(M_PI / 6.);
const R3 v = m2.transformed(a);
CHECK_THAT(v.x(), WithinRel(1.0, epsilon));
CHECK(v.y() == 2.0);
CHECK_THAT(v.z(), WithinAbs(0.0, 1e-9));
}
TEST_CASE("RotMatrixTest:RotateZ")
{
const R3 a(0.5, std::sqrt(3.) / 2., 2.);
const RotMatrix m3 = RotMatrix::AroundZ(M_PI / 6.);
const R3 v = m3.transformed(a);
CHECK_THAT(v.x(), WithinAbs(0.0, 1e-9));
CHECK_THAT(v.y(), WithinRel(1.0, epsilon));
CHECK(v.z() == 2.0);
const RotMatrix m4 = m3.Inverse();
const R3 w = m4.transformed(v);
CHECK_THAT(w.x(), WithinRel(a.x(), epsilon));
CHECK_THAT(w.y(), WithinRel(a.y(), epsilon));
CHECK_THAT(w.z(), WithinRel(a.z(), epsilon));
}
TEST_CASE("RotMatrixTest:RecoverEulerAngles")
{
auto angles = mEul.zxzEulerAngles();
CHECK_THAT(angles[0], WithinRel(w0, epsilon));
CHECK_THAT(angles[1], WithinRel(w1, epsilon));
CHECK_THAT(angles[2], WithinRel(w2, epsilon));
}
TEST_CASE("RotMatrixTest:InvertXMatrix")
{
InversionTest(RotMatrix::AroundX(M_PI / 7.), R3(4, 5, 6));
}
TEST_CASE("RotMatrixTest:InvertYMatrix")
{
InversionTest(RotMatrix::AroundY(M_PI / 7.), R3(4, 5, 6));
}
TEST_CASE("RotMatrixTest:InvertZMatrix")
{
InversionTest(RotMatrix::AroundZ(M_PI / 7.), R3(4, 5, 6));
}
TEST_CASE("RotMatrixTest:InvertEulerMatrix")
{
InversionTest(mEul, R3(3, 4, 7));
}
|
