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 |