GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/log.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 169 177 95.5%
Branches: 397 927 42.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_log_hxx__
6 #define __pinocchio_spatial_log_hxx__
7
8 namespace pinocchio
9 {
10
11 namespace internal
12 {
13 template<long i0, typename Matrix3, typename Vector3>
14 98658 void compute_theta_axis(
15 const typename Matrix3::Scalar & val,
16 const Eigen::MatrixBase<Matrix3> & R,
17 typename Matrix3::Scalar & angle,
18 const Eigen::MatrixBase<Vector3> & _axis)
19 {
20 typedef typename Matrix3::Scalar Scalar;
21
3/8
✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 18 static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
22
23 static const long i1 = (i0 + 1) % 3;
24 static const long i2 = (i0 + 2) % 3;
25 98658 Vector3 & axis = _axis.const_cast_derived();
26
27
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 98676 const Scalar s =
28 98640 math::sqrt(val + eps + eps * eps)
29 197280 * if_then_else(
30
4/8
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 98676 GE, R.coeff(i2, i1), R.coeff(i1, i2), Scalar(1.),
31
1/2
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
 98658 Scalar(-1.)); // Ensure value in sqrt is non negative and that s is non zero
32
4/8
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 98658 axis[i0] = s / Scalar(2);
33
10/20
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49329 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 9 times.
✗ Branch 29 not taken.
 98658 axis[i1] = Scalar(1) / (2 * s) * (R.coeff(i1, i0) + R.coeff(i0, i1));
34
10/20
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49329 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 9 times.
✗ Branch 29 not taken.
 98658 axis[i2] = Scalar(1) / (2 * s) * (R.coeff(i2, i0) + R.coeff(i0, i2));
35
8/16
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
 98676 const Scalar w = Scalar(1) / (2 * s) * (R.coeff(i2, i1) - R.coeff(i1, i2));
36
37
1/2
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
 98658 const Scalar axis_norm = axis.norm();
38
4/8
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 98658 angle = 2 * math::atan2(axis_norm, w);
39
1/2
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
 98658 axis /= axis_norm;
40 98658 }
41 } // namespace internal
42
43 /// \brief Renormalize a rotation matrix.
44 template<typename Matrix3>
45 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
46 17603 renormalize_rotation_matrix(const Eigen::MatrixBase<Matrix3> & R)
47 {
48 17603 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) Rout;
49
13/20
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 16460 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 16460 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 16460 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✓ Branch 11 taken 16460 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 3 times.
✓ Branch 14 taken 16460 times.
✗ Branch 15 not taken.
✓ Branch 16 taken 3 times.
✓ Branch 17 taken 16460 times.
✗ Branch 18 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 17603 Rout.col(0).noalias() = R.col(0) / R.col(0).norm();
50
13/20
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 16460 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 16460 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 16460 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✓ Branch 11 taken 16460 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 3 times.
✓ Branch 14 taken 16460 times.
✗ Branch 15 not taken.
✓ Branch 16 taken 3 times.
✓ Branch 17 taken 16460 times.
✗ Branch 18 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 17603 Rout.col(1).noalias() = R.col(1) / R.col(1).norm();
51
11/17
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 16460 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 16460 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 16460 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✓ Branch 11 taken 16460 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 3 times.
✓ Branch 14 taken 16460 times.
✗ Branch 15 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
 17603 Rout.col(2).noalias() = Rout.col(0).cross(Rout.col(1));
52
11/17
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 16460 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 16460 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 16460 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✓ Branch 11 taken 16460 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 3 times.
✓ Branch 14 taken 16460 times.
✗ Branch 15 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
 17603 Rout.col(0).noalias() = Rout.col(1).cross(Rout.col(2));
53 17603 return Rout;
54 }
55
56 /// \brief Generic evaluation of log3 function
57 template<typename _Scalar>
58 struct log3_impl
59 {
60 template<typename Matrix3Like, typename Vector3Out>
61 17563 static void run(
62 const Eigen::MatrixBase<Matrix3Like> & R,
63 typename Matrix3Like::Scalar & theta,
64 const Eigen::MatrixBase<Vector3Out> & angle_axis)
65 {
66
2/4
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
 17563 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
67
2/4
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
 17563 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, angle_axis, 3, 1);
68 using namespace internal;
69
70 typedef typename Matrix3Like::Scalar Scalar;
71 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
72
3/8
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 4 static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
73
74
4/8
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 16418 times.
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 17563 const static Scalar PI_value = PI<Scalar>();
75 17563 Vector3Out & angle_axis_ = angle_axis.const_cast_derived();
76
77 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) Matrix3;
78
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 const Matrix3 Rnormed = renormalize_rotation_matrix(R);
79
80
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 const Scalar tr = Rnormed.trace();
81
4/8
✓ 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.
 17567 const Scalar cos_value = (tr - Scalar(1)) / Scalar(2);
82
83
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 const Scalar prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
84 // Singular cases when theta == PI
85
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 Vector3 angle_axis_singular;
86
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 4 Scalar theta_singular;
87
88 {
89
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 Vector3 val_singular;
90
8/16
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16443 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 16443 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 16443 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 16443 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
 17563 val_singular.array() = 2 * Rnormed.diagonal().array() - tr + Scalar(1);
91
3/6
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16443 times.
✗ Branch 8 not taken.
 17563 Vector3 axis_0, axis_1, axis_2;
92
3/6
✓ 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.
 4 Scalar theta_0, theta_1, theta_2;
93
94
2/4
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
 17563 internal::compute_theta_axis<0>(val_singular[0], Rnormed, theta_0, axis_0);
95
2/4
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
 17563 internal::compute_theta_axis<1>(val_singular[1], Rnormed, theta_1, axis_1);
96
2/4
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
 17563 internal::compute_theta_axis<2>(val_singular[2], Rnormed, theta_2, axis_2);
97
98
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 35122 theta_singular = if_then_else(
99
3/6
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 17563 GE, val_singular[0], val_singular[1],
100
3/6
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16443 times.
✗ Branch 8 not taken.
 17563 if_then_else(GE, val_singular[0], val_singular[2], theta_0, theta_2),
101
4/8
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16440 times.
✗ Branch 11 not taken.
 17563 if_then_else(GE, val_singular[1], val_singular[2], theta_1, theta_2));
102
103
2/2
✓ Branch 0 taken 49329 times.
✓ Branch 1 taken 16443 times.
 70252 for (int k = 0; k < 3; ++k)
104
2/4
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 52689 angle_axis_singular[k] = if_then_else(
105
3/6
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 52689 GE, val_singular[0], val_singular[1],
106
5/10
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49329 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 49329 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 49329 times.
✗ Branch 14 not taken.
 52689 if_then_else(GE, val_singular[0], val_singular[2], axis_0[k], axis_2[k]),
107
6/12
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49329 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 49329 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 49329 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 49320 times.
✗ Branch 17 not taken.
 105366 if_then_else(GE, val_singular[1], val_singular[2], axis_1[k], axis_2[k]));
108 4 }
109
7/14
✓ 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.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 17567 const Scalar acos_expansion = math::sqrt(2 * (1 - cos_value) + eps * eps);
110
3/6
✓ 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.
 35138 const Scalar theta_nominal = if_then_else(
111
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 8 LE, tr, static_cast<Scalar>(Scalar(3) - prec),
112
1/2
✓ Branch 1 taken 16440 times.
✗ Branch 2 not taken.
 17559 if_then_else(
113
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 8 GE, tr, static_cast<Scalar>(Scalar(-1) + prec),
114
1/2
✓ Branch 1 taken 16440 times.
✗ Branch 2 not taken.
 17559 math::acos(cos_value), // then
115
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 8 static_cast<Scalar>(PI_value - acos_expansion) // else
116 ),
117
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 17563 static_cast<Scalar>(acos_expansion) // else
118 );
119
5/11
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16440 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 17563 assert(
120 check_expression_if_real<Scalar>(theta_nominal == theta_nominal)
121 && "theta contains some NaN"); // theta != NaN
122
123
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 Vector3 antisymmetric_R;
124
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 unSkew(Rnormed, antisymmetric_R);
125
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm();
126
127
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 35122 const Scalar t = if_then_else(
128 GE, theta_nominal, prec,
129
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 8 static_cast<Scalar>(theta_nominal / sin(theta_nominal)), // then
130
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.
 24 static_cast<Scalar>(
131
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 17575 Scalar(1.) + norm_antisymmetric_R_squared / Scalar(6)
132
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 17567 + norm_antisymmetric_R_squared * norm_antisymmetric_R_squared * Scalar(3)
133
1/2
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
 17563 / Scalar(40)) // else
134 );
135
136
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 35122 theta = if_then_else(
137
2/4
✓ Branch 1 taken 16443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 17567 GE, cos_value, static_cast<Scalar>(Scalar(-1.) + prec), theta_nominal, theta_singular);
138
139
2/2
✓ Branch 0 taken 49329 times.
✓ Branch 1 taken 16443 times.
 70252 for (int k = 0; k < 3; ++k)
140
3/6
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 52689 angle_axis_[k] = if_then_else(
141
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 24 GE, cos_value, static_cast<Scalar>(Scalar(-1.) + prec),
142
2/4
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 52701 static_cast<Scalar>(t * antisymmetric_R[k]),
143
2/4
✓ Branch 1 taken 49329 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49329 times.
✗ Branch 5 not taken.
 105378 static_cast<Scalar>(theta_singular * angle_axis_singular[k]));
144 17563 }
145 };
146
147 /// \brief Generic evaluation of Jlog3 function
148 template<typename _Scalar>
149 struct Jlog3_impl
150 {
151 template<typename Scalar, typename Vector3Like, typename Matrix3Like>
152 1159 static void run(
153 const Scalar & theta,
154 const Eigen::MatrixBase<Vector3Like> & log,
155 const Eigen::MatrixBase<Matrix3Like> & Jlog)
156 {
157
2/4
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 641 times.
✗ Branch 5 not taken.
 1159 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, log, 3, 1);
158
2/4
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 641 times.
✗ Branch 5 not taken.
 1159 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3);
159
160 using namespace internal;
161 ✗ Scalar ct, st;
162
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1159 SINCOS(theta, &st, &ct);
163
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.
 1159 const Scalar st_1mct = st / (Scalar(1) - ct);
164
1/2
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
 1159 const Scalar prec = TaylorSeriesExpansion<Scalar>::template precision<3>();
165
166
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2318 const Scalar alpha = if_then_else(
167 LT, theta, prec,
168 ✗ static_cast<Scalar>(Scalar(1) / Scalar(12) + theta * theta / Scalar(720)), // then
169
1/14
✓ Branch 1 taken 641 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.
 1159 static_cast<Scalar>(Scalar(1) / (theta * theta) - st_1mct / (Scalar(2) * theta)) // else
170 );
171
172
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2318 const Scalar diag_value = if_then_else(
173 LT, theta, prec,
174 ✗ static_cast<Scalar>(Scalar(0.5) * (2 - theta * theta / Scalar(6))), // then
175
1/6
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1159 static_cast<Scalar>(Scalar(0.5) * (theta * st_1mct)) // else
176 );
177
178 1159 Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog);
179
5/10
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 641 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 641 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 641 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 641 times.
✗ Branch 14 not taken.
 1159 Jlog_.noalias() = alpha * log * log.transpose();
180
3/6
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 641 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 641 times.
✗ Branch 8 not taken.
 1159 Jlog_.diagonal().array() += diag_value;
181
182 // Jlog += r_{\times}/2
183
2/6
✓ Branch 1 taken 641 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 641 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1159 addSkew(Scalar(0.5) * log, Jlog_);
184 1159 }
185 };
186
187 /// \brief Generic evaluation of log6 function
188 template<typename _Scalar>
189 struct log6_impl
190 {
191 template<typename Scalar, int Options, typename MotionDerived>
192 14826 static void run(const SE3Tpl<Scalar, Options> & M, MotionDense<MotionDerived> & mout)
193 {
194 typedef SE3Tpl<Scalar, Options> SE3;
195 typedef typename SE3::Vector3 Vector3;
196
197
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 typename SE3::ConstAngularRef R = M.rotation();
198
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 typename SE3::ConstLinearRef p = M.translation();
199
200 using namespace internal;
201
202
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 Vector3 antisymmetric_R;
203
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 unSkew(R, antisymmetric_R);
204
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm();
205
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Scalar theta;
207
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 const Scalar tr = R.trace();
208
1/2
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
 14826 const Vector3 w(log3(R, theta)); // t in [0,Ï€]
209 14826 const Scalar & t2 = norm_antisymmetric_R_squared;
210
211
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Scalar st, ct;
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 14826 SINCOS(theta, &st, &ct);
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 29651 const Scalar alpha = if_then_else(
214 GE, tr,
215
3/6
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 14827 static_cast<Scalar>(Scalar(3) - TaylorSeriesExpansion<Scalar>::template precision<2>()),
216
8/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 taken 1 times.
✗ Branch 23 not taken.
 2 static_cast<Scalar>(Scalar(1) - t2 / Scalar(12) - t2 * t2 / Scalar(720)), // then
217
6/12
✓ Branch 1 taken 14826 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.
 14827 static_cast<Scalar>(theta * st / (Scalar(2) * (Scalar(1) - ct))) // else
218 );
219
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 29651 const Scalar beta = if_then_else(
221 GE, tr,
222
3/6
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 14827 static_cast<Scalar>(Scalar(3) - TaylorSeriesExpansion<Scalar>::template precision<2>()),
223
6/12
✓ 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.
 2 static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)), // then
224
7/14
✓ 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.
 6 static_cast<Scalar>(
225
3/6
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 14830 Scalar(1) / (theta * theta) - st / (Scalar(2) * theta * (Scalar(1) - ct))) // else
226 );
227
228
12/24
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14826 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14826 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14826 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14826 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14826 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 14826 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 14826 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 14826 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 14826 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 14826 mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w;
229
2/4
✓ Branch 1 taken 14826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14826 times.
✗ Branch 5 not taken.
 14826 mout.angular() = w;
230 14826 }
231
232 template<typename Vector3Like, typename QuaternionLike, typename MotionDerived>
233 8075 static void run(
234 const Eigen::QuaternionBase<QuaternionLike> & quat,
235 const Eigen::MatrixBase<Vector3Like> & vec,
236 MotionDense<MotionDerived> & mout)
237 {
238
2/4
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8075 times.
✗ Branch 5 not taken.
 8075 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, vec, 3, 1);
239
240 typedef typename Vector3Like::Scalar Scalar;
241 enum
242 {
243 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options
244 };
245 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
246
1/2
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
 8075 const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
247
248 using namespace internal;
249
250
5/10
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8075 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 165 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 165 times.
✗ Branch 14 not taken.
 8240 const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));
251
252
1/2
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
 165 Scalar theta;
253
1/2
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
 8075 Vector3 w(quaternion::log3(quat, theta)); // theta nonsingular by construction
254
1/2
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
 8075 const Scalar t2 = w.squaredNorm();
255
256 // Scalar st,ct; SINCOS(theta,&st,&ct);
257
2/4
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
 165 Scalar st_2, ct_2;
258
3/6
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165 times.
✗ Branch 8 not taken.
 8075 ct_2 = pos_neg * quat.w();
259
6/12
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8075 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 165 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 165 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 165 times.
✗ Branch 17 not taken.
 8075 st_2 = math::sqrt(quat.vec().squaredNorm() + eps * eps);
260
1/2
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
 8075 const Scalar cot_th_2 = ct_2 / st_2;
261 // const Scalar cot_th_2 = ( st / (Scalar(1) - ct) ); // cotan of half angle
262
263 // we use formula (9.26) from
264 // https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf for the linear
265 // part of the Log map. A Taylor series expansion of cotan can be used up to order 4
266
2/4
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
 8075 const Scalar th_2_squared = t2 / Scalar(4); // (theta / 2) squared
267
268 // const Scalar alpha = if_then_else(LE,theta,TaylorSeriesExpansion<Scalar>::template
269 // precision<3>(),
270 // static_cast<Scalar>(Scalar(1) - t2/Scalar(12) -
271 // t2*t2/Scalar(720)), // then
272 // static_cast<Scalar>(theta * cot_th_2 /(Scalar(2)))
273 // // else
274 // );
275
276
8/16
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 165 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 165 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 165 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 165 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 165 times.
✗ Branch 23 not taken.
 8240 const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);
277
2/4
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
 16150 const Scalar beta = if_then_else(
278
1/2
✓ Branch 1 taken 7910 times.
✗ Branch 2 not taken.
 7910 LE, theta, TaylorSeriesExpansion<Scalar>::template precision<3>(),
279
1/2
✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
 330 static_cast<Scalar>(beta_alt), // then
280
6/12
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 165 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 165 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 165 times.
✗ Branch 17 not taken.
 8240 static_cast<Scalar>(Scalar(1) / t2 - cot_th_2 * Scalar(0.5) / theta) // else
281 // static_cast<Scalar>(Scalar(1) / t2 - st/(Scalar(2)*theta*(Scalar(1)-ct))) // else
282 );
283
284 // mout.linear().noalias() = alpha * vec - Scalar(0.5) * w.cross(vec) + (beta * w.dot(vec)) *
285 // w;
286
11/22
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8075 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8075 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8075 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8075 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8075 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8075 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 8075 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 8075 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 8075 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 165 times.
✗ Branch 32 not taken.
 8075 mout.linear().noalias() = vec - Scalar(0.5) * w.cross(vec) + beta * w.cross(w.cross(vec));
287
2/4
✓ Branch 1 taken 8075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8075 times.
✗ Branch 5 not taken.
 8075 mout.angular() = w;
288 8075 }
289 };
290
291 template<typename _Scalar>
292 struct Jlog6_impl
293 {
294 template<typename Scalar, int Options, typename Matrix6Like>
295 420 static void run(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Like> & Jlog)
296 {
297
2/4
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
 420 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6);
298
299 typedef SE3Tpl<Scalar, Options> SE3;
300 typedef typename SE3::Vector3 Vector3;
301 420 Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jlog);
302
303
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 typename SE3::ConstAngularRef R = M.rotation();
304
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 typename SE3::ConstLinearRef p = M.translation();
305
306 using namespace internal;
307
308 ✗ Scalar t;
309
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 Vector3 w(log3(R, t));
310
311 // value is decomposed as following:
312 // value = [ A, B;
313 // C, D ]
314 typedef Eigen::Block<Matrix6Like, 3, 3> Block33;
315
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 Block33 A = value.template topLeftCorner<3, 3>();
316
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 Block33 B = value.template topRightCorner<3, 3>();
317
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 Block33 C = value.template bottomLeftCorner<3, 3>();
318
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 Block33 D = value.template bottomRightCorner<3, 3>();
319
320
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 Jlog3(t, w, A);
321
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 D = A;
322
323
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 420 const Scalar t2 = t * t;
324
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.
 420 const Scalar tinv = Scalar(1) / t, t2inv = tinv * tinv;
325
326 ✗ Scalar st, ct;
327
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 420 SINCOS(t, &st, &ct);
328
0/12
✗ 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 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 420 const Scalar inv_2_2ct = Scalar(1) / (Scalar(2) * (Scalar(1) - ct));
329
330
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 840 const Scalar beta = if_then_else(
331
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
332 ✗ static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)), // then
333
1/6
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 420 static_cast<Scalar>(t2inv - st * tinv * inv_2_2ct) // else
334 );
335
336
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 840 const Scalar beta_dot_over_theta = if_then_else(
337
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
338 ✗ static_cast<Scalar>(Scalar(1) / Scalar(360)), // then
339 ✗ static_cast<Scalar>(
340
1/4
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 420 -Scalar(2) * t2inv * t2inv + (Scalar(1) + st * tinv) * t2inv * inv_2_2ct) // else
341 );
342
343
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 const Scalar wTp = w.dot(p);
344
2/16
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 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.
 420 const Vector3 v3_tmp(
345
2/4
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
 420 (beta_dot_over_theta * wTp) * w - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p);
346 // C can be treated as a temporary variable
347
4/8
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 374 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 374 times.
✗ Branch 11 not taken.
 420 C.noalias() = v3_tmp * w.transpose();
348
5/10
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 374 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 374 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 374 times.
✗ Branch 14 not taken.
 420 C.noalias() += beta * w * p.transpose();
349
3/8
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 374 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 420 C.diagonal().array() += wTp * beta;
350
2/6
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 420 addSkew(Scalar(.5) * p, C);
351
352
3/6
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 374 times.
✗ Branch 8 not taken.
 420 B.noalias() = C * A;
353
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 420 C.setZero();
354 420 }
355 };
356
357 } // namespace pinocchio
358
359 #endif // ifndef __pinocchio_spatial_log_hxx__
360