GCC Code Coverage Report


Directory: ./
File: include/pinocchio/spatial/symmetric3.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 272 282 96.5%
Branches: 427 1622 26.3%

Line Branch Exec Source
1 //
2 // Copyright (c) 2014-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_symmetric3__
6 #define __pinocchio_spatial_symmetric3__
7
8 #include "pinocchio/spatial/fwd.hpp"
9
10 #include "pinocchio/math/matrix.hpp"
11
12 namespace pinocchio
13 {
14 template<typename _Scalar, int _Options>
15 struct traits<Symmetric3Tpl<_Scalar, _Options>>
16 {
17 typedef _Scalar Scalar;
18 };
19
20 template<typename _Scalar, int _Options>
21 class Symmetric3Tpl : public NumericalBase<Symmetric3Tpl<_Scalar, _Options>>
22 {
23 public:
24 typedef _Scalar Scalar;
25 enum
26 {
27 Options = _Options
28 };
29 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
30 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
31 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
32 typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
33 typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32;
34
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36
37 public:
38 308152 Symmetric3Tpl()
39 308152 {
40 308152 }
41
42 template<typename Sc, int Opt>
43 3046 explicit Symmetric3Tpl(const Eigen::Matrix<Sc, 3, 3, Opt> & I)
44 3046 {
45
5/16
✗ Branch 1 not taken.
✓ Branch 2 taken 3046 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3046 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3046 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3046 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 3046 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
3046 assert(check_expression_if_real<Scalar>(pinocchio::isZero((I - I.transpose()))));
46
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
3046 m_data(0) = I(0, 0);
47
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
3046 m_data(1) = I(1, 0);
48
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
3046 m_data(2) = I(1, 1);
49
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
3046 m_data(3) = I(2, 0);
50
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
3046 m_data(4) = I(2, 1);
51
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
3046 m_data(5) = I(2, 2);
52 3046 }
53
54 161923 explicit Symmetric3Tpl(const Vector6 & I)
55 161923 : m_data(I)
56 {
57 161923 }
58
59 1061083 Symmetric3Tpl(const Symmetric3Tpl & other)
60 1061083 {
61
1/2
✓ Branch 1 taken 4710 times.
✗ Branch 2 not taken.
1061083 *this = other;
62 1061083 }
63
64 template<typename S2, int O2>
65 explicit Symmetric3Tpl(const Symmetric3Tpl<S2, O2> & other)
66 {
67 *this = other.template cast<Scalar>();
68 }
69
70 ///
71 /// \brief Copy assignment operator.
72 ///
73 /// \param[in] other Symmetric3 to copy
74 ///
75 1278157 Symmetric3Tpl & operator=(const Symmetric3Tpl & clone) // Copy assignment operator
76 {
77 1278157 m_data = clone.m_data;
78 1278157 return *this;
79 }
80
81 336685 Symmetric3Tpl(
82 const Scalar & a0,
83 const Scalar & a1,
84 const Scalar & a2,
85 const Scalar & a3,
86 const Scalar & a4,
87 const Scalar & a5)
88 336685 {
89
11/17
✓ Branch 1 taken 234 times.
✓ Branch 2 taken 336335 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 234 times.
✓ Branch 5 taken 336335 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 234 times.
✓ Branch 8 taken 336335 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 234 times.
✓ Branch 11 taken 336335 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 234 times.
✓ Branch 14 taken 336335 times.
✗ Branch 15 not taken.
✓ Branch 16 taken 234 times.
✗ Branch 17 not taken.
336685 m_data << a0, a1, a2, a3, a4, a5;
90 336685 }
91
92 53119 static Symmetric3Tpl Zero()
93 {
94
2/4
✓ Branch 2 taken 52062 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52062 times.
✗ Branch 6 not taken.
53119 return Symmetric3Tpl(Vector6::Zero());
95 }
96 2400 void setZero()
97 {
98 2402 m_data.setZero();
99 2402 }
100
101 7 static Symmetric3Tpl Random()
102 {
103 7 return RandomPositive();
104 }
105 3 void setRandom()
106 {
107
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
3 Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
108
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
3 b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
109
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
3 c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
110
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
3 d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
111
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
3 e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
112
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
3 f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
113
114
6/12
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
3 m_data << a, b, c, d, e, f;
115 3 }
116
117 294 static Symmetric3Tpl Identity()
118 {
119
1/13
✓ Branch 1 taken 294 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
588 return Symmetric3Tpl(Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1));
120 }
121 1 void setIdentity()
122 {
123
6/28
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
1 m_data << Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1);
124 1 }
125
126 template<typename Vector3Like>
127 7 void setDiagonal(const Eigen::MatrixBase<Vector3Like> & diag)
128 {
129 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
130
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
7 m_data[0] = diag[0];
131
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
7 m_data[2] = diag[1];
132
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
7 m_data[5] = diag[2];
133 7 }
134
135 /* Required by Inertia::operator== */
136 11758 bool operator==(const Symmetric3Tpl & other) const
137 {
138 11758 return m_data == other.m_data;
139 }
140
141 1 bool operator!=(const Symmetric3Tpl & other) const
142 {
143 1 return !(*this == other);
144 }
145
146 1311 bool isApprox(
147 const Symmetric3Tpl & other,
148 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
149 {
150 1311 return m_data.isApprox(other.m_data, prec);
151 }
152
153 394 bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
154 {
155 394 return m_data.isZero(prec);
156 }
157
158 void fill(const Scalar value)
159 {
160 m_data.fill(value);
161 }
162
163 template<typename Matrix3Like>
164 2 void inverse(const Eigen::MatrixBase<Matrix3Like> & res_) const
165 {
166 2 Matrix3Like & res = res_.const_cast_derived();
167
4/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
2 const Scalar &a11 = m_data[0], a21 = m_data[1], a22 = m_data[2], a31 = m_data[3],
168
2/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
2 a32 = m_data[4], a33 = m_data[5];
169
170
1/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
2 res(0, 0) = a33 * a22 - a32 * a32;
171
2/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
2 res(1, 0) = res(0, 1) = -(a33 * a21 - a32 * a31);
172
2/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
2 res(2, 0) = res(0, 2) = a32 * a21 - a22 * a31;
173
1/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
2 res(1, 1) = a33 * a11 - a31 * a31;
174
2/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
2 res(2, 1) = res(1, 2) = -(a32 * a11 - a21 * a31);
175
1/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
2 res(2, 2) = a22 * a11 - a21 * a21;
176
177
3/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
2 const Scalar det = a11 * res(0, 0) + a21 * res(0, 1) + a31 * res(0, 2);
178
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
2 res /= det;
179 2 }
180
181 1 Matrix3 inverse() const
182 {
183 1 Matrix3 res;
184 1 inverse(res);
185 1 return res;
186 }
187
188 struct SkewSquare
189 {
190 const Vector3 & v;
191 205571 SkewSquare(const Vector3 & v)
192 205571 : v(v)
193 {
194 205571 }
195 61 operator Symmetric3Tpl() const
196 {
197 61 const Scalar &x = v[0], &y = v[1], &z = v[2];
198
1/31
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
122 return Symmetric3Tpl(-y * y - z * z, x * y, -x * x - z * z, x * z, y * z, -x * x - y * y);
199 }
200 }; // struct SkewSquare
201
202 1 Symmetric3Tpl operator-(const SkewSquare & v) const
203 {
204 1 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
205 return Symmetric3Tpl(
206
3/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 not taken.
✗ Branch 11 not taken.
1 m_data[0] + y * y + z * z, m_data[1] - x * y, m_data[2] + x * x + z * z, m_data[3] - x * z,
207
3/40
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 59 not taken.
✗ Branch 60 not taken.
3 m_data[4] - y * z, m_data[5] + x * x + y * y);
208 }
209
210 1 Symmetric3Tpl & operator-=(const SkewSquare & v)
211 {
212 1 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
213
0/8
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
1 m_data[0] += y * y + z * z;
214
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
1 m_data[1] -= x * y;
215
0/8
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
1 m_data[2] += x * x + z * z;
216
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
1 m_data[3] -= x * z;
217
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
1 m_data[4] -= y * z;
218
0/8
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
1 m_data[5] += x * x + y * y;
219 1 return *this;
220 }
221
222 struct AlphaSkewSquare
223 {
224 const Scalar & m;
225 const Vector3 & v;
226
227 205508 AlphaSkewSquare(const Scalar & m, const SkewSquare & v)
228 205508 : m(m)
229 205508 , v(v.v)
230 {
231 205508 }
232 24832 AlphaSkewSquare(const Scalar & m, const Vector3 & v)
233 24892 : m(m)
234 24892 , v(v)
235 {
236 24892 }
237
238 142 operator Symmetric3Tpl() const
239 {
240 142 const Scalar &x = v[0], &y = v[1], &z = v[2];
241 return Symmetric3Tpl(
242 -m * (y * y + z * z), m * x * y, -m * (x * x + z * z), m * x * z, m * y * z,
243
1/43
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 59 not taken.
✗ Branch 60 not taken.
✗ Branch 62 not taken.
✗ Branch 63 not taken.
284 -m * (x * x + y * y));
244 }
245 };
246
247 205508 friend AlphaSkewSquare operator*(const Scalar & m, const SkewSquare & sk)
248 {
249 205508 return AlphaSkewSquare(m, sk);
250 }
251
252 124762 Symmetric3Tpl operator-(const AlphaSkewSquare & v) const
253 {
254 124762 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
255 return Symmetric3Tpl(
256
2/4
✓ Branch 1 taken 124647 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
125057 m_data[0] + v.m * (y * y + z * z), m_data[1] - v.m * x * y,
257
3/6
✓ Branch 1 taken 124647 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 124647 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 124647 times.
✗ Branch 8 not taken.
125352 m_data[2] + v.m * (x * x + z * z), m_data[3] - v.m * x * z, m_data[4] - v.m * y * z,
258
25/50
✓ Branch 2 taken 124647 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 124647 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 234 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 234 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 234 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 234 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 234 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 234 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 234 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 234 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 234 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 234 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 234 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 234 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 234 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 234 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 234 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 234 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 234 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 234 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 234 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 234 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 234 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 234 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 234 times.
✗ Branch 75 not taken.
375171 m_data[5] + v.m * (x * x + y * y));
259 }
260
261 105490 Symmetric3Tpl & operator-=(const AlphaSkewSquare & v)
262 {
263 105490 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
264
5/10
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 98 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 98 times.
✗ Branch 15 not taken.
105490 m_data[0] += v.m * (y * y + z * z);
265
3/6
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
105490 m_data[1] -= v.m * x * y;
266
5/10
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 98 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 98 times.
✗ Branch 15 not taken.
105490 m_data[2] += v.m * (x * x + z * z);
267
3/6
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
105490 m_data[3] -= v.m * x * z;
268
3/6
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
105490 m_data[4] -= v.m * y * z;
269
5/10
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 98 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 98 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 98 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 98 times.
✗ Branch 15 not taken.
105490 m_data[5] += v.m * (x * x + y * y);
270 105490 return *this;
271 }
272
273 6867 const Vector6 & data() const
274 {
275 6867 return m_data;
276 }
277 4127 Vector6 & data()
278 {
279 4127 return m_data;
280 }
281
282 // static Symmetric3Tpl SkewSq( const Vector3 & v )
283 // {
284 // const Scalar & x = v[0], & y = v[1], & z = v[2];
285 // return Symmetric3Tpl(-y*y-z*z,
286 // x*y, -x*x-z*z,
287 // x*z, y*z, -x*x-y*y );
288 // }
289
290 /* Shoot a positive definite matrix. */
291 211351 static Symmetric3Tpl RandomPositive()
292 {
293
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
211351 Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
294
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
211351 b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
295
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
211351 c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
296
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
211351 d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
297
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
211351 e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
298
0/14
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
211351 f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
299 return Symmetric3Tpl(
300 a * a + b * b + d * d, a * b + b * c + d * e, b * b + c * c + e * e, a * d + b * e + d * f,
301
1/62
✓ Branch 1 taken 211351 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 67 not taken.
✗ Branch 68 not taken.
✗ Branch 70 not taken.
✗ Branch 71 not taken.
✗ Branch 73 not taken.
✗ Branch 74 not taken.
✗ Branch 76 not taken.
✗ Branch 77 not taken.
✗ Branch 79 not taken.
✗ Branch 80 not taken.
✗ Branch 82 not taken.
✗ Branch 83 not taken.
✗ Branch 85 not taken.
✗ Branch 86 not taken.
✗ Branch 88 not taken.
✗ Branch 89 not taken.
✗ Branch 91 not taken.
✗ Branch 92 not taken.
422702 b * d + c * e + e * f, d * d + e * e + f * f);
302 }
303
304 124750 Matrix3 matrix() const
305 {
306 124750 Matrix3 res;
307
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(0, 0) = m_data(0);
308
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(0, 1) = m_data(1);
309
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(0, 2) = m_data(3);
310
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(1, 0) = m_data(1);
311
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(1, 1) = m_data(2);
312
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(1, 2) = m_data(4);
313
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(2, 0) = m_data(3);
314
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(2, 1) = m_data(4);
315
3/6
✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 234 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 234 times.
✗ Branch 8 not taken.
124750 res(2, 2) = m_data(5);
316 124750 return res;
317 }
318 1 operator Matrix3() const
319 {
320 1 return matrix();
321 }
322
323 7524 Scalar vtiv(const Vector3 & v) const
324 {
325
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar & x = v[0];
326
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar & y = v[1];
327
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar & z = v[2];
328
329
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar xx = x * x;
330
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar xy = x * y;
331
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar xz = x * z;
332
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar yy = y * y;
333
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar yz = y * z;
334
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
7524 const Scalar zz = z * z;
335
336
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.
7526 return m_data(0) * xx + m_data(2) * yy + m_data(5) * zz
337
16/32
✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
7528 + 2. * (m_data(1) * xy + m_data(3) * xz + m_data(4) * yz);
338 1 }
339
340 ///
341 /// \brief Performs the operation \f$ M = [v]_{\times} S_{3} \f$.
342 /// This operation is equivalent to applying the cross product of v on each column of S.
343 ///
344 /// \tparam Vector3, Matrix3
345 ///
346 /// \param[in] v a vector of dimension 3.
347 /// \param[in] S3 a symmetric matrix of dimension 3x3.
348 /// \param[out] M an output matrix of dimension 3x3.
349 ///
350 template<typename Vector3, typename Matrix3>
351 264 static void vxs(
352 const Eigen::MatrixBase<Vector3> & v,
353 const Symmetric3Tpl & S3,
354 const Eigen::MatrixBase<Matrix3> & M)
355 {
356 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
357 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
358
359
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
264 const Scalar & a = S3.data()[0];
360
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
264 const Scalar & b = S3.data()[1];
361
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
264 const Scalar & c = S3.data()[2];
362
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
264 const Scalar & d = S3.data()[3];
363
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
264 const Scalar & e = S3.data()[4];
364
1/2
✓ Branch 2 taken 262 times.
✗ Branch 3 not taken.
264 const Scalar & f = S3.data()[5];
365
366
1/2
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
264 const typename Vector3::RealScalar & v0 = v[0];
367
1/2
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
264 const typename Vector3::RealScalar & v1 = v[1];
368
1/2
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
264 const typename Vector3::RealScalar & v2 = v[2];
369
370 264 Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
371
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(0, 0) = d * v1 - b * v2;
372
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(1, 0) = a * v2 - d * v0;
373
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(2, 0) = b * v0 - a * v1;
374
375
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(0, 1) = e * v1 - c * v2;
376
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(1, 1) = b * v2 - e * v0;
377
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(2, 1) = c * v0 - b * v1;
378
379
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(0, 2) = f * v1 - e * v2;
380
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(1, 2) = d * v2 - f * v0;
381
1/14
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
264 M_(2, 2) = e * v0 - d * v1;
382 264 }
383
384 ///
385 /// \brief Performs the operation \f$ [v]_{\times} S \f$.
386 /// This operation is equivalent to applying the cross product of v on each column of S.
387 ///
388 /// \tparam Vector3
389 ///
390 /// \param[in] v a vector of dimension 3.
391 ///
392 /// \returns the result \f$ [v]_{\times} S \f$.
393 ///
394 template<typename Vector3>
395 263 Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
396 {
397 263 Matrix3 M;
398
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
263 vxs(v, *this, M);
399 263 return M;
400 }
401
402 ///
403 /// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
404 ///
405 /// \tparam Vector3, Matrix3
406 ///
407 /// \param[in] v a vector of dimension 3.
408 /// \param[in] S3 a symmetric matrix of dimension 3x3.
409 /// \param[out] M an output matrix of dimension 3x3.
410 ///
411 template<typename Vector3, typename Matrix3>
412 5 static void svx(
413 const Eigen::MatrixBase<Vector3> & v,
414 const Symmetric3Tpl & S3,
415 const Eigen::MatrixBase<Matrix3> & M)
416 {
417 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
418 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
419
420
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
5 const Scalar & a = S3.data()[0];
421
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
5 const Scalar & b = S3.data()[1];
422
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
5 const Scalar & c = S3.data()[2];
423
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
5 const Scalar & d = S3.data()[3];
424
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
5 const Scalar & e = S3.data()[4];
425
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
5 const Scalar & f = S3.data()[5];
426
427
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
5 const typename Vector3::RealScalar & v0 = v[0];
428
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
5 const typename Vector3::RealScalar & v1 = v[1];
429
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
5 const typename Vector3::RealScalar & v2 = v[2];
430
431 5 Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
432
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(0, 0) = b * v2 - d * v1;
433
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(1, 0) = c * v2 - e * v1;
434
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(2, 0) = e * v2 - f * v1;
435
436
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(0, 1) = d * v0 - a * v2;
437
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(1, 1) = e * v0 - b * v2;
438
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(2, 1) = f * v0 - d * v2;
439
440
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(0, 2) = a * v1 - b * v0;
441
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(1, 2) = b * v1 - c * v0;
442
1/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
5 M_(2, 2) = d * v1 - e * v0;
443 5 }
444
445 /// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
446 ///
447 /// \tparam Vector3
448 ///
449 /// \param[in] v a vector of dimension 3.
450 ///
451 /// \returns the result \f$ S [v]_{\times} \f$.
452 ///
453 template<typename Vector3>
454 4 Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
455 {
456 4 Matrix3 M;
457
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
4 svx(v, *this, M);
458 4 return M;
459 }
460
461 100015 Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
462 {
463
2/4
✓ Branch 2 taken 100015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100015 times.
✗ Branch 6 not taken.
100015 return Symmetric3Tpl(m_data + s2.m_data);
464 }
465
466 1 Symmetric3Tpl operator-(const Symmetric3Tpl & s2) const
467 {
468
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
1 return Symmetric3Tpl(m_data - s2.m_data);
469 }
470
471 105494 Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
472 {
473 105494 m_data += s2.m_data;
474 105494 return *this;
475 }
476
477 3 Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2)
478 {
479 3 m_data -= s2.m_data;
480 3 return *this;
481 }
482
483 Symmetric3Tpl & operator*=(const Scalar s)
484 {
485 m_data *= s;
486 return *this;
487 }
488
489 template<typename V3in, typename V3out>
490 353530 static void rhsMult(
491 const Symmetric3Tpl & S3,
492 const Eigen::MatrixBase<V3in> & vin,
493 const Eigen::MatrixBase<V3out> & vout)
494 {
495 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in, Vector3);
496 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out, Vector3);
497
498 353530 V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out, vout);
499
500
12/34
✓ Branch 2 taken 886 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 886 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 886 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 886 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 886 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 886 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 886 times.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 886 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 886 times.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 886 times.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 886 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 886 times.
✗ Branch 36 not taken.
353530 vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
501
12/34
✓ Branch 2 taken 886 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 886 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 886 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 886 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 886 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 886 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 886 times.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 886 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 886 times.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 886 times.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 886 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 886 times.
✗ Branch 36 not taken.
353530 vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
502
12/34
✓ Branch 2 taken 886 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 886 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 886 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 886 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 886 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 886 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 886 times.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 886 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 886 times.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 886 times.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 886 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 886 times.
✗ Branch 36 not taken.
353530 vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
503 353530 }
504
505 template<typename V3>
506 766 Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
507 {
508 766 Vector3 res;
509
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
766 rhsMult(*this, v, res);
510 766 return res;
511 }
512
513 // Matrix3 operator*(const Matrix3 &a) const
514 // {
515 // Matrix3 r;
516 // for(unsigned int i=0; i<3; ++i)
517 // {
518 // r(0,i) = m_data(0) * a(0,i) + m_data(1) * a(1,i) + m_data(3) * a(2,i);
519 // r(1,i) = m_data(1) * a(0,i) + m_data(2) * a(1,i) + m_data(4) * a(2,i);
520 // r(2,i) = m_data(3) * a(0,i) + m_data(4) * a(1,i) + m_data(5) * a(2,i);
521 // }
522 // return r;
523 // }
524
525 1842 const Scalar & operator()(const int i, const int j) const
526 {
527
4/4
✓ Branch 0 taken 1473 times.
✓ Branch 1 taken 369 times.
✓ Branch 2 taken 982 times.
✓ Branch 3 taken 491 times.
1842 return ((i != 2) && (j != 2)) ? m_data[i + j] : m_data[i + j + 1];
528 }
529
530 template<typename Matrix3Like>
531 1 Symmetric3Tpl operator-(const Eigen::MatrixBase<Matrix3Like> & S) const
532 {
533
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 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 13 not taken.
✓ Branch 14 taken 1 times.
1 assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
534 return Symmetric3Tpl(
535
7/16
✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
1 m_data(0) - S(0, 0), m_data(1) - S(1, 0), m_data(2) - S(1, 1), m_data(3) - S(2, 0),
536
4/20
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
3 m_data(4) - S(2, 1), m_data(5) - S(2, 2));
537 }
538
539 template<typename Matrix3Like>
540 2 Symmetric3Tpl operator+(const Eigen::MatrixBase<Matrix3Like> & S) const
541 {
542
5/10
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 2 times.
2 assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
543 return Symmetric3Tpl(
544
7/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
2 m_data(0) + S(0, 0), m_data(1) + S(1, 0), m_data(2) + S(1, 1), m_data(3) + S(2, 0),
545
4/20
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
6 m_data(4) + S(2, 1), m_data(5) + S(2, 2));
546 }
547
548 /* --- Symmetric R*S*R' and R'*S*R products --- */
549 public: // private:
550 /** \brief Computes L for a symmetric matrix A.
551 */
552 206168 Matrix32 decomposeltI() const
553 {
554 206168 Matrix32 L;
555
27/44
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 205919 times.
✓ Branch 4 taken 126 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 205919 times.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 205919 times.
✓ Branch 10 taken 126 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 205919 times.
✓ Branch 13 taken 126 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 205919 times.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 205919 times.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 21 taken 205919 times.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 205919 times.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
✓ Branch 27 taken 205919 times.
✓ Branch 28 taken 126 times.
✗ Branch 29 not taken.
✓ Branch 30 taken 205919 times.
✓ Branch 31 taken 126 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 126 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 126 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 126 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 126 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 126 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 126 times.
✗ Branch 50 not taken.
206357 L << m_data(0) - m_data(5), m_data(1), m_data(1), m_data(2) - m_data(5), 2 * m_data(3),
556
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
206168 m_data(4) + m_data(4);
557 206168 return L;
558 }
559
560 /* R*S*R' */
561 template<typename D>
562 306229 Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
563 {
564 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
565
12/22
✓ Branch 1 taken 126 times.
✓ Branch 2 taken 205919 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 126 times.
✓ Branch 5 taken 205919 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 126 times.
✓ Branch 8 taken 205919 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 126 times.
✓ Branch 11 taken 205919 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 125 times.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 125 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
306229 assert(
566 check_expression_if_real<Scalar>(isUnitary(R.transpose() * R))
567 && "R is not a Unitary matrix");
568
569
1/2
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
306229 Symmetric3Tpl Sres;
570
571 // 4 a
572
1/2
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
306229 const Matrix32 L(decomposeltI());
573
574 // Y = R' L ===> (12 m + 8 a)
575
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
306229 const Matrix2 Y(R.template block<2, 3>(1, 0) * L);
576
577 // Sres= Y R ===> (16 m + 8a)
578
9/18
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
306229 Sres.m_data(1) = Y(0, 0) * R(0, 0) + Y(0, 1) * R(0, 1);
579
9/18
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
306229 Sres.m_data(2) = Y(0, 0) * R(1, 0) + Y(0, 1) * R(1, 1);
580
9/18
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
306229 Sres.m_data(3) = Y(1, 0) * R(0, 0) + Y(1, 1) * R(0, 1);
581
9/18
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
306229 Sres.m_data(4) = Y(1, 0) * R(1, 0) + Y(1, 1) * R(1, 1);
582
9/18
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
306229 Sres.m_data(5) = Y(1, 0) * R(2, 0) + Y(1, 1) * R(2, 1);
583
584 // r=R' v ( 6m + 3a)
585
7/26
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 126 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 126 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
306796 const Vector3 r(
586
12/24
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 206045 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 206045 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 206045 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 126 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 126 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 126 times.
✗ Branch 35 not taken.
306418 -R(0, 0) * m_data(4) + R(0, 1) * m_data(3), -R(1, 0) * m_data(4) + R(1, 1) * m_data(3),
587
6/12
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
306229 -R(2, 0) * m_data(4) + R(2, 1) * m_data(3));
588
589 // Sres_11 (3a)
590
9/18
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206045 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206045 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206045 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 126 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 126 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 126 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 126 times.
✗ Branch 26 not taken.
306229 Sres.m_data(0) = L(0, 0) + L(1, 1) - Sres.m_data(2) - Sres.m_data(5);
591
592 // Sres + D + (Ev)x ( 9a)
593
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
306229 Sres.m_data(0) += m_data(5);
594
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
306229 Sres.m_data(1) += r(2);
595
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
306229 Sres.m_data(2) += m_data(5);
596
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
306229 Sres.m_data(3) -= r(1);
597
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
306229 Sres.m_data(4) += r(0);
598
3/6
✓ Branch 1 taken 206045 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206045 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
306229 Sres.m_data(5) += m_data(5);
599
600 612458 return Sres;
601 189 }
602
603 /// \returns An expression of *this with the Scalar type casted to NewScalar.
604 template<typename NewScalar>
605 6976 Symmetric3Tpl<NewScalar, Options> cast() const
606 {
607
2/4
✓ Branch 2 taken 1261 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1261 times.
✗ Branch 6 not taken.
6976 return Symmetric3Tpl<NewScalar, Options>(m_data.template cast<NewScalar>());
608 }
609
610 friend std::ostream & operator<<(std::ostream & os, const Symmetric3Tpl<Scalar, Options> & S3)
611 {
612 os << "m_data: " << S3.m_data.transpose() << "\n";
613 return os;
614 }
615
616 // TODO: adjust code
617 // bool isValid() const
618 // {
619 // return
620 // m_data(0) >= Scalar(0)
621 // && m_data(2) >= Scalar(0)
622 // && m_data(5) >= Scalar(0);
623 // }
624
625 protected:
626 Vector6 m_data;
627
628 }; // class Symmetric3Tpl
629
630 } // namespace pinocchio
631
632 #endif // ifndef __pinocchio_spatial_symmetric3__
633