GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/symmetric3.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 158 275 57.5%
Branches: 100 426 23.5%
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 62129 Symmetric3Tpl()
39 62129 {
40 62129 }
41
42 template<typename Sc, int Opt>
43 1622 explicit Symmetric3Tpl(const Eigen::Matrix<Sc, 3, 3, Opt> & I)
44 1622 {
45
5/10
✓ Branch 2 taken 1622 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1622 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1622 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1622 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1622 times.
 1622 assert(check_expression_if_real<Scalar>(pinocchio::isZero((I - I.transpose()))));
46 1622 m_data(0) = I(0, 0);
47 1622 m_data(1) = I(1, 0);
48 1622 m_data(2) = I(1, 1);
49 1622 m_data(3) = I(2, 0);
50 1622 m_data(4) = I(2, 1);
51 1622 m_data(5) = I(2, 2);
52 1622 }
53
54 20469 explicit Symmetric3Tpl(const Vector6 & I)
55 20469 : m_data(I)
56 {
57 20469 }
58
59 117614 Symmetric3Tpl(const Symmetric3Tpl & other)
60 117614 {
61 117614 *this = other;
62 117614 }
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 210168 Symmetric3Tpl & operator=(const Symmetric3Tpl & clone) // Copy assignment operator
76 {
77 210168 m_data = clone.m_data;
78 210168 return *this;
79 }
80
81 9614 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 9614 {
89
5/10
✓ Branch 2 taken 8260 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8260 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8260 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 8260 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8260 times.
✗ Branch 15 not taken.
 9614 m_data << a0, a1, a2, a3, a4, a5;
90 9614 }
91
92 23102 static Symmetric3Tpl Zero()
93 {
94
2/4
✓ Branch 2 taken 23102 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23102 times.
✗ Branch 6 not taken.
 23102 return Symmetric3Tpl(Vector6::Zero());
95 }
96 2201 void setZero()
97 {
98 2201 m_data.setZero();
99 2201 }
100
101 ✗ static Symmetric3Tpl Random()
102 {
103 ✗ return RandomPositive();
104 }
105 ✗ void setRandom()
106 {
107 ✗ Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
108 ✗ b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
109 ✗ c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
110 ✗ d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
111 ✗ e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
112 ✗ f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
113
114 ✗ m_data << a, b, c, d, e, f;
115 }
116
117 137 static Symmetric3Tpl Identity()
118 {
119
1/2
✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
 274 return Symmetric3Tpl(Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1));
120 }
121 ✗ void setIdentity()
122 {
123 ✗ m_data << Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1);
124 }
125
126 template<typename Vector3Like>
127 6 void setDiagonal(const Eigen::MatrixBase<Vector3Like> & diag)
128 {
129 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
130 6 m_data[0] = diag[0];
131 6 m_data[2] = diag[1];
132 6 m_data[5] = diag[2];
133 6 }
134
135 /* Required by Inertia::operator== */
136 106 bool operator==(const Symmetric3Tpl & other) const
137 {
138 106 return m_data == other.m_data;
139 }
140
141 ✗ bool operator!=(const Symmetric3Tpl & other) const
142 {
143 ✗ return !(*this == other);
144 }
145
146 43 bool isApprox(
147 const Symmetric3Tpl & other,
148 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
149 {
150 43 return m_data.isApprox(other.m_data, prec);
151 }
152
153 20 bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
154 {
155 20 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 ✗ void inverse(const Eigen::MatrixBase<Matrix3Like> & res_) const
165 {
166 ✗ Matrix3Like & res = res_.const_cast_derived();
167 ✗ const Scalar &a11 = m_data[0], a21 = m_data[1], a22 = m_data[2], a31 = m_data[3],
168 ✗ a32 = m_data[4], a33 = m_data[5];
169
170 ✗ res(0, 0) = a33 * a22 - a32 * a32;
171 ✗ res(1, 0) = res(0, 1) = -(a33 * a21 - a32 * a31);
172 ✗ res(2, 0) = res(0, 2) = a32 * a21 - a22 * a31;
173 ✗ res(1, 1) = a33 * a11 - a31 * a31;
174 ✗ res(2, 1) = res(1, 2) = -(a32 * a11 - a21 * a31);
175 ✗ res(2, 2) = a22 * a11 - a21 * a21;
176
177 ✗ const Scalar det = a11 * res(0, 0) + a21 * res(0, 1) + a31 * res(0, 2);
178 ✗ res /= det;
179 }
180
181 ✗ Matrix3 inverse() const
182 {
183 ✗ Matrix3 res;
184 ✗ inverse(res);
185 ✗ return res;
186 }
187
188 struct SkewSquare
189 {
190 const Vector3 & v;
191 65399 SkewSquare(const Vector3 & v)
192 65399 : v(v)
193 {
194 65399 }
195 30 operator Symmetric3Tpl() const
196 {
197 30 const Scalar &x = v[0], &y = v[1], &z = v[2];
198
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 60 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 ✗ Symmetric3Tpl operator-(const SkewSquare & v) const
203 {
204 ✗ const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
205 return Symmetric3Tpl(
206 ✗ 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 ✗ m_data[4] - y * z, m_data[5] + x * x + y * y);
208 }
209
210 ✗ Symmetric3Tpl & operator-=(const SkewSquare & v)
211 {
212 ✗ const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
213 ✗ m_data[0] += y * y + z * z;
214 ✗ m_data[1] -= x * y;
215 ✗ m_data[2] += x * x + z * z;
216 ✗ m_data[3] -= x * z;
217 ✗ m_data[4] -= y * z;
218 ✗ m_data[5] += x * x + y * y;
219 ✗ return *this;
220 }
221
222 struct AlphaSkewSquare
223 {
224 const Scalar & m;
225 const Vector3 & v;
226
227 65369 AlphaSkewSquare(const Scalar & m, const SkewSquare & v)
228 65369 : m(m)
229 65369 , v(v.v)
230 {
231 65369 }
232 7634 AlphaSkewSquare(const Scalar & m, const Vector3 & v)
233 7634 : m(m)
234 7634 , v(v)
235 {
236 7634 }
237
238 2 operator Symmetric3Tpl() const
239 {
240 2 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/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 -m * (x * x + y * y));
244 }
245 };
246
247 65369 friend AlphaSkewSquare operator*(const Scalar & m, const SkewSquare & sk)
248 {
249 65369 return AlphaSkewSquare(m, sk);
250 }
251
252 7628 Symmetric3Tpl operator-(const AlphaSkewSquare & v) const
253 {
254 7628 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
255 return Symmetric3Tpl(
256
1/2
✓ Branch 1 taken 7628 times.
✗ Branch 2 not taken.
 7628 m_data[0] + v.m * (y * y + z * z), m_data[1] - v.m * x * y,
257
3/6
✓ Branch 1 taken 7628 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7628 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7628 times.
✗ Branch 8 not taken.
 7628 m_data[2] + v.m * (x * x + z * z), m_data[3] - v.m * x * z, m_data[4] - v.m * y * z,
258
2/4
✓ Branch 2 taken 7628 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7628 times.
✗ Branch 6 not taken.
 22884 m_data[5] + v.m * (x * x + y * y));
259 }
260
261 77820 Symmetric3Tpl & operator-=(const AlphaSkewSquare & v)
262 {
263 77820 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
264 77820 m_data[0] += v.m * (y * y + z * z);
265 77820 m_data[1] -= v.m * x * y;
266 77820 m_data[2] += v.m * (x * x + z * z);
267 77820 m_data[3] -= v.m * x * z;
268 77820 m_data[4] -= v.m * y * z;
269 77820 m_data[5] += v.m * (x * x + y * y);
270 77820 return *this;
271 }
272
273 1995 const Vector6 & data() const
274 {
275 1995 return m_data;
276 }
277 341 Vector6 & data()
278 {
279 341 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 7539 static Symmetric3Tpl RandomPositive()
292 {
293 7539 Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
294 7539 b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
295 7539 c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
296 7539 d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
297 7539 e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
298 7539 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/2
✓ Branch 1 taken 7539 times.
✗ Branch 2 not taken.
 15078 b * d + c * e + e * f, d * d + e * e + f * f);
302 }
303
304 7810 Matrix3 matrix() const
305 {
306 7810 Matrix3 res;
307 7810 res(0, 0) = m_data(0);
308 7810 res(0, 1) = m_data(1);
309 7810 res(0, 2) = m_data(3);
310 7810 res(1, 0) = m_data(1);
311 7810 res(1, 1) = m_data(2);
312 7810 res(1, 2) = m_data(4);
313 7810 res(2, 0) = m_data(3);
314 7810 res(2, 1) = m_data(4);
315 7810 res(2, 2) = m_data(5);
316 7810 return res;
317 }
318 ✗ operator Matrix3() const
319 {
320 ✗ return matrix();
321 }
322
323 7222 Scalar vtiv(const Vector3 & v) const
324 {
325 7222 const Scalar & x = v[0];
326 7222 const Scalar & y = v[1];
327 7222 const Scalar & z = v[2];
328
329 7222 const Scalar xx = x * x;
330 7222 const Scalar xy = x * y;
331 7222 const Scalar xz = x * z;
332 7222 const Scalar yy = y * y;
333 7222 const Scalar yz = y * z;
334 7222 const Scalar zz = z * z;
335
336 7222 return m_data(0) * xx + m_data(2) * yy + m_data(5) * zz
337 7222 + 2. * (m_data(1) * xy + m_data(3) * xz + m_data(4) * yz);
338 }
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 ✗ 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 ✗ const Scalar & a = S3.data()[0];
360 ✗ const Scalar & b = S3.data()[1];
361 ✗ const Scalar & c = S3.data()[2];
362 ✗ const Scalar & d = S3.data()[3];
363 ✗ const Scalar & e = S3.data()[4];
364 ✗ const Scalar & f = S3.data()[5];
365
366 ✗ const typename Vector3::RealScalar & v0 = v[0];
367 ✗ const typename Vector3::RealScalar & v1 = v[1];
368 ✗ const typename Vector3::RealScalar & v2 = v[2];
369
370 ✗ Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
371 ✗ M_(0, 0) = d * v1 - b * v2;
372 ✗ M_(1, 0) = a * v2 - d * v0;
373 ✗ M_(2, 0) = b * v0 - a * v1;
374
375 ✗ M_(0, 1) = e * v1 - c * v2;
376 ✗ M_(1, 1) = b * v2 - e * v0;
377 ✗ M_(2, 1) = c * v0 - b * v1;
378
379 ✗ M_(0, 2) = f * v1 - e * v2;
380 ✗ M_(1, 2) = d * v2 - f * v0;
381 ✗ M_(2, 2) = e * v0 - d * v1;
382 }
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 ✗ Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
396 {
397 ✗ Matrix3 M;
398 ✗ vxs(v, *this, M);
399 ✗ 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 ✗ 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 ✗ const Scalar & a = S3.data()[0];
421 ✗ const Scalar & b = S3.data()[1];
422 ✗ const Scalar & c = S3.data()[2];
423 ✗ const Scalar & d = S3.data()[3];
424 ✗ const Scalar & e = S3.data()[4];
425 ✗ const Scalar & f = S3.data()[5];
426
427 ✗ const typename Vector3::RealScalar & v0 = v[0];
428 ✗ const typename Vector3::RealScalar & v1 = v[1];
429 ✗ const typename Vector3::RealScalar & v2 = v[2];
430
431 ✗ Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
432 ✗ M_(0, 0) = b * v2 - d * v1;
433 ✗ M_(1, 0) = c * v2 - e * v1;
434 ✗ M_(2, 0) = e * v2 - f * v1;
435
436 ✗ M_(0, 1) = d * v0 - a * v2;
437 ✗ M_(1, 1) = e * v0 - b * v2;
438 ✗ M_(2, 1) = f * v0 - d * v2;
439
440 ✗ M_(0, 2) = a * v1 - b * v0;
441 ✗ M_(1, 2) = b * v1 - c * v0;
442 ✗ M_(2, 2) = d * v1 - e * v0;
443 }
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 ✗ Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
455 {
456 ✗ Matrix3 M;
457 ✗ svx(v, *this, M);
458 ✗ return M;
459 }
460
461 ✗ Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
462 {
463 ✗ return Symmetric3Tpl(m_data + s2.m_data);
464 }
465
466 ✗ Symmetric3Tpl operator-(const Symmetric3Tpl & s2) const
467 {
468 ✗ return Symmetric3Tpl(m_data - s2.m_data);
469 }
470
471 77822 Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
472 {
473 77822 m_data += s2.m_data;
474 77822 return *this;
475 }
476
477 2 Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2)
478 {
479 2 m_data -= s2.m_data;
480 2 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 289574 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 289574 V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out, vout);
499
500 289574 vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
501 289574 vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
502 289574 vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
503 289574 }
504
505 template<typename V3>
506 513 Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
507 {
508 513 Vector3 res;
509 513 rhsMult(*this, v, res);
510 513 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 ✗ const Scalar & operator()(const int i, const int j) const
526 {
527 ✗ return ((i != 2) && (j != 2)) ? m_data[i + j] : m_data[i + j + 1];
528 }
529
530 template<typename Matrix3Like>
531 ✗ Symmetric3Tpl operator-(const Eigen::MatrixBase<Matrix3Like> & S) const
532 {
533 ✗ assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
534 return Symmetric3Tpl(
535 ✗ 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 ✗ m_data(4) - S(2, 1), m_data(5) - S(2, 2));
537 }
538
539 template<typename Matrix3Like>
540 ✗ Symmetric3Tpl operator+(const Eigen::MatrixBase<Matrix3Like> & S) const
541 {
542 ✗ assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
543 return Symmetric3Tpl(
544 ✗ 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 ✗ 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 62482 Matrix32 decomposeltI() const
553 {
554 62482 Matrix32 L;
555
10/20
✓ Branch 3 taken 62482 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 62482 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 62482 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 62482 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 62482 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 62482 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 62482 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 62482 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 62482 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 62482 times.
✗ Branch 31 not taken.
 62482 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 62482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62482 times.
✗ Branch 8 not taken.
 62482 m_data(4) + m_data(4);
557 62482 return L;
558 }
559
560 /* R*S*R' */
561 template<typename D>
562 63156 Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
563 {
564 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
565
5/10
✓ Branch 2 taken 63156 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 63156 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 63156 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 63156 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 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 63156 times.
✗ Branch 2 not taken.
 63156 Symmetric3Tpl Sres;
570
571 // 4 a
572
1/2
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
 63156 const Matrix32 L(decomposeltI());
573
574 // Y = R' L ===> (12 m + 8 a)
575
3/6
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
 63156 const Matrix2 Y(R.template block<2, 3>(1, 0) * L);
576
577 // Sres= Y R ===> (16 m + 8a)
578
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 Sres.m_data(1) = Y(0, 0) * R(0, 0) + Y(0, 1) * R(0, 1);
579
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 Sres.m_data(2) = Y(0, 0) * R(1, 0) + Y(0, 1) * R(1, 1);
580
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 Sres.m_data(3) = Y(1, 0) * R(0, 0) + Y(1, 1) * R(0, 1);
581
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 Sres.m_data(4) = Y(1, 0) * R(1, 0) + Y(1, 1) * R(1, 1);
582
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 Sres.m_data(5) = Y(1, 0) * R(2, 0) + Y(1, 1) * R(2, 1);
583
584 // r=R' v ( 6m + 3a)
585 63156 const Vector3 r(
586
8/16
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 63156 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 63156 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 63156 times.
✗ Branch 23 not taken.
 63156 -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
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 -R(2, 0) * m_data(4) + R(2, 1) * m_data(3));
588
589 // Sres_11 (3a)
590
5/10
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 63156 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 63156 times.
✗ Branch 14 not taken.
 63156 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
2/4
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
 63156 Sres.m_data(0) += m_data(5);
594
2/4
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
 63156 Sres.m_data(1) += r(2);
595
2/4
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
 63156 Sres.m_data(2) += m_data(5);
596
2/4
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
 63156 Sres.m_data(3) -= r(1);
597
2/4
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
 63156 Sres.m_data(4) += r(0);
598
2/4
✓ Branch 1 taken 63156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63156 times.
✗ Branch 5 not taken.
 63156 Sres.m_data(5) += m_data(5);
599
600 126312 return Sres;
601 }
602
603 /// \returns An expression of *this with the Scalar type casted to NewScalar.
604 template<typename NewScalar>
605 1146 Symmetric3Tpl<NewScalar, Options> cast() const
606 {
607 1146 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