GCC Code Coverage Report


Directory: ./
File: unittest/symmetric.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 156 156 100.0%
Branches: 616 1218 50.6%

Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 /* --- Unitary test symmetric.cpp This code tests and compares two ways of
6 * expressing symmetric matrices. In addition to the unitary validation (test
7 * of the basic operations), the code is validating the computation
8 * performances of each methods.
9 *
10 * The three methods are:
11 * - Eigen SelfAdjoint (a mask atop of a classical dense matrix) ==> the least efficient.
12 * - Pinocchio rewritting of Metapod code with LTI factor as well and minor improvement.
13 *
14 * IMPORTANT: the following timings seems outdated.
15 * Expected time scores on a I7 2.1GHz:
16 * - Eigen: 2.5us
17 * - Pinocchio: 6us
18 */
19
20 #include "pinocchio/spatial/fwd.hpp"
21 #include "pinocchio/spatial/skew.hpp"
22 #include "pinocchio/utils/timer.hpp"
23
24 #include <boost/random.hpp>
25 #include <Eigen/Geometry>
26
27 #include "pinocchio/spatial/symmetric3.hpp"
28
29 #include <boost/test/unit_test.hpp>
30 #include <boost/utility/binary.hpp>
31
32 100000 void timeSym3(
33 const pinocchio::Symmetric3 & S,
34 const pinocchio::Symmetric3::Matrix3 & R,
35 pinocchio::Symmetric3 & res)
36 {
37
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
100000 res = S.rotate(R);
38 100000 }
39
40 #ifdef WITH_METAPOD
41
42 #include <metapod/tools/spatial/lti.hh>
43 #include <metapod/tools/spatial/rm-general.hh>
44
45 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>)
46 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>)
47
48 void timeLTI(
49 const metapod::Spatial::ltI<double> & S,
50 const metapod::Spatial::RotationMatrixTpl<double> & R,
51 metapod::Spatial::ltI<double> & res)
52 {
53 res = R.rotTSymmetricMatrix(S);
54 }
55
56 #endif
57
58 100001 void timeSelfAdj(const Eigen::Matrix3d & A, const Eigen::Matrix3d & Sdense, Eigen::Matrix3d & ASA)
59 {
60 typedef Eigen::SelfAdjointView<const Eigen::Matrix3d, Eigen::Upper> Sym3;
61
1/2
✓ Branch 1 taken 100001 times.
✗ Branch 2 not taken.
100001 Sym3 S(Sdense);
62
5/10
✓ Branch 1 taken 100001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100001 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100001 times.
✗ Branch 14 not taken.
100001 ASA.triangularView<Eigen::Upper>() = A * S * A.transpose();
63 100001 }
64
65 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
66
67 /* --- PINOCCHIO ------------------------------------------------------------ */
68 /* --- PINOCCHIO ------------------------------------------------------------ */
69 /* --- PINOCCHIO ------------------------------------------------------------ */
70
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(test_pinocchio_Sym3)
71 {
72 using namespace pinocchio;
73 typedef Symmetric3::Matrix3 Matrix3;
74 typedef Symmetric3::Vector3 Vector3;
75
76 {
77 // op(Matrix3)
78 {
79
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 M = Matrix3::Random();
80
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 M = M * M.transpose();
81
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S(M);
82
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
2 BOOST_CHECK(S.matrix().isApprox(M, 1e-12));
83 }
84
85 // S += S
86 {
87
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Symmetric3 S = Symmetric3::Random(), S2 = Symmetric3::Random();
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 Scopy = S;
89
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 S += S2;
90
11/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
2 BOOST_CHECK(S.matrix().isApprox(S2.matrix() + Scopy.matrix(), 1e-12));
91 }
92
93 // S + M
94 {
95
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::Random();
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 M = Matrix3::Random();
97
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 M = M * M.transpose();
98
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S2 = S + M;
100
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK(S2.matrix().isApprox(S.matrix() + M, 1e-12));
101
102
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 S2 = S - M;
103
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK(S2.matrix().isApprox(S.matrix() - M, 1e-12));
104 }
105
106 // S*v
107 {
108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::Random();
109
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Vector3 v = Vector3::Random();
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Vector3 Sv = S * v;
111
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
2 BOOST_CHECK(Sv.isApprox(S.matrix() * v, 1e-12));
112 }
113
114 // Random
115
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
202 for (int i = 0; i < 100; ++i)
116 {
117
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
200 Matrix3 M = Matrix3::Random();
118
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
200 M = M * M.transpose();
119
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
200 Symmetric3 S = Symmetric3::RandomPositive();
120
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
200 Vector3 v = Vector3::Random();
121
9/18
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 100 times.
200 BOOST_CHECK_GT((v.transpose() * (S * v))[0], 0);
122 }
123
124 // Identity
125 {
126
9/18
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
2 BOOST_CHECK(Symmetric3::Identity().matrix().isApprox(Matrix3::Identity(), 1e-12));
127 }
128
129 // Set diagonal
130 {
131
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S0 = Symmetric3::Zero();
132 const Symmetric3::Vector3 diag_elt =
133
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
2 (Symmetric3::Vector3::Constant(1.) + Symmetric3::Vector3::Random());
134
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 S0.setDiagonal(diag_elt);
135
136
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
2 BOOST_CHECK(S0.matrix().diagonal().isApprox(diag_elt));
137 }
138
139 // Skew2
140 {
141
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Vector3 v = Vector3::Random();
142
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 Symmetric3 vxvx = Symmetric3::SkewSquare(v);
143
144
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Vector3 p = Vector3::UnitX();
145
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK((vxvx * p).isApprox(v.cross(v.cross(p)), 1e-12));
146
147
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 p = Vector3::UnitY();
148
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK((vxvx * p).isApprox(v.cross(v.cross(p)), 1e-12));
149
150
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 p = Vector3::UnitZ();
151
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK((vxvx * p).isApprox(v.cross(v.cross(p)), 1e-12));
152
153
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Matrix3 vx = skew(v);
154
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 vxvx2 = (vx * vx).eval();
155
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
2 BOOST_CHECK(vxvx.matrix().isApprox(vxvx2, 1e-12));
156
157
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::RandomPositive();
158
11/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1 times.
2 BOOST_CHECK((S - Symmetric3::SkewSquare(v)).matrix().isApprox(S.matrix() - vxvx2, 1e-12));
159
160
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 double m = Eigen::internal::random<double>() + 1;
161
13/26
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 1 times.
✗ Branch 40 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 1 times.
2 BOOST_CHECK(
162 (S - m * Symmetric3::SkewSquare(v)).matrix().isApprox(S.matrix() - m * vxvx2, 1e-12));
163
164
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S2 = S;
165
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 S -= Symmetric3::SkewSquare(v);
166
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK(S.matrix().isApprox(S2.matrix() - vxvx2, 1e-12));
167
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 S = S2;
169
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 S -= m * Symmetric3::SkewSquare(v);
170
11/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
2 BOOST_CHECK(S.matrix().isApprox(S2.matrix() - m * vxvx2, 1e-12));
171 }
172
173 // (i,j)
174 {
175
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 M = Matrix3::Random();
176
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 M = M * M.transpose();
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S(M);
178
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
8 for (int i = 0; i < 3; ++i)
179
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
24 for (int j = 0; j < 3; ++j)
180
7/14
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 9 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 9 times.
18 BOOST_CHECK_SMALL(S(i, j) - M(i, j), Eigen::NumTraits<double>::dummy_precision());
181 }
182 }
183
184 // SRS
185 {
186
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::RandomPositive();
187
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
2 Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
188
189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 RSRt = S.rotate(R);
190
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 1 times.
2 BOOST_CHECK(RSRt.matrix().isApprox(R * S.matrix() * R.transpose(), 1e-12));
191
192
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Symmetric3 RtSR = S.rotate(R.transpose());
193
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 1 times.
2 BOOST_CHECK(RtSR.matrix().isApprox(R.transpose() * S.matrix() * R, 1e-12));
194 }
195
196 // Test operator vtiv
197 {
198
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::RandomPositive();
199
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Vector3 v = Vector3::Random();
200
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
2 double kinetic_ref = v.transpose() * S.matrix() * v;
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 double kinetic = S.vtiv(v);
202
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
2 BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
203 }
204
205 // Test v x S3
206 {
207
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::RandomPositive();
208
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Vector3 v = Vector3::Random();
209
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Matrix3 Vcross = skew(v);
210
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 Matrix3 M_ref(Vcross * S.matrix());
211
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Matrix3 M_res;
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3::vxs(v, S, M_res);
214
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(M_res.isApprox(M_ref));
215
216
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK(S.vxs(v).isApprox(M_ref));
217 }
218
219 // Test S3 vx
220 {
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::RandomPositive();
222
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Vector3 v = Vector3::Random();
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Matrix3 Vcross = skew(v);
224
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
2 Matrix3 M_ref(S.matrix() * Vcross);
225
226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Matrix3 M_res;
227
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3::svx(v, S, M_res);
228
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(M_res.isApprox(M_ref));
229
230
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK(S.svx(v).isApprox(M_ref));
231 }
232
233 // Test isZero
234 {
235
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S_not_zero = Symmetric3::Identity();
236
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(!S_not_zero.isZero());
237
238
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S_zero = Symmetric3::Zero();
239
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(S_zero.isZero());
240 }
241
242 // Test isApprox
243 {
244
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S1 = Symmetric3::RandomPositive();
245
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S2 = S1;
246
247
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(S1.isApprox(S2));
248
249
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S3 = S1;
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 S3 += S3;
251
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(!S1.isApprox(S3));
252 }
253
254 // Test inverse
255 {
256
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S1 = Symmetric3::RandomPositive();
257
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3::Matrix3 inv = S1.inverse();
258
259
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
2 BOOST_CHECK(inv.isApprox(S1.matrix().inverse()));
260 }
261
262 // Time test
263 {
264 2 const size_t NBT = 100000;
265
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 S = Symmetric3::RandomPositive();
266
267
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 std::vector<Symmetric3> Sres(NBT);
268
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 std::vector<Matrix3> Rs(NBT);
269
2/2
✓ Branch 0 taken 100000 times.
✓ Branch 1 taken 1 times.
200002 for (size_t i = 0; i < NBT; ++i)
270
4/8
✓ Branch 1 taken 100000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100000 times.
✗ Branch 11 not taken.
200000 Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
271
272
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 std::cout << "Pinocchio: ";
273
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 PinocchioTicToc timer(PinocchioTicToc::US);
274
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timer.tic();
275
2/2
✓ Branch 0 taken 100000 times.
✓ Branch 1 taken 1 times.
200002 SMOOTH(NBT)
276 {
277
1/2
✓ Branch 3 taken 100000 times.
✗ Branch 4 not taken.
200000 timeSym3(S, Rs[_smooth], Sres[_smooth]);
278 }
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timer.toc(std::cout, NBT);
280 2 }
281 2 }
282
283 /* --- EIGEN SYMMETRIC ------------------------------------------------------ */
284 /* --- EIGEN SYMMETRIC ------------------------------------------------------ */
285 /* --- EIGEN SYMMETRIC ------------------------------------------------------ */
286
287
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(test_eigen_SelfAdj)
288 {
289 using namespace pinocchio;
290 typedef Eigen::Matrix3d Matrix3;
291 typedef Eigen::SelfAdjointView<Matrix3, Eigen::Upper> Sym3;
292
293
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 M = Matrix3::Random();
294
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Sym3 S(M);
295 {
296
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Matrix3 Scp = S;
297
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
2 BOOST_CHECK((Scp - Scp.transpose()).isApprox(Matrix3::Zero(), 1e-16));
298 }
299
300
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 M2 = Matrix3::Random();
301
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 M.triangularView<Eigen::Upper>() = M2;
302
303
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
2 Matrix3 A = Matrix3::Random(), ASA1, ASA2;
304
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
2 ASA1.triangularView<Eigen::Upper>() = A * S * A.transpose();
305
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timeSelfAdj(A, M, ASA2);
306
307 {
308
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
309
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
310
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK(Masa1.isApprox(Masa2, 1e-16));
311 }
312
313 2 const size_t NBT = 100000;
314
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 std::vector<Eigen::Matrix3d> Sres(NBT);
315
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 std::vector<Eigen::Matrix3d> Rs(NBT);
316
2/2
✓ Branch 0 taken 100000 times.
✓ Branch 1 taken 1 times.
200002 for (size_t i = 0; i < NBT; ++i)
317
4/8
✓ Branch 1 taken 100000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100000 times.
✗ Branch 11 not taken.
200000 Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
318
319
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 std::cout << "Eigen: ";
320
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 PinocchioTicToc timer(PinocchioTicToc::US);
321
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timer.tic();
322
2/2
✓ Branch 0 taken 100000 times.
✓ Branch 1 taken 1 times.
200002 SMOOTH(NBT)
323 {
324
1/2
✓ Branch 3 taken 100000 times.
✗ Branch 4 not taken.
200000 timeSelfAdj(Rs[_smooth], M, Sres[_smooth]);
325 }
326
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 timer.toc(std::cout, NBT);
327 2 }
328
329
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(comparison)
330 {
331 using namespace pinocchio;
332
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 sym1(Symmetric3::Random());
333
334
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 sym2(sym1);
335
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 sym2.data() *= 2;
336
337
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK(sym2 != sym1);
338
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK(sym1 == sym1);
339 2 }
340
341
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(cast)
342 {
343 using namespace pinocchio;
344
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 Symmetric3 sym(Symmetric3::Random());
345
346
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
2 BOOST_CHECK(sym.cast<double>() == sym);
347
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
2 BOOST_CHECK(sym.cast<long double>().cast<double>() == sym);
348 2 }
349 BOOST_AUTO_TEST_SUITE_END()
350