| 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 |