Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/math/matrix-twist.cpp	Lines:	74	74	100.0 %
Date:	2023-03-13 12:09:37	Branches:	596	1154	51.6 %


// Copyright 2011 Thomas Moulard.

#include <sstream>

#define BOOST_TEST_MODULE matrix_twist

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/output_test_stream.hpp>
#include <boost/test/unit_test.hpp>
#include <sot/core/matrix-geometry.hh>

using boost::test_tools::output_test_stream;

#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
  for (unsigned i = 0; i < N; ++i)                               \
    for (unsigned j = 0; j < M; ++j)                             \
  BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)

#define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
  MATRIX_BOOST_REQUIRE_CLOSE(6, 6, LEFT, RIGHT, TOLERANCE)

#define MATRIX_4x4_INIT(M, A00, A01, A02, A03, A10, A11, A12, A13, A20, A21, \
                        A22, A23, A30, A31, A32, A33)                        \
  M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03;                \
  M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13;                \
  M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23;                \
  M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33

#define MATRIX_6x6_INIT(M, A00, A01, A02, A03, A04, A05, A10, A11, A12, A13,   \
                        A14, A15, A20, A21, A22, A23, A24, A25, A30, A31, A32, \
                        A33, A34, A35, A40, A41, A42, A43, A44, A45, A50, A51, \
                        A52, A53, A54, A55)                                    \
  M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03, M(0, 4) = A04,   \
       M(0, 5) = A05;                                                          \
  M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13, M(1, 4) = A14,   \
       M(1, 5) = A15;                                                          \
  M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23, M(2, 4) = A24,   \
       M(2, 5) = A25;                                                          \
  M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33, M(3, 4) = A34,   \
       M(3, 5) = A35;                                                          \
  M(4, 0) = A40, M(4, 1) = A41, M(4, 2) = A42, M(4, 3) = A43, M(4, 4) = A44,   \
       M(4, 5) = A45;                                                          \
  M(5, 0) = A50, M(5, 1) = A51, M(5, 2) = A52, M(5, 3) = A53, M(5, 4) = A54,   \
       M(5, 5) = A55

// For reminder:
//
// R \in R^3 x R^3
// M = [R ; t] \in R^4 x R^4
//     [0 ; 1]
//
// Twist(M) = [R ; t^ R] \in R^6 x R^6
//            [0 ; R]
//
// t^ R \in R^3xR^3
//
//
// Twist(M)^{-1} = [R^T ; -R^T t^]
//                 [0   ; R^T    ]

BOOST_AUTO_TEST_CASE(constructor_trivial_identity) {
  dynamicgraph::sot::MatrixHomogeneous M(Eigen::Matrix4d::Identity());
  dynamicgraph::sot::MatrixTwist twist;
  dynamicgraph::sot::buildFrom(M, twist);

  dynamicgraph::Matrix twistRef(6, 6);

  for (unsigned i = 0; i < 6; ++i)
    for (unsigned j = 0; j < 6; ++j) twistRef(i, j) = (i == j) ? 1. : 0.;

  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}

BOOST_AUTO_TEST_CASE(constructor_rotation_only) {
  dynamicgraph::sot::MatrixHomogeneous M;

  MATRIX_4x4_INIT(M, 0., 0., 1., 0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0.,
                  0., 1.);
  dynamicgraph::sot::MatrixTwist twist;
  dynamicgraph::sot::buildFrom(M, twist);
  dynamicgraph::Matrix twistRef(6, 6);
  MATRIX_6x6_INIT(twistRef, 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
                  -1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 1.,
                  0., 0., 0., 0., 0., 0., -1., 0.);

  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}

BOOST_AUTO_TEST_CASE(constructor_translation_only) {
  dynamicgraph::sot::MatrixHomogeneous M;

  double tx = 11.;
  double ty = 22.;
  double tz = 33.;

  MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
                  1.);
  dynamicgraph::sot::MatrixTwist twist;
  dynamicgraph::sot::buildFrom(M, twist);

  dynamicgraph::Matrix twistRef(6, 6);
  MATRIX_6x6_INIT(twistRef, 1., 0., 0., 0., -tz, ty, 0., 1., 0., tz, 0., -tx,
                  0., 0., 1., -ty, tx, 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
                  0., 1., 0., 0., 0., 0., 0., 0., 1.);

  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}

BOOST_AUTO_TEST_CASE(constructor_rotation_translation) {
  dynamicgraph::sot::MatrixHomogeneous M;

  double tx = 11.;
  double ty = 22.;
  double tz = 33.;

  MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
                  0., 1.);
  dynamicgraph::sot::MatrixTwist twist;
  dynamicgraph::sot::buildFrom(M, twist);

  dynamicgraph::Matrix twistRef(6, 6);
  MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, tz, 0., 0., -1., 0., -tx, 0., tz,
                  1., 0., 0., 0., -tx, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
                  0., -1., 0., 0., 0., 0., 1., 0., 0.);

  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}

BOOST_AUTO_TEST_CASE(inverse_translation_only) {
  dynamicgraph::sot::MatrixHomogeneous M;

  double tx = 11.;
  double ty = 22.;
  double tz = 33.;

  MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
                  1.);
  dynamicgraph::sot::MatrixTwist twist;
  dynamicgraph::sot::buildFrom(M, twist);

  dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());

  dynamicgraph::sot::MatrixTwist twistInv_;
  twistInv_ = twist.inverse();

  dynamicgraph::Matrix twistRef(6, 6);
  MATRIX_6x6_INIT(twistRef, 1., 0., 0., 0., tz, -ty, 0., 1., 0., -tz, 0., tx,
                  0., 0., 1., ty, -tx, 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
                  0., 1., 0., 0., 0., 0., 0., 0., 1.);

  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
}

BOOST_AUTO_TEST_CASE(inverse_translation_rotation) {
  dynamicgraph::sot::MatrixHomogeneous M;

  double tx = 11.;
  double ty = 22.;
  double tz = 33.;

  MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
                  0., 1.);
  dynamicgraph::sot::MatrixTwist twist;
  dynamicgraph::sot::buildFrom(M, twist);

  dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());

  dynamicgraph::sot::MatrixTwist twistInv_;
  twistInv_ = twist.inverse();

  dynamicgraph::Matrix twistRef(6, 6);
  MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, -tx, -0., 0., -1., 0., tz, -0., -tx,
                  1., 0., 0., -0., tz, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
                  0., -1., 0., 0., 0., 0., 1., 0., 0.);

  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			// Copyright 2011 Thomas Moulard.
2
3			#include <sstream>
4
5			#define BOOST_TEST_MODULE matrix_twist
6
7			#include <boost/test/floating_point_comparison.hpp>
8			#include <boost/test/output_test_stream.hpp>
9			#include <boost/test/unit_test.hpp>
10			#include <sot/core/matrix-geometry.hh>
11
12			using boost::test_tools::output_test_stream;
13
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)
18
19			#define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
20			MATRIX_BOOST_REQUIRE_CLOSE(6, 6, LEFT, RIGHT, TOLERANCE)
21
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
28
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, \
32			A52, A53, A54, A55) \
33			M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03, M(0, 4) = A04, \
34			M(0, 5) = A05; \
35			M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13, M(1, 4) = A14, \
36			M(1, 5) = A15; \
37			M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23, M(2, 4) = A24, \
38			M(2, 5) = A25; \
39			M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33, M(3, 4) = A34, \
40			M(3, 5) = A35; \
41			M(4, 0) = A40, M(4, 1) = A41, M(4, 2) = A42, M(4, 3) = A43, M(4, 4) = A44, \
42			M(4, 5) = A45; \
43			M(5, 0) = A50, M(5, 1) = A51, M(5, 2) = A52, M(5, 3) = A53, M(5, 4) = A54, \
44			M(5, 5) = A55
45
46			// For reminder:
47			//
48			// R \in R^3 x R^3
49			// M = [R ; t] \in R^4 x R^4
50			// [0 ; 1]
51			//
52			// Twist(M) = [R ; t^ R] \in R^6 x R^6
53			// [0 ; R]
54			//
55			// t^ R \in R^3xR^3
56			//
57			//
58			// Twist(M)^{-1} = [R^T ; -R^T t^]
59			// [0 ; R^T ]
60
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(constructor_trivial_identity) {
62	✓✗✓✗	2	dynamicgraph::sot::MatrixHomogeneous M(Eigen::Matrix4d::Identity());
63	✓✗	2	dynamicgraph::sot::MatrixTwist twist;
64	✓✗	2	dynamicgraph::sot::buildFrom(M, twist);
65
66	✓✗	4	dynamicgraph::Matrix twistRef(6, 6);
67
68	✓✓	14	for (unsigned i = 0; i < 6; ++i)
69	✓✓✓✓ ✓✗	84	for (unsigned j = 0; j < 6; ++j) twistRef(i, j) = (i == j) ? 1. : 0.;
70
71	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
72		2	}
73
74	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(constructor_rotation_only) {
75	✓✗	2	dynamicgraph::sot::MatrixHomogeneous M;
76
77	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	MATRIX_4x4_INIT(M, 0., 0., 1., 0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0.,
78			0., 1.);
79	✓✗	2	dynamicgraph::sot::MatrixTwist twist;
80	✓✗	2	dynamicgraph::sot::buildFrom(M, twist);
81	✓✗	4	dynamicgraph::Matrix twistRef(6, 6);
82	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	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.);
85
86	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
87		2	}
88
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(constructor_translation_only) {
90	✓✗	2	dynamicgraph::sot::MatrixHomogeneous M;
91
92		2	double tx = 11.;
93		2	double ty = 22.;
94		2	double tz = 33.;
95
96	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
97			1.);
98	✓✗	2	dynamicgraph::sot::MatrixTwist twist;
99	✓✗	2	dynamicgraph::sot::buildFrom(M, twist);
100
101	✓✗	4	dynamicgraph::Matrix twistRef(6, 6);
102	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	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.);
105
106	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
107		2	}
108
109	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(constructor_rotation_translation) {
110	✓✗	2	dynamicgraph::sot::MatrixHomogeneous M;
111
112		2	double tx = 11.;
113		2	double ty = 22.;
114		2	double tz = 33.;
115
116	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
117			0., 1.);
118	✓✗	2	dynamicgraph::sot::MatrixTwist twist;
119	✓✗	2	dynamicgraph::sot::buildFrom(M, twist);
120
121	✓✗	4	dynamicgraph::Matrix twistRef(6, 6);
122	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	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.);
125
126	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
127		2	}
128
129	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(inverse_translation_only) {
130	✓✗	2	dynamicgraph::sot::MatrixHomogeneous M;
131
132		2	double tx = 11.;
133		2	double ty = 22.;
134		2	double tz = 33.;
135
136	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
137			1.);
138	✓✗	2	dynamicgraph::sot::MatrixTwist twist;
139	✓✗	2	dynamicgraph::sot::buildFrom(M, twist);
140
141	✓✗✓✗	2	dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());
142
143	✓✗	2	dynamicgraph::sot::MatrixTwist twistInv_;
144	✓✗✓✗	2	twistInv_ = twist.inverse();
145
146	✓✗	4	dynamicgraph::Matrix twistRef(6, 6);
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	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.);
150
151	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
152	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
153		2	}
154
155	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(inverse_translation_rotation) {
156	✓✗	2	dynamicgraph::sot::MatrixHomogeneous M;
157
158		2	double tx = 11.;
159		2	double ty = 22.;
160		2	double tz = 33.;
161
162	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
163			0., 1.);
164	✓✗	2	dynamicgraph::sot::MatrixTwist twist;
165	✓✗	2	dynamicgraph::sot::buildFrom(M, twist);
166
167	✓✗✓✗	2	dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());
168
169	✓✗	2	dynamicgraph::sot::MatrixTwist twistInv_;
170	✓✗✓✗	2	twistInv_ = twist.inverse();
171
172	✓✗	4	dynamicgraph::Matrix twistRef(6, 6);
173	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	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.);
176
177	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
178	✓✓✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	86	MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
179		2	}