5 #define BOOST_TEST_MODULE matrix_twist
7 #include <boost/test/tools/floating_point_comparison.hpp>
8 #include <boost/test/tools/output_test_stream.hpp>
9 #include <boost/test/unit_test.hpp>
12 using boost::test_tools::output_test_stream;
14 #define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
15 for (unsigned i = 0; i < N; ++i) \
16 for (unsigned j = 0; j < M; ++j) \
17 BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)
19 #define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
20 MATRIX_BOOST_REQUIRE_CLOSE(6, 6, LEFT, RIGHT, TOLERANCE)
22 #define MATRIX_4x4_INIT(M, A00, A01, A02, A03, A10, A11, A12, A13, A20, A21, \
23 A22, A23, A30, A31, A32, A33) \
24 M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03; \
25 M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13; \
26 M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23; \
27 M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33
29 #define MATRIX_6x6_INIT(M, A00, A01, A02, A03, A04, A05, A10, A11, A12, A13, \
30 A14, A15, A20, A21, A22, A23, A24, A25, A30, A31, A32, \
31 A33, A34, A35, A40, A41, A42, A43, A44, A45, A50, A51, \
33 M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03, M(0, 4) = A04, \
35 M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13, M(1, 4) = A14, \
37 M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23, M(2, 4) = A24, \
39 M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33, M(3, 4) = A34, \
41 M(4, 0) = A40, M(4, 1) = A41, M(4, 2) = A42, M(4, 3) = A43, M(4, 4) = A44, \
43 M(5, 0) = A50, M(5, 1) = A51, M(5, 2) = A52, M(5, 3) = A53, M(5, 4) = A54, \
68 for (
unsigned i = 0;
i < 6; ++
i)
69 for (
unsigned j = 0; j < 6; ++j) twistRef(
i, j) = (
i == j) ? 1. : 0.;
77 MATRIX_4x4_INIT(
M, 0., 0., 1., 0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0.,
82 MATRIX_6x6_INIT(twistRef, 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
83 -1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 1.,
84 0., 0., 0., 0., 0., 0., -1., 0.);
96 MATRIX_4x4_INIT(
M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
102 MATRIX_6x6_INIT(twistRef, 1., 0., 0., 0., -tz, ty, 0., 1., 0., tz, 0., -tx,
103 0., 0., 1., -ty, tx, 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
104 0., 1., 0., 0., 0., 0., 0., 0., 1.);
116 MATRIX_4x4_INIT(
M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
122 MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, tz, 0., 0., -1., 0., -tx, 0., tz,
123 1., 0., 0., 0., -tx, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
124 0., -1., 0., 0., 0., 0., 1., 0., 0.);
136 MATRIX_4x4_INIT(
M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
144 twistInv_ = twist.inverse();
147 MATRIX_6x6_INIT(twistRef, 1., 0., 0., 0., tz, -ty, 0., 1., 0., -tz, 0., tx,
148 0., 0., 1., ty, -tx, 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
149 0., 1., 0., 0., 0., 0., 0., 0., 1.);
162 MATRIX_4x4_INIT(
M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
170 twistInv_ = twist.inverse();
173 MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, -tx, -0., 0., -1., 0., tz, -0., -tx,
174 1., 0., 0., -0., tz, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
175 0., -1., 0., 0., 0., 0., 1., 0., 0.);