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 |